#!/bin/sh
# @name Gazebo - Zero Turn Lawnmower
# @type Rover
# @class Rover

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

PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=lawn}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}

param set-default SIM_GZ_EN 1 # Gazebo bridge

param set-default NAV_ACC_RAD 0.5

# We can arm and drive in manual mode when it slides and GPS check fails:
param set-default COM_ARM_WO_GPS 1

# Differential Parameters
param set-default RD_WHEEL_TRACK 0.9
param set-default RD_TRANS_DRV_TRN 0.349066
param set-default RD_TRANS_TRN_DRV 0.174533

# Rover Control Parameters
param set-default RO_ACCEL_LIM 1.5
param set-default RO_DECEL_LIM 5
param set-default RO_JERK_LIM 15
param set-default RO_MAX_THR_SPEED 2.7

# Rover Rate Control Parameters
param set-default RO_YAW_RATE_I 0.2
param set-default RO_YAW_RATE_P 1.0
param set-default RO_YAW_RATE_LIM 60
param set-default RO_YAW_ACCEL_LIM 50
param set-default RO_YAW_DECEL_LIM 1000
param set-default RO_YAW_RATE_CORR 1.0

# Rover Attitude Control Parameters
param set-default RO_YAW_P 5

# Rover Velocity Control Parameters
param set-default RO_SPEED_LIM 2.1
param set-default RO_SPEED_I 0.01
param set-default RO_SPEED_P 0.1
param set-defatul RO_SPEED_RED 1.0

# 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 - set SITL motors/servos output parameters:

# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
param set-default SIM_GZ_WH_MIN1 87
param set-default SIM_GZ_WH_MAX1 113
param set-default SIM_GZ_WH_DIS1 100
param set-default SIM_GZ_WH_FAIL1 100

param set-default SIM_GZ_WH_FUNC2 102 # left wheel
param set-default SIM_GZ_WH_MIN2 87
param set-default SIM_GZ_WH_MAX2 113
param set-default SIM_GZ_WH_DIS2 100
param set-default SIM_GZ_WH_FAIL2 100

param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels

# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
# controls in practical scenarios.

# Cutter deck blades clutch, PCA9685 servo channel 3,  "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_MIN1 0
param set-default SIM_GZ_SV_MAX1 1000
param set-default SIM_GZ_SV_DIS1 500
param set-default SIM_GZ_SV_FAIL1 500
param set-default SIM_GZ_SV_MAXA1 90
param set-default SIM_GZ_SV_MINA1 -90

# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
#    - on minimum when disarmed or failed:
param set-default SIM_GZ_SV_FUNC2 202
param set-default SIM_GZ_SV_MIN2 0
param set-default SIM_GZ_SV_MAX2 1000
param set-default SIM_GZ_SV_DIS2 500
param set-default SIM_GZ_SV_FAIL2 500
param set-default SIM_GZ_SV_MAXA2 90
param set-default SIM_GZ_SV_MINA2 -90

param set-default CA_SV_CS_COUNT 2

# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:

# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
#param set-default SIM_GZ_SV_FUNC5 205
#param set-default SIM_GZ_SV_MIN5 1000
#param set-default SIM_GZ_SV_MAX5 2000
#param set-default SIM_GZ_SV_DIS5 1000
#param set-default SIM_GZ_SV_FAIL5 1000

# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
#param set-default SIM_GZ_SV_FUNC6 206

# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
#param set-default SIM_GZ_SV_FUNC7 207

# Spare PCA9685 servo channel 8 - "Servo 8" (208):
#param set-default SIM_GZ_SV_FUNC8 208