diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 4a7dcde579..d1ffe3d496 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -117,19 +117,39 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con return math::constrain(val, min, max); } +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; + } +} + void FlightTaskAutoLineSmoothVel::_prepareSetpoints() { // Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified // 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. - // Check if a reset event has happened. - if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) { - // Reset the XY axes - _trajectory[0].setCurrentPosition(_position(0)); - _trajectory[1].setCurrentPosition(_position(1)); - _reset_counter = _sub_vehicle_local_position->get().xy_reset_counter; - } + _checkEkfResetCounters(); if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 39ea776b42..17c2b6e617 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -69,12 +69,20 @@ protected: void _setDefaultConstraints() override; inline float _constrainOneSide(float val, float constrain); + void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ void _updateTrajConstraints(); void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ void _generateTrajectory(); VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions - float _yaw_sp_prev{NAN}; - uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + float _yaw_sp_prev; + + /* 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/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 0aca829607..709d489773 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -89,6 +89,32 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero) _position_setpoint_z_locked = NAN; } +void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters() +{ + // Check if a reset event has happened. + if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) { + _smoothing[0].setCurrentPosition(_position(0)); + _smoothing[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) { + _smoothing[0].setCurrentVelocity(_velocity(0)); + _smoothing[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) { + _smoothing[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) { + _smoothing[2].setCurrentVelocity(_velocity(2)); + _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; + } +} + void FlightTaskManualPositionSmoothVel::_updateSetpoints() { /* Get yaw setpont, un-smoothed position setpoints.*/ @@ -111,6 +137,8 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()}; + _checkEkfResetCounters(); + /* Check for position unlock * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint * is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null @@ -161,16 +189,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only - if (_position_lock_xy_active) { - // Check if a reset event has happened. - if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) { - // Reset the XY axes - _smoothing[0].setCurrentPosition(_position(0)); - _smoothing[1].setCurrentPosition(_position(1)); - _reset_counter = _sub_vehicle_local_position->get().xy_reset_counter; - } - } - if (!_position_lock_xy_active) { _smoothing[0].setCurrentPosition(_position(0)); _smoothing[1].setCurrentPosition(_position(1)); diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index 038d9ddd85..ac6a77508b 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -70,6 +70,7 @@ private: * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states */ void reset(Axes axes, bool force_z_zero = false); + void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions matrix::Vector3f _vel_sp_smooth; @@ -78,5 +79,11 @@ private: matrix::Vector2f _position_setpoint_xy_locked; float _position_setpoint_z_locked{NAN}; - uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-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}; };