Send MISSION_CURRENT regularly

mavlink · Aug 2, 2021 · 3a269d8 · 3a269d8
1 parent 3e97c89
commit 3a269d8
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 11 deletions.
diff --git a/src/plugins/mission_raw_server/mission_raw_server_impl.cpp b/src/plugins/mission_raw_server/mission_raw_server_impl.cpp
@@ -225,13 +225,6 @@ void MissionRawServerImpl::init()
                 _parent->send_message(status_message);
             } else {
                 set_current_seq(set_current.seq);
-                mavlink_message_t mission_current;
-                mavlink_msg_mission_current_pack(
-                    _parent->get_own_system_id(),
-                    _parent->get_own_component_id(),
-                    &mission_current,
-                    set_current.seq);
-                _parent->send_message(mission_current);
             }
         },
         this);
@@ -321,21 +314,31 @@ void MissionRawServerImpl::set_current_item_complete()
     _parent->send_message(mission_reached);
     if (_current_seq + 1 == _current_mission.size()) {
         _mission_completed = true;
+        set_current_seq(0);
     } else {
         set_current_seq(_current_seq + 1);
     }
 }
 
-void MissionRawServerImpl::set_current_seq(int32_t seq)
+void MissionRawServerImpl::set_current_seq(uint16_t seq)
 {
-    if (_current_mission.size() <= static_cast<size_t>(seq))
+    if (_current_mission.size() <= static_cast<size_t>(seq)) {
         return;
+    }
 
     _current_seq = seq;
 
     auto converted_item = convert_item(_current_mission.at(_current_seq));
     _parent->call_user_callback(
         [this, converted_item]() { _current_item_changed_callback(converted_item); });
+
+    mavlink_message_t mission_current;
+    mavlink_msg_mission_current_pack(
+        _parent->get_own_system_id(),
+        _parent->get_own_component_id(),
+        &mission_current,
+        _current_seq);
+    _parent->send_message(mission_current);
 }
 
 } // namespace mavsdk
diff --git a/src/plugins/mission_raw_server/mission_raw_server_impl.h b/src/plugins/mission_raw_server/mission_raw_server_impl.h
@@ -41,7 +41,7 @@ class MissionRawServerImpl : public PluginImplBase {
     std::atomic<bool> _stop_work_thread = false;
 
     std::vector<MAVLinkMissionTransfer::ItemInt> _current_mission;
-    uint32_t _current_seq;
+    uint16_t _current_seq;
 
     struct MissionData {
         mutable std::recursive_mutex mutex{};
@@ -66,7 +66,7 @@ class MissionRawServerImpl : public PluginImplBase {
         MAVLinkMissionTransfer::Result result,
         const std::vector<MAVLinkMissionTransfer::ItemInt>& int_items);
 
-    void set_current_seq(int32_t seq);
+    void set_current_seq(uint16_t seq);
 
     void add_task(std::function<void()> task)
     {