diff --git a/msg/Mission.msg b/msg/Mission.msg index 3bc66f19fe..c7740d4f7a 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -4,5 +4,6 @@ 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 +int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5e59dcbf7e..b0230f792c 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -62,6 +62,7 @@ uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; int32_t MavlinkMissionManager::_current_seq = 0; bool MavlinkMissionManager::_transfer_in_progress = false; constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; +uint16_t MavlinkMissionManager::_mission_update_counter = 0; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; @@ -146,6 +147,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.mission_update_counter = _mission_update_counter; mission.geofence_update_counter = _geofence_update_counter; mission.safe_points_update_counter = _safepoint_update_counter; @@ -869,6 +871,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: + ++_mission_update_counter; + /* alternate dataman ID anyway to let navigator know about changes */ if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { @@ -1155,6 +1159,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: + ++_mission_update_counter; update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); break; @@ -1209,6 +1214,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: + ++_mission_update_counter; update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); break; @@ -1222,6 +1228,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_ALL: + ++_mission_update_counter; update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); ret = update_geofence_count(0); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index ae8407b038..d66eddc524 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -132,6 +132,7 @@ private: uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; + static uint16_t _mission_update_counter; static uint16_t _geofence_update_counter; static uint16_t _safepoint_update_counter; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 5465c65053..d513556922 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -624,29 +624,31 @@ Mission::update_mission() /* otherwise, just leave it */ } - check_mission_valid(true); + if (old_mission.mission_update_counter != _mission.mission_update_counter) { + 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 {