49 #include <px4_platform_common/defines.h> 50 #include <mathlib/mathlib.h> 68 #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ 69 ((_msg.target_component == mavlink_system.compid) || \ 70 (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ 71 (_msg.target_component == MAV_COMP_ID_ALL))) 88 if (dm_lock_ret != 0) {
89 PX4_ERR(
"DM_KEY_MISSION_STATE lock failed");
96 if (dm_lock_ret == 0) {
102 _count[MAV_MISSION_TYPE_MISSION] = mission_state.
count;
105 }
else if (ret < 0) {
106 PX4_ERR(
"offboard mission init failed (%i)", ret);
155 mission.dataman_id = dataman_id;
156 mission.count = count;
157 mission.current_seq = seq;
164 if (dm_lock_ret != 0) {
165 PX4_ERR(
"DM_KEY_MISSION_STATE lock failed");
171 if (dm_lock_ret == 0) {
178 _count[MAV_MISSION_TYPE_MISSION] = count;
188 PX4_ERR(
"WPM: can't save mission state");
208 _count[MAV_MISSION_TYPE_FENCE] = count;
234 _count[MAV_MISSION_TYPE_RALLY] = count;
251 mavlink_mission_ack_t wpa;
253 wpa.target_system = sysid;
254 wpa.target_component = compid;
256 wpa.mission_type = _mission_type;
260 PX4_DEBUG(
"WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system);
266 unsigned item_count =
_count[MAV_MISSION_TYPE_MISSION];
268 if (seq < item_count) {
269 mavlink_mission_current_t wpc;
275 }
else if (seq == 0 && item_count == 0) {
279 PX4_DEBUG(
"WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq);
291 mavlink_mission_count_t wpc;
293 wpc.target_system = sysid;
294 wpc.target_component = compid;
296 wpc.mission_type = mission_type;
300 PX4_DEBUG(
"WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type);
310 switch (_mission_type) {
312 case MAV_MISSION_TYPE_MISSION: {
317 case MAV_MISSION_TYPE_FENCE: {
323 mission_item.
frame = mission_fence_point.
frame;
324 mission_item.
lat = mission_fence_point.
lat;
325 mission_item.
lon = mission_fence_point.
lon;
328 if (mission_fence_point.
nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
329 mission_fence_point.
nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
338 case MAV_MISSION_TYPE_RALLY: {
343 mission_item.
nav_cmd = MAV_CMD_NAV_RALLY_POINT;
344 mission_item.
frame = mission_safe_point.
frame;
345 mission_item.
lat = mission_safe_point.
lat;
346 mission_item.
lon = mission_safe_point.
lon;
356 if (read_result > 0) {
360 mavlink_mission_item_int_t wp = {};
363 wp.target_system = sysid;
364 wp.target_component = compid;
370 PX4_DEBUG(
"WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system);
373 mavlink_mission_item_t wp = {};
376 wp.target_system = sysid;
377 wp.target_component = compid;
383 PX4_DEBUG(
"WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system);
393 PX4_DEBUG(
"WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq,
_dataman_id);
401 PX4_ERR(
"WPM: MAX_COUNT out of bounds (%u)", _mission_type);
411 if (_mission_type >=
sizeof(
_count) /
sizeof(
_count[0])) {
412 PX4_ERR(
"WPM: _count out of bounds (%u)", _mission_type);
416 return _count[_mission_type];
426 mavlink_mission_request_int_t wpr;
427 wpr.target_system = sysid;
428 wpr.target_component = compid;
430 wpr.mission_type = _mission_type;
433 PX4_DEBUG(
"WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system);
437 mavlink_mission_request_t wpr;
438 wpr.target_system = sysid;
439 wpr.target_component = compid;
441 wpr.mission_type = _mission_type;
445 PX4_DEBUG(
"WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system);
451 PX4_DEBUG(
"WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq);
459 mavlink_mission_item_reached_t wp_reached;
461 wp_reached.seq = seq;
465 PX4_DEBUG(
"WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq);
484 PX4_DEBUG(
"WPM: got mission result, new current_seq: %u",
_current_seq);
495 PX4_DEBUG(
"WPM: got mission result, new seq_reached: %d",
_last_reached);
500 if (mission_result.item_do_jump_changed) {
504 _mission_type = MAV_MISSION_TYPE_MISSION;
506 (uint16_t)mission_result.item_changed_index);
534 PX4_DEBUG(
"WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state);
552 switch (msg->msgid) {
553 case MAVLINK_MSG_ID_MISSION_ACK:
557 case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
561 case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
565 case MAVLINK_MSG_ID_MISSION_REQUEST:
569 case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
573 case MAVLINK_MSG_ID_MISSION_COUNT:
577 case MAVLINK_MSG_ID_MISSION_ITEM:
581 case MAVLINK_MSG_ID_MISSION_ITEM_INT:
585 case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
598 mavlink_mission_ack_t wpa;
599 mavlink_msg_mission_ack_decode(msg, &wpa);
607 PX4_DEBUG(
"WPM: MISSION_ACK OK all items sent, switch to state IDLE");
612 PX4_DEBUG(
"WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway");
620 if (wpa.type == MAV_MISSION_UNSUPPORTED) {
631 }
else if (wpa.type != MAV_MISSION_ACCEPTED) {
632 PX4_WARN(
"Mission ack result was %d", wpa.type);
639 PX4_DEBUG(
"WPM: MISSION_ACK ERR: ID mismatch");
648 mavlink_mission_set_current_t wpc;
649 mavlink_msg_mission_set_current_decode(msg, &wpc);
655 if (wpc.seq <
_count[MAV_MISSION_TYPE_MISSION]) {
657 PX4_DEBUG(
"WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq);
660 PX4_DEBUG(
"WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq);
666 PX4_ERR(
"WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq);
672 PX4_DEBUG(
"WPM: MISSION_SET_CURRENT ERROR: busy");
683 mavlink_mission_request_list_t wprl;
684 mavlink_msg_mission_request_list_decode(msg, &wprl);
688 && (uint8_t)_mission_type == wprl.mission_type)) {
692 _mission_type = (MAV_MISSION_TYPE)wprl.mission_type;
695 switch (_mission_type) {
696 case MAV_MISSION_TYPE_FENCE:
700 case MAV_MISSION_TYPE_RALLY:
714 PX4_DEBUG(
"WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i",
_transfer_count, _mission_type);
717 PX4_DEBUG(
"WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type);
723 PX4_DEBUG(
"WPM: MISSION_REQUEST_LIST ERROR: busy");
758 mavlink_mission_request_t wpr;
759 mavlink_msg_mission_request_decode(msg, &wpr);
765 if (_mission_type != wpr.mission_type) {
766 PX4_WARN(
"WPM: Unexpected mission type (%u %u)", wpr.mission_type, _mission_type);
774 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u", wpr.seq, msg->sysid);
779 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u (again)", wpr.seq, msg->sysid);
783 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid,
787 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid,
791 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid,
807 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", wpr.seq, wpr.seq,
817 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer");
823 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state);
831 PX4_DEBUG(
"WPM: MISSION_ITEM_REQUEST(_INT) ERROR: rejected, partner ID mismatch");
840 mavlink_mission_count_t wpc;
841 mavlink_msg_mission_count_decode(msg, &wpc);
853 _mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
856 PX4_DEBUG(
"WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count,
current_max_item_count());
863 if (wpc.count == 0) {
864 PX4_DEBUG(
"WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE");
866 switch (_mission_type) {
867 case MAV_MISSION_TYPE_MISSION:
880 case MAV_MISSION_TYPE_FENCE:
884 case MAV_MISSION_TYPE_RALLY:
889 PX4_ERR(
"mission type %u not handled", _mission_type);
898 PX4_DEBUG(
"WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
909 if (_mission_type == MAV_MISSION_TYPE_FENCE) {
912 PX4_DEBUG(
"locking fence dataman items");
920 PX4_ERR(
"locking failed (%i)", errno);
929 PX4_DEBUG(
"WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid);
932 PX4_DEBUG(
"WPM: MISSION_COUNT ERROR: busy, already receiving seq %u",
_transfer_seq);
941 PX4_DEBUG(
"WPM: MISSION_COUNT ERROR: busy, state %i", _state);
961 PX4_DEBUG(
"unlocking geofence");
997 mavlink_mission_item_t wp;
998 mavlink_msg_mission_item_decode(msg, &wp);
1002 if (wp.mission_type != _mission_type) {
1003 PX4_WARN(
"WPM: Unexpected mission type (%u %u)", (
int)wp.mission_type, (
int)_mission_type);
1012 PX4_DEBUG(
"WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq,
_transfer_seq);
1026 PX4_DEBUG(
"WPM: MISSION_ITEM ERROR: no transfer");
1035 PX4_DEBUG(
"WPM: MISSION_ITEM ERROR: busy, state %i", _state);
1046 if (ret != PX4_OK) {
1047 PX4_DEBUG(
"WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq);
1057 bool write_failed =
false;
1058 bool check_failed =
false;
1060 switch (_mission_type) {
1062 case MAV_MISSION_TYPE_MISSION: {
1065 if (mission_item.
nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
1066 mission_item.
nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION ||
1067 mission_item.
nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION ||
1068 mission_item.
nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION ||
1069 mission_item.
nav_cmd == MAV_CMD_NAV_RALLY_POINT) {
1070 check_failed =
true;
1078 if (!write_failed) {
1088 case MAV_MISSION_TYPE_FENCE: {
1091 mission_fence_point.
lat = mission_item.
lat;
1092 mission_fence_point.
lon = mission_item.
lon;
1095 if (mission_item.
nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
1096 mission_item.
nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
1100 PX4_ERR(
"Fence: too few vertices");
1101 check_failed =
true;
1109 mission_fence_point.
frame = mission_item.
frame;
1111 if (!check_failed) {
1119 case MAV_MISSION_TYPE_RALLY: {
1121 mission_safe_point.
lat = mission_item.
lat;
1122 mission_safe_point.
lon = mission_item.
lon;
1124 mission_safe_point.
frame = mission_item.
frame;
1135 if (write_failed || check_failed) {
1136 PX4_DEBUG(
"WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq,
_transfer_dataman_id);
1154 PX4_DEBUG(
"WPM: MISSION_ITEM seq %u received", wp.seq);
1160 PX4_DEBUG(
"WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE",
1165 switch (_mission_type) {
1166 case MAV_MISSION_TYPE_MISSION:
1170 case MAV_MISSION_TYPE_FENCE:
1174 case MAV_MISSION_TYPE_RALLY:
1179 PX4_ERR(
"mission type %u not handled", _mission_type);
1187 if (ret == PX4_OK) {
1207 mavlink_mission_clear_all_t wpca;
1208 mavlink_msg_mission_clear_all_decode(msg, &wpca);
1216 _mission_type = (MAV_MISSION_TYPE)wpca.mission_type;
1219 switch (wpca.mission_type) {
1220 case MAV_MISSION_TYPE_MISSION:
1225 case MAV_MISSION_TYPE_FENCE:
1229 case MAV_MISSION_TYPE_RALLY:
1233 case MAV_MISSION_TYPE_ALL:
1235 DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
1241 PX4_ERR(
"mission type %u not handled", _mission_type);
1245 if (ret == PX4_OK) {
1246 PX4_DEBUG(
"WPM: CLEAR_ALL OK (mission_type=%i)", _mission_type);
1257 PX4_DEBUG(
"WPM: CLEAR_ALL IGNORED: busy");
1266 if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
1267 mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
1268 (
_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
1269 mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) {
1272 if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
1273 mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) {
1281 const mavlink_mission_item_int_t *item_int
1282 =
reinterpret_cast<const mavlink_mission_item_int_t *
>(mavlink_mission_item);
1283 mission_item->
lat = ((double)item_int->x) * 1e-7;
1284 mission_item->
lon = ((double)item_int->y) * 1e-7;
1287 mission_item->
lat = (double)mavlink_mission_item->x;
1288 mission_item->
lon = (
double)mavlink_mission_item->y;
1291 mission_item->
altitude = mavlink_mission_item->z;
1293 if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
1294 mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT) {
1297 }
else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
1298 mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
1309 switch (mavlink_mission_item->command) {
1310 case MAV_CMD_NAV_WAYPOINT:
1312 mission_item->
time_inside = mavlink_mission_item->param1;
1317 case MAV_CMD_NAV_LOITER_UNLIM:
1323 case MAV_CMD_NAV_LOITER_TIME:
1325 mission_item->
time_inside = mavlink_mission_item->param1;
1333 case MAV_CMD_NAV_LAND:
1340 case MAV_CMD_NAV_TAKEOFF:
1342 mission_item->
pitch_min = mavlink_mission_item->param1;
1346 case MAV_CMD_NAV_LOITER_TO_ALT:
1348 mission_item->
force_heading = (mavlink_mission_item->param1 > 0);
1353 case MAV_CMD_NAV_ROI:
1354 case MAV_CMD_DO_SET_ROI:
1355 if ((
int)mavlink_mission_item->param1 == MAV_ROI_LOCATION) {
1357 mission_item->
params[0] = MAV_ROI_LOCATION;
1359 mission_item->
params[6] = mavlink_mission_item->z;
1361 }
else if ((
int)mavlink_mission_item->param1 == MAV_ROI_NONE) {
1363 mission_item->
params[0] = MAV_ROI_NONE;
1366 return MAV_MISSION_INVALID_PARAM1;
1371 case MAV_CMD_DO_SET_ROI_LOCATION:
1373 mission_item->
params[6] = mavlink_mission_item->z;
1376 case MAV_CMD_NAV_VTOL_TAKEOFF:
1377 case MAV_CMD_NAV_VTOL_LAND:
1382 case MAV_CMD_NAV_FENCE_RETURN_POINT:
1386 case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
1387 case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
1389 mission_item->
vertex_count = (uint16_t)(mavlink_mission_item->param1 + 0.5f);
1392 case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
1393 case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
1398 case MAV_CMD_NAV_RALLY_POINT:
1405 PX4_DEBUG(
"Unsupported command %d", mavlink_mission_item->command);
1407 return MAV_MISSION_UNSUPPORTED;
1410 mission_item->
frame = mavlink_mission_item->frame;
1412 }
else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
1416 mission_item->
params[0] = mavlink_mission_item->param1;
1417 mission_item->
params[1] = mavlink_mission_item->param2;
1418 mission_item->
params[2] = mavlink_mission_item->param3;
1419 mission_item->
params[3] = mavlink_mission_item->param4;
1420 mission_item->
params[4] = mavlink_mission_item->x;
1421 mission_item->
params[5] = mavlink_mission_item->y;
1422 mission_item->
params[6] = mavlink_mission_item->z;
1424 switch (mavlink_mission_item->command) {
1425 case MAV_CMD_DO_JUMP:
1432 case MAV_CMD_NAV_ROI:
1433 case MAV_CMD_DO_SET_ROI: {
1434 const int roi_mode = mavlink_mission_item->param1;
1436 if (roi_mode == MAV_ROI_NONE || roi_mode == MAV_ROI_WPNEXT || roi_mode == MAV_ROI_WPINDEX) {
1440 return MAV_MISSION_INVALID_PARAM1;
1445 case MAV_CMD_DO_CHANGE_SPEED:
1446 case MAV_CMD_DO_SET_HOME:
1447 case MAV_CMD_DO_SET_SERVO:
1448 case MAV_CMD_DO_LAND_START:
1449 case MAV_CMD_DO_TRIGGER_CONTROL:
1450 case MAV_CMD_DO_DIGICAM_CONTROL:
1451 case MAV_CMD_DO_MOUNT_CONFIGURE:
1452 case MAV_CMD_DO_MOUNT_CONTROL:
1453 case MAV_CMD_IMAGE_START_CAPTURE:
1454 case MAV_CMD_IMAGE_STOP_CAPTURE:
1455 case MAV_CMD_VIDEO_START_CAPTURE:
1456 case MAV_CMD_VIDEO_STOP_CAPTURE:
1457 case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
1458 case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
1459 case MAV_CMD_SET_CAMERA_MODE:
1460 case MAV_CMD_DO_VTOL_TRANSITION:
1461 case MAV_CMD_NAV_DELAY:
1462 case MAV_CMD_NAV_RETURN_TO_LAUNCH:
1463 case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
1464 case MAV_CMD_DO_SET_ROI_NONE:
1471 PX4_DEBUG(
"Unsupported command %d", mavlink_mission_item->command);
1473 return MAV_MISSION_UNSUPPORTED;
1476 mission_item->
frame = MAV_FRAME_MISSION;
1479 PX4_DEBUG(
"Unsupported frame %d", mavlink_mission_item->frame);
1481 return MAV_MISSION_UNSUPPORTED_FRAME;
1484 mission_item->
autocontinue = mavlink_mission_item->autocontinue;
1489 return MAV_MISSION_ACCEPTED;
1495 mavlink_mission_item_t *mavlink_mission_item)
1497 mavlink_mission_item->frame = mission_item->
frame;
1498 mavlink_mission_item->command = mission_item->
nav_cmd;
1499 mavlink_mission_item->autocontinue = mission_item->
autocontinue;
1502 if (mission_item->
frame == MAV_FRAME_MISSION) {
1503 mavlink_mission_item->param1 = mission_item->
params[0];
1504 mavlink_mission_item->param2 = mission_item->
params[1];
1505 mavlink_mission_item->param3 = mission_item->
params[2];
1506 mavlink_mission_item->param4 = mission_item->
params[3];
1507 mavlink_mission_item->x = mission_item->
params[4];
1508 mavlink_mission_item->y = mission_item->
params[5];
1509 mavlink_mission_item->z = mission_item->
params[6];
1511 switch (mavlink_mission_item->command) {
1541 mavlink_mission_item->param1 = 0.0f;
1542 mavlink_mission_item->param2 = 0.0f;
1543 mavlink_mission_item->param3 = 0.0f;
1544 mavlink_mission_item->param4 = 0.0f;
1550 mavlink_mission_item_int_t *item_int =
1551 reinterpret_cast<mavlink_mission_item_int_t *
>(mavlink_mission_item);
1553 item_int->x = round(mission_item->
lat * 1e7);
1554 item_int->y = round(mission_item->
lon * 1e7);
1557 mavlink_mission_item->x = (float)mission_item->
lat;
1558 mavlink_mission_item->y = (
float)mission_item->
lon;
1561 mavlink_mission_item->z = mission_item->
altitude;
1565 mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
1568 mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
1573 mavlink_mission_item->frame = MAV_FRAME_GLOBAL_INT;
1576 mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
1580 switch (mission_item->
nav_cmd) {
1582 mavlink_mission_item->param1 = mission_item->
time_inside;
1593 mavlink_mission_item->param1 = mission_item->
time_inside;
1605 mavlink_mission_item->param1 = mission_item->
pitch_min;
1615 case MAV_CMD_NAV_VTOL_TAKEOFF:
1616 case MAV_CMD_NAV_VTOL_LAND:
1620 case MAV_CMD_NAV_FENCE_RETURN_POINT:
1623 case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
1624 case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
1625 mavlink_mission_item->param1 = (float)mission_item->
vertex_count;
1628 case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
1629 case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
1633 case MAV_CMD_NAV_RALLY_POINT:
1654 PX4_DEBUG(
"WPM: New mission detected (possibly over different Mavlink instance) Updating");
1658 MAV_MISSION_TYPE_MISSION);
int update_geofence_count(unsigned count)
store the geofence count to dataman
Global position setpoint in WGS84 coordinates.
void handle_mission_request(const mavlink_message_t *msg)
bool _int_mode
Use accurate int32 instead of float.
static uint16_t _count[3]
Count of items in (active) mission for each MAV_MISSION_TYPE.
uint8_t _transfer_partner_sysid
Partner system ID for current transmission.
static bool _dataman_init
Dataman initialized.
uint16_t frame
mission frame
void check_active_mission(void)
Definition of a mission consisting of mission items.
Definition of geo / math functions to perform geodesic calculations.
uint16_t _transfer_count
Items count in current transmission.
static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT
Protocol action timeout in us.
uint16_t vertex_count
Polygon vertex count (geofence)
float altitude
altitude in meters (AMSL)
void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type)
uint8_t _transfer_partner_compid
Partner component ID for current transmission.
uint16_t autocontinue
true if next waypoint should follow after this one
float acceptance_radius
default radius in which the mission is accepted as reached in meters
#define CHECK_SYSID_COMPID_MISSION(_msg)
void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
__EXPORT int dm_lock(dm_item_t item)
Lock all items of a type.
void handle_mission_request_int(const mavlink_message_t *msg)
Implementation of the MAVLink mission protocol.
int load_geofence_stats()
load geofence stats from dataman
static int32_t _current_seq
Current item sequence in active mission.
void send_mission_current(uint16_t seq)
Broadcasts the new target waypoint and directs the MAV to fly there.
MAVLink 2.0 protocol interface definition.
dm_item_t
Types of items that the data manager can store.
uint16_t origin
how the mission item was generated
uint16_t vertex_count
number of vertices in this polygon
uint16_t do_jump_repeat_count
how many times do jump needs to be done
uORB::Publication< mission_s > _offboard_mission_pub
enum MAVLINK_MODE get_mode()
constexpr T degrees(T radians)
void handle_mission_set_current(const mavlink_message_t *msg)
void init_offboard_mission()
uint16_t current_max_item_count()
get the maximum number of item count for the current _mission_type
float circle_radius
geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*)
High-resolution timer with callouts and timekeeping.
static dm_item_t _dataman_id
Global Dataman storage ID for active mission.
double lon
longitude in degrees
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
uORB::Subscription _mission_result_sub
static uint16_t _geofence_update_counter
MavlinkRateLimiter _slow_rate_limiter
Rate limit sending of the current WP sequence to 10 Hz.
uint16_t _transfer_seq
Item sequence in current transmission.
__EXPORT ssize_t dm_read(dm_item_t item, unsigned index, void *buf, size_t count)
Retrieve from the data manager file.
bool publish(const T &data)
Publish the struct.
void handle_mission_clear_all(const mavlink_message_t *msg)
dm_item_t _my_dataman_id
class Dataman storage ID
static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT
Protocol retry timeout in us.
float circle_radius
geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*)
void handle_mission_request_both(const mavlink_message_t *msg)
void handle_message(const mavlink_message_t *msg)
bool check(const hrt_abstime &t)
uint16_t nav_cmd
navigation command
constexpr T radians(T degrees)
void handle_mission_request_list(const mavlink_message_t *msg)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
uint16_t do_jump_current_count
count how many times the jump has been done
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
dataman housekeeping information for a specific item.
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
Format mission_item_s as mavlink MISSION_ITEM(_INT) message.
MavlinkMissionManager(Mavlink *mavlink)
mavlink_channel_t get_channel() const
float time_inside
time that the MAV should stay inside the radius before advancing in seconds
int update_safepoint_count(unsigned count)
store the safepoint count to dataman
Safe Point (Rally Point).
static uint16_t _safepoint_update_counter
uint8_t _reached_sent_count
last time when the vehicle reached a waypoint
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
void handle_mission_item(const mavlink_message_t *msg)
static bool _transfer_in_progress
Global variable checking for current transmission.
void handle_mission_item_int(const mavlink_message_t *msg)
float params[7]
array to store mission command values for MAV_FRAME_MISSION
void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
Sends an waypoint ack message.
__EXPORT void dm_unlock(dm_item_t item)
Unlock a data Item.
uint16_t land_precision
Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing...
uint16_t nav_cmd
navigation command (one of MAV_CMD_NAV_FENCE_*)
void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
uint16_t current_item_count()
get the number of item count for the current _mission_type
uint16_t num_items
total number of items stored (excluding this one)
void send_mission_item_reached(uint16_t seq)
emits a message that a waypoint reached
int16_t do_jump_mission_index
index where the do jump will go to
void send(const hrt_abstime t)
Handle sending of messages.
uint16_t update_counter
This counter is increased when (some) items change (this can wrap)
void switch_to_idle_state()
set _state to idle (and do necessary cleanup)
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT
Error count limit before stopping to report FS errors.
void handle_mission_count(const mavlink_message_t *msg)
void handle_mission_item_both(const mavlink_message_t *msg)
int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
Publish mission topic to notify navigator about changes.
void send_statustext_critical(const char *string)
Send a status text with loglevel CRITICAL.
__EXPORT ssize_t dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count)
Write to the data manager file.
bool _geofence_locked
if true, we currently hold the dm_lock for the geofence (transaction in progress) ...
float pitch_min
minimal pitch angle for fixed wing takeoff waypoints
uint16_t loiter_exit_xtrack
exit xtrack location: 0 for center of loiter wp, 1 for exit location
void handle_mission_ack(const mavlink_message_t *msg)
bool update(void *dst)
Update the struct.
int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
Parse mavlink MISSION_ITEM message to get mission_item_s.
static constexpr uint16_t MAX_COUNT[]
Maximum number of mission items for each type (fence & safe points use the first item for the stats) ...
unsigned _filesystem_errcount
File system error count.
uint16_t force_heading
heading needs to be reached
dm_item_t _transfer_dataman_id
Dataman storage ID for current transmission.
float yaw
in radians NED -PI..+PI, NAN means don't change yaw
float loiter_radius
loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise
int load_safepoint_stats()
load safe point stats from dataman
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
int32_t _transfer_current_seq
Current item ID for current transmission (-1 means not initialized)
int32_t _last_reached
Last reached waypoint in active mission (-1 means nothing reached)
uint16_t altitude_is_relative
true if altitude is relative from start point
double lat
latitude in degrees