differential: update module (PX4#23629)

Improve the slow down effect and add support for speed change in mission mode.
Seperate code related to turning setpoints into motor commands into its own folder and refactor code.

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

@@ -14,15 +14,17 @@ 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_YAW_RATE_P 5
+param set-default RD_YAW_RATE_I 0
+param set-default RD_YAW_P 5
+param set-default RD_YAW_I 0
+param set-default RD_SPEED_P 1
+param set-default RD_SPEED_I 0
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 7
+param set-default RD_MISS_SPD_DEF 5
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 

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

@@ -27,8 +27,8 @@ param set-default RD_YAW_RATE_P 5
 param set-default RD_MAX_ACCEL 1
 param set-default RD_MAX_JERK 3
 param set-default RD_MAX_SPEED 8
-param set-default RD_HEADING_P 5
-param set-default RD_HEADING_I 0.1
+param set-default RD_YAW_P 5
+param set-default RD_YAW_I 0.1
 param set-default RD_MAX_YAW_RATE 30
 param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066

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

@@ -20,3 +20,25 @@ param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
 param set-default RBCLW_REV 1 # reverse right wheels
+
+# Rover parameters
+param set-default RD_WHEEL_TRACK 0.3
+param set-default RD_MAN_YAW_SCALE 1
+param set-default RD_MAX_ACCEL 5
+param set-default RD_MAX_JERK 50
+param set-default RD_MAX_SPEED 2
+param set-default RD_YAW_RATE_P 0.1
+param set-default RD_YAW_RATE_I 0
+param set-default RD_YAW_P 5
+param set-default RD_YAW_I 0
+param set-default RD_SPEED_P 0.5
+param set-default RD_SPEED_I 0.1
+param set-default RD_MAX_YAW_RATE 300
+param set-default RD_MISS_SPD_DEF 1.8
+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 10
+param set-default PP_LOOKAHD_MIN 1
+param set-default PP_LOOKAHD_GAIN 1

msg/CMakeLists.txt

@@ -182,6 +182,7 @@ set(msg_files
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
+	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.msg

msg/RoverDifferentialGuidanceStatus.msg

@@ -1,10 +1,7 @@
 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/RoverDifferentialSetpoint.msg

@@ -0,0 +1,9 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 forward_speed_setpoint 		  # [m/s] Desired forward speed for the rover
+float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover
+float32 yaw_rate_setpoint      		  # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used)
+float32 yaw_rate_setpoint_normalized      # [-1, 1] Normalized yaw rate for the rover
+float32 yaw_setpoint 	       		  # [rad] Desired yaw (heading) for the rover
+
+# TOPICS rover_differential_setpoint

msg/RoverDifferentialStatus.msg

@@ -1,8 +1,11 @@
 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_deg         # [deg] Actual yaw of the rover
 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
+float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller
+float32 pid_yaw_integral       # Integral of the PID for the closed loop yaw controller
+float32 pid_yaw_rate_integral  # Integral of the PID for the closed loop yaw rate controller
+float32 pid_throttle_integral  # Integral of the PID for the closed loop speed controller
 
 # TOPICS rover_differential_status

src/modules/logger/logged_topics.cpp

@@ -105,6 +105,7 @@ void LoggedTopics::add_default_topics()
 	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_setpoint", 100);
 	add_optional_topic("rover_differential_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);

src/modules/rover_differential/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2024 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
@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(RoverDifferentialGuidance)
+add_subdirectory(RoverDifferentialControl)
 
 px4_add_module(
 	MODULE modules__rover_differential
@@ -41,8 +42,8 @@ px4_add_module(
 		RoverDifferential.hpp
 	DEPENDS
 		RoverDifferentialGuidance
+		RoverDifferentialControl
 		px4_work_queue
-		modules__control_allocator # for parameter CA_R_REV
 		pure_pursuit
 	MODULE_CONFIG
 		module.yaml

src/modules/rover_differential/RoverDifferential.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2024 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
@@ -32,16 +32,13 @@
  ****************************************************************************/
 
 #include "RoverDifferential.hpp"
-using namespace matrix;
-using namespace time_literals;
 
 RoverDifferential::RoverDifferential() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	updateParams();
-	_rover_differential_status_pub.advertise();
-	pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
+	_rover_differential_setpoint_pub.advertise();
 }
 
 bool RoverDifferential::init()
@@ -53,16 +50,7 @@ bool RoverDifferential::init()
 void RoverDifferential::updateParams()
 {
 	ModuleParams::updateParams();
-
 	_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
-
-	pid_set_parameters(&_pid_yaw_rate,
-			   _param_rd_p_gain_yaw_rate.get(), // Proportional gain
-			   _param_rd_i_gain_yaw_rate.get(), // Integral gain
-			   0.f, // Derivative gain
-			   1.f, // Integral limit
-			   1.f); // Output limit
-
 }
 
 void RoverDifferential::Run()
@@ -73,135 +61,99 @@ void RoverDifferential::Run()
 		return;
 	}
 
-	hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	// uORB subscriber updates
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&parameter_update);
-		updateParams();
-	}
+	updateSubscriptions();
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status{};
-		_vehicle_status_sub.copy(&vehicle_status);
-		_nav_state = vehicle_status.nav_state;
-	}
+	// Generate and publish attitude and velocity setpoints
+	hrt_abstime timestamp = hrt_absolute_time();
 
-	if (_vehicle_angular_velocity_sub.updated()) {
-		vehicle_angular_velocity_s vehicle_angular_velocity{};
-		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
-		_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
-	}
-
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-	}
-
-	if (_vehicle_local_position_sub.updated()) {
-		vehicle_local_position_s vehicle_local_position{};
-		_vehicle_local_position_sub.copy(&vehicle_local_position);
-		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		_vehicle_forward_speed = velocity_in_body_frame(0);
-	}
-
-	// Navigation modes
 	switch (_nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
 			manual_control_setpoint_s manual_control_setpoint{};
 
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_differential_setpoint.throttle = manual_control_setpoint.throttle;
-				_differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
-
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = NAN;
+				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+				rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
+				rover_differential_setpoint.yaw_rate_setpoint = NAN;
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 			}
-
-			_differential_setpoint.closed_loop_yaw_rate = false;
 		} break;
 
 	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
 			manual_control_setpoint_s manual_control_setpoint{};
 
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_differential_setpoint.throttle = manual_control_setpoint.throttle;
-				_differential_setpoint.yaw_rate = math::interpolate<float>(manual_control_setpoint.roll,
-								  -1.f, 1.f,
-								  -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = NAN;
+				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 			}
 
-			_differential_setpoint.closed_loop_yaw_rate = true;
 		} break;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-		_differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed,
-					 _nav_state);
+		_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
 		break;
 
 	default: // Unimplemented nav states will stop the rover
-		_differential_setpoint.throttle = 0.f;
-		_differential_setpoint.yaw_rate = 0.f;
-		_differential_setpoint.closed_loop_yaw_rate = false;
+		rover_differential_setpoint_s rover_differential_setpoint{};
+		rover_differential_setpoint.forward_speed_setpoint = NAN;
+		rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
+		rover_differential_setpoint.yaw_rate_setpoint = NAN;
+		rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f;
+		rover_differential_setpoint.yaw_setpoint = NAN;
+		_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 		break;
 	}
 
-	float speed_diff_normalized = _differential_setpoint.yaw_rate;
-
-	// Closed loop yaw rate control
-	if (_differential_setpoint.closed_loop_yaw_rate) {
-		if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) {
-			speed_diff_normalized = 0.f;
-			pid_reset_integral(&_pid_yaw_rate);
-
-		} else {
-			const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward
-			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_speed.get(),
-						_param_rd_max_speed.get(), -1.f, 1.f);
-			speed_diff_normalized = math::constrain(speed_diff_normalized +
-								pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt),
-								-1.f, 1.f); // Feedback
-		}
-
-	} else {
-		pid_reset_integral(&_pid_yaw_rate);
-	}
-
-	// Publish rover differential status (logging)
-	rover_differential_status_s rover_differential_status{};
-	rover_differential_status.timestamp = _timestamp;
-	rover_differential_status.actual_speed = _vehicle_forward_speed;
-	rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate;
-	rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate;
-	rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
-	_rover_differential_status_pub.publish(rover_differential_status);
-
-	// Publish to motors
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control);
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
+	_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
 
 }
 
-matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff)
+void RoverDifferential::updateSubscriptions()
 {
-	float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff);
 
-	if (combined_velocity > 1.0f) { // Prioritize yaw rate
-		float excess_velocity = fabsf(combined_velocity - 1.0f);
-		forward_speed -= sign(forward_speed) * excess_velocity;
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+		updateParams();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
+		_nav_state = vehicle_status.nav_state;
 	}
 
-	// Calculate the left and right wheel speeds
-	return Vector2f(forward_speed - speed_diff,
-			forward_speed + speed_diff);
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
+		_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		_vehicle_forward_speed = velocity_in_body_frame(0);
+	}
 }
 
 int RoverDifferential::task_spawn(int argc, char *argv[])