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 <silvan@auterion.com>

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

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;
+	}
+}

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

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<px4::params::NAV_TRAFF_A_VER>)  _param_nav_traff_a_ver,	/**< avoidance Distance Vertical*/
 		(ParamInt<px4::params::NAV_TRAFF_COLL_T>)   _param_nav_traff_collision_time,
 		(ParamFloat<px4::params::NAV_MIN_LTR_ALT>)   _param_min_ltr_alt,	/**< minimum altitude in Loiter mode*/
+		(ParamFloat<px4::params::NAV_MIN_GND_DIST>)
+		_param_nav_min_gnd_dist,	/**< minimum distance to ground (Mission and RTL)*/
 
 		// non-navigator parameters: Mission (MIS_*)
 		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,

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) {

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

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