MC PositionControl: Add timeout for invalid TrajectorySetpoint (#25283) (#25341)

* MulticopterPositionControl: Add timeout before triggering emergency setpoint on invalid TrajectorySetpoint

* Apply suggestions from code review



* Cleanup & address review comments

* Safegaurd against using old setpoint if states aren't valid anymore

---------

Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

Author: mahima-yoga

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -565,14 +565,33 @@ void MulticopterPositionControl::Run()
 
 			_control.setState(states);
 
+			const hrt_abstime now = hrt_absolute_time();
+
 			// Run position control
-			if (!_control.update(dt)) {
-				// Failsafe
-				_vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints
+			if (_control.update(dt)) {
+
+				// Valid control update - store for fallback
+				_last_valid_setpoint = _setpoint;
+
+			} else {
+
+				// Initial update failed - Try fallback if within timeout
+				if (now < _last_valid_setpoint.timestamp + 200_ms) {
+					// Use last valid setpoint
+					adjustSetpointForEKFResets(vehicle_local_position, _last_valid_setpoint);
+					_control.setInputSetpoint(_last_valid_setpoint);
+				}
+
+				// Still failing / not within timeout - Go to failsafe
+				if (!_control.update(dt)) {
+
+					_vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints
 
-				_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true));
-				_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
-				_control.update(dt);
+					_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true));
+					_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
+
+					_control.update(dt);
+				}
 			}
 
 			// Publish internal position control setpoints

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -113,6 +113,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 	hrt_abstime _time_position_control_enabled{0};
 
 	trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint};
+	trajectory_setpoint_s _last_valid_setpoint{PositionControl::empty_trajectory_setpoint};
 	vehicle_control_mode_s _vehicle_control_mode{};
 
 	vehicle_constraints_s _vehicle_constraints {