diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 20cbf7d9e3e2..502c38de2a44 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/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()) { diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 897a7199ef4d..8dfd4f7affff 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -111,6 +111,7 @@ class RoverDifferential : public ModuleBase, public ModulePar float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; int _nav_state{0}; + bool _armed{false}; DEFINE_PARAMETERS( (ParamFloat) _param_rd_man_yaw_scale, diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index c1f96c0ce066..8b16020a0a05 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/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); +} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp index d3ef7a076fd6..9f49186ad8b6 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -72,6 +72,11 @@ class RoverDifferentialControl : public ModuleParams */ 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.