From b8fb8c610ecdae15bf03897b799bde242ae6bfd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 24 Mar 2017 08:42:01 +0100 Subject: [PATCH] mavlink_mission: implement geofence & rally point protocol - retrieve & store the geofence & rally point data from/to dataman - interleaved transmissions (of different types) are not possible. trying to do so will NACK the new transmission - only one storage backend for polygons & rally points (not alternating between 2 as the mission does) --- src/modules/mavlink/mavlink_mission.cpp | 416 +++++++++++++++++++++--- src/modules/mavlink/mavlink_mission.h | 34 +- 2 files changed, 409 insertions(+), 41 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 9c18d4a742..86c42c9400 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -43,23 +43,24 @@ #include "mavlink_mission.h" #include "mavlink_main.h" +#include #include #include #include #include #include -#include #include #include #include int MavlinkMissionManager::_dataman_id = 0; bool MavlinkMissionManager::_dataman_init = false; -unsigned MavlinkMissionManager::_count = 0; +unsigned MavlinkMissionManager::_count[3] = { 0, 0, 0 }; int MavlinkMissionManager::_current_seq = 0; int MavlinkMissionManager::_last_reached = -1; bool MavlinkMissionManager::_transfer_in_progress = false; +constexpr unsigned MavlinkMissionManager::MAX_COUNT[]; #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ ((_msg.target_component == mavlink_system.compid) || \ @@ -68,13 +69,13 @@ bool MavlinkMissionManager::_transfer_in_progress = false; MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _state(MAVLINK_WPM_STATE_IDLE), + _mission_type(MAV_MISSION_TYPE_MISSION), _time_last_recv(0), _time_last_sent(0), _time_last_reached(0), _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _int_mode(false), - _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), _filesystem_errcount(0), _my_dataman_id(0), _transfer_dataman_id(0), @@ -113,7 +114,7 @@ MavlinkMissionManager::init_offboard_mission() if (ret > 0) { _dataman_id = mission_state.dataman_id; - _count = mission_state.count; + _count[(uint8_t)MAV_MISSION_TYPE_MISSION] = mission_state.count; _current_seq = mission_state.current_seq; } else if (ret == 0) { @@ -127,11 +128,43 @@ MavlinkMissionManager::init_offboard_mission() _count = 0; _current_seq = 0; } + + load_geofence_stats(); + + load_safepoint_stats(); } _my_dataman_id = _dataman_id; } +int +MavlinkMissionManager::load_geofence_stats() +{ + mission_stats_entry_s stats; + // initialize fence points count + int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + + if (ret == sizeof(mission_stats_entry_s)) { + _count[(uint8_t)MAV_MISSION_TYPE_FENCE] = stats.num_items; + } + + return ret; +} + +int +MavlinkMissionManager::load_safepoint_stats() +{ + mission_stats_entry_s stats; + // initialize safe points count + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + + if (ret == sizeof(mission_stats_entry_s)) { + _count[(uint8_t)MAV_MISSION_TYPE_RALLY] = stats.num_items; + } + + return ret; +} + /** * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. */ @@ -150,7 +183,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int if (res == sizeof(mission_s)) { /* update active mission state */ _dataman_id = dataman_id; - _count = count; + _count[(uint8_t)MAV_MISSION_TYPE_MISSION] = count; _current_seq = seq; _my_dataman_id = _dataman_id; @@ -174,6 +207,57 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int return PX4_ERROR; } } +int +MavlinkMissionManager::update_geofence_count(unsigned count) +{ + mission_stats_entry_s stats; + stats.num_items = count; + + /* update stats in dataman */ + int res = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + + if (res == sizeof(mission_stats_entry_s)) { + _count[(uint8_t)MAV_MISSION_TYPE_FENCE] = count; + // TODO: notify via orb + + } else { + warnx("WPM: ERROR: can't save mission state"); + + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } + + return PX4_ERROR; + } + + return PX4_OK; + +} + +int +MavlinkMissionManager::update_safepoint_count(unsigned count) +{ + mission_stats_entry_s stats; + stats.num_items = count; + + /* update stats in dataman */ + int res = dm_write(DM_KEY_SAFE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + + if (res == sizeof(mission_stats_entry_s)) { + _count[(uint8_t)MAV_MISSION_TYPE_RALLY] = count; + + } else { + warnx("WPM: ERROR: can't save mission state"); + + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } + + return PX4_ERROR; + } + + return PX4_OK; +} void MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) @@ -183,6 +267,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t wpa.target_system = sysid; wpa.target_component = compid; wpa.type = type; + wpa.mission_type = _mission_type; mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); @@ -193,14 +278,14 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t void MavlinkMissionManager::send_mission_current(uint16_t seq) { - if (seq < _count) { + if (seq < current_item_count()) { mavlink_mission_current_t wpc; wpc.seq = seq; mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc); - } else if (seq == 0 && _count == 0) { + } else if (seq == 0 && current_item_count() == 0) { /* don't broadcast if no WPs */ } else { @@ -220,7 +305,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ wpc.target_system = sysid; wpc.target_component = compid; - wpc.count = _count; + wpc.count = count; mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); @@ -231,10 +316,50 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id); - struct mission_item_s mission_item; + dm_item_t dm_item; + struct mission_item_s mission_item {}; + bool read_success = false; - if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + switch (_mission_type) { + + case MAV_MISSION_TYPE_MISSION: { + dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id); + read_success = dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == + sizeof(struct mission_item_s); + } + break; + + case MAV_MISSION_TYPE_FENCE: { // Read a geofence point + mission_fence_point_s mission_fence_point; + read_success = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) == + sizeof(mission_fence_point_s); + mission_item.nav_cmd = mission_fence_point.nav_cmd; + mission_item.frame = mission_fence_point.frame; + mission_item.lat = mission_fence_point.lat; + mission_item.lon = mission_fence_point.lon; + mission_item.altitude = mission_fence_point.alt; + mission_item.vertex_count = mission_fence_point.vertex_count; + } + break; + + case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point + mission_save_point_s mission_save_point; + read_success = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_save_point, sizeof(mission_save_point_s)) == + sizeof(mission_save_point_s); + mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; + mission_item.frame = mission_save_point.frame; + mission_item.lat = mission_save_point.lat; + mission_item.lon = mission_save_point.lon; + mission_item.altitude = mission_save_point.alt; + } + break; + + default: + _mavlink->send_statustext_critical("Received unknown mission type, abort."); + break; + } + + if (read_success) { _time_last_sent = hrt_absolute_time(); if (_int_mode) { @@ -279,12 +404,32 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } } +unsigned +MavlinkMissionManager::current_max_item_count() +{ + if ((unsigned)_mission_type >= sizeof(MAX_COUNT) / sizeof(MAX_COUNT[0])) { + PX4_ERR("WPM: MAX_COUNT out of bounds (%u)", (unsigned)_mission_type); + return 0; + } + + return MAX_COUNT[(unsigned)_mission_type]; +} + +unsigned +MavlinkMissionManager::current_item_count() +{ + if ((unsigned)_mission_type >= sizeof(_count) / sizeof(_count[0])) { + PX4_ERR("WPM: _count out of bounds (%u)", (unsigned)_mission_type); + return 0; + } + + return _count[(unsigned)_mission_type]; +} void MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) { - if (seq < _max_count) { - + if (seq < current_max_item_count()) { _time_last_sent = hrt_absolute_time(); if (_int_mode) { @@ -292,6 +437,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; + wpr.mission_type = _mission_type; mavlink_msg_mission_request_int_send_struct(_mavlink->get_channel(), &wpr); if (_verbose) { @@ -304,6 +450,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; + wpr.mission_type = _mission_type; mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr); @@ -469,10 +616,10 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) if (CHECK_SYSID_COMPID_MISSION(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { - if (_state == MAVLINK_WPM_STATE_SENDLIST) { + if (_state == MAVLINK_WPM_STATE_SENDLIST && _mission_type == wpa.mission_type) { _time_last_recv = hrt_absolute_time(); - if (_transfer_seq == _count) { + if (_transfer_seq == current_item_count()) { if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } } else { @@ -515,14 +662,31 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); - if (wpc.seq < _count) { - if (update_active_mission(_dataman_id, _count, wpc.seq) == PX4_OK) { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + if (wpc.seq < current_item_count()) { + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + if (update_active_mission(_dataman_id, _count[(uint8_t)_mission_type], wpc.seq) == PX4_OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } - _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); + } + + break; + + case MAV_MISSION_TYPE_FENCE: + update_geofence_count(_count[(uint8_t)_mission_type]); + break; + + case MAV_MISSION_TYPE_RALLY: + update_safepoint_count(_count[(uint8_t)_mission_type]); + break; + + default: + PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + break; } } else { @@ -547,23 +711,40 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_msg_mission_request_list_decode(msg, &wprl); if (CHECK_SYSID_COMPID_MISSION(wprl)) { - if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + if (_state == MAVLINK_WPM_STATE_IDLE || (_state == MAVLINK_WPM_STATE_SENDLIST + && (uint8_t)_mission_type == wprl.mission_type)) { _time_last_recv = hrt_absolute_time(); _state = MAVLINK_WPM_STATE_SENDLIST; + _mission_type = (MAV_MISSION_TYPE)wprl.mission_type; + + // make sure our item counts are up-to-date + switch (_mission_type) { + case MAV_MISSION_TYPE_FENCE: + load_geofence_stats(); + break; + + case MAV_MISSION_TYPE_RALLY: + load_safepoint_stats(); + break; + + default: + break; + } + _transfer_seq = 0; - _transfer_count = _count; + _transfer_count = current_item_count(); _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; - if (_count > 0) { + if (_transfer_count > 0) { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } } - send_mission_count(msg->sysid, msg->compid, _count); + send_mission_count(msg->sysid, msg->compid, _transfer_count); } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } @@ -607,6 +788,12 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) if (CHECK_SYSID_COMPID_MISSION(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { + + if ((uint8_t)_mission_type != wpr.mission_type) { + PX4_WARN("WPM: Unexpected mission type (%u %u)", (int)wpr.mission_type, (int)_mission_type); + return; + } + _time_last_recv = hrt_absolute_time(); /* _transfer_seq contains sequence of expected request */ @@ -637,11 +824,11 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) } /* double check bounds in case of items count changed */ - if (wpr.seq < _count) { + if (wpr.seq < current_item_count()) { send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); } + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)current_item_count() - 1); } _state = MAVLINK_WPM_STATE_IDLE; @@ -686,9 +873,10 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) } _transfer_in_progress = true; + _mission_type = (MAV_MISSION_TYPE)wpc.mission_type; - if (wpc.count > _max_count) { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + if (wpc.count > current_max_item_count()) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count()); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); _transfer_in_progress = false; @@ -698,8 +886,24 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) if (wpc.count == 0) { if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } - /* alternate dataman ID anyway to let navigator know about changes */ - update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + break; + + case MAV_MISSION_TYPE_FENCE: + update_geofence_count(0); + break; + + case MAV_MISSION_TYPE_RALLY: + update_safepoint_count(0); + break; + + default: + PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + break; + } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); _transfer_in_progress = false; @@ -775,6 +979,12 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mavlink_msg_mission_item_decode(msg, &wp); if (CHECK_SYSID_COMPID_MISSION(wp)) { + + if (wp.mission_type != _mission_type) { + PX4_WARN("WPM: Unexpected mission type (%u %u)", (int)wp.mission_type, (int)_mission_type); + return; + } + if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -813,14 +1023,71 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) return; } - dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + bool write_failed = false; + bool check_failed = false; - if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, - sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + switch (_mission_type) { + + case MAV_MISSION_TYPE_MISSION: { + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + + write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, + sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + + if (!write_failed) { + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + } + } + break; + + case MAV_MISSION_TYPE_FENCE: { // Write a geofence point + mission_fence_point_s mission_fence_point; + mission_fence_point.nav_cmd = mission_item.nav_cmd; + mission_fence_point.lat = mission_item.lat; + mission_fence_point.lon = mission_item.lon; + mission_fence_point.alt = mission_item.altitude; + mission_fence_point.vertex_count = mission_item.vertex_count; + mission_fence_point.frame = mission_item.frame; + + write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); + + if (mission_item.vertex_count < 3) { // feasibility check + PX4_ERR("Fence: too few vertices"); + check_failed = true; + update_geofence_count(0); + } + } + break; + + case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point + mission_save_point_s mission_save_point; + mission_save_point.lat = mission_item.lat; + mission_save_point.lon = mission_item.lon; + mission_save_point.alt = mission_item.altitude; + mission_save_point.frame = mission_item.frame; + write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_save_point, + sizeof(mission_save_point_s)) != sizeof(mission_save_point_s); + } + break; + + default: + _mavlink->send_statustext_critical("Received unknown mission type, abort."); + break; + } + + if (write_failed || check_failed) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext_critical("Unable to write on micro SD"); + + if (write_failed) { + _mavlink->send_statustext_critical("Unable to write on micro SD"); + } + _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; @@ -841,7 +1108,27 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) _state = MAVLINK_WPM_STATE_IDLE; - if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == PX4_OK) { + ret = 0; + + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + break; + + case MAV_MISSION_TYPE_FENCE: + ret = update_geofence_count(_transfer_count); + break; + + case MAV_MISSION_TYPE_RALLY: + ret = update_safepoint_count(_transfer_count); + break; + + default: + PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + break; + } + + if (ret == PX4_OK) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); } else { @@ -870,7 +1157,34 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) /* don't touch mission items storage itself, but only items count in mission state */ _time_last_recv = hrt_absolute_time(); - if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == PX4_OK) { + _mission_type = (MAV_MISSION_TYPE)wpca.mission_type; // this is needed for the returned ack + int ret = 0; + + switch (wpca.mission_type) { + case MAV_MISSION_TYPE_MISSION: + ret = update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + break; + + case MAV_MISSION_TYPE_FENCE: + ret = update_geofence_count(0); + break; + + case MAV_MISSION_TYPE_RALLY: + ret = update_safepoint_count(0); + break; + + case MAV_MISSION_TYPE_ALL: + ret = update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + ret = update_geofence_count(0) || ret; + ret = update_safepoint_count(0) || ret; + break; + + default: + PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + break; + } + + if (ret == PX4_OK) { if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); @@ -980,6 +1294,20 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; + case MAV_CMD_NAV_FENCE_RETURN_POINT: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; + + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->vertex_count = (uint16_t)(mavlink_mission_item->param1 + 0.5f); + break; + + case MAV_CMD_NAV_RALLY_POINT: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; + default: mission_item->nav_cmd = NAV_CMD_INVALID; @@ -1192,6 +1520,18 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; break; + case MAV_CMD_NAV_FENCE_RETURN_POINT: + break; + + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + mavlink_mission_item->param1 = (float)mission_item->vertex_count; + break; + + case MAV_CMD_NAV_RALLY_POINT: + break; + + default: return PX4_ERROR; } @@ -1207,6 +1547,6 @@ void MavlinkMissionManager::check_active_mission() if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } _my_dataman_id = _dataman_id; - this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count); + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[(uint8_t)MAV_MISSION_TYPE_MISSION]); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b8d96936dd..bb31a66fe7 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -33,7 +33,10 @@ /** * @file mavlink_mission.h - * MAVLink mission manager interface definition. + * Implementation of the MAVLink mission protocol. + * Documentation: + * - http://qgroundcontrol.org/mavlink/mission_interface + * - http://qgroundcontrol.org/mavlink/waypoint_protocol * * @author Lorenz Meier * @author Julian Oes @@ -42,6 +45,7 @@ #pragma once +#include #include #include "mavlink_bridge_header.h" @@ -89,6 +93,7 @@ public: private: enum MAVLINK_WPM_STATES _state; ///< Current state + enum MAV_MISSION_TYPE _mission_type; ///< mission type of current transmission (only one at a time possible) uint64_t _time_last_recv; uint64_t _time_last_sent; @@ -99,14 +104,13 @@ private: bool _int_mode; ///< Use accurate int32 instead of float - unsigned _max_count; ///< Maximum number of mission items unsigned _filesystem_errcount; ///< File system error count static int _dataman_id; ///< Global Dataman storage ID for active mission int _my_dataman_id; ///< class Dataman storage ID static bool _dataman_init; ///< Dataman initialized - static unsigned _count; ///< Count of items in active mission + static unsigned _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE static int _current_seq; ///< Current item sequence in active mission static int _last_reached; ///< Last reached waypoint in active mission (-1 means nothing reached) @@ -131,6 +135,18 @@ private: static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors + static constexpr unsigned MAX_COUNT[] = { + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_FENCE_POINTS_MAX - 1, + DM_KEY_SAFE_POINTS_MAX - 1 + }; /**< Maximum number of mission items for each type + (fence & save points use the first item for the stats) */ + + /** get the maximum number of item count for the current _mission_type */ + inline unsigned current_max_item_count(); + + /** get the number of item count for the current _mission_type */ + inline unsigned current_item_count(); /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); @@ -140,6 +156,18 @@ private: int update_active_mission(int dataman_id, unsigned count, int seq); + /** store the geofence count to dataman */ + int update_geofence_count(unsigned count); + + /** store the safepoint count to dataman */ + int update_safepoint_count(unsigned count); + + /** load geofence stats from dataman */ + int load_geofence_stats(); + + /** load safe point stats from dataman */ + int load_safepoint_stats(); + /** * @brief Sends an waypoint ack message */