navigator: implemented protection against flying into terrain during landing

- introduced NAV_VTL_MIN_HGT to specify min distance to terrain which should be
maintained during the landing approach

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/mission.cpp

@@ -91,42 +91,6 @@ Mission::on_activation()
 	MissionBase::on_activation();
 }
 
-bool
-Mission::isLanding()
-{
-	if (get_land_start_available()) {
-		static constexpr size_t max_num_next_items{1u};
-		int32_t next_mission_items_index[max_num_next_items];
-		size_t num_found_items;
-
-		getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
-
-		// vehicle is currently landing if
-		//  mission valid, still flying, and in the landing portion of mission (past land start marker)
-		bool on_landing_stage = (num_found_items > 0U) &&  _mission.current_seq > next_mission_items_index[0U];
-
-		// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
-		// distance to the WP is below the loiter radius + acceptance.
-		if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U]
-		    && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
-			const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-			// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
-			const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
-					&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
-					_navigator->get_loiter_radius();
-
-			on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
-		}
-
-		return _navigator->get_mission_result()->valid && on_landing_stage;
-
-	} else {
-		return false;
-	}
-}
-
 bool
 Mission::set_current_mission_index(uint16_t index)
 {

src/modules/navigator/mission.h

@@ -67,8 +67,6 @@ public:
 	uint16_t get_land_start_index() const { return _mission.land_start_index; }
 	bool get_land_start_available() const { return hasMissionLandStart(); }
 
-	bool isLanding();
-
 private:
 
 	bool setNextMissionItem() override;

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();
 	}
+
+	updateMinAltDuringVtolLandingAndRepublishTriplet(_mission_item);
 }
 
 void MissionBase::update_mission()
@@ -1413,3 +1415,41 @@ void MissionBase::updateMissionAltAfterHomeChanged()
 		_home_update_counter = _navigator->get_home_position()->update_count;
 	}
 }
+
+bool
+MissionBase::isLanding()
+{
+	if (hasMissionLandStart()) {
+		static constexpr size_t max_num_next_items{1u};
+		int32_t next_mission_items_index[max_num_next_items];
+		size_t num_found_items;
+
+		getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
+
+		// vehicle is currently landing if
+		//  mission valid, still flying, and in the landing portion of mission (past land start marker)
+		bool on_landing_stage = (num_found_items > 0U) &&  _mission.current_seq > next_mission_items_index[0U];
+
+		// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
+		// distance to the WP is below the loiter radius + acceptance.
+		if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U]
+		    && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+			const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+
+			// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
+			const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
+					&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
+					_navigator->get_loiter_radius();
+
+			on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
+		}
+
+
+
+		return _navigator->get_mission_result()->valid && on_landing_stage;
+
+	} else {
+		return false;
+	}
+}

src/modules/navigator/mission_base.h

@@ -73,6 +73,8 @@ public:
 	virtual void on_activation() override;
 	virtual void on_active() override;
 
+	bool isLanding();
+
 protected:
 
 	/**
@@ -321,6 +323,7 @@ protected:
 	 */
 	void setMissionIndex(int32_t index);
 
+
 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
 	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/

src/modules/navigator/mission_block.cpp

@@ -55,6 +55,9 @@
 
 using matrix::wrap_pi;
 
+static constexpr float max_height_correction =
+	30.0f; // maximum altitude we can add on top of setpoint to avoid terrain during landing
+
 MissionBlock::MissionBlock(Navigator *navigator) :
 	NavigatorMode(navigator)
 {
@@ -1011,3 +1014,33 @@ void MissionBlock::startPrecLand(uint16_t land_precision)
 		_navigator->get_precland()->on_activation();
 	}
 }
+
+void MissionBlock::updateMinAltDuringVtolLandingAndRepublishTriplet(mission_item_s mission_item)
+{
+	// APX4 Custom start: Whenever we get too close to terrain in fixed wing mode, change the altitude setpoint in the triplet to maintain the
+	// minimum distance to terrain and republish the triplet. We only do this when we are executing a landing.
+	// We also change the mission item altitude used for acceptance calculations to prevent getting stuck in a loop.
+	const float fw_min_height = _navigator->get_fw_min_height();
+
+	float alt_sp_msl = get_absolute_altitude_for_item(mission_item);
+
+	if (fw_min_height > FLT_EPSILON && _navigator->get_vstatus()->is_vtol && _navigator->force_vtol()
+	    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+	    && _navigator->get_local_position()->dist_bottom_valid && _navigator->get_local_position()->dist_bottom < fw_min_height
+	    && _navigator->get_global_position()->alt > alt_sp_msl && _navigator->isLanding()) {
+		const float alt_corrected =  _navigator->get_global_position()->alt + fw_min_height -
+					     _navigator->get_local_position()->dist_bottom;
+
+		alt_sp_msl = math::constrain(alt_corrected, alt_sp_msl,
+					     alt_sp_msl + max_height_correction);
+
+		struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+		curr_sp->alt = alt_sp_msl;
+		_navigator->set_position_setpoint_triplet_updated();
+
+		_mission_item.altitude = alt_sp_msl;
+		_mission_item.altitude_is_relative = false;
+	}
+
+	// APX4 Custom end
+}

src/modules/navigator/mission_block.h

@@ -248,4 +248,6 @@ protected:
 	bool _payload_deploy_ack_successful{false};	// Flag to keep track of whether we received an acknowledgement for a successful payload deployment
 	hrt_abstime _payload_deployed_time{0};		// Last payload deployment start time to handle timeouts
 	float _payload_deploy_timeout_s{0.0f};		// Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received
+
+	void updateMinAltDuringVtolLandingAndRepublishTriplet(mission_item_s mission_item);
 };

src/modules/navigator/navigator.h

@@ -255,6 +255,11 @@ public:
 
 	bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
 
+	bool isLanding()
+	{
+		return (_navigation_mode == &_rtl && _rtl.isLanding()) || (_navigation_mode == &_mission && _mission.isLanding());
+	}
+
 	// RTL
 	bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
 
@@ -268,6 +273,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_fw_min_height() const { return _param_fw_min_height.get(); }
 
 	float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
 
@@ -408,6 +414,7 @@ private:
 		(ParamFloat<px4::params::MIS_YAW_TMT>)     _param_mis_yaw_tmt,
 		(ParamFloat<px4::params::MIS_YAW_ERR>)     _param_mis_yaw_err,
 		(ParamFloat<px4::params::MIS_PD_TO>)       _param_mis_payload_delivery_timeout,
-		(ParamInt<px4::params::MIS_LND_ABRT_ALT>)  _param_mis_lnd_abrt_alt
+		(ParamInt<px4::params::MIS_LND_ABRT_ALT>)  _param_mis_lnd_abrt_alt,
+		(ParamFloat<px4::params::NAV_VTL_MIN_HGT>)   _param_fw_min_height
 	)
-};
+};

src/modules/navigator/navigator_params.c

@@ -187,3 +187,20 @@ PARAM_DEFINE_INT32(NAV_FORCE_VT, 1);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_MIN_LTR_ALT, -1.f);
+
+
+/**
+ * Minimum height above ground for vtol landings.
+ *
+ * Minimum height above ground in fixed wing mode during a vtol landing (mission landing or rtl with landing pattern).
+ * When the estimated distance to the ground is below this value, the system will
+ * increase the current altitude setpoint (by max 30m) to maintain this distance.
+ * Setting the value to 0 will disable the feature.
+ *
+ * @unit m
+ * @min 0
+ * @decimal 1
+ * @increment 1
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_VTL_MIN_HGT, 0.0f);

src/modules/navigator/rtl.cpp

@@ -712,3 +712,26 @@ land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint rtl_position)
 
 	return vtol_land_approaches;
 }
+
+bool RTL::isLanding()
+{
+	bool is_landing = false;
+
+	switch (_rtl_type) {
+	case RtlType::RTL_MISSION_FAST: // Fall through
+	case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through
+	case RtlType::RTL_DIRECT_MISSION_LAND:
+		is_landing = _rtl_mission_type_handle && _rtl_mission_type_handle->isLanding();
+		break;
+
+	case RtlType::RTL_DIRECT:
+		is_landing = _rtl_direct.isLoiterindDownOrMovingToTransitionPoint();
+		break;
+
+	default:
+		break;
+
+	}
+
+	return is_landing;
+}

src/modules/navigator/rtl.h

@@ -89,6 +89,8 @@ public:
 
 	void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }
 
+	bool isLanding();
+
 private:
 	enum class DestinationType {
 		DESTINATION_TYPE_HOME,

src/modules/navigator/rtl_direct.cpp

@@ -104,6 +104,8 @@ void RtlDirect::on_active()
 		set_rtl_item();
 	}
 
+	updateMinAltDuringVtolLandingAndRepublishTriplet(_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();

src/modules/navigator/rtl_direct.h

@@ -102,6 +102,14 @@ public:
 
 	void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos);
 
+	bool isLoiterindDownOrMovingToTransitionPoint() const
+	{
+		// during the following states the vehicle is loitering down to transition altitude or moving to the
+		// land waypoint at transition altitude
+		return _rtl_state == RTLState::LOITER_HOLD || _rtl_state == RTLState::MOVE_TO_LAND
+		       || _rtl_state == RTLState::TRANSITION_TO_MC;
+	}
+
 private:
 	/**
 	 * @brief Return to launch state machine.