fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 12:14:19 +01:00 committed by Daniel Agar
parent de650cab9e
commit 8d296a50f9
3 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/flight_mode_manager/FlightModeManager.cpp
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;

11
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
View File

@ -11,6 +11,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
{
_resetSetpoints();
_setDefaultConstraints();
initEkfResetCounters();
_time_stamp_activate = hrt_absolute_time();
_gear = empty_landing_gear_default_keep;
return true;
@ -24,6 +25,16 @@ void FlightTask::reActivate()
activate(setpoint_preserve_vertical);
}
void FlightTask::initEkfResetCounters()
{
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
}
bool FlightTask::updateInitialize()
{
_time_stamp_current = hrt_absolute_time();

23
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View File

@ -55,15 +55,6 @@
#include <uORB/topics/home_position.h>
#include <lib/geo/geo.h>
struct ekf_reset_counters_s {
uint8_t xy;
uint8_t vxy;
uint8_t z;
uint8_t vz;
uint8_t heading;
uint8_t hagl;
};
class FlightTask : public ModuleParams
{
public:
@ -88,6 +79,8 @@ public:
*/
virtual void reActivate();
virtual void initEkfResetCounters();
/**
* To be called to adopt parameters from an arrived vehicle command
* @param command received command message containing the parameters
@ -115,9 +108,6 @@ public:
*/
const trajectory_setpoint_s getTrajectorySetpoint();
const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
/**
* Get vehicle constraints.
* The constraints can vary with task.
@ -236,7 +226,14 @@ protected:
float _yaw_setpoint{};
float _yawspeed_setpoint{};
ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
struct ekf_reset_counters_s {
uint8_t xy;
uint8_t vxy;
uint8_t z;
uint8_t vz;
uint8_t heading;
uint8_t hagl;
} _reset_counters{}; ///< Counters for estimator local position resets
/**
* Vehicle constraints.