From 11fb67cbd8d721ef4dd5182b5dcc2703475f56d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 19 Aug 2021 14:06:52 +0200 Subject: [PATCH] mavlink_mission: restore old sequence number when a new mission is infeasible --- src/modules/mavlink/mavlink_mission.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 067b4f4cb273..939c766f0184 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -487,6 +487,12 @@ MavlinkMissionManager::send() if (_mission_result_sub.update(&mission_result)) { + /* If an infeasible mission is uploaded and the old feasible mission exists item count could be wrong. + * This is restoring item count from the feasible mission. */ + if (_count[MAV_MISSION_TYPE_MISSION] != mission_result.seq_total) { + _count[MAV_MISSION_TYPE_MISSION] = mission_result.seq_total; + } + if (_current_seq != mission_result.seq_current) { _current_seq = mission_result.seq_current;