diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 4453b7ab00..d8dea330c8 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -503,6 +503,8 @@ MavlinkMissionManager::send() if (_mission_result_sub.update()) { const mission_result_s &mission_result = _mission_result_sub.get(); + bool send_current = false; + if (_current_seq != mission_result.seq_current) { _current_seq = mission_result.seq_current; @@ -511,7 +513,7 @@ MavlinkMissionManager::send() if (mission_result.seq_total > 0) { if (mission_result.seq_current < mission_result.seq_total) { - send_mission_current(_current_seq); + send_current = true; } else { _mavlink.send_statustext_critical("ERROR: wp index out of bounds\t"); @@ -521,6 +523,16 @@ MavlinkMissionManager::send() } } + // Send MISSION_CURRENT when finished state changes (to notify MISSION_STATE_COMPLETE) + if (_last_finished != mission_result.finished) { + _last_finished = mission_result.finished; + send_current = true; + } + + if (send_current) { + send_mission_current(_current_seq); + } + if (_last_reached != mission_result.seq_reached) { _last_reached = mission_result.seq_reached; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index e35c668c91..192dcaafca 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -132,6 +132,7 @@ private: static int32_t _current_seq; ///< Current item sequence in active mission int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) + bool _last_finished{false}; ///< Last mission finished state dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission