[RTL] Update MAVLINK Mission logic to send optional loiter points to …

…mission rally points.

src/modules/dataman/dataman.cpp

@@ -989,7 +989,7 @@ dataman_main(int argc, char *argv[])
 }
 
 static_assert(sizeof(dataman_request_s::data) == sizeof(dataman_response_s::data), "request and response data are not the same size");
-static_assert(sizeof(dataman_response_s::data) >= MISSION_SAFE_POINT_SIZE, "mission_safe_point_s can't fit in the response data");
+static_assert(sizeof(dataman_response_s::data) >= MISSION_SAFE_POINT_SIZE, "mission_item_s can't fit in the response data");
 static_assert(sizeof(dataman_response_s::data) >= MISSION_FENCE_POINT_SIZE, "mission_fance_point_s can't fit in the response data");
 static_assert(sizeof(dataman_response_s::data) >= MISSION_ITEM_SIZE, "mission_item_s can't fit in the response data");
 static_assert(sizeof(dataman_response_s::data) >= MISSION_SIZE, "mission_s can't fit in the response data");

src/modules/dataman/dataman.h

@@ -74,7 +74,7 @@ enum {
 };
 #else
 enum {
-	DM_KEY_SAFE_POINTS_MAX = 8,
+	DM_KEY_SAFE_POINTS_MAX = 32,
 	DM_KEY_FENCE_POINTS_MAX = 64,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -93,7 +93,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 	DM_KEY_COMPAT_MAX
 };
 
-constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_safe_point_s);
+constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_item_s);
 constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s);
 constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s);
 constexpr uint32_t MISSION_SIZE = sizeof(struct mission_s);
@@ -114,9 +114,9 @@ struct dataman_compat_s {
 };
 
 /* increment this define whenever a binary incompatible change is performed */
-#define DM_COMPAT_VERSION	3ULL
+#define DM_COMPAT_VERSION	4ULL
 
 #define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \
 		       (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \
-		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \
+		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_item_s) << 4) + \
 		       sizeof(struct dataman_compat_s))

src/modules/mavlink/mavlink_mission.cpp

@@ -309,15 +309,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		break;
 
 	case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
-			mission_safe_point_s mission_safe_point;
-			read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast<uint8_t *>(&mission_safe_point),
-								sizeof(mission_safe_point_s));
-
-			mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
-			mission_item.frame = mission_safe_point.frame;
-			mission_item.lat = mission_safe_point.lat;
-			mission_item.lon = mission_safe_point.lon;
-			mission_item.altitude = mission_safe_point.alt;
+			read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast<uint8_t *>(&mission_item),
+								sizeof(mission_item_s));
 		}
 		break;
 
@@ -1140,13 +1133,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			break;
 
 		case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
-				mission_safe_point_s mission_safe_point;
-				mission_safe_point.lat = mission_item.lat;
-				mission_safe_point.lon = mission_item.lon;
-				mission_safe_point.alt = mission_item.altitude;
-				mission_safe_point.frame = mission_item.frame;
 				write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1,
-						reinterpret_cast<uint8_t *>(&mission_safe_point), sizeof(mission_safe_point_s), 2_s);
+						reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item_s), 2_s);
 			}
 			break;
 
@@ -1436,6 +1424,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 		case MAV_CMD_NAV_RALLY_POINT:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->is_mission_rally_point = (mavlink_mission_item->param1 > 0.0f);
 			break;
 
 		default:
@@ -1717,6 +1706,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 			break;
 
 		case MAV_CMD_NAV_RALLY_POINT:
+			mavlink_mission_item->param1 = mission_item->is_mission_rally_point ? 1 : 0;
 			break;
 
 

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 */
@@ -159,6 +160,7 @@ struct mission_item_s {
 			union {
 				float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 				float circle_radius;		/**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
+				bool is_mission_rally_point;	/**< only used for NAV_CMD_RALLY_POINT */
 			};
 			float acceptance_radius;		/**< default radius in which the mission is accepted as reached in meters */
 			float loiter_radius;			/**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */
@@ -226,19 +228,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,19 @@ 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)};
+			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && mission_safe_point.is_mission_rally_point == 1) {
+				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,22 +398,22 @@ 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
 	switch (mission_safe_point.frame) {
 	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 @@ class RTL : public NavigatorMode, public ModuleParams
 	 *
 	 * @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