From 24811cf550b8bdbfd43f28fdd4d3df026857d480 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 2 Jul 2019 16:39:48 +0200 Subject: [PATCH] FlightTask smoothVel - Initialize ekf reset counters when task is activated --- .../AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 10 ++++++++++ .../AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp | 1 + .../FlightTaskManualAltitudeSmoothVel.cpp | 8 ++++++++ .../FlightTaskManualAltitudeSmoothVel.hpp | 1 + .../FlightTaskManualPositionSmoothVel.cpp | 9 +++++++++ .../FlightTaskManualPositionSmoothVel.hpp | 1 + 6 files changed, 30 insertions(+) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index e2bb34db49..55c84f53b3 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -56,6 +56,7 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s sta _yaw_sp_prev = _yaw; _updateTrajConstraints(); + _initEkfResetCounters(); return ret; } @@ -68,6 +69,7 @@ void FlightTaskAutoLineSmoothVel::reActivate() } _trajectory[2].reset(0.f, 0.7f, _position(2)); + _initEkfResetCounters(); } void FlightTaskAutoLineSmoothVel::_generateSetpoints() @@ -115,6 +117,14 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con return math::constrain(val, min, 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. diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index c4ddf26f80..c178b7c394 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -70,6 +70,7 @@ protected: bool _checkTakeoff() override { return _want_takeoff; }; inline float _constrainOneSide(float val, float constrain); + void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index df75642d99..4b04206e30 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -50,6 +50,7 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint _smoothing.reset(state_prev.acc_z, state_prev.vz, state_prev.z); + _initEkfResetCounters(); _resetPositionLock(); return ret; @@ -61,6 +62,7 @@ void FlightTaskManualAltitudeSmoothVel::reActivate() // using the generated jerk, reset the z derivatives to zero _smoothing.reset(0.f, 0.f, _position(2)); + _initEkfResetCounters(); _resetPositionLock(); } @@ -71,6 +73,12 @@ void FlightTaskManualAltitudeSmoothVel::_resetPositionLock() _position_setpoint_z_locked = NAN; } +void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters() +{ + _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; + _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; +} + void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters() { if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index b6b1534d71..3460d9f08f 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -64,6 +64,7 @@ protected: private: void _resetPositionLock(); + void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ VelocitySmoothing _smoothing; ///< Smoothing in z direction diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 21271bac80..8d70a881d1 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -52,6 +52,7 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint _smoothing[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); } + _initEkfResetCounters(); _resetPositionLock(); return ret; @@ -80,6 +81,14 @@ void FlightTaskManualPositionSmoothVel::_resetPositionLock() _position_setpoint_z_locked = NAN; } +void FlightTaskManualPositionSmoothVel::_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 FlightTaskManualPositionSmoothVel::_checkEkfResetCounters() { // Check if a reset event has happened. diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index 465c7974fd..b500b66bd3 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -63,6 +63,7 @@ protected: ) private: void _resetPositionLock(); + void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions