differential: fix closed loop control

removed thresholds for closed loop setpoints and added minimum thresholds for yaw rate and speed measurements instead to avoid moving due to measurement noise

src/modules/rover_differential/RoverDifferential.cpp

@@ -142,7 +142,7 @@ void RoverDifferential::updateSubscriptions()
 	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];
+		_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f;
 	}
 
 	if (_vehicle_attitude_sub.updated()) {
@@ -157,7 +157,7 @@ void RoverDifferential::updateSubscriptions()
 		_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);
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
 	}
 }
 

src/modules/rover_differential/RoverDifferential.hpp

@@ -113,6 +113,12 @@ class RoverDifferential : public ModuleBase<RoverDifferential>, public ModulePar
 	int _nav_state{0};
 	bool _armed{false};
 
+	// Thresholds to avoid moving at rest due to measurement noise
+	static constexpr float YAW_RATE_THRESHOLD =
+		0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
+	static constexpr float SPEED_THRESHOLD =
+		0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
+
 	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

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -84,32 +84,20 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 
 	// Closed loop yaw control (Overrides yaw rate 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);
-
-		} else {
-			const float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
-			_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
-		}
+		float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
+		_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
 	}
 
 	// Yaw rate control
 	float speed_diff_normalized{0.f};
 
 	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);
-
-		} else {
-			const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _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, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
-								-1.f, 1.f); // Feedback
-		}
+		const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _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, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
+							-1.f, 1.f); // Feedback
 
 	} else { // Use normalized setpoint
 		speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ?
@@ -120,20 +108,15 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	float throttle{0.f};
 
 	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);
-
-		} else {
-			throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
-						 dt);
-
-			if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term
-				throttle += math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
-								     0.f, _param_rd_max_speed.get(),
-								     0.f, 1.f);
-			}
+		if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feedforward
+			throttle += math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
+							     0.f, _param_rd_max_speed.get(),
+							     0.f, 1.f);
 		}
 
+		throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
+					 dt); // Feedback
+
 	} 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;