From c811cf4784a06bc4b8a6efe317df89ec3f9d8a3d Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 23 Sep 2019 13:42:06 +0200 Subject: [PATCH] FlightTask - Move ekf reset counter monitoring logic in the base FlihtTask Each child FlightTask can simply implement what it wants to do in case of an EKF reset event by overriding one of the _ekfResetHandler functions --- .../FlightTaskAutoLineSmoothVel.cpp | 69 +++++++++---------- .../FlightTaskAutoLineSmoothVel.hpp | 15 ++-- .../tasks/FlightTask/FlightTask.cpp | 42 ++++++++++- .../tasks/FlightTask/FlightTask.hpp | 24 ++++++- .../FlightTaskManualAltitude.cpp | 15 ++-- .../FlightTaskManualAltitude.hpp | 1 + .../FlightTaskManualAltitudeSmoothVel.cpp | 30 ++------ .../FlightTaskManualAltitudeSmoothVel.hpp | 12 ++-- .../FlightTaskManualPositionSmoothVel.cpp | 60 +++------------- .../FlightTaskManualPositionSmoothVel.hpp | 22 ++---- 10 files changed, 138 insertions(+), 152 deletions(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index adf8cae8cb..ac770cc509 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -56,7 +56,6 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s las _yaw_sp_prev = last_setpoint.yaw; _updateTrajConstraints(); - _initEkfResetCounters(); return ret; } @@ -69,7 +68,6 @@ void FlightTaskAutoLineSmoothVel::reActivate() } _trajectory[2].reset(0.f, 0.7f, _position(2)); - _initEkfResetCounters(); } void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) @@ -98,6 +96,38 @@ void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } } +/** + * EKF reset handling functions + * Those functions are called by the base FlightTask in + * case of an EKF reset event + */ +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY() +{ + _trajectory[0].setCurrentPosition(_position(0)); + _trajectory[1].setCurrentPosition(_position(1)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY() +{ + _trajectory[0].setCurrentVelocity(_velocity(0)); + _trajectory[1].setCurrentVelocity(_velocity(1)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ() +{ + _trajectory[2].setCurrentPosition(_position(2)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ() +{ + _trajectory[2].setCurrentVelocity(_velocity(2)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi) +{ + _yaw_sp_prev += delta_psi; +} + void FlightTaskAutoLineSmoothVel::_generateSetpoints() { _prepareSetpoints(); @@ -151,40 +181,6 @@ float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max)); } -void FlightTaskAutoLineSmoothVel::_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.vz = _sub_vehicle_local_position->get().vz_reset_counter; -} - -void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters() -{ - // Check if a reset event has happened. - if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) { - _trajectory[0].setCurrentPosition(_position(0)); - _trajectory[1].setCurrentPosition(_position(1)); - _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; - } - - if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) { - _trajectory[0].setCurrentVelocity(_velocity(0)); - _trajectory[1].setCurrentVelocity(_velocity(1)); - _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; - } - - if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { - _trajectory[2].setCurrentPosition(_position(2)); - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; - } - - if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) { - _trajectory[2].setCurrentVelocity(_velocity(2)); - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; - } -} - float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() { // Compute the maximum allowed speed at the waypoint assuming that we want to @@ -239,7 +235,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() // that one is used as a velocity limit. // If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here. - _checkEkfResetCounters(); _want_takeoff = false; if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) { diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 06b2957473..507f06cb19 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -71,12 +71,16 @@ protected: /** determines when to trigger a takeoff (ignored in flight) */ bool _checkTakeoff() override { return _want_takeoff; }; + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionXY() override; + void _ekfResetHandlerVelocityXY() override; + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + void _ekfResetHandlerHeading(float delta_psi) override; inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */ - void _initEkfResetCounters(); - void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ void _updateTrajConstraints(); @@ -86,11 +90,4 @@ protected: bool _want_takeoff{false}; - /* counters for estimator local position resets */ - struct { - uint8_t xy; - uint8_t vxy; - uint8_t z; - uint8_t vz; - } _reset_counters{0, 0, 0, 0}; }; diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index b29cd3f573..6c2d6ed7ed 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -26,8 +26,8 @@ bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint) { _resetSetpoints(); _setDefaultConstraints(); + _initEkfResetCounters(); _time_stamp_activate = hrt_absolute_time(); - _heading_reset_counter = _sub_attitude->get().quat_reset_counter; _gear = empty_landing_gear_default_keep; return true; } @@ -44,9 +44,49 @@ bool FlightTask::updateInitialize() _deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f; _time_stamp_last = _time_stamp_current; _evaluateVehicleLocalPosition(); + _checkEkfResetCounters(); return true; } +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.vz = _sub_vehicle_local_position->get().vz_reset_counter; + _reset_counters.quat = _sub_attitude->get().quat_reset_counter; +} + +void FlightTask::_checkEkfResetCounters() +{ + // Check if a reset event has happened + if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) { + _ekfResetHandlerPositionXY(); + _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; + } + + if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) { + _ekfResetHandlerVelocityXY(); + _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; + } + + if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { + _ekfResetHandlerPositionZ(); + _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; + } + + if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) { + _ekfResetHandlerVelocityZ(); + _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; + } + + if (_sub_attitude->get().quat_reset_counter != _reset_counters.quat) { + float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); + _ekfResetHandlerHeading(delta_psi); + _reset_counters.quat = _sub_attitude->get().quat_reset_counter; + } +} + const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() { /* fill position setpoint message */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 6de157339f..6bfdc31e20 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -178,7 +178,6 @@ protected: uORB::SubscriptionPollable *_sub_vehicle_local_position{nullptr}; uORB::SubscriptionPollable *_sub_attitude{nullptr}; - uint8_t _heading_reset_counter{0}; /**< estimator heading reset */ /** Reset all setpoints to NAN */ void _resetSetpoints(); @@ -189,9 +188,21 @@ protected: /** Set constraints to default values */ virtual void _setDefaultConstraints(); - /** determines when to trigger a takeoff (ignored in flight) */ + /** Determine when to trigger a takeoff (ignored in flight) */ virtual bool _checkTakeoff(); + /** + * Monitor the EKF reset counters and + * call the appropriate handling functions in case of a reset event + */ + void _initEkfResetCounters(); + void _checkEkfResetCounters(); + virtual void _ekfResetHandlerPositionXY() {}; + virtual void _ekfResetHandlerVelocityXY() {}; + virtual void _ekfResetHandlerPositionZ() {}; + virtual void _ekfResetHandlerVelocityZ() {}; + virtual void _ekfResetHandlerHeading(float delta_psi) {}; + /* Time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ float _time = 0; /**< passed time in seconds since the task was activated */ @@ -225,6 +236,15 @@ protected: matrix::Vector3f _velocity_setpoint_feedback; matrix::Vector3f _thrust_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{0, 0, 0, 0, 0}; + /** * Vehicle constraints. * The constraints can vary with tasks. diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 5a030d802e..8ee135ba3f 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -310,17 +310,18 @@ void FlightTaskManualAltitude::_updateHeadingSetpoints() // hold the current heading when no more rotation commanded if (!PX4_ISFINITE(_yaw_setpoint)) { _yaw_setpoint = _yaw; - - } else { - // check reset counter and update yaw setpoint if necessary - if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) { - _yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); - _heading_reset_counter = _sub_attitude->get().quat_reset_counter; - } } } } +void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) +{ + // Only reset the yaw setpoint when the heading is locked + if (PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint += delta_psi; + } +} + void FlightTaskManualAltitude::_updateSetpoints() { _updateHeadingSetpoints(); // get yaw setpoint diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 8460719fda..faddb0dde0 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -54,6 +54,7 @@ public: protected: void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ + void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ bool _checkTakeoff() override; diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 9a1328d024..fb19e80d93 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -50,8 +50,6 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint _smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z); - _initEkfResetCounters(); - return ret; } @@ -60,8 +58,6 @@ void FlightTaskManualAltitudeSmoothVel::reActivate() // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly // using the generated jerk, reset the z derivatives to zero _smoothing.reset(0.f, 0.f, _position(2)); - - _initEkfResetCounters(); } void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) @@ -76,17 +72,18 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } } -void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters() +void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ() { - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; + _smoothing.setCurrentPosition(_position(2)); +} + +void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ() +{ + _smoothing.setCurrentVelocity(_velocity(2)); } void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() { - // Reset trajectories if EKF did a reset - _checkEkfResetCounters(); - // Set max accel/vel/jerk // Has to be done before _updateTrajectories() _updateTrajConstraints(); @@ -104,19 +101,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() _setOutputState(); } -void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters() -{ - if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { - _smoothing.setCurrentPosition(_position(2)); - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; - } - - if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) { - _smoothing.setCurrentVelocity(_velocity(2)); - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; - } -} - void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints() { _smoothing.setMaxJerk(_param_mpc_jerk_max.get()); diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 28bd8b37be..9be1385ddb 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -55,6 +55,10 @@ protected: virtual void _updateSetpoints() override; + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + DEFINE_PARAMETERS( (ParamFloat) _param_mpc_jerk_max, (ParamFloat) _param_mpc_acc_up_max, @@ -65,16 +69,8 @@ private: void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - void _initEkfResetCounters(); - void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _updateTrajConstraints(); void _setOutputState(); ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction - - /* counters for estimator local position resets */ - struct { - uint8_t z; - uint8_t vz; - } _reset_counters{0, 0}; }; diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 9b172d1a3c..3e9b7394d8 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -51,8 +51,6 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint _smoothing_xy.reset(accel_prev, vel_prev, pos_prev); _smoothing_z.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z); - _initEkfResetCounters(); - return ret; } @@ -62,8 +60,6 @@ void FlightTaskManualPositionSmoothVel::reActivate() // using the generated jerk, reset the z derivatives to zero _smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position)); _smoothing_z.reset(0.f, 0.f, _position(2)); - - _initEkfResetCounters(); } void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) @@ -90,29 +86,28 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } } -void FlightTaskManualPositionSmoothVel::_initEkfResetCounters() +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY() { - _initEkfResetCountersXY(); - _initEkfResetCountersZ(); + _smoothing_xy.setCurrentPosition(_position.xy()); } -void FlightTaskManualPositionSmoothVel::_initEkfResetCountersXY() +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY() { - _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; - _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; + _smoothing_xy.setCurrentVelocity(_velocity.xy()); } -void FlightTaskManualPositionSmoothVel::_initEkfResetCountersZ() +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ() { - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; + _smoothing_z.setCurrentPosition(_position(2)); +} + +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ() +{ + _smoothing_z.setCurrentVelocity(_velocity(2)); } void FlightTaskManualPositionSmoothVel::_updateSetpoints() { - // Reset trajectories if EKF did a reset - _checkEkfResetCounters(); - // Set max accel/vel/jerk // Has to be done before _updateTrajectories() _updateTrajConstraints(); @@ -128,39 +123,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _setOutputState(); } -void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters() -{ - // Check if a reset event has happened. - _checkEkfResetCountersXY(); - _checkEkfResetCountersZ(); -} - -void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersXY() -{ - if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) { - _smoothing_xy.setCurrentPosition(Vector2f(_position)); - _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; - } - - if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) { - _smoothing_xy.setCurrentVelocity(Vector2f(_velocity)); - _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; - } -} - -void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersZ() -{ - if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { - _smoothing_z.setCurrentPosition(_position(2)); - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; - } - - if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) { - _smoothing_z.setCurrentVelocity(_velocity(2)); - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; - } -} - void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() { _updateTrajConstraintsXY(); diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index b6399645c5..5427563ec5 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -60,6 +60,12 @@ protected: virtual void _updateSetpoints() override; + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionXY() override; + void _ekfResetHandlerVelocityXY() override; + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, (ParamFloat) _param_mpc_jerk_max, (ParamFloat) _param_mpc_acc_up_max, @@ -69,14 +75,6 @@ protected: private: void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - void _initEkfResetCounters(); - void _initEkfResetCountersXY(); - void _initEkfResetCountersZ(); - - void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ - void _checkEkfResetCountersXY(); - void _checkEkfResetCountersZ(); - void _updateTrajConstraints(); void _updateTrajConstraintsXY(); void _updateTrajConstraintsZ(); @@ -92,12 +90,4 @@ private: ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction - - /* counters for estimator local position resets */ - struct { - uint8_t xy; - uint8_t vxy; - uint8_t z; - uint8_t vz; - } _reset_counters{0, 0, 0, 0}; };