fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Konrad
2023-10-31 14:16:02 +01:00
committed by Roman Bapst
parent 26fd4c852c
commit e2cbf5be94
6 changed files with 21 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/dataman/dataman.cpp
+1 -1
View File
@@ -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");
src/modules/dataman/dataman.h
+4 -4
View File
@@ -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
+3 -15
View File
@@ -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;
src/modules/navigator/navigation.h
+1 -13
View File
@@ -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
+11 -6
View File
@@ -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
+1 -1
View File
@@ -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