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

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

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

param set-default SIM_GZ_EN 1 # Gazebo bridge

param set-default NAV_ACC_RAD 0.5

# Ackermann Parameters
param set-default RA_WHEEL_BASE 0.5
param set-default RA_ACC_RAD_GAIN 2
param set-default RA_ACC_RAD_MAX 1.5
param set-default RA_MAX_STR_ANG 0.5236
param set-default RA_STR_RATE_LIM 360

# 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 130
param set-default RO_YAW_ACCEL_LIM 800
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 3

# 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 3.1
param set-default RO_SPEED_LIM 2.5
param set-default RO_SPEED_I 0.01
param set-default RO_SPEED_P 1
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

# Wheels
param set-default SIM_GZ_WH_FUNC1 101
param set-default SIM_GZ_WH_MIN1 70
param set-default SIM_GZ_WH_MAX1 130
param set-default SIM_GZ_WH_DIS1 100

# Steering
param set-default SIM_GZ_SV_MAXA1 30
param set-default SIM_GZ_SV_MINA1 -30
param set-default CA_SV_CS_COUNT 1
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_REV 1