fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 08:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualAltitude: do terrain following when distance sensor active and

Browse Source 
below minimum distance to ground.
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent bd85f5f993
commit c4c01358b9
1 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+10 -5
View File
@@ -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();