From a7e01b18d86724186949d70162766a7245e01aca Mon Sep 17 00:00:00 2001 From: Mahima Yoga Date: Wed, 30 Jul 2025 17:18:34 +0200 Subject: [PATCH] MC PositionControl: Add timeout for invalid TrajectorySetpoint (#25283) * MulticopterPositionControl: Add timeout before triggering emergency setpoint on invalid TrajectorySetpoint * Apply suggestions from code review Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * 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> --- .../MulticopterPositionControl.cpp | 33 +++++++++++++++---- .../MulticopterPositionControl.hpp | 1 + 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 89731dc7258..0501dadcc81 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -568,14 +568,33 @@ void MulticopterPositionControl::Run() _control.setState(states); - // Run position control - if (!_control.update(dt)) { - // Failsafe - _vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints + const hrt_abstime now = hrt_absolute_time(); - _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); + // Run position control + 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); + } } // Publish internal position control setpoints diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 6d58b85cd7a..5ae11224cf4 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -113,6 +113,7 @@ private: 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 {