ackermann: reset controllers on disarm

chfriedrich98 · Sep 2, 2024 · 3e90f51 · 3e90f51
1 parent 60b27c0
commit 3e90f51
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 3 deletions.
diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp
@@ -66,6 +66,8 @@ void RoverAckermann::Run()
 	hrt_abstime timestamp = hrt_absolute_time();
 
 	if (_armed) {
+		_was_armed = true;
+
 		switch (_nav_state) {
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
 				manual_control_setpoint_s manual_control_setpoint{};
@@ -98,18 +100,20 @@ void RoverAckermann::Run()
 			break;
 		}
 
-	} else { // Reset on disarm
+		_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
+
+	} else if (_was_armed) { // Reset on disarm
+		_was_armed = false;
 		rover_ackermann_setpoint_s rover_ackermann_setpoint{};
 		rover_ackermann_setpoint.timestamp =  timestamp;
 		rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
 		rover_ackermann_setpoint.forward_speed_setpoint_normalized =  0.f;
 		rover_ackermann_setpoint.steering_setpoint = NAN;
 		rover_ackermann_setpoint.steering_setpoint_normalized = 0.f;
 		_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
+		_ackermann_control.resetControllers();
 	}
 
-	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
-
 }
 
 void RoverAckermann::updateSubscriptions()

diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp
@@ -114,5 +114,6 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	float _vehicle_forward_speed{0.f};
 	float _vehicle_yaw{0.f};
 	bool _armed{false};
+	bool _was_armed{false};
 
 };
diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp
@@ -185,3 +185,10 @@ void RoverAckermannControl::publishMotorSetpoints(motor_setpoint_struct motor_se
 	actuator_servos.timestamp = _timestamp;
 	_actuator_servos_pub.publish(actuator_servos);
 }
+
+void RoverAckermannControl::resetControllers()
+{
+	pid_reset_integral(&_pid_throttle);
+	_throttle_with_accel_limit.setForcedValue(0.f);
+	_steering_with_rate_limit.setForcedValue(0.f);
+}
diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp
@@ -74,6 +74,11 @@ class RoverAckermannControl : public ModuleParams
 	 */
 	void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw);
 
+	/**
+	 * @brief Reset PID controllers and slew rates
+	 */
+	void resetControllers();
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.