From be2bb4a479e5d87ebb1903c182736f6ca8328b4f Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Wed, 22 Apr 2020 13:18:35 +0200 Subject: [PATCH] FlightTask: Fix ekf2 reset race condition during task switch (#14692) * FlightTask: Fix ekf2 reset race condition during task switch During a loss of GPS data when using GPS as primary height source, the height is reset to baro and the local position gets invalid at the same time. This triggers a switch to altitude flight task and a setpoint reset. This combination of events had the effect to ignore the height reset, the large sudden height error could create an abrupt change of altitude or even a crash. The ekf2 reset is now done at the beginning of each update call. --- src/lib/flight_tasks/FlightTasks.cpp | 15 +++++++++- src/lib/flight_tasks/FlightTasks.hpp | 6 ++++ .../AutoFollowMe/FlightTaskAutoFollowMe.cpp | 5 ++-- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 3 +- .../tasks/Descend/FlightTaskDescend.cpp | 4 ++- .../tasks/Failsafe/FlightTaskFailsafe.cpp | 4 ++- .../tasks/FlightTask/FlightTask.cpp | 12 +++----- .../tasks/FlightTask/FlightTask.hpp | 29 ++++++++++++------- .../FlightTaskManualAltitude.cpp | 3 +- .../tasks/Offboard/FlightTaskOffboard.cpp | 12 ++++---- .../tasks/Orbit/FlightTaskOrbit.cpp | 4 +-- .../tasks/Transition/FlightTaskTransition.cpp | 3 +- .../Utility/ManualVelocitySmoothingXY.hpp | 14 +++++---- .../Utility/ManualVelocitySmoothingZ.hpp | 13 ++++++--- 14 files changed, 85 insertions(+), 42 deletions(-) diff --git a/src/lib/flight_tasks/FlightTasks.cpp b/src/lib/flight_tasks/FlightTasks.cpp index 8f6cd60716..b6aacf3d69 100644 --- a/src/lib/flight_tasks/FlightTasks.cpp +++ b/src/lib/flight_tasks/FlightTasks.cpp @@ -36,6 +36,16 @@ const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint() } } +const ekf_reset_counters_s FlightTasks::getResetCounters() +{ + if (isAnyTaskActive()) { + return _current_task.task->getResetCounters(); + + } else { + return FlightTask::zero_reset_counters; + } +} + const vehicle_constraints_s FlightTasks::getConstraints() { if (isAnyTaskActive()) { @@ -64,7 +74,8 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index) } // Save current setpoints for the next FlightTask - vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint(); + const vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint(); + const ekf_reset_counters_s last_reset_counters = getResetCounters(); if (_initTask(new_task_index)) { // invalid task @@ -84,6 +95,8 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index) return FlightTaskError::ActivationFailed; } + _current_task.task->setResetCounters(last_reset_counters); + return FlightTaskError::NoError; } diff --git a/src/lib/flight_tasks/FlightTasks.hpp b/src/lib/flight_tasks/FlightTasks.hpp index 7215557965..54c1af6154 100644 --- a/src/lib/flight_tasks/FlightTasks.hpp +++ b/src/lib/flight_tasks/FlightTasks.hpp @@ -81,6 +81,12 @@ public: */ const vehicle_local_position_setpoint_s getPositionSetpoint(); + /** + * Get the local frame and attitude reset counters of the estimator from the current task + * @return the reset counters + */ + const ekf_reset_counters_s getResetCounters(); + /** * Get task dependent constraints * @return setpoint constraints that has to be respected by the position controller diff --git a/src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp b/src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp index 4487256735..dda7374f66 100644 --- a/src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp +++ b/src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp @@ -42,8 +42,9 @@ using namespace matrix; bool FlightTaskAutoFollowMe::update() { + bool ret = FlightTaskAuto::update(); _position_setpoint = _target; matrix::Vector2f vel_sp = _getTargetVelocityXY(); _velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f); - return true; -} \ No newline at end of file + return ret; +} diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 7ae06d5108..3c342bbed1 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -49,6 +49,7 @@ bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpo bool FlightTaskAutoMapper::update() { + bool ret = FlightTaskAuto::update(); // always reset constraints because they might change depending on the type _setDefaultConstraints(); @@ -107,7 +108,7 @@ bool FlightTaskAutoMapper::update() // update previous type _type_previous = _type; - return true; + return ret; } void FlightTaskAutoMapper::_reset() diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp index a28b1fec5a..735e4d2318 100644 --- a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp +++ b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp @@ -48,6 +48,8 @@ bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint bool FlightTaskDescend::update() { + bool ret = FlightTask::update(); + if (PX4_ISFINITE(_velocity(2))) { // land with landspeed _velocity_setpoint(2) = _param_mpc_land_speed.get(); @@ -59,5 +61,5 @@ bool FlightTaskDescend::update() _acceleration_setpoint(2) = .3f; } - return true; + return ret; } diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp index 8ed6f6c58e..2911f9e5f1 100644 --- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp +++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -49,6 +49,8 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin bool FlightTaskFailsafe::update() { + bool ret = FlightTask::update(); + if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) { // stay at current position setpoint _velocity_setpoint(0) = _velocity_setpoint(1) = 0.f; @@ -72,5 +74,5 @@ bool FlightTaskFailsafe::update() _acceleration_setpoint(2) = NAN; } - return true; + return ret; } diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index 95bf42402f..5fcac4b003 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -6,6 +6,7 @@ constexpr uint64_t FlightTask::_timeout; // First index of empty_setpoint corresponds to time-stamp and requires a finite number. const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}}; +const ekf_reset_counters_s FlightTask::zero_reset_counters = {}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; @@ -13,7 +14,6 @@ bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint) { _resetSetpoints(); _setDefaultConstraints(); - _initEkfResetCounters(); _time_stamp_activate = hrt_absolute_time(); _gear = empty_landing_gear_default_keep; return true; @@ -37,17 +37,13 @@ bool FlightTask::updateInitialize() _evaluateVehicleLocalPosition(); _evaluateDistanceToGround(); - _checkEkfResetCounters(); return true; } -void FlightTask::_initEkfResetCounters() +bool FlightTask::update() { - _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.vz = _sub_vehicle_local_position.get().vz_reset_counter; - _reset_counters.quat = _sub_attitude.get().quat_reset_counter; + _checkEkfResetCounters(); + return true; } void FlightTask::_checkEkfResetCounters() diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp index c3db9c8aca..f3a3b44eb0 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp @@ -55,6 +55,14 @@ #include #include +struct ekf_reset_counters_s { + uint8_t xy; + uint8_t vxy; + uint8_t z; + uint8_t vz; + uint8_t quat; +}; + class FlightTask : public ModuleParams { public: @@ -97,7 +105,7 @@ public: * To be called regularly in the control loop cycle to execute the task * @return true on success, false on error */ - virtual bool update() = 0; + virtual bool update(); /** * Call after update() @@ -113,6 +121,9 @@ public: */ const vehicle_local_position_setpoint_s getPositionSetpoint(); + 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. @@ -139,6 +150,11 @@ public: */ static const vehicle_local_position_setpoint_s empty_setpoint; + /**. + * All counters are set to 0. + */ + static const ekf_reset_counters_s zero_reset_counters; + /** * Empty constraints. * All constraints are set to NAN. @@ -193,8 +209,8 @@ protected: /** * Monitor the EKF reset counters and * call the appropriate handling functions in case of a reset event + * TODO: add the delta values to all the handlers */ - void _initEkfResetCounters(); void _checkEkfResetCounters(); virtual void _ekfResetHandlerPositionXY() {}; virtual void _ekfResetHandlerVelocityXY() {}; @@ -234,14 +250,7 @@ protected: matrix::Vector3f _velocity_setpoint_feedback; matrix::Vector3f _acceleration_setpoint_feedback; - /* Counters for estimator local position resets */ - struct { - uint8_t xy; - uint8_t vxy; - uint8_t z; - uint8_t vz; - uint8_t quat; - } _reset_counters{}; + ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets /** * Vehicle constraints. diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 48ccc4dde6..85be3474e4 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -361,10 +361,11 @@ bool FlightTaskManualAltitude::_checkTakeoff() bool FlightTaskManualAltitude::update() { + bool ret = FlightTaskManual::update(); _updateConstraintsFromEstimator(); _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); - return true; + return ret; } diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp index ca45d56302..8b6cd46a84 100644 --- a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -68,6 +68,8 @@ bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s last_setpoin bool FlightTaskOffboard::update() { + bool ret = FlightTask::update(); + // reset setpoint for every loop _resetSetpoints(); @@ -106,7 +108,7 @@ bool FlightTaskOffboard::update() } // don't have to continue - return true; + return ret; } else { _position_lock.setAll(NAN); @@ -124,7 +126,7 @@ bool FlightTaskOffboard::update() } // don't have to continue - return true; + return ret; } else { _position_lock.setAll(NAN); @@ -144,7 +146,7 @@ bool FlightTaskOffboard::update() } // don't have to continue - return true; + return ret; } else { _position_lock.setAll(NAN); @@ -155,7 +157,7 @@ bool FlightTaskOffboard::update() _position_setpoint.setNaN(); // Don't require any position/velocity setpoints _velocity_setpoint.setNaN(); _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust - return true; + return ret; } // Possible inputs: @@ -240,5 +242,5 @@ bool FlightTaskOffboard::update() // use default conditions of upwards position or velocity to take off _constraints.want_takeoff = _checkTakeoff(); - return true; + return ret; } diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 30af1448ab..f742c2956a 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -162,7 +162,7 @@ bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint) bool FlightTaskOrbit::update() { // update altitude - FlightTaskManualAltitudeSmooth::update(); + bool ret = FlightTaskManualAltitudeSmooth::update(); // stick input adjusts parameters within a fixed time frame const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f); @@ -186,7 +186,7 @@ bool FlightTaskOrbit::update() // publish information to UI sendTelemetry(); - return true; + return ret; } void FlightTaskOrbit::generate_circle_approach_setpoints() diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp index 3f67c23f51..0288e79558 100644 --- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp @@ -60,6 +60,7 @@ void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &set bool FlightTaskTransition::update() { + bool ret = FlightTask::update(); _acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f); // demand zero vertical velocity and level attitude // tailsitters will override attitude and thrust setpoint @@ -68,5 +69,5 @@ bool FlightTaskTransition::update() _velocity_setpoint(2) = 0.0f; _yaw_setpoint = NAN; - return true; + return ret; } diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp index 9ff5c1a397..aa35cabe9d 100644 --- a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp +++ b/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp @@ -92,7 +92,11 @@ public: _state.x = pos; _trajectory[0].setCurrentPosition(pos(0)); _trajectory[1].setCurrentPosition(pos(1)); - // TODO: check lock/unlock in case of EKF reset + _position_estimate = pos; + + if (_position_lock_active) { + _position_setpoint_locked = pos; + } } Vector2f getCurrentPosition() const { return _position_setpoint_locked; } @@ -108,15 +112,15 @@ private: bool _position_lock_active{false}; - Vector2f _position_setpoint_locked; + Vector2f _position_setpoint_locked{}; - Vector2f _velocity_setpoint_feedback; - Vector2f _position_estimate; + Vector2f _velocity_setpoint_feedback{}; + Vector2f _position_estimate{}; struct { Vector2f j; Vector2f a; Vector2f v; Vector2f x; - } _state; + } _state{}; }; diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp index b24ee78639..affeaeb98b 100644 --- a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp +++ b/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp @@ -84,6 +84,11 @@ public: { _state.x = pos; _trajectory.setCurrentPosition(pos); + _position_estimate = pos; + + if (_position_lock_active) { + _position_setpoint_locked = pos; + } } float getCurrentPosition() const { return _position_setpoint_locked; } void setCurrentPositionEstimate(float pos) { _position_estimate = pos; } @@ -99,17 +104,17 @@ private: bool _position_lock_active{false}; - float _position_setpoint_locked; + float _position_setpoint_locked{}; - float _velocity_setpoint_feedback; - float _position_estimate; + float _velocity_setpoint_feedback{}; + float _position_estimate{}; struct { float j; float a; float v; float x; - } _state; + } _state{}; float _max_accel_up{0.f}; float _max_accel_down{0.f};