From 63718bf27b63aac113312bb52fd2137b6cf34593 Mon Sep 17 00:00:00 2001 From: lamping7 Date: Sun, 10 Dec 2017 02:30:58 -0500 Subject: [PATCH] fix MISSION_ITEM REACHED message broadcast (#8332) --- msg/mission.msg | 5 +- msg/mission_result.msg | 11 +- src/modules/logger/logger.cpp | 2 + src/modules/mavlink/mavlink_mission.cpp | 123 ++++++++++------------- src/modules/mavlink/mavlink_mission.h | 67 ++++++------ src/modules/navigator/land.cpp | 1 - src/modules/navigator/mission.cpp | 15 ++- src/modules/navigator/navigator_main.cpp | 1 - src/modules/navigator/takeoff.cpp | 1 - 9 files changed, 106 insertions(+), 120 deletions(-) diff --git a/msg/mission.msg b/msg/mission.msg index b25940c07a..323f7a4e51 100644 --- a/msg/mission.msg +++ b/msg/mission.msg @@ -1,5 +1,6 @@ -int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 -uint32 count # count of the missions stored in the dataman +int8 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 + +uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest # TOPICS mission offboard_mission onboard_mission diff --git a/msg/mission_result.msg b/msg/mission_result.msg index 5f6d5bcb14..a780ba8463 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -1,12 +1,11 @@ uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified -uint32 seq_reached # Sequence of the mission item which has been reached -uint32 seq_current # Sequence of the current mission item -uint32 seq_total # Total number of mission items +int32 seq_reached # Sequence of the mission item which has been reached, default -1 +uint16 seq_current # Sequence of the current mission item +uint16 seq_total # Total number of mission items bool valid # true if mission is valid bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings -bool reached # true if mission has been reached bool finished # true if mission has been completed bool failure # true if the mission cannot continue or be completed for some reason @@ -14,5 +13,5 @@ bool stay_in_failsafe # true if the commander should not switch out of the fail bool flight_termination # true if the navigator demands a flight termination from the commander app bool item_do_jump_changed # true if the number of do jumps remaining has changed -uint32 item_changed_index # indicate which item has changed -uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item +uint16 item_changed_index # indicate which item has changed +uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index e4b066651c..5902b7a2bf 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -593,6 +593,8 @@ void Logger::add_default_topics() add_topic("estimator_status", 200); add_topic("input_rc", 200); add_topic("manual_control_setpoint", 200); + add_topic("mission_result"); + add_topic("offboard_mission"); add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); add_topic("sensor_combined", 100); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 0189096f66..0254a7ffd1 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -54,13 +54,12 @@ #include #include -int MavlinkMissionManager::_dataman_id = 0; +int8_t MavlinkMissionManager::_dataman_id = 0; bool MavlinkMissionManager::_dataman_init = false; -unsigned MavlinkMissionManager::_count[3] = { 0, 0, 0 }; -int MavlinkMissionManager::_current_seq = 0; -int MavlinkMissionManager::_last_reached = -1; +uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; +int32_t MavlinkMissionManager::_current_seq = 0; bool MavlinkMissionManager::_transfer_in_progress = false; -constexpr unsigned MavlinkMissionManager::MAX_COUNT[]; +constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ @@ -69,27 +68,6 @@ uint16_t MavlinkMissionManager::_geofence_update_counter = 0; (_msg.target_component == MAV_COMP_ID_ALL))) 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), - _filesystem_errcount(0), - _my_dataman_id(0), - _transfer_dataman_id(0), - _transfer_count(0), - _transfer_seq(0), - _transfer_current_seq(-1), - _transfer_partner_sysid(0), - _transfer_partner_compid(0), - _offboard_mission_sub(-1), - _mission_result_sub(-1), - _offboard_mission_pub(nullptr), - _geofence_locked(false), - _slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz _verbose(mavlink->verbose()), _mavlink(mavlink) { @@ -116,7 +94,7 @@ MavlinkMissionManager::init_offboard_mission() if (ret > 0) { _dataman_id = mission_state.dataman_id; - _count[(uint8_t)MAV_MISSION_TYPE_MISSION] = mission_state.count; + _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; _current_seq = mission_state.current_seq; } else if (ret < 0) { @@ -139,7 +117,7 @@ MavlinkMissionManager::load_geofence_stats() 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; + _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; _geofence_update_counter = stats.update_counter; } @@ -154,7 +132,7 @@ MavlinkMissionManager::load_safepoint_stats() 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; + _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; } return ret; @@ -164,10 +142,10 @@ MavlinkMissionManager::load_safepoint_stats() * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. */ int -MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +MavlinkMissionManager::update_active_mission(int8_t dataman_id, uint16_t count, int32_t seq) { - struct mission_s mission; - + mission_s mission; + mission.timestamp = hrt_absolute_time(); mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; @@ -178,7 +156,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[(uint8_t)MAV_MISSION_TYPE_MISSION] = count; + _count[MAV_MISSION_TYPE_MISSION] = count; _current_seq = seq; _my_dataman_id = _dataman_id; @@ -213,7 +191,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) 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; + _count[MAV_MISSION_TYPE_FENCE] = count; } else { PX4_ERR("WPM: can't save mission state"); @@ -239,7 +217,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) 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; + _count[MAV_MISSION_TYPE_RALLY] = count; } else { PX4_ERR("WPM: can't save mission state"); @@ -273,7 +251,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t void MavlinkMissionManager::send_mission_current(uint16_t seq) { - unsigned item_count = _count[(uint8_t)MAV_MISSION_TYPE_MISSION]; + unsigned item_count = _count[MAV_MISSION_TYPE_MISSION]; if (seq < item_count) { mavlink_mission_current_t wpc; @@ -409,26 +387,26 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } } -unsigned +uint16_t 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); + if (_mission_type >= sizeof(MAX_COUNT) / sizeof(MAX_COUNT[0])) { + PX4_ERR("WPM: MAX_COUNT out of bounds (%u)", _mission_type); return 0; } - return MAX_COUNT[(unsigned)_mission_type]; + return MAX_COUNT[_mission_type]; } -unsigned +uint16_t MavlinkMissionManager::current_item_count() { - if ((unsigned)_mission_type >= sizeof(_count) / sizeof(_count[0])) { - PX4_ERR("WPM: _count out of bounds (%u)", (unsigned)_mission_type); + if (_mission_type >= sizeof(_count) / sizeof(_count[0])) { + PX4_ERR("WPM: _count out of bounds (%u)", _mission_type); return 0; } - return _count[(unsigned)_mission_type]; + return _count[_mission_type]; } void @@ -495,17 +473,21 @@ MavlinkMissionManager::send(const hrt_abstime now) mission_result_s mission_result; orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); - _current_seq = mission_result.seq_current; + if (_current_seq != mission_result.seq_current) { + _current_seq = mission_result.seq_current; - if (_verbose) { PX4_INFO("WPM: got mission result, new current_seq: %d", _current_seq); } + if (_verbose) { PX4_INFO("WPM: got mission result, new current_seq: %u", _current_seq); } + } - if (mission_result.reached) { - _time_last_reached = now; + if (_last_reached != mission_result.seq_reached) { _last_reached = mission_result.seq_reached; - send_mission_item_reached((uint16_t)mission_result.seq_reached); + _reached_sent_count = 0; - } else { - _last_reached = -1; + if (_last_reached >= 0) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + if (_verbose) { PX4_INFO("WPM: got mission result, new seq_reached: %d", _last_reached); } } send_mission_current(_current_seq); @@ -520,21 +502,24 @@ MavlinkMissionManager::send(const hrt_abstime now) if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); - // send the reached message a couple of times after reaching the waypoint - if (_last_reached >= 0 && (now - _time_last_reached) < 300 * 1000) { + // send the reached message another 10 times + if (_last_reached >= 0 && (_reached_sent_count < 10)) { send_mission_item_reached((uint16_t)_last_reached); + _reached_sent_count++; } } } /* check for timed-out operations */ if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0) - && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) { + // try to request item again after timeout send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } else if (_state == MAVLINK_WPM_STATE_SENDLIST && (_time_last_sent > 0) - && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) { + if (_transfer_seq == 0) { /* try to send items count again after timeout */ send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count, _mission_type); @@ -547,7 +532,8 @@ MavlinkMissionManager::send(const hrt_abstime now) } } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) - && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) { + _mavlink->send_statustext_critical("Operation timeout"); if (_verbose) { PX4_INFO("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } @@ -665,8 +651,8 @@ 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[(uint8_t)MAV_MISSION_TYPE_MISSION]) { - if (update_active_mission(_dataman_id, _count[(uint8_t)MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { + if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { + if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { if (_verbose) { PX4_INFO("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } } else { @@ -725,13 +711,12 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) if (_transfer_count > 0) { if (_verbose) { - PX4_INFO("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count, - (int)_mission_type); + PX4_INFO("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count, _mission_type); } } else { if (_verbose) { - PX4_INFO("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", (int)_mission_type); + PX4_INFO("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type); } } @@ -780,8 +765,8 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) 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); + if (_mission_type != wpr.mission_type) { + PX4_WARN("WPM: Unexpected mission type (%u %u)", wpr.mission_type, _mission_type); return; } @@ -819,7 +804,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); } else { - if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)current_item_count() - 1); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", wpr.seq, wpr.seq, current_item_count() - 1); } switch_to_idle_state(); @@ -892,7 +877,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) break; default: - PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + PX4_ERR("mission type %u not handled", _mission_type); break; } @@ -1002,7 +987,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) 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); + PX4_WARN("WPM: Unexpected mission type (%u %u)", wp.mission_type, _mission_type); return; } @@ -1165,7 +1150,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) break; default: - PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + PX4_ERR("mission type %u not handled", _mission_type); break; } @@ -1225,7 +1210,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) break; default: - PX4_ERR("mission type %u not handled", (unsigned)_mission_type); + PX4_ERR("mission type %u not handled", _mission_type); break; } @@ -1626,7 +1611,7 @@ void MavlinkMissionManager::check_active_mission() if (_verbose) { PX4_INFO("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } _my_dataman_id = _dataman_id; - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[(uint8_t)MAV_MISSION_TYPE_MISSION], + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], MAV_MISSION_TYPE_MISSION); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b9f4d02377..f20a33901c 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -67,8 +67,8 @@ enum MAVLINK_WPM_CODES { MAVLINK_WPM_CODE_ENUM_END }; -#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds -#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds +static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT = 5000000; ///< Protocol action timeout in us +static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 500000; ///< Protocol retry timeout in us class Mavlink; @@ -92,45 +92,48 @@ public: void check_active_mission(void); 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) + enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state + enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) - uint64_t _time_last_recv; - uint64_t _time_last_sent; - uint64_t _time_last_reached; ///< last time when the vehicle reached a waypoint + uint64_t _time_last_recv{0}; + uint64_t _time_last_sent{0}; - uint32_t _action_timeout; - uint32_t _retry_timeout; + uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint - bool _int_mode; ///< Use accurate int32 instead of float + bool _int_mode{false}; ///< Use accurate int32 instead of float - unsigned _filesystem_errcount; ///< File system error count + unsigned _filesystem_errcount{0}; ///< File system error count - static int _dataman_id; ///< Global Dataman storage ID for active mission - int _my_dataman_id; ///< class Dataman storage ID + static int8_t _dataman_id; ///< Global Dataman storage ID for active mission + int8_t _my_dataman_id{0}; ///< class Dataman storage ID static bool _dataman_init; ///< Dataman initialized - 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 uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE + static int32_t _current_seq; ///< Current item sequence in active mission - static int _last_reached; ///< Last reached waypoint in active mission (-1 means nothing reached) + int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) + + int8_t _transfer_dataman_id{0}; ///< Dataman storage ID for current transmission + + uint16_t _transfer_count{0}; ///< Items count in current transmission + uint16_t _transfer_seq{0}; ///< Item sequence in current transmission + + int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized) + + uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission + uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission - int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - unsigned _transfer_seq; ///< Item sequence in current transmission - unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission - unsigned _transfer_partner_compid; ///< Partner component ID for current transmission static bool _transfer_in_progress; ///< Global variable checking for current transmission - int _offboard_mission_sub; - int _mission_result_sub; - orb_advert_t _offboard_mission_pub; + int _offboard_mission_sub{-1}; + int _mission_result_sub{-1}; + + orb_advert_t _offboard_mission_pub{nullptr}; - static uint16_t _geofence_update_counter; - bool _geofence_locked; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) + static uint16_t _geofence_update_counter; + bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) - MavlinkRateLimiter _slow_rate_limiter; + MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz bool _verbose; @@ -138,7 +141,7 @@ private: static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors - static constexpr unsigned MAX_COUNT[] = { + static constexpr uint16_t MAX_COUNT[] = { DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_FENCE_POINTS_MAX - 1, DM_KEY_SAFE_POINTS_MAX - 1 @@ -146,10 +149,10 @@ private: (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(); + uint16_t current_max_item_count(); /** get the number of item count for the current _mission_type */ - inline unsigned current_item_count(); + uint16_t current_item_count(); /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); @@ -157,7 +160,7 @@ private: void init_offboard_mission(); - int update_active_mission(int dataman_id, unsigned count, int seq); + int update_active_mission(int8_t dataman_id, uint16_t count, int32_t seq); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 15d554a75f..4fa0e8ad54 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -74,7 +74,6 @@ Land::on_activation() { /* set current mission item to Land */ set_land_item(&_mission_item, true); - _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 653b059f47..729b264acc 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -139,6 +139,9 @@ Mission::on_inactive() /* reset so current mission item gets restarted if mission was paused */ _work_item_type = WORK_ITEM_TYPE_DEFAULT; + + /* reset so MISSION_ITEM_REACHED isn't published */ + _navigator->get_mission_result()->seq_reached = -1; } void @@ -329,9 +332,8 @@ Mission::update_onboard_mission() /* reset mission failure if we have an updated valid mission */ _navigator->get_mission_result()->failure = false; - /* reset reached info as well */ - _navigator->get_mission_result()->reached = false; - _navigator->get_mission_result()->seq_reached = 0; + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; _navigator->get_mission_result()->seq_total = _onboard_mission.count; /* reset work item if new mission has been accepted */ @@ -384,9 +386,8 @@ Mission::update_offboard_mission() /* reset mission failure if we have an updated valid mission */ _navigator->get_mission_result()->failure = false; - /* reset reached info as well */ - _navigator->get_mission_result()->reached = false; - _navigator->get_mission_result()->seq_reached = 0; + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; _navigator->get_mission_result()->seq_total = _offboard_mission.count; /* reset work item if new mission has been accepted */ @@ -1406,7 +1407,6 @@ Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) void Mission::set_mission_item_reached() { - _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; _navigator->set_mission_result_updated(); reset_mission_item_reached(); @@ -1415,7 +1415,6 @@ Mission::set_mission_item_reached() void Mission::set_current_offboard_mission_item() { - _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; _navigator->set_mission_result_updated(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4b3dcba346..4830cce0ff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1131,7 +1131,6 @@ Navigator::publish_mission_result() //_mission_result.seq_current = 0; /* reset some of the flags */ - _mission_result.reached = false; _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 1f15e2913f..f186671646 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -142,7 +142,6 @@ Takeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, abs_altitude); - _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached();