differential: reset integrators when disarmed (#23637)

src/modules/rover_differential/RoverDifferential.cpp

@@ -115,6 +115,10 @@ void RoverDifferential::Run()
 		break;
 	}
 
+	if (!_armed) { // Reset on disarm
+		_rover_differential_control.resetControllers();
+	}
+
 	_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
 
 }
@@ -132,6 +136,7 @@ void RoverDifferential::updateSubscriptions()
 		vehicle_status_s vehicle_status{};
 		_vehicle_status_sub.copy(&vehicle_status);
 		_nav_state = vehicle_status.nav_state;
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 	}
 
 	if (_vehicle_angular_velocity_sub.updated()) {

src/modules/rover_differential/RoverDifferential.hpp

@@ -111,6 +111,7 @@ private:
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	int _nav_state{0};
+	bool _armed{false};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp

@@ -174,3 +174,10 @@ matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forwar
 	return Vector2f(forward_speed_normalized - speed_diff_normalized,
 			forward_speed_normalized + speed_diff_normalized);
 }
+
+void RoverDifferentialControl::resetControllers()
+{
+	pid_reset_integral(&_pid_throttle);
+	pid_reset_integral(&_pid_yaw_rate);
+	pid_reset_integral(&_pid_yaw);
+}

src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp

@@ -72,6 +72,11 @@ public:
 	 */
 	void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed);
 
+	/**
+	 * @brief Reset PID controllers
+	 */
+	void resetControllers();
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.