Differential Rover: PR fixes

ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover

@@ -3,7 +3,7 @@
 # @name Rover
 #
 
-. ${R}etc/init.d/rc.rover_ackermann_defaults
+. ${R}etc/init.d/rc.rover_defaults
 
 param set-default GND_L1_DIST 5
 param set-default GND_L1_PERIOD 10

ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover

@@ -6,29 +6,7 @@
 
 . ${R}etc/init.d/rc.rover_differential_defaults
 
-param set-default GND_L1_DIST 5
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_SPEED_D 3
-param set-default GND_SPEED_I 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_THR_SC 1
-param set-default GND_SPEED_TRIM 4
-param set-default GND_THR_CRUISE 0.3
-param set-default GND_THR_MAX 0.5
-param set-default GND_THR_MIN 0
-
-param set-default NAV_ACC_RAD 0.5
-param set-default NAV_LOITER_RAD 2
-
-param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
-
-param set-default CA_AIRFRAME 6
-
-param set-default CA_R_REV 3
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 101
 param set-default PWM_MAIN_FUNC6 102
 param set-default PWM_MAIN_FUNC7 102
-

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

@@ -1,5 +1,4 @@
 #!/bin/sh
-#
 # @name Aion Robotics R1 Rover
 # @type Rover
 # @class Rover
@@ -10,39 +9,23 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
 
-param set-default SIM_GZ_EN 1
+param set-default SIM_GZ_EN 1 # Gazebo bridge
 
+# 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 COM_PREARM_MODE 2
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
 
-param set-default NAV_ACC_RAD 0.5
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
-param set-default NAV_LOITER_RAD 2
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
 
-param set-default GND_MAX_ANG 0.6
+param set-default SIM_GZ_WH_REV 1 # reverse right wheel
-param set-default GND_WHEEL_BASE 2.0
-
-param set-default CA_R_REV 3
-
-param set-default SIM_GZ_EC_FUNC1 101
-param set-default SIM_GZ_EC_MIN1 0
-param set-default SIM_GZ_EC_MAX1 200
-param set-default SIM_GZ_EC_DIS1 100
-
-param set-default SIM_GZ_EC_FUNC2 102
-param set-default SIM_GZ_EC_MIN2 0
-param set-default SIM_GZ_EC_MAX2 200
-param set-default SIM_GZ_EC_DIS2 100
-
-param set-default SIM_GZ_MT_FUNC1 101
-param set-default SIM_GZ_MT_MIN1 0
-param set-default SIM_GZ_MT_MAX1 200
-param set-default SIM_GZ_MT_DIS1 100
-
-param set-default SIM_GZ_MT_FUNC2 102
-param set-default SIM_GZ_MT_MIN2 0
-param set-default SIM_GZ_MT_MAX2 200
-param set-default SIM_GZ_MT_DIS2 100

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -53,8 +53,6 @@ px4_add_romfs_files(
 	rc.rover_defaults
 	rc.rover_differential_apps
 	rc.rover_differential_defaults
-	rc.rover_ackermann_apps
-	rc.rover_ackermann_defaults
 	rc.uuv_apps
 	rc.uuv_defaults
 	rc.vehicle_setup

ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle

@@ -13,7 +13,7 @@
 # @board bitcraze_crazyflie exclude
 #
 
-. ${R}etc/init.d/rc.rover_ackermann_defaults
+. ${R}etc/init.d/rc.rover_defaults
 
 
 param set-default BAT1_N_CELLS 2

ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover

@@ -13,50 +13,13 @@
 
 . ${R}etc/init.d/rc.rover_differential_defaults
 
-
-
 param set-default BAT1_N_CELLS 4
 
 param set-default EKF2_GBIAS_INIT 0.01
 param set-default EKF2_ANGERR_INIT 0.01
-param set-default EKF2_MAG_TYPE 1
-
-param set-default FW_AIRSPD_MIN 0
-param set-default FW_AIRSPD_TRIM 1
-param set-default FW_AIRSPD_MAX 3
-
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_L1_DIST 5
-param set-default GND_L1_PERIOD 3
-param set-default GND_THR_CRUISE 0.7
-param set-default GND_THR_MAX 0.5
-
-# Because this is differential drive, it can make a turn with radius 0.
-# This corresponds to a turn angle of pi radians.
-# If a special case is made for differential-drive, this will need to change.
-param set-default GND_MAX_ANG 3.142
-param set-default GND_WHEEL_BASE 0.3
-
-# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
-# to support negative throttle.
-param set-default GND_THR_MIN 0
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_I 3
-param set-default GND_SPEED_D 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_THR_SC 1
-
-param set-default NAV_ACC_RAD 0.5
-
-# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
-param set-default GND_MAX_ANG 3.1415
 
 # Set geometry & output configration
-param set-default CA_AIRFRAME 6
-param set-default CA_R_REV 3
-
-
 param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
-param set-default RBCLW_REV 1
+param set-default RBCLW_REV 1 # reverse right wheels

ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps

@@ -1,28 +0,0 @@
-#!/bin/sh
-#
-# Standard apps for unmanned ground vehicles (UGV).
-#
-# NOTE: Script variables are declared/initialized/unset in the rcS script.
-#
-
-#
-# Start the attitude and position estimator.
-#
-ekf2 start &
-#attitude_estimator_q start
-#local_position_estimator start
-
-#
-# Start Control Allocator
-#
-control_allocator start
-
-#
-# Start attitude controllers.
-#
-rover_pos_control start
-
-#
-# Start Land Detector.
-#
-land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults

@@ -1,22 +0,0 @@
-#!/bin/sh
-#
-# UGV default parameters.
-#
-# NOTE: Script variables are declared/initialized/unset in the rcS script.
-#
-
-set VEHICLE_TYPE rover_ackermann
-
-# MAV_TYPE_GROUND_ROVER 10
-param set-default MAV_TYPE 10
-
-# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
-param set-default CBRK_AIRSPD_CHK 162128
-
-param set-default MIS_TAKEOFF_ALT 0.01
-
-param set-default NAV_ACC_RAD 2
-param set-default NAV_LOITER_RAD 2
-
-# Temporary.
-param set-default NAV_FW_ALT_RAD 1000

ROMFS/px4fmu_common/init.d/rc.rover_apps

@@ -9,8 +9,6 @@
 # Start the attitude and position estimator.
 #
 ekf2 start &
-#attitude_estimator_q start
-#local_position_estimator start
 
 #
 # Start Control Allocator

ROMFS/px4fmu_common/init.d/rc.rover_differential_apps

@@ -1,24 +1,11 @@
 #!/bin/sh
-#
+# Standard apps for a differential drive rover.
-# Standard apps for unmanned ground vehicles (UGV).
-#
-# NOTE: Script variables are declared/initialized/unset in the rcS script.
-#
 
-#
 # Start the attitude and position estimator.
-#
 ekf2 start &
-#attitude_estimator_q start
-#local_position_estimator start
 
-#
+# Start rover differential drive controller.
-# Start manual rover differential drive controller.
-#
 differential_drive_control start
 
-
-#
 # Start Land Detector.
-#
 land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults

@@ -1,11 +1,11 @@
 #!/bin/sh
-#
+# Differential rover parameters.
-# UGV default parameters.
-#
-# NOTE: Script variables are declared/initialized/unset in the rcS script.
-#
 
 set VEHICLE_TYPE rover_differential
 
-# MAV_TYPE_GROUND_ROVER 10
+param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
-param set-default MAV_TYPE 10
+
+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

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -23,24 +23,6 @@ then
 	. ${R}etc/init.d/rc.mc_apps
 fi
 
-#
-# Differential Rover setup.
-#
-if [ $VEHICLE_TYPE = rover_differential ]
-then
-	# Start standard UGV apps.
-	. ${R}etc/init.d/rc.rover_differential_apps
-fi
-
-#
-# Ackermann Rover setup.
-#
-if [ $VEHICLE_TYPE = rover_ackermann ]
-then
-	# Start standard UGV apps.
-	. ${R}etc/init.d/rc.rover_ackermann_apps
-fi
-
 #
 # UGV setup.
 #
@@ -50,6 +32,15 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi
 
+#
+# Differential Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_differential ]
+then
+	# Start differential drive rover apps.
+	. ${R}etc/init.d/rc.rover_differential_apps
+fi
+
 #
 # VTOL setup.
 #

boards/px4/fmu-v5x/default.px4board

@@ -7,7 +7,6 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
-CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -54,6 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y

boards/px4/sitl/default.px4board

@@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y

platforms/common/include/px4_platform_common/module.h

@@ -91,7 +91,7 @@ extern pthread_mutex_t px4_modules_mutex;
  *      static int custom_command(int argc, char *argv[])
  *      {
  *              // support for custom commands
- *              // it none are supported, just do:
+ *              // if none are supported, just do:
  *              return print_usage("unrecognized command");
  *      }
  *

platforms/ros2/include/px4_platform_common/module.h

@@ -75,7 +75,7 @@
  *      static int custom_command(int argc, char *argv[])
  *      {
  *              // support for custom commands
- *              // it none are supported, just do:
+ *              // if none are supported, just do:
  *              return print_usage("unrecognized command");
  *      }
  *

src/modules/control_allocator/ControlAllocator.cpp

@@ -61,7 +61,6 @@ ControlAllocator::ControlAllocator() :
 	_actuator_servos_pub.advertise();
 	_actuator_servos_trim_pub.advertise();
 
-
 	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
 		char buffer[17];
 		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
@@ -240,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
 			break;
 
 		case EffectivenessSource::ROVER_DIFFERENTIAL:
-			// actuator_motors message is published directly from the differential drive controller
+			// differential_drive_control does allocation and publishes directly to actuator_motors topic
 			break;
 
 		case EffectivenessSource::FIXED_WING:

src/modules/differential_drive_control/CMakeLists.txt

@@ -30,17 +30,18 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
 add_subdirectory(DifferentialDriveKinematics)
 
 px4_add_module(
 	MODULE modules__differential_drive_control
 	MAIN differential_drive_control
-	COMPILE_FLAGS
-		#-DDEBUG_BUILD
 	SRCS
 		DifferentialDriveControl.cpp
 		DifferentialDriveControl.hpp
 	DEPENDS
 		DifferentialDriveKinematics
 		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
 )

src/modules/differential_drive_control/DifferentialDriveControl.cpp

@@ -34,7 +34,7 @@
 #include "DifferentialDriveControl.hpp"
 
 using namespace time_literals;
-
+using namespace matrix;
 namespace differential_drive_control
 {
 
@@ -42,136 +42,118 @@ DifferentialDriveControl::DifferentialDriveControl() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_outputs_pub.advertise();
+	updateParams();
-	_last_timestamp = hrt_absolute_time();
-	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
-	_differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get());
 }
 
-int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+bool DifferentialDriveControl::init()
-{
-	DifferentialDriveControl *obj = new DifferentialDriveControl();
-
-	if (!obj) {
-		PX4_ERR("alloc failed");
-		return -1;
-	}
-
-	_object.store(obj);
-	_task_id = task_id_is_work_queue;
-
-	obj->start();
-
-	return 0;
-}
-
-void DifferentialDriveControl::start()
 {
 	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void DifferentialDriveControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
+	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
+
+	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+	_differential_drive_kinematics.setMaxSpeed(_max_speed);
+	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
 }
 
 void DifferentialDriveControl::Run()
 {
-
 	if (should_exit()) {
 		ScheduleClear();
 		exit_and_cleanup();
 	}
 
-	_current_timestamp = hrt_absolute_time();
+	hrt_abstime now = hrt_absolute_time();
-
-	vehicle_control_mode_poll();
 
 	if (_parameter_update_sub.updated()) {
 		parameter_update_s pupdate;
 		_parameter_update_sub.copy(&pupdate);
 
 		updateParams();
-
-		_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
-		_differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get());
 	}
 
-	if (_vehicle_control_mode.flag_armed) {
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
 
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+			_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
+		}
+	}
+
+	if (_manual_driving) {
+		// Manual mode
+		// directly produce setpoints from the manual control setpoint (joystick)
 		if (_manual_control_setpoint_sub.updated()) {
 			manual_control_setpoint_s manual_control_setpoint{};
-			_manual_control_setpoint_sub.copy(&manual_control_setpoint);
-
-			// Manual mode
-			if (_vehicle_control_mode.flag_control_manual_enabled) {
-				// directly get the input from the manual control setpoint (joystick)
-
-				_differential_drive_setpoint.timestamp = hrt_absolute_time();
-
-				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_max_speed.get();
-				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_max_angular_velocity.get();
 
+			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+				_differential_drive_setpoint.timestamp = now;
+				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
+				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
+									_max_angular_velocity;
 				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
 			}
 		}
+	}
 
-		// if manual is not enabled, it will look for a differential drive setpoint topic (differential_drive_setpoint) and subscribe to it from a ROS2 mode over DDS
+	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
-		if (_differential_drive_setpoint_sub.updated()) {
-			_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
 
-			_velocity_control_inputs(0) = _differential_drive_setpoint.speed;
+	// publish data to actuator_motors (output module)
-			_velocity_control_inputs(1) = _differential_drive_setpoint.yaw_rate;
+	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
+	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
+					_differential_drive_setpoint.speed,
+					_differential_drive_setpoint.yaw_rate);
 
-		} else if (_differential_drive_setpoint.timestamp <= 100_ms) {
+	// Check if max_angular_wheel_speed is zero
-			_velocity_control_inputs(0) = 0.0f;
+	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
-			_velocity_control_inputs(1) = 0.0f;
+	const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
+
+	if (!_armed || setpoint_timeout || !valid_max_speed) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+{
+	DifferentialDriveControl *instance = new DifferentialDriveControl();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
 		}
 
 	} else {
-		// if the system is in an error state, stop the vehicle
+		PX4_ERR("alloc failed");
-		_velocity_control_inputs = {0.0f, 0.0f};
 	}
 
-	// publish data to actuator_motors (output module)
+	delete instance;
-	publishWheelControl();
+	_object.store(nullptr);
-
+	_task_id = -1;
-	_last_timestamp = _current_timestamp;
 
+	return PX4_ERROR;
 }
 
-void DifferentialDriveControl::publishWheelControl()
+int DifferentialDriveControl::custom_command(int argc, char *argv[])
 {
-	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
+	return print_usage("unknown command");
-	_output_inverse = _differential_drive_kinematics.computeInverseKinematics(_velocity_control_inputs(0),
-			  _velocity_control_inputs(1));
-
-	// Superpose Linear and Angular velocity vector
-	float max_angular_wheel_speed = ((_param_rdd_max_speed.get() + (_param_rdd_max_angular_velocity.get() *
-					  _param_rdd_wheel_base.get() / 2.f)) / _param_rdd_wheel_radius.get());
-
-	// Check if max_angular_wheel_speed is zero
-	if (fabsf(max_angular_wheel_speed) < FLT_EPSILON) {
-		// Guard against division by zero
-		max_angular_wheel_speed = 0.001f;
-	}
-
-	_actuator_motors.timestamp = hrt_absolute_time();
-
-	// bitset which motors are configured to be reversible (3 -> ..00 0000 0011 this means that the first two motors are reversible)
-	_actuator_motors.reversible_flags = 3;
-	_actuator_motors.control[0] = _output_inverse(0) / max_angular_wheel_speed;
-	_actuator_motors.control[1] = _output_inverse(1) / max_angular_wheel_speed;
-
-	_outputs_pub.publish(_actuator_motors);
-}
-
-void DifferentialDriveControl::vehicle_control_mode_poll()
-{
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
-}
-
-int DifferentialDriveControl::print_status()
-{
-	PX4_INFO("Diffential Drive Controller running");
-	return 0;
 }
 
 int DifferentialDriveControl::print_usage(const char *reason)
@@ -186,8 +168,8 @@ int DifferentialDriveControl::print_usage(const char *reason)
 Rover Differential Drive controller.
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("differential_drive", "system");
+	PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
+	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	return 0;
 }

src/modules/differential_drive_control/DifferentialDriveControl.hpp

@@ -51,9 +51,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
-
-// Cruise mode
-#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/differential_drive_setpoint.h>
 
 // Standard library includes
@@ -72,55 +69,47 @@ public:
 	DifferentialDriveControl();
 	~DifferentialDriveControl() override = default;
 
+	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
 
 	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[])
+	static int custom_command(int argc, char *argv[]);
-	{
-		return print_usage("unknown command");
-	}
 
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	/** @see ModuleBase::print_status() */
+	bool init();
-	int print_status() override;
 
-	void start();
+protected:
+	void updateParams() override;
 
 private:
-
 	void Run() override;
-	void publishWheelControl();
-	void vehicle_control_mode_poll();
 
-	uORB::PublicationMulti<actuator_motors_s> _outputs_pub{ORB_ID(actuator_motors)};
+	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
-	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
-
-
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
 
 	differential_drive_setpoint_s _differential_drive_setpoint{};
-	vehicle_control_mode_s _vehicle_control_mode{};
+	DifferentialDriveKinematics _differential_drive_kinematics{};
-	actuator_motors_s _actuator_motors{};
 
-	DifferentialDriveKinematics _differential_drive_kinematics;
+	bool _armed = false;
-
+	bool _manual_driving = false;
-	matrix::Vector2f _velocity_control_inputs{0.f, 0.f}; // [m/s] collective roll-off speed in body x-axis, [rad/s] yaw rate
+	float _max_speed{0.f};
-	matrix::Vector2f _output_inverse{0.0f, 0.0f}; // [rad/s] Right Motor, [rad/s] Left Motor
+	float _max_angular_velocity{0.f};
-
-	float _last_timestamp{0.f};
-	float _current_timestamp{0.f};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RDD_MAX_SPEED>) _param_rdd_max_speed,
+		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
-		(ParamFloat<px4::params::RDD_MAX_ANG_VEL>) _param_rdd_max_angular_velocity,
+		(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
+		(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
 		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
-		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
+		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
 	)
 };
 

src/modules/differential_drive_control/DifferentialDriveKinematics/CMakeLists.txt

@@ -33,10 +33,8 @@
 
 px4_add_library(DifferentialDriveKinematics
 	DifferentialDriveKinematics.cpp
-	DifferentialDriveKinematics.hpp
 )
 
-target_compile_options(DifferentialDriveKinematics PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp

@@ -33,20 +33,36 @@
 
 #include "DifferentialDriveKinematics.hpp"
 
-void DifferentialDriveKinematics::setWheelBase(float wheel_base)
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
 {
-	_wheel_base = wheel_base;
+	if (_max_speed < FLT_EPSILON) {
+		return Vector2f();
+	}
+
+	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
+	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
+
+	const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
+	float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
+
+	// Compute an initial gain
+	float gain = 1.0f;
+
+	if (combined_velocity > _max_speed) {
+		float excess_velocity = fabsf(combined_velocity - _max_speed);
+		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
+	}
+
+	// Apply the gain
+	linear_velocity_x *= gain;
+
+	// Calculate the left and right wheel speeds
+	return Vector2f(linear_velocity_x - rotational_velocity,
+			linear_velocity_x + rotational_velocity) / _max_speed;
 }
 
-void DifferentialDriveKinematics::setWheelRadius(float wheel_radius)
-{
-	_wheel_radius = wheel_radius;
-}
-
-matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_vel_x, float yaw_rate)
-{
-	float motor_vel_right = linear_vel_x / _wheel_radius - _wheel_base / 2.f * yaw_rate / _wheel_radius;
-	float motor_vel_left = linear_vel_x / _wheel_radius + _wheel_base / 2.f * yaw_rate / _wheel_radius;
-
-	return matrix::Vector2f(motor_vel_right, motor_vel_left);
-}

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp

@@ -47,30 +47,38 @@ public:
 	DifferentialDriveKinematics() = default;
 	~DifferentialDriveKinematics() = default;
 
-	/**
-	 * @brief Computes the inverse kinematics for differential drive.
-	 *
-	 * @param linear_vel_x Linear velocity along the x-axis.
-	 * @param yaw_rate Yaw rate of the robot.
-	 * @return matrix::Vector2f Motor velocities for the right and left motors.
-	 */
-	matrix::Vector2f computeInverseKinematics(float linear_vel_x, float yaw_rate);
-
 	/**
 	 * @brief Sets the wheel base of the robot.
 	 *
 	 * @param wheel_base The distance between the centers of the wheels.
 	 */
-	void setWheelBase(float wheel_base);
+	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
 
 	/**
-	 * @brief Sets the radius of the wheels of the robot.
+	 * @brief Sets the maximum speed of the robot.
 	 *
-	 * @param wheel_radius The radius of the wheels.
+	 * @param max_speed The maximum speed of the robot.
 	 */
-	void setWheelRadius(float wheel_radius);
+	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
+
+	/**
+	 * @brief Sets the maximum angular speed of the robot.
+	 *
+	 * @param max_angular_speed The maximum angular speed of the robot.
+	 */
+	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
+
+	/**
+	 * @brief Computes the inverse kinematics for differential drive.
+	 *
+	 * @param linear_velocity_x Linear velocity along the x-axis.
+	 * @param yaw_rate Yaw rate of the robot.
+	 * @return matrix::Vector2f Motor velocities for the right and left motors.
+	 */
+	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
 
 private:
 	float _wheel_base{0.f};
-	float _wheel_radius{0.f};
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
 };

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp

@@ -37,48 +37,138 @@
 
 using namespace matrix;
 
-TEST(DifferentialDriveKinematicsTest, AllZeroCaseInverse)
+TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
 {
 	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
-	kinematics.setWheelRadius(1.f);
+	kinematics.setMaxSpeed(10.f);
-	Vector2f rate_setpoint = {0.f, 0.f};
+	kinematics.setMaxAngularVelocity(10.f);
-	kinematics.setInput(rate_setpoint, true);
+
-	Vector2f wheel_output = kinematics.getOutput(true);
+	// Test with zero linear velocity and zero yaw rate (stationary vehicle)
-	EXPECT_EQ(wheel_output, Vector2f());
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
 }
 
-TEST(DifferentialDriveKinematicsTest, InvalidCaseInverse)
+
+TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
 {
 	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(0.f);
-	kinematics.setWheelRadius(0.f);
+	kinematics.setMaxSpeed(10.f);
-	Vector2f rate_setpoint = {0.f, 0.f};
+	kinematics.setMaxAngularVelocity(10.f);
-	kinematics.setInput(rate_setpoint, true);
+
-	Vector2f wheel_output = kinematics.getOutput(true);
+	// Test with invalid parameters (zero wheel base and wheel radius)
-	EXPECT_EQ(wheel_output, Vector2f());
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
 }
 
-TEST(DifferentialDriveKinematicsTest, UnitCaseInverse)
-{
-	DifferentialDriveKinematics kinematics;
-	kinematics.setWheelBase(1.f);
-	kinematics.setWheelRadius(1.f);
-	Vector2f rate_setpoint = {1.f, 1.f};
-	kinematics.setInput(rate_setpoint, true);
-	Vector2f wheel_output = kinematics.getOutput(true);
-	Vector2f expected_output = {1.5f, 0.5f};
-	EXPECT_EQ(wheel_output, expected_output);
-}
 
 TEST(DifferentialDriveKinematicsTest, UnitCase)
 {
 	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
-	kinematics.setWheelRadius(1.f);
+	kinematics.setMaxSpeed(10.f);
-	Vector2f wheel_input = {1.f, 1.f};
+	kinematics.setMaxAngularVelocity(10.f);
-	kinematics.setInput(wheel_input, false);
+
-	Vector2f rate_output = kinematics.getOutput(false);
+	// Test with unit values for linear velocity and yaw rate
-	Vector2f expected_output = {1.f, 0.f};
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
-	EXPECT_EQ(rate_output, expected_output);
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
+}
+
+TEST(DifferentialDriveKinematicsTest, RandomCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
+}
+
+TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test rotating in place (zero linear velocity, non-zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
+}
+
+TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test moving straight (non-zero linear velocity, zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(FLT_MIN);
+	kinematics.setMaxSpeed(FLT_MIN);
+	kinematics.setMaxAngularVelocity(FLT_MIN);
+
+	// Test with minimum possible input values
+	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
 }

src/modules/differential_drive_control/Kconfig

@@ -2,4 +2,4 @@ menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
 	bool "differential_drive_control"
 	default y
 	---help---
-		Enable support for differential_drive_control
+		Enable support for control of differential drive rovers

src/modules/differential_drive_control/module.yaml

@@ -0,0 +1,55 @@
+module_name: Differential Drive Control
+
+parameters:
+  - group: Rover Differential Drive
+    definitions:
+      RDD_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the center of the right wheel to the center of the left wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+      RDD_WHEEL_RADIUS:
+        description:
+          short: Wheel radius
+          long: Size of the wheel, half the diameter of the wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.1
+      RDD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_WHL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 10

src/modules/differential_drive_control/params.c

@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Wheel Base
- *
- * Distance from the center of the right wheel to the center of the left wheel
- *
- * @unit m
- * @min 0.001
- * @max 100
- * @increment 0.001
- * @decimal 3
- * @group Rover Differential Drive
- */
-PARAM_DEFINE_FLOAT(RDD_WHEEL_BASE, 0.5f);
-
-/**
- * Wheel radius
- *
- * Size of the wheel, half the diameter of the wheel
- *
- * @unit m
- * @min 0.001
- * @max 100
- * @increment 0.001
- * @decimal 3
- * @group Rover Differential Drive
- */
-PARAM_DEFINE_FLOAT(RDD_WHEEL_RADIUS, 0.1f);
-
-/**
- * Max Speed
- *
- * @unit m/s
- * @min 0.0
- * @max 100
- * @increment 0.01
- * @decimal 2
- * @group Rover Differential Drive
- */
-PARAM_DEFINE_FLOAT(RDD_MAX_SPEED, 0.5f);
-
-/**
- * Max Angular Velocity
- *
- * @unit rad/s
- * @min 0.0
- * @max 100
- * @increment 0.01
- * @decimal 2
- * @group Rover Differential Drive
- */
-PARAM_DEFINE_FLOAT(RDD_MAX_ANG_VEL, 0.3f);

src/modules/logger/logged_topics.cpp

@@ -57,7 +57,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
-	add_topic("differential_drive_setpoint", 100);
+	add_optional_topic("differential_drive_setpoint", 100);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -226,7 +226,7 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
-	if (!_mixing_interface_motor.init(_model_name)) {
+	if (!_mixing_interface_wheel.init(_model_name)) {
 		PX4_ERR("failed to init motor output");
 		return PX4_ERROR;
 	}
@@ -729,7 +729,7 @@ void GZBridge::Run()
 
 		_mixing_interface_esc.stop();
 		_mixing_interface_servo.stop();
-		_mixing_interface_motor.stop();
+		_mixing_interface_wheel.stop();
 
 		exit_and_cleanup();
 		return;
@@ -745,7 +745,7 @@ void GZBridge::Run()
 
 		_mixing_interface_esc.updateParams();
 		_mixing_interface_servo.updateParams();
-		_mixing_interface_motor.updateParams();
+		_mixing_interface_wheel.updateParams();
 	}
 
 	ScheduleDelayed(10_ms);
@@ -761,8 +761,8 @@ int GZBridge::print_status()
 	PX4_INFO_RAW("Servo outputs:\n");
 	_mixing_interface_servo.mixingOutput().printStatus();
 
-	PX4_INFO_RAW("Motor outputs:\n");
+	PX4_INFO_RAW("Wheel outputs:\n");
-	_mixing_interface_motor.mixingOutput().printStatus();
+	_mixing_interface_wheel.mixingOutput().printStatus();
 
 	return 0;
 }

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -132,7 +132,7 @@ private:
 
 	GZMixingInterfaceESC   _mixing_interface_esc{_node, _node_mutex};
 	GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
-	GZMixingInterfaceWheel _mixing_interface_motor{_node, _node_mutex};
+	GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
 
 	px4::atomic<uint64_t> _world_time_us{0};
 

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp

@@ -77,8 +77,9 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
 		wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
 
 		for (unsigned i = 0; i < active_output_count; i++) {
-			float output_scaler = 100.0f;
+			// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
-			float scaled_output = (float)outputs[i] - output_scaler;
+			static constexpr float output_offset = 100.0f;
+			float scaled_output = (float)outputs[i] - output_offset;
 			wheel_velocity_message.set_velocity(i, scaled_output);
 		}
 

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp

@@ -80,7 +80,7 @@ private:
 	gz::transport::Node &_node;
 	pthread_mutex_t &_node_mutex;
 
-	MixingOutput _mixing_output{"SIM_GZ_MT", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 
 	gz::transport::Node::Publisher _actuators_pub;
 

src/modules/simulation/gz_bridge/module.yaml

@@ -25,9 +25,9 @@ actuator_output:
         max: { min: 0, max: 1000, default: 1000 }
         failsafe: { min: 0, max: 1000 }
       num_channels: 8
-    - param_prefix: SIM_GZ_MT
+    - param_prefix: SIM_GZ_WH
-      group_label: 'Motors'
+      group_label: 'Wheels'
-      channel_label: 'Motors'
+      channel_label: 'Wheels'
       standard_params:
         disarmed: { min: 0, max: 200, default: 100 }
         min: { min: 0, max: 200, default: 0 }

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -76,9 +76,6 @@ subscriptions:
   - topic: /fmu/in/register_ext_component_request
     type: px4_msgs::msg::RegisterExtComponentRequest
 
-  - topic: /fmu/in/differential_drive_setpoint
-    type: px4_msgs::msg::DifferentialDriveSetpoint
-
   - topic: /fmu/in/unregister_ext_component
     type: px4_msgs::msg::UnregisterExtComponent
 
@@ -130,6 +127,9 @@ subscriptions:
   - topic: /fmu/in/vehicle_rates_setpoint
     type: px4_msgs::msg::VehicleRatesSetpoint
 
+  - topic: /fmu/in/differential_drive_setpoint
+    type: px4_msgs::msg::DifferentialDriveSetpoint
+
   - topic: /fmu/in/vehicle_visual_odometry
     type: px4_msgs::msg::VehicleOdometry