Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 09:26:25 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

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

Browse Source 
* 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>
This commit is contained in:
Mahima Yoga
2025-07-30 17:18:34 +02:00
committed by GitHub
parent 5736b948ac
commit a7e01b18d8
2 changed files with 27 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+26 -7
View File
@@ -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
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+1
View File
@@ -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 {