From 9ec47f2b7439a5bae638c4dd9098fe114358adb1 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Mon, 30 Apr 2018 09:40:02 +0200 Subject: [PATCH] replace reset counter logic for estimator reset for manual controlled mode --- .../FlightTasks/tasks/FlightTaskManualAltitude.cpp | 10 +++++++++- .../FlightTasks/tasks/FlightTaskManualAltitude.hpp | 2 ++ .../FlightTasks/tasks/FlightTaskManualPosition.cpp | 12 +++++++++++- .../FlightTasks/tasks/FlightTaskManualPosition.hpp | 1 + 4 files changed, 23 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index ffe46b0b50..b4e2cc44d3 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -82,7 +82,15 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // ensure that minimum altitude is respected _respectMinAltitude(); - } else if (!apply_brake) { + } else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) { + // Position is locked but check if a reset event has happened. + // We will shift the setpoints. + if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counter) { + _position_setpoint(2) = _position(2); + _reset_counter = _sub_vehicle_local_position->get().z_reset_counter; + } + + } else { // user demands velocity change _position_setpoint(2) = NAN; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index f1940019d9..ef45a1b0d6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -65,6 +65,8 @@ protected: (ParamInt) MPC_ALT_MODE ) private: + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ + /** * Distance to ground during terrain following. * If user does not demand velocity change in D-direction and the vehcile diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index 9d264a9218..3809bc03d2 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -111,7 +111,17 @@ void FlightTaskManualPosition::_updateXYlock() _position_setpoint(0) = _position(0); _position_setpoint(1) = _position(1); - } else if (!apply_brake) { + } else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) { + // Position is locked but check if a reset event has happened. + // We will shift the setpoints. + if (_sub_vehicle_local_position->get().xy_reset_counter + != _reset_counter) { + _position_setpoint(0) = _position(0); + _position_setpoint(1) = _position(1); + _reset_counter = _sub_vehicle_local_position->get().xy_reset_counter; + } + + } else { /* don't lock*/ _position_setpoint(0) = NAN; _position_setpoint(1) = NAN; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp index 29dedd6b2f..ff789eaa04 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp @@ -63,4 +63,5 @@ protected: ) private: float _velocity_scale; //scales the stick input to velocity + uint8_t _reset_counter = 0; /**< counter for estimator resets in xy-direction */ };