diff --git a/msg/mission.msg b/msg/mission.msg index 70fc68ccf2..bc19b70dec 100644 --- a/msg/mission.msg +++ b/msg/mission.msg @@ -3,3 +3,5 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest + +bool clear_mission # true if mission is cleared in dataman \ No newline at end of file diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7c8a8f94ca..995b9557dd 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -148,7 +148,7 @@ MavlinkMissionManager::load_safepoint_stats() * Publish mission topic to notify navigator about changes. */ int -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission) { // We want to make sure the whole struct is initialized including padding before getting written by dataman. mission_s mission{}; @@ -156,6 +156,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; + mission.clear_mission = clear_mission; /* update mission state in dataman */ @@ -1303,7 +1304,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true); break; case MAV_MISSION_TYPE_FENCE: @@ -1316,7 +1317,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) case MAV_MISSION_TYPE_ALL: ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true); ret = update_geofence_count(0) || ret; ret = update_safepoint_count(0) || ret; break; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 93847b3ff4..0b6b8a82ea 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -159,7 +159,7 @@ private: void init_offboard_mission(); - int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission = false); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 485dc63b43..c6844c3bff 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -482,7 +482,7 @@ void Mission::update_mission() { - bool failed = true; + bool failed = false; /* Reset vehicle_roi * Missions that do not explicitly configure ROI would not override @@ -493,46 +493,58 @@ Mission::update_mission() _old_mission.current_seq = _current_mission_index; if (_mission_sub.copy(&_mission)) { - /* determine current index */ - if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { - _current_mission_index = _mission.current_seq; + + /* If _mission.clear_mission flag is set to true, mission is deleted from dataman. + * No mission validation needed. */ + if (_mission.clear_mission) { + _mission.count = 0; + _mission.current_seq = 0; + _current_mission_index = 0; } else { - /* if less items available, reset to first item */ - if (_current_mission_index >= (int)_mission.count) { - _current_mission_index = 0; - } else if (_current_mission_index < 0) { - /* if not initialized, set it to 0 */ - _current_mission_index = 0; + /* determine current index */ + if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { + _current_mission_index = _mission.current_seq; + + } else { + /* if less items available, reset to first item */ + if (_current_mission_index >= (int)_mission.count) { + _current_mission_index = 0; + + } else if (_current_mission_index < 0) { + /* if not initialized, set it to 0 */ + _current_mission_index = 0; + } + + /* otherwise, just leave it */ } - /* otherwise, just leave it */ - } + check_mission_valid(true); - check_mission_valid(true); + failed = !_navigator->get_mission_result()->valid; - failed = !_navigator->get_mission_result()->valid; + if (!failed) { + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; - if (!failed) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; + /* reset work item if new mission has been accepted */ + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _mission_changed = true; + } - /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_changed = true; - } + /* check if the mission waypoints changed while the vehicle is in air + * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ + if (((_mission.count != _old_mission.count) || + (_mission.dataman_id != _old_mission.dataman_id)) && + !_navigator->get_land_detected()->landed) { + _mission_waypoints_changed = true; + } - /* check if the mission waypoints changed while the vehicle is in air - * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != _old_mission.count) || - (_mission.dataman_id != _old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { - _mission_waypoints_changed = true; } } else {