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}; };