differential: add yaw rate slew rate to normalized commands

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -73,6 +73,7 @@ void RoverDifferentialControl::updateParams()
 			   _max_yaw_rate,  // Integral limit
 			   _max_yaw_rate);  // Output limit
 
+	// Update slew rates
 	if (_max_yaw_rate > FLT_EPSILON) {
 		_yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate);
 	}
@@ -110,29 +111,15 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	float speed_diff_normalized{0.f};
 
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
-		if (_max_yaw_accel > FLT_EPSILON) {
-			_yaw_rate_with_accel_limit.update(_rover_differential_setpoint.yaw_rate_setpoint, dt);
-
-		} else {
-			_yaw_rate_with_accel_limit.setForcedValue(_rover_differential_setpoint.yaw_rate_setpoint);
-		}
-
-		_rover_differential_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState();
-
-		if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
-			const float speed_diff = _yaw_rate_with_accel_limit.getState() * _param_rd_wheel_track.get() /
-						 2.f;
-			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_thr_yaw_r.get(),
-						_param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
-		}
-
-		speed_diff_normalized = math::constrain(speed_diff_normalized +
-							pid_calculate(&_pid_yaw_rate, _yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0, dt),
-							-1.f, 1.f); // Feedback
+		speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate,
+					_param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
+					_pid_yaw_rate, false);
 
 	} 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_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint_normalized,
+					vehicle_yaw_rate,
+					_param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
+					_pid_yaw_rate, true);
 	}
 
 	// Speed control
@@ -166,6 +153,49 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	_actuator_motors_pub.publish(actuator_motors);
 }
 
+float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate,
+		const float max_thr_yaw_r,
+		const float max_yaw_accel, const float wheel_track, const float dt, SlewRate<float> &yaw_rate_with_accel_limit,
+		PID_t &pid_yaw_rate, const bool normalized)
+{
+	float slew_rate_normalization{1.f};
+
+	if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
+		slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f;
+	}
+
+	if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > 0.f) {
+		yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization);
+		yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt);
+
+	} else {
+		yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint);
+	}
+
+	// Transform yaw rate into speed difference
+	float speed_diff_normalized{0.f};
+
+	if (normalized) {
+		speed_diff_normalized = yaw_rate_with_accel_limit.getState();
+
+	} else {
+		_rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState();
+
+		if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
+			const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track /
+						 2.f;
+			speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
+						max_thr_yaw_r, -1.f, 1.f);
+		}
+
+		speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0,
+						       dt); // Feedback
+	}
+
+	return math::constrain(speed_diff_normalized, -1.f, 1.f);
+
+}
+
 float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
 		const float vehicle_forward_speed, const float dt, const bool normalized)
 {

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -85,8 +85,25 @@ class RoverDifferentialControl : public ModuleParams
 	void updateParams() override;
 
 private:
+
+	/**
+	 * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates.
+	 * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]].
+	 * @param vehicle_yaw_rate Measured yaw rate [rad/s].
+	 * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
+	 * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2].
+	 * @param wheel_track Wheel track [m].
+	 * @param dt Time since last update [s].
+	 * @param yaw_rate_with_accel_limit Slew rate for yaw rate acceleration.
+	 * @param pid_yaw_rate Yaw rate PID
+	 * @param normalized Indicates if the forward speed setpoint is already normalized.
+	 * @return Normalized speed differece setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel,
+				      float wheel_track,
+				      float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized);
 	/**
-	 * @brief Compute normalized forward speed setpoint by applying slew rates
+	 * @brief Compute normalized forward speed setpoint and apply slew rates.
 	 * to the forward speed setpoint and doing closed loop speed control if enabled.
 	 * @param forward_speed_setpoint Forward speed setpoint [m/s].
 	 * @param vehicle_forward_speed Actual forward speed [m/s].
@@ -98,7 +115,6 @@ class RoverDifferentialControl : public ModuleParams
 
 	/**
 	 * @brief Compute normalized motor commands based on normalized setpoints.
-	 *
 	 * @param forward_speed_normalized Normalized forward speed [-1, 1].
 	 * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
 	 * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1].