differential: restructure and update module

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -6,16 +6,36 @@
 . ${R}etc/init.d/rc.rover_differential_defaults
 
 PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
+# Rover parameters
+param set-default RD_WHEEL_TRACK 0.3
+param set-default RD_MAN_YAW_SCALE 0.1
+param set-default RD_YAW_RATE_I 0.1
+param set-default RD_YAW_RATE_P 5
+param set-default RD_MAX_ACCEL 6
+param set-default RD_MAX_JERK 30
+param set-default RD_MAX_SPEED 7
+param set-default RD_HEADING_P 5
+param set-default RD_HEADING_I 0.1
+param set-default RD_MAX_YAW_RATE 180
+param set-default RD_MISS_SPD_DEF 7
+param set-default RD_TRANS_DRV_TRN 0.349066
+param set-default RD_TRANS_TRN_DRV 0.174533
+
+# Pure pursuit parameters
+param set-default PP_LOOKAHD_MAX 30
+param set-default PP_LOOKAHD_MIN 2
+param set-default PP_LOOKAHD_GAIN 1
+
 # Simulated sensors
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 0
 param set-default SENS_EN_MAGSIM 1
-param set-default SENS_EN_ARSPDSIM 1
+param set-default SENS_EN_ARSPDSIM 0
 
 # Actuator mapping
 param set-default SIM_GZ_WH_FUNC1 101 # right wheel

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -20,9 +20,9 @@ param set-default SENS_EN_ARSPDSIM 1
 param set-default COM_ARM_WO_GPS 1
 
 # Set Differential Drive Kinematics Library parameters:
-param set RDD_WHEEL_BASE 0.9
-param set RDD_WHEEL_RADIUS 0.22
-param set RDD_WHEEL_SPEED 10.0	# Maximum wheel speed rad/s, approx 8 km/h
+param set RD_WHEEL_BASE 0.9
+param set RD_WHEEL_RADIUS 0.22
+param set RD_WHEEL_SPEED 10.0	# Maximum wheel speed rad/s, approx 8 km/h
 
 # Actuator mapping - set SITL motors/servos output parameters:
 
@@ -41,9 +41,9 @@ param set-default SIM_GZ_WH_FUNC2 102 # left wheel
 
 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 
+# 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):

ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults

@@ -3,9 +3,8 @@
 
 set VEHICLE_TYPE rover_differential
 
-param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
-
-param set-default CA_AIRFRAME 6 # Rover (Differential)
-param set-default CA_R_REV 3 # Right and left motors reversible
-
-param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
+param set-default MAV_TYPE 10     # MAV_TYPE_GROUND_ROVER
+param set-default CA_AIRFRAME 6   # Rover (Differential)
+param set-default CA_R_REV 3 	  # Right and left motors reversible
+param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
+param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying

boards/px4/fmu-v5/rover.px4board

@@ -19,3 +19,4 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v5x/rover.px4board

@@ -16,3 +16,4 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6c/rover.px4board

@@ -15,3 +15,4 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6u/rover.px4board

@@ -15,3 +15,4 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6x/rover.px4board

@@ -15,3 +15,4 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -16,3 +16,4 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_DRIVERS_ROBOCLAW=y

msg/CMakeLists.txt

@@ -180,6 +180,8 @@ set(msg_files
 	RegisterExtComponentRequest.msg
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
+	RoverDifferentialGuidanceStatus.msg
+	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg

msg/RoverDifferentialGuidanceStatus.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 desired_speed 	      # [m/s] Desired forward speed for the rover
+float32 lookahead_distance    # [m] Lookahead distance of pure the pursuit controller
+float32 heading_error_deg 	      # [deg] Heading error of the pure pursuit controller
+float32 pid_heading_integral  # Integral of the PID for the desired yaw rate during missions
+float32 pid_throttle_integral # Integral of the PID for the throttle during missions
+uint8   state_machine         # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED]
+
+# TOPICS rover_differential_guidance_status

msg/RoverDifferentialStatus.msg

@@ -0,0 +1,8 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 actual_speed	       # [m/s] Actual forward speed of the rover
+float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate
+float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
+float32 pid_yaw_rate_integral  # Integral of the PID for the desired yaw rate controller
+
+# TOPICS rover_differential_status

src/modules/logger/logged_topics.cpp

@@ -103,6 +103,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("radio_status");
 	add_optional_topic("rover_ackermann_guidance_status", 100);
 	add_optional_topic("rover_ackermann_status", 100);
+	add_optional_topic("rover_differential_guidance_status", 100);
+	add_optional_topic("rover_differential_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

src/modules/rover_differential/CMakeLists.txt

@@ -31,9 +31,7 @@
 #
 ############################################################################
 
-add_subdirectory(RoverDifferentialControl)
 add_subdirectory(RoverDifferentialGuidance)
-add_subdirectory(RoverDifferentialKinematics)
 
 px4_add_module(
 	MODULE modules__rover_differential
@@ -42,11 +40,10 @@ px4_add_module(
 		RoverDifferential.cpp
 		RoverDifferential.hpp
 	DEPENDS
-		RoverDifferentialControl
 		RoverDifferentialGuidance
-		RoverDifferentialKinematics
 		px4_work_queue
 		modules__control_allocator # for parameter CA_R_REV
+		pure_pursuit
 	MODULE_CONFIG
 		module.yaml
 )