[RTL] Update MAVLINK Mission logic to send optional loiter points to mission rally points.

src/modules/navigator/navigation.h

@@ -104,6 +104,7 @@ enum NAV_CMD {
 	NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002,
 	NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
 	NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
+	NAV_CMD_RALLY_POINT = 5100,
 	NAV_CMD_CONDITION_GATE = 4501,
 	NAV_CMD_DO_WINCH = 42600,
 	NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
@@ -226,19 +227,6 @@ struct mission_fence_point_s {
 	uint8_t _padding0[5];				/**< padding struct size to alignment boundary  */
 };
 
-/**
- * Safe Point (Rally Point).
- * Corresponds to the DM_KEY_SAFE_POINTS dataman item
- */
-struct mission_safe_point_s {
-	double lat;
-	double lon;
-	float alt;
-	uint8_t frame;					/**< MAV_FRAME */
-
-	uint8_t _padding0[3];				/**< padding struct size to alignment boundary  */
-};
-
 #if (__GNUC__ >= 5) || __clang__
 #pragma GCC diagnostic pop
 #endif // GCC >= 5 || Clang

src/modules/navigator/rtl.cpp

@@ -357,18 +357,23 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 	if (_safe_points_updated) {
 
 		for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) {
-			mission_safe_point_s mission_safe_point;
+			mission_item_s mission_safe_point;
 
 			bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq,
 					reinterpret_cast<uint8_t *>(&mission_safe_point),
-					sizeof(mission_safe_point_s), 500_ms);
+					sizeof(mission_item_s), 500_ms);
 
 			if (!success) {
 				PX4_ERR("dm_read failed");
 				continue;
 			}
 
-			float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
+			// Ignore safepoints which are too close to the homepoint
+			const float dist_to_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
+						   mission_safe_point.lat, mission_safe_point.lon);
+
+			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
+				float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
 
 			if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
 				min_dist = dist;
@@ -397,7 +402,7 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_
 }
 
 void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
-				    const mission_safe_point_s &mission_safe_point)
+				    const mission_item_s &mission_safe_point)
 {
 	// There is a safe point closer than home/mission landing
 	// TODO: handle all possible mission_safe_point.frame cases
@@ -405,14 +410,14 @@ void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
 	case 0: // MAV_FRAME_GLOBAL
 		rtl_position.lat = mission_safe_point.lat;
 		rtl_position.lon = mission_safe_point.lon;
-		rtl_position.alt = mission_safe_point.alt;
+		rtl_position.alt = mission_safe_point.altitude;
 		rtl_position.yaw = _home_pos_sub.get().yaw;;
 		break;
 
 	case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
 		rtl_position.lat = mission_safe_point.lat;
 		rtl_position.lon = mission_safe_point.lon;
-		rtl_position.alt = mission_safe_point.alt + _home_pos_sub.get().alt; // alt of safe point is rel to home
+		rtl_position.alt = mission_safe_point.altitude + _home_pos_sub.get().alt; // alt of safe point is rel to home
 		rtl_position.yaw = _home_pos_sub.get().yaw;;
 		break;
 

src/modules/navigator/rtl.h

@@ -114,7 +114,7 @@ private:
 	 *
 	 * @param mission_safe_point is the mission safe point/rally point to set as destination.
 	 */
-	void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point);
+	void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_item_s &mission_safe_point);
 
 	/**
 	 * @brief calculate return altitude from cone half angle