differential: update setpoint message and add clarifications

msg/RoverDifferentialSetpoint.msg

@@ -1,10 +1,9 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 forward_speed_setpoint # [m/s or normalized [-1, 1]] Desired forward speed for the rover
-float32 yaw_rate_setpoint      # [rad/s or normalized [-1, 1]] Desired yaw rate for the rover
-float32 yaw_setpoint 	       # [rad] Desired yaw (heading) for the rover
-bool closed_loop_speed         # Use closed loop velocity controller
-bool closed_loop_yaw_rate      # Use closed loop yaw rate controller
-bool closed_loop_yaw           # Use closed loop yaw controller
+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

@@ -2,9 +2,10 @@ uint64 timestamp # time since system start (microseconds)
 
 float32 actual_speed	       # [m/s] Actual forward speed of the rover
 float32 actual_yaw_deg         # [deg] Actual yaw of the rover
-float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate (deviates from the yaw_rate_setpoint if the yaw setpoint is used)
 float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
+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 yaw controller
-float32 pid_yaw_integral   # Integral of the PID for the closed loop heading controller
+float32 pid_throttle_integral  # Integral of the PID for the closed loop speed controller
+
 # TOPICS rover_differential_status

src/modules/rover_differential/RoverDifferential.cpp

@@ -73,12 +73,11 @@ void RoverDifferential::Run()
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 				rover_differential_setpoint_s rover_differential_setpoint{};
 				rover_differential_setpoint.timestamp = timestamp;
-				rover_differential_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle;
-				rover_differential_setpoint.yaw_rate_setpoint = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
+				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.closed_loop_yaw_rate = false;
-				rover_differential_setpoint.closed_loop_yaw = false;
-				rover_differential_setpoint.closed_loop_speed = false;
+				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);
 			}
 		} break;
@@ -89,13 +88,12 @@ void RoverDifferential::Run()
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 				rover_differential_setpoint_s rover_differential_setpoint{};
 				rover_differential_setpoint.timestamp = timestamp;
-				rover_differential_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle;
+				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.closed_loop_yaw_rate = true;
-				rover_differential_setpoint.closed_loop_yaw = false;
-				rover_differential_setpoint.closed_loop_speed = false;
 				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 			}
 
@@ -108,13 +106,11 @@ void RoverDifferential::Run()
 
 	default: // Unimplemented nav states will stop the rover
 		rover_differential_setpoint_s rover_differential_setpoint{};
-		rover_differential_setpoint.timestamp = timestamp;
-		rover_differential_setpoint.forward_speed_setpoint = 0.f;
-		rover_differential_setpoint.yaw_rate_setpoint = 0.f;
+		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.closed_loop_yaw_rate = false;
-		rover_differential_setpoint.closed_loop_yaw = false;
-		rover_differential_setpoint.closed_loop_speed = false;
 		_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 		break;
 	}

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -83,7 +83,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	_rover_differential_setpoint_sub.update(&_rover_differential_setpoint);
 
 	// Closed loop yaw control (Overrides yaw rate setpoint)
-	if (_rover_differential_setpoint.closed_loop_yaw && PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
+	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
 		if (fabsf(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw) < FLT_EPSILON) {
 			_rover_differential_setpoint.yaw_rate_setpoint = 0.f;
 			pid_reset_integral(&_pid_yaw);
@@ -97,8 +97,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	// Yaw rate control
 	float speed_diff_normalized{0.f};
 
-	if (_rover_differential_setpoint.closed_loop_yaw_rate
-	    && PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
+	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
 		if (fabsf(_rover_differential_setpoint.yaw_rate_setpoint) < FLT_EPSILON) {
 			speed_diff_normalized = 0.f;
 			pid_reset_integral(&_pid_yaw_rate);
@@ -112,16 +111,15 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 								-1.f, 1.f); // Feedback
 		}
 
-	} else { // Interpret yaw rate setpoint as already normalized
-		speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint) ?
-					math::constrain(_rover_differential_setpoint.yaw_rate_setpoint, -1.f, 1.f) : 0.f;
+	} else { // Use normalized setpoint
+		speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ?
+					math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f;
 	}
 
 	// Speed control
 	float throttle{0.f};
 
-	if (_rover_differential_setpoint.closed_loop_speed
-	    && PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
+	if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
 		if (fabsf(_rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) {
 			pid_reset_integral(&_pid_throttle);
 
@@ -136,9 +134,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 			}
 		}
 
-	} else { // Interpret speed setpoint as already normalized
-		throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint) ?
-			   math::constrain(_rover_differential_setpoint.forward_speed_setpoint, -1.f, 1.f) : 0.f;
+	} else { // Use normalized setpoint
+		throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
+			   math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
 	}
 
 	// Publish rover differential status (logging)
@@ -165,11 +163,11 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized,
 		const float speed_diff_normalized)
 {
-	float combined_velocity = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized);
+	float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized);
 
-	if (combined_velocity > 1.0f) { // Prioritize yaw rate
-		float excess_velocity = fabsf(combined_velocity - 1.0f);
-		forward_speed_normalized -= sign(forward_speed_normalized) * excess_velocity;
+	if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1
+		float excess = fabsf(max_motor_command - 1.0f);
+		forward_speed_normalized -= sign(forward_speed_normalized) * excess;
 	}
 
 	// Calculate the left and right wheel speeds

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp

@@ -128,11 +128,10 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f
 	rover_differential_setpoint_s rover_differential_setpoint{};
 	rover_differential_setpoint.timestamp = timestamp;
 	rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed;
+	rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
 	rover_differential_setpoint.yaw_rate_setpoint = NAN;
+	rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
 	rover_differential_setpoint.yaw_setpoint = desired_yaw;
-	rover_differential_setpoint.closed_loop_yaw_rate = true;
-	rover_differential_setpoint.closed_loop_yaw = true;
-	rover_differential_setpoint.closed_loop_speed = true;
 	_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
 }