dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Morten Fyhn Amundsen 2020-02-12 10:46:51 +01:00 committed by Mathieu Bresciani
parent d7c3e1066a
commit 86bf4b2289
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@ -136,7 +136,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
// Adjust the setpoint to maintain the same height error to reduce control transients // 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)) { 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 { } else {
_position_setpoint(2) = _position(2); _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 // Adjust the setpoint to maintain the same height error to reduce control transients
if (PX4_ISFINITE(_position_setpoint(2))) { 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));
} }
} }
} }