From 75ce550db39cc5d61ba970ca5f4b6096a287827e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 19 Jul 2024 11:25:56 +0200 Subject: [PATCH] Navigator: add terrain collision avoidance logic for Mission/RTL 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 --- src/modules/navigator/mission_base.cpp | 2 ++ src/modules/navigator/mission_block.cpp | 31 ++++++++++++++++++++++++ src/modules/navigator/mission_block.h | 1 + src/modules/navigator/navigator.h | 5 ++++ src/modules/navigator/navigator_main.cpp | 7 ++++++ src/modules/navigator/navigator_params.c | 18 ++++++++++++++ src/modules/navigator/rtl_direct.cpp | 5 ++++ 7 files changed, 69 insertions(+) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 595f548c83..0a9a76bd2b 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -352,6 +352,8 @@ MissionBase::on_active() } else if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } + + updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } void MissionBase::update_mission() diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 17dd3e620c..1f0c9f0dd0 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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; + } +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index b97731567a..13815b8602 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -215,6 +215,7 @@ protected: void setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const; void startPrecLand(uint16_t land_precision); + void updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_item_s mission_item); /** * @brief Issue a command for mission items with a nav_cmd that specifies an action diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 03a1ea621e..93f630dbf5 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -268,6 +268,7 @@ public: float get_param_mis_takeoff_alt() const { return _param_mis_takeoff_alt.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } + float get_nav_min_gnd_dist_param() const { return _param_nav_min_gnd_dist.get(); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } @@ -284,6 +285,8 @@ public: void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); + void sendWarningDescentStoppedDueToTerrain(); + private: int _local_pos_sub{-1}; @@ -402,6 +405,8 @@ private: (ParamFloat) _param_nav_traff_a_ver, /**< avoidance Distance Vertical*/ (ParamInt) _param_nav_traff_collision_time, (ParamFloat) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/ + (ParamFloat) + _param_nav_min_gnd_dist, /**< minimum distance to ground (Mission and RTL)*/ // non-navigator parameters: Mission (MIS_*) (ParamFloat) _param_mis_takeoff_alt, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 434be34e77..f2694b595a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1522,6 +1522,13 @@ void Navigator::set_gimbal_neutral() publish_vehicle_cmd(&vcmd); } +void Navigator::sendWarningDescentStoppedDueToTerrain() +{ + mavlink_log_critical(&_mavlink_log_pub, "Terrain collision risk, descent is stopped\t"); + events::send(events::ID("navigator_terrain_collision_risk"), events::Log::Critical, + "Terrain collision risk, descent is stopped"); +} + int Navigator::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index c300c8e3c8..325b19abf6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -187,3 +187,21 @@ PARAM_DEFINE_INT32(NAV_FORCE_VT, 1); * @group Mission */ PARAM_DEFINE_FLOAT(NAV_MIN_LTR_ALT, -1.f); + +/** + * Minimum height above ground during Mission and RTL + * + * Minimum height above ground the vehicle is allowed to descend to during Mission and RTL, + * excluding landing commands. + * Requires a distance sensor to be set up. + * Note: only prevents the vehicle from descending further, but does not force it to climb. + * + * Set to a negative value to disable. + * + * @unit m + * @min -1 + * @decimal 1 + * @increment 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_MIN_GND_DIST, -1.f); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index d2936986db..b028c7fb35 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -104,6 +104,11 @@ void RtlDirect::on_active() set_rtl_item(); } + if (_rtl_state == RTLState::LOITER_HOLD) { //TODO: rename _rtl_state to _rtl_state_next + //check for terrain collision and update altitude if needed + updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); + } + if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) { // Need to update the position and type on the current setpoint triplet. _navigator->get_precland()->on_active();