From 86bf4b2289c1630833c920c84dfc95a82e2ac6b5 Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Wed, 12 Feb 2020 10:46:51 +0100 Subject: [PATCH] FlightTaskManualAltitude: Fix sign error in terrain hold setpoint reset The modified statements are intended to 1. Set a new Z position setpoint that is equivalent to the current distance to ground setpoint, and 2. Set a new distance to ground setpoint that is equivalent to the current Z position setpoint. They are only called in terrain hold mode, when activating/deactivating the holding (typically when coming to a stop and when starting to move again). The setpoints take the current control error into account, but because the control error is added, not subtracted, the result is that the new setpoint is 2 times the control error off from the old setpoint, instead of being at the same spot as the old setpoint. --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 4eb7490c1b..dacdb6ac56 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -136,7 +136,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) { - _position_setpoint(2) = _position(2) + (_dist_to_ground_lock - _dist_to_bottom); + _position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom); } else { _position_setpoint(2) = _position(2); @@ -153,7 +153,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_position_setpoint(2))) { - _dist_to_ground_lock = _dist_to_bottom + (_position_setpoint(2) - _position(2)); + _dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); } } }