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();