differential: add slew rates for speed, yaw and yaw rate setpoints

msg/RoverDifferentialStatus.msg

@@ -1,13 +1,14 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 actual_speed	         # [m/s] Actual forward speed of the rover
-float32 actual_yaw               # [rad] Actual yaw of the rover
-float32 actual_yaw_rate          # [rad/s] Actual yaw rate of the rover
-float32 desired_yaw_rate         # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller
-float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint
-float32 speed_diff_normalized    # [-1, 1] Normalized speed difference setpoint between the left and right motor
-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
+float32 measured_forward_speed	        # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ
+float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_yaw                    # [rad] Measured yaw
+float32 adjusted_yaw_setpoint           # [rad] Yaw setpoint after applying slew rate
+float32 clyaw_yaw_rate_setpoint         # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
+float32 measured_yaw_rate               # [rad/s] Measured yaw rate
+float32 adjusted_yaw_rate_setpoint      # [rad/s] Yaw rate setpoint from the closed loop yaw 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/rover_differential/RoverDifferential.cpp

@@ -63,7 +63,7 @@ void RoverDifferential::Run()
 
 	updateSubscriptions();
 
-	// Generate and publish attitude and velocity setpoints
+	// Generate and publish attitude, rate and speed setpoints
 	hrt_abstime timestamp = hrt_absolute_time();
 
 	switch (_nav_state) {

src/modules/rover_differential/RoverDifferential.hpp

@@ -130,8 +130,8 @@ class RoverDifferential : public ModuleBase<RoverDifferential>, public ModulePar
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
-		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
-		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
-		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
+		(ParamFloat<px4::params::RD_MAX_YAW_RATE>)  _param_rd_max_yaw_rate,
+		(ParamFloat<px4::params::RD_MAX_SPEED>)     _param_rd_max_speed,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max
 	)
 };

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -51,6 +51,9 @@ void RoverDifferentialControl::updateParams()
 {
 	ModuleParams::updateParams();
 	_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
+	_max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F;
+
+	// Update PID
 	pid_set_parameters(&_pid_yaw_rate,
 			   _param_rd_yaw_rate_p.get(), // Proportional gain
 			   _param_rd_yaw_rate_i.get(), // Integral gain
@@ -69,6 +72,16 @@ void RoverDifferentialControl::updateParams()
 			   0.f,  // Derivative gain
 			   _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);
+	}
+
+	if (_max_yaw_accel > FLT_EPSILON) {
+		_yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel);
+	}
+
 }
 
 void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
@@ -84,70 +97,156 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 
 	// Closed loop yaw control (Overrides yaw rate setpoint)
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
-		float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
+		_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt);
+		_rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
+		const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw);
 		_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
+		_rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint;
+
+	} else {
+		_yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw);
 	}
 
 	// Yaw rate control
 	float speed_diff_normalized{0.f};
 
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
-		if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
-			const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _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, _rover_differential_setpoint.yaw_rate_setpoint, 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
 	float forward_speed_normalized{0.f};
 
-	if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
-		if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
-			forward_speed_normalized = math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
-						   -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(),
-						   -1.f, 1.f);
-		}
+	if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) {
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint,
+					   vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle,
+					   _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false);
 
-		forward_speed_normalized = math::constrain(forward_speed_normalized + pid_calculate(&_pid_throttle,
-					   _rover_differential_setpoint.forward_speed_setpoint,
-					   vehicle_forward_speed, 0,
-					   dt), -1.f, 1.f); // Feedback
+	} else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized,
+					   vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle,
+					   _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true);
 
-	} else { // Use normalized setpoint
-		forward_speed_normalized = 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)
-	rover_differential_status_s rover_differential_status{};
-	rover_differential_status.timestamp = _timestamp;
-	rover_differential_status.actual_speed = vehicle_forward_speed;
-	rover_differential_status.actual_yaw = vehicle_yaw;
-	rover_differential_status.desired_yaw_rate = _rover_differential_setpoint.yaw_rate_setpoint;
-	rover_differential_status.actual_yaw_rate = vehicle_yaw_rate;
-	rover_differential_status.forward_speed_normalized = forward_speed_normalized;
-	rover_differential_status.speed_diff_normalized = speed_diff_normalized;
-	rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
-	rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
-	rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
-	_rover_differential_status_pub.publish(rover_differential_status);
+	_rover_differential_status.timestamp = _timestamp;
+	_rover_differential_status.measured_forward_speed = vehicle_forward_speed;
+	_rover_differential_status.measured_yaw = vehicle_yaw;
+	_rover_differential_status.measured_yaw_rate = vehicle_yaw_rate;
+	_rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
+	_rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
+	_rover_differential_status.pid_yaw_integral = _pid_yaw.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();
 	computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control);
 	actuator_motors.timestamp = _timestamp;
 	_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 max_thr_spd, SlewRate<float> &forward_speed_setpoint_with_accel_limit,
+		PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, 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_spd > FLT_EPSILON ? max_thr_spd : 0.f;
+	}
+
+	// Apply acceleration and deceleration limit
+	if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) {
+		if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+			forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization);
+			forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
+
+		} else {
+			forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
+
+		}
+
+	} else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+		forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization);
+		forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
+
+	} else {
+		forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
+	}
+
+	// Calculate normalized forward speed setpoint
+	float forward_speed_normalized{0.f};
+
+	if (normalized) {
+		forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState();
+
+	} else { // Closed loop speed control
+		_rover_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
+
+		if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			forward_speed_normalized = math::interpolate<float>(_forward_speed_setpoint_with_accel_limit.getState(),
+						   -max_thr_spd, max_thr_spd,
+						   -1.f, 1.f);
+		}
+
+		forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
+					    vehicle_forward_speed, 0, dt); // Feedback
+	}
+
+	return math::constrain(forward_speed_normalized, -1.f, 1.f);
 
 }
 

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -43,7 +43,8 @@
 #include <uORB/topics/rover_differential_setpoint.h>
 #include <uORB/topics/rover_differential_status.h>
 #include <uORB/topics/actuator_motors.h>
-
+#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/slew_rate/SlewRateYaw.hpp>
 
 // Standard libraries
 #include <lib/pid/pid.h>
@@ -84,9 +85,42 @@ 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 Yaw rate slew rate.
+	 * @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 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].
+	 * @param max_thr_spd Speed the rover drives at maximum throttle [m/s].
+	 * @param forward_speed_setpoint_with_accel_limit Speed slew rate.
+	 * @param pid_throttle Throttle PID
+	 * @param max_accel Maximum acceleration [m/s^2]
+	 * @param max_decel Maximum deceleration [m/s^2]
+	 * @param dt Time since last update [s].
+	 * @param normalized Indicates if the forward speed setpoint is already normalized.
+	 * @return Normalized forward speed setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
+					  SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel,
+					  float dt, bool normalized);
+
 	/**
 	 * @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].
@@ -99,29 +133,37 @@ class RoverDifferentialControl : public ModuleParams
 	// uORB publications
 	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
 	uORB::Publication<rover_differential_status_s> _rover_differential_status_pub{ORB_ID(rover_differential_status)};
+	rover_differential_status_s _rover_differential_status{};
 
 	// Variables
 	rover_differential_setpoint_s _rover_differential_setpoint{};
 	hrt_abstime _timestamp{0};
 	float _max_yaw_rate{0.f};
+	float _max_yaw_accel{0.f};
 
 	// Controllers
-	PID_t _pid_throttle; // The PID controller for the closed loop speed control
-	PID_t _pid_yaw; // The PID controller for the closed loop yaw control
-	PID_t _pid_yaw_rate; // The PID controller for the closed loop yaw rate control
+	PID_t _pid_throttle; // Closed loop speed control
+	PID_t _pid_yaw;      // Closed loop yaw control
+	PID_t _pid_yaw_rate; // Closed loop yaw rate control
+	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
+	SlewRate<float> _yaw_rate_with_accel_limit{0.f};
+	SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
-		(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
+		(ParamFloat<px4::params::RD_WHEEL_TRACK>)   _param_rd_wheel_track,
+		(ParamFloat<px4::params::RD_MAX_THR_SPD>)   _param_rd_max_thr_spd,
+		(ParamFloat<px4::params::RD_MAX_ACCEL>)     _param_rd_max_accel,
+		(ParamFloat<px4::params::RD_MAX_DECEL>)     _param_rd_max_decel,
 		(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
-		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
-		(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
-		(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
-		(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
-		(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
-		(ParamFloat<px4::params::RD_YAW_P>) _param_rd_p_gain_yaw,
-		(ParamFloat<px4::params::RD_YAW_I>) _param_rd_i_gain_yaw,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamFloat<px4::params::RD_MAX_YAW_RATE>)  _param_rd_max_yaw_rate,
+		(ParamFloat<px4::params::RD_MAX_YAW_ACCEL>) _param_rd_max_yaw_accel,
+		(ParamFloat<px4::params::RD_YAW_RATE_P>)    _param_rd_yaw_rate_p,
+		(ParamFloat<px4::params::RD_YAW_RATE_I>)    _param_rd_yaw_rate_i,
+		(ParamFloat<px4::params::RD_SPEED_P>)       _param_rd_p_gain_speed,
+		(ParamFloat<px4::params::RD_SPEED_I>)       _param_rd_i_gain_speed,
+		(ParamFloat<px4::params::RD_YAW_P>)         _param_rd_p_gain_yaw,
+		(ParamFloat<px4::params::RD_YAW_I>)         _param_rd_i_gain_yaw,
+		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev
 	)
 };

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp

@@ -91,9 +91,9 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f
 			desired_forward_speed = _max_forward_speed;
 
 			if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) {
-				if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) {
+				if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) {
 					desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
-								_param_rd_max_accel.get(), distance_to_curr_wp, 0.0f);
+								_param_rd_max_decel.get(), distance_to_curr_wp, 0.0f);
 					desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed);
 				}
 			}