fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-04 21:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTask: properly initialize EKF reset counters

Browse Source 
This fixes a race condition when switching from a flight mode that is
not a flight task (e.g.: stabilized). In this case, the reset counters
were initialized to 0 and deltas were applied to the first setpoints if
the EKF had any of its reset counters different from 0.
This commit is contained in:
bresch
2025-02-05 13:21:32 -05:00
committed by Daniel Agar
parent de650cab9e
commit 8d296a50f9
3 changed files with 21 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
-3
View File
@@ -374,11 +374,9 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
// Save current setpoints for the next FlightTask
trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
ekf_reset_counters_s last_reset_counters{};
if (isAnyTaskActive()) {
last_setpoint = _current_task.task->getTrajectorySetpoint();
last_reset_counters = _current_task.task->getResetCounters();
}
if (_initTask(new_task_index)) {
@@ -399,7 +397,6 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
return FlightTaskError::ActivationFailed;
}
_current_task.task->setResetCounters(last_reset_counters);
_command_failed = false;
return FlightTaskError::NoError;