#!/bin/sh
# @name Generic Differential Rover
# @type Rover
# @class Rover

. ${R}etc/init.d/rc.rover_differential_defaults

PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_differential}

param set-default SIM_GZ_EN 1 # Gazebo bridge

param set-default NAV_ACC_RAD 0.5

# Differential Parameters
param set-default RD_WHEEL_TRACK 0.6
param set-default RD_TRANS_DRV_TRN 0.785398
param set-default RD_TRANS_TRN_DRV 0.174533
param set-default RD_YAW_STK_GAIN 0.6

# Rate Control Parameters
param set-default RO_YAW_RATE_I 0.01
param set-default RO_YAW_RATE_P 0.25
param set-default RO_YAW_RATE_LIM 250
param set-default RO_YAW_ACCEL_LIM 400
param set-default RO_YAW_DECEL_LIM 800
param set-default RO_YAW_RATE_CORR 1
param set-default RO_YAW_EXPO 0.85
param set-default RO_YAW_SUPEXPO 0.3

# Attitude Control Parameters
param set-default RO_YAW_P 5

# Velocity Control Parameters
param set-default RO_ACCEL_LIM 4
param set-default RO_DECEL_LIM 6
param set-default RO_JERK_LIM 10
param set-default RO_MAX_THR_SPEED 2
param set-default RO_SPEED_LIM 1.8
param set-default RO_SPEED_I 0.01
param set-default RO_SPEED_P 0.01
param set-default RO_SPEED_RED 1

# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
param set-default PP_LOOKAHD_MAX 10
param set-default PP_LOOKAHD_MIN 1

# Actuator mapping
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
param set-default SIM_GZ_WH_MIN1 80
param set-default SIM_GZ_WH_MAX1 120
param set-default SIM_GZ_WH_DIS1 100

param set-default SIM_GZ_WH_FUNC2 102 # left wheel
param set-default SIM_GZ_WH_MIN2 80
param set-default SIM_GZ_WH_MAX2 120
param set-default SIM_GZ_WH_DIS2 100