From e2cbf5be9438d6593b78b07f7c7122118232e0d0 Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 25 Aug 2023 15:41:07 +0200 Subject: [PATCH] [RTL] Update MAVLINK Mission logic to send optional loiter points to mission rally points. --- src/modules/dataman/dataman.cpp | 2 +- src/modules/dataman/dataman.h | 8 ++++---- src/modules/mavlink/mavlink_mission.cpp | 18 +++--------------- src/modules/navigator/navigation.h | 14 +------------- src/modules/navigator/rtl.cpp | 17 +++++++++++------ src/modules/navigator/rtl.h | 2 +- 6 files changed, 21 insertions(+), 40 deletions(-) diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index fc42770a96..57f5cdfcf8 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -991,7 +991,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"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 48d048ca44..49fd036aec 100644 --- a/src/modules/dataman/dataman.h +++ b/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)) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 95ee75817d..330fd429de 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/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(&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(&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(&mission_safe_point), sizeof(mission_safe_point_s), 2_s); + reinterpret_cast(&mission_item), sizeof(mission_item_s), 2_s); } break; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index c194b80532..54296a99b1 100644 --- a/src/modules/navigator/navigation.h +++ b/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 diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d28952c9f5..c7ededb4eb 100644 --- a/src/modules/navigator/rtl.cpp +++ b/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(&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; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index fab59e65d6..2a96cf04b0 100644 --- a/src/modules/navigator/rtl.h +++ b/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