From c4c01358b96d7a03ff00dd1358052522c0ade9a7 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 24 May 2018 18:03:21 +0200 Subject: [PATCH] FlightTaskManualAltitude: do terrain following when distance sensor active and below minimum distance to ground. --- .../tasks/FlightTaskManualAltitude.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 1884d39581..f5c54c9cf8 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -84,14 +84,20 @@ void FlightTaskManualAltitude::_updateAltitudeLock() _terrain_following(apply_brake, stopped); } else { - // altitude based on locale coordinate system - _dist_to_ground_lock = NAN; // reset since not used + // normal mode where height is dependent on local frame if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) { // lock position _position_setpoint(2) = _position(2); - // ensure that minimum altitude is respected - _respectMinAltitude(); + + // Ensure that minimum altitude is respected if + // there is a distance sensor and distance to bottome is below minimum. + if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get()) { + _terrain_following(apply_brake, stopped); + + } else { + _dist_to_ground_lock = NAN; + } } else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) { // Position is locked but check if a reset event has happened. @@ -110,7 +116,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() void FlightTaskManualAltitude::_respectMinAltitude() { - const bool respectAlt = _sub_vehicle_local_position->get().limit_hagl && PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get();