fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-15 02:37:35 +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-01-15 17:05:00 +01:00
parent 81105d256a
commit 4c7f840f87
8 changed files with 31 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/dataman/dataman.cpp
+2 -2
View File
@@ -158,7 +158,7 @@ static unsigned g_func_counts[dm_number_of_funcs];
/* table of maximum number of instances for each item type */
static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_SAFE_POINTS_MAX,
DM_KEY_SAFE_POINT_ITEMS_MAX,
DM_KEY_FENCE_POINTS_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
@@ -170,7 +170,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
/* Table of the len of each item type */
static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = {
sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE,
sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE,
sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
src/modules/dataman/dataman.h
+4 -4
View File
@@ -59,7 +59,7 @@ typedef enum {
#if defined(MEMORY_CONSTRAINED_SYSTEM)
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_SAFE_POINT_ITEMS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = 16,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -69,7 +69,7 @@ enum {
#else
/** The maximum number of instances for each item type */
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_SAFE_POINT_ITEMS_MAX = 64,
DM_KEY_FENCE_POINTS_MAX = 64,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -83,11 +83,11 @@ struct dataman_compat_s {
};
/* increment this define whenever a binary incompatible change is performed */
#define DM_COMPAT_VERSION 2ULL
#define DM_COMPAT_VERSION 3ULL
#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))
/** Retrieve from the data manager store */
src/modules/mavlink/mavlink_mission.cpp
+6 -16
View File
@@ -331,15 +331,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;
bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s));
read_success = (bytes_read == 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;
bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_item, sizeof(mission_item_s));
read_success = (bytes_read == sizeof(mission_item_s));
}
break;
@@ -1175,13 +1168,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 = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point,
sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s);
write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_item,
sizeof(mission_item)) != sizeof(mission_item);
}
break;
@@ -1462,6 +1450,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:
@@ -1746,6 +1735,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/mavlink/mavlink_mission.h
+1 -1
View File
@@ -143,7 +143,7 @@ private:
static constexpr uint16_t MAX_COUNT[] = {
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_FENCE_POINTS_MAX - 1,
DM_KEY_SAFE_POINTS_MAX - 1
DM_KEY_SAFE_POINT_ITEMS_MAX - 1
}; /**< Maximum number of mission items for each type
(fence & safe points use the first item for the stats) */
src/modules/navigator/navigation.h
+2 -13
View File
@@ -105,6 +105,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 */
@@ -160,6 +161,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 */
@@ -227,19 +229,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
+14 -11
View File
@@ -301,20 +301,23 @@ void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPo
}
for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
mission_safe_point_s mission_safe_point;
mission_item_s mission_safe_point;
if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) !=
sizeof(mission_safe_point_s)) {
if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_item_s)) !=
sizeof(mission_item_s)) {
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) {
if (dist < min_dist) {
min_dist = dist;
setSafepointAsDestination(rtl_position, mission_safe_point);
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
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) {
min_dist = dist;
setSafepointAsDestination(rtl_position, mission_safe_point);
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
}
}
}
@@ -338,7 +341,7 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position)
}
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
@@ -346,14 +349,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
@@ -110,7 +110,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
src/systemcmds/tests/test_dataman.c
+1 -1
View File
@@ -80,7 +80,7 @@ task_main(int argc, char *argv[])
}
/* try to read an invalid index */
if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINT_ITEMS_MAX, buffer, sizeof(buffer)) >= 0) {
PX4_ERR("%d read an invalid index failed", my_id);
goto fail;
}