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

Navigator: add terrain collision avoidance logic for Mission/RTL

Browse Source 
Avoid flying into terrain using the distance sensor.
Enable through the parameter NAV_MIN_GND_DIST.
Only active during commanded descents with vz>0 (to prevent climb-aways),
excluding landing and VTOL transitions.
It changes the altitude setpoint in the triplet to maintain the current altitude
and republish the triplet. We also change the mission item altitude used for
acceptance calculations to prevent getting stuck in a loop.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-07-30 17:50:54 +02:00
parent b74e46b1ac
commit 75ce550db3
7 changed files with 69 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+31
View File
@@ -1011,3 +1011,34 @@ void MissionBlock::startPrecLand(uint16_t land_precision)
_navigator->get_precland()->on_activation();
}
}
void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_item_s mission_item)
{
// Avoid flying into terrain using the distance sensor. Enable through the parameter NAV_MIN_GND_DIST.
// Only active during commanded descents with vz>0 (to prevent climb-aways), excluding landing and VTOL transitions.
// It changes the altitude setpoint in the triplet to maintain the current altitude and republish the triplet.
// We also change the mission item altitude used for acceptance calculations to prevent getting stuck in a loop.
// This threshold is needed to prevent the check from re-triggering due to small altitude over-shoots while
// tracking the new altitude setpoint.
static constexpr float kAltitudeDifferenceForDescentCondition = 2.f;
if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND
&& _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION
&& _navigator->get_local_position()->dist_bottom_valid
&& _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param()
&& _navigator->get_local_position()->vz > FLT_EPSILON
&& _navigator->get_global_position()->alt - get_absolute_altitude_for_item(mission_item) >
kAltitudeDifferenceForDescentCondition) {
_navigator->sendWarningDescentStoppedDueToTerrain();
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
curr_sp->alt = _navigator->get_global_position()->alt;
_navigator->set_position_setpoint_triplet_updated();
_mission_item.altitude = _navigator->get_global_position()->alt;
_mission_item.altitude_is_relative = false;
}
}