Merge pull request #2596 from mavlink/pr-use-mission-current-extended

Improve handling of mission finished

Author: julianoes

docs/en/cpp/api_reference/classmavsdk_1_1_mission_raw.md

@@ -69,6 +69,7 @@ void | [unsubscribe_mission_progress](#classmavsdk_1_1_mission_raw_1ac46f08b5270
 void | [unsubscribe_mission_changed](#classmavsdk_1_1_mission_raw_1ac6cd7602b2e5b46ad0ea1cf8bf602a0c) ([MissionChangedHandle](classmavsdk_1_1_mission_raw.md#classmavsdk_1_1_mission_raw_1a46da6d8a53822fd5fbd7b2a414624c5c) handle) | Unsubscribe from subscribe_mission_changed.
 std::pair< [Result](classmavsdk_1_1_mission_raw.md#classmavsdk_1_1_mission_raw_1a7ea2a624818ebb5a3e209cc275d58eaf), [MissionRaw::MissionImportData](structmavsdk_1_1_mission_raw_1_1_mission_import_data.md) > | [import_qgroundcontrol_mission](#classmavsdk_1_1_mission_raw_1a2a4ca261c37737e691c6954693d6d0a5) (std::string qgc_plan_path)const | Import a QGroundControl missions in JSON .plan format, from a file.
 std::pair< [Result](classmavsdk_1_1_mission_raw.md#classmavsdk_1_1_mission_raw_1a7ea2a624818ebb5a3e209cc275d58eaf), [MissionRaw::MissionImportData](structmavsdk_1_1_mission_raw_1_1_mission_import_data.md) > | [import_qgroundcontrol_mission_from_string](#classmavsdk_1_1_mission_raw_1a4a1b55650120d8af0ce7fa037f6b5ce9) (std::string qgc_plan)const | Import a QGroundControl missions in JSON .plan format, from a string.
+std::pair< [Result](classmavsdk_1_1_mission_raw.md#classmavsdk_1_1_mission_raw_1a7ea2a624818ebb5a3e209cc275d58eaf), bool > | [is_mission_finished](#classmavsdk_1_1_mission_raw_1a998db451c2718d9728276ca2d01ce315) () const | Check if the mission is finished.
 const [MissionRaw](classmavsdk_1_1_mission_raw.md) & | [operator=](#classmavsdk_1_1_mission_raw_1a2b8cdc1fbee72224a9ef6eb9266b2e2a) (const [MissionRaw](classmavsdk_1_1_mission_raw.md) &)=delete | Equality operator (object is not copyable).
 
 
@@ -734,6 +735,23 @@ This function is blocking.
 
 &emsp;std::pair< [Result](classmavsdk_1_1_mission_raw.md#classmavsdk_1_1_mission_raw_1a7ea2a624818ebb5a3e209cc275d58eaf), [MissionRaw::MissionImportData](structmavsdk_1_1_mission_raw_1_1_mission_import_data.md) > - Result of request.
 
+### is_mission_finished() {#classmavsdk_1_1_mission_raw_1a998db451c2718d9728276ca2d01ce315}
+```cpp
+std::pair< Result, bool > mavsdk::MissionRaw::is_mission_finished() const
+```
+
+
+Check if the mission is finished.
+
+Returns true if the mission is finished, false otherwise.
+
+
+This function is blocking.
+
+**Returns**
+
+&emsp;std::pair< [Result](classmavsdk_1_1_mission_raw.md#classmavsdk_1_1_mission_raw_1a7ea2a624818ebb5a3e209cc275d58eaf), bool > - Result of request.
+
 ### operator=() {#classmavsdk_1_1_mission_raw_1a2b8cdc1fbee72224a9ef6eb9266b2e2a}
 ```cpp
 const MissionRaw & mavsdk::MissionRaw::operator=(const MissionRaw &)=delete

proto

@@ -70,6 +70,7 @@ void MissionImpl::process_mission_current(const mavlink_message_t& message)
 
     std::lock_guard<std::mutex> lock(_mission_data.mutex);
     _mission_data.last_current_mavlink_mission_item = mission_current.seq;
+    _mission_data.mission_state = static_cast<MissionState>(mission_current.mission_state);
     report_progress_locked();
 }
 
@@ -993,17 +994,30 @@ std::pair<Mission::Result, bool> MissionImpl::is_mission_finished_locked() const
         return std::make_pair<Mission::Result, bool>(Mission::Result::Success, false);
     }
 
-    // It is not straightforward to look at "current" because it jumps to 0
-    // once the last item has been done. Therefore we have to lo decide using
-    // "reached" here.
-    // It seems that we never receive a reached when RTL is initiated after
-    // a mission, and we need to account for that.
-    const unsigned rtl_correction = _enable_return_to_launch_after_mission ? 2 : 1;
-
-    return std::make_pair<Mission::Result, bool>(
-        Mission::Result::Success,
-        unsigned(_mission_data.last_reached_mavlink_mission_item + rtl_correction) ==
-            _mission_data.mavlink_mission_item_to_mission_item_indices.size());
+    // If mission_state is Unknown, fall back to the previous behavior
+    if (_mission_data.mission_state == MissionState::Unknown) {
+        // It is not straightforward to look at "current" because it jumps to 0
+        // once the last item has been done. Therefore we have to lo decide using
+        // "reached" here.
+        // With PX4, it seems that we didn't use to receive a reached when RTL is initiated
+        // after a mission, and we needed to account for that all the way to PX4 v1.16.
+        const unsigned rtl_correction = _enable_return_to_launch_after_mission ? 2 : 1;
+
+        // By accepting the current to be larger than the total, we are accepting the case
+        // where we no longer require the rtl_correction, as this is now getting fixed in PX4.
+        return std::make_pair<Mission::Result, bool>(
+            Mission::Result::Success,
+            unsigned(_mission_data.last_reached_mavlink_mission_item + rtl_correction) >=
+                _mission_data.mavlink_mission_item_to_mission_item_indices.size());
+    }
+
+    // If mission_state is Completed, the mission is finished
+    if (_mission_data.mission_state == MissionState::Completed) {
+        return std::make_pair<Mission::Result, bool>(Mission::Result::Success, true);
+    }
+
+    // If mission_state is NotCompleted, the mission is not finished
+    return std::make_pair<Mission::Result, bool>(Mission::Result::Success, false);
 }
 
 int MissionImpl::current_mission_item() const

src/mavsdk/plugins/mission/mission_impl.cpp

@@ -16,6 +16,8 @@ namespace mavsdk {
 
 class MissionImpl : public PluginImplBase {
 public:
+    enum class MissionState { Unknown = 0, NotCompleted = 1, Completed = 2 };
+
     explicit MissionImpl(System& system);
     explicit MissionImpl(std::shared_ptr<System> system);
     ~MissionImpl() override;
@@ -127,6 +129,7 @@ class MissionImpl : public PluginImplBase {
         std::weak_ptr<MavlinkMissionTransferClient::WorkItem> last_upload{};
         std::weak_ptr<MavlinkMissionTransferClient::WorkItem> last_download{};
         bool gimbal_v2_in_control{false};
+        MissionState mission_state{MissionState::Unknown};
     } _mission_data{};
 
     TimeoutHandler::Cookie _timeout_cookie{};

src/mavsdk/plugins/mission/mission_impl.h

@@ -507,6 +507,17 @@ class MissionRaw : public PluginBase {
     std::pair<Result, MissionRaw::MissionImportData>
     import_qgroundcontrol_mission_from_string(std::string qgc_plan) const;
 
+    /**
+     * @brief Check if the mission is finished.
+     *
+     * Returns true if the mission is finished, false otherwise.
+     *
+     * This function is blocking.
+     *
+     * @return Result of request.
+     */
+    std::pair<Result, bool> is_mission_finished() const;
+
     /**
      * @brief Copy constructor.
      */

src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h

@@ -180,6 +180,11 @@ MissionRaw::import_qgroundcontrol_mission_from_string(std::string qgc_plan) cons
     return _impl->import_qgroundcontrol_mission_from_string(qgc_plan);
 }
 
+std::pair<MissionRaw::Result, bool> MissionRaw::is_mission_finished() const
+{
+    return _impl->is_mission_finished();
+}
+
 bool operator==(const MissionRaw::MissionProgress& lhs, const MissionRaw::MissionProgress& rhs)
 {
     return (rhs.current == lhs.current) && (rhs.total == lhs.total);

src/mavsdk/plugins/mission_raw/mission_raw.cpp

@@ -105,6 +105,7 @@ void MissionRawImpl::process_mission_current(const mavlink_message_t& message)
         if (mission_current.seq != _mission_progress.last_reached) {
             _mission_progress.last.current = mission_current.seq;
         }
+        _mission_progress.mission_state = static_cast<MissionState>(mission_current.mission_state);
     }
     {
         std::lock_guard<std::mutex> lock(_mission_changed.mutex);
@@ -693,4 +694,36 @@ MissionRaw::Result MissionRawImpl::convert_result(MavlinkMissionTransferClient::
             return MissionRaw::Result::Unknown;
     }
 }
+
+std::pair<MissionRaw::Result, bool> MissionRawImpl::is_mission_finished() const
+{
+    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
+
+    if (_mission_progress.last.current < 0) {
+        return std::make_pair<MissionRaw::Result, bool>(MissionRaw::Result::Success, false);
+    }
+
+    if (_mission_progress.last_reached < 0) {
+        return std::make_pair<MissionRaw::Result, bool>(MissionRaw::Result::Success, false);
+    }
+
+    if (_mission_progress.last.total <= 0) {
+        return std::make_pair<MissionRaw::Result, bool>(MissionRaw::Result::Success, false);
+    }
+
+    // If mission_state is Unknown, fall back to the previous behavior
+    if (_mission_progress.mission_state == MissionState::Unknown) {
+        return std::make_pair<MissionRaw::Result, bool>(
+            MissionRaw::Result::Success,
+            _mission_progress.last_reached == _mission_progress.last.total - 1);
+    }
+
+    // If mission_state is Completed, the mission is finished
+    if (_mission_progress.mission_state == MissionState::Completed) {
+        return std::make_pair<MissionRaw::Result, bool>(MissionRaw::Result::Success, true);
+    }
+
+    // If mission_state is NotCompleted, the mission is not finished
+    return std::make_pair<MissionRaw::Result, bool>(MissionRaw::Result::Success, false);
+}
 } // namespace mavsdk

src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp

@@ -12,6 +12,8 @@ namespace mavsdk {
 
 class MissionRawImpl : public PluginImplBase {
 public:
+    enum class MissionState { Unknown = 0, NotCompleted = 1, Completed = 2 };
+
     explicit MissionRawImpl(System& system);
     explicit MissionRawImpl(std::shared_ptr<System> system);
     ~MissionRawImpl() override;
@@ -66,6 +68,8 @@ class MissionRawImpl : public PluginImplBase {
     subscribe_mission_progress(const MissionRaw::MissionProgressCallback& callback);
     void unsubscribe_mission_progress(MissionRaw::MissionProgressHandle handle);
 
+    std::pair<MissionRaw::Result, bool> is_mission_finished() const;
+
     std::pair<MissionRaw::Result, MissionRaw::MissionImportData>
     import_qgroundcontrol_mission(std::string qgc_plan_path);
 
@@ -119,11 +123,12 @@ class MissionRawImpl : public PluginImplBase {
     std::weak_ptr<MavlinkMissionTransferClient::WorkItem> _last_download{};
 
     struct {
-        std::mutex mutex{};
+        mutable std::mutex mutex{};
         MissionRaw::MissionProgress last{};
         MissionRaw::MissionProgress last_reported{};
         CallbackList<MissionRaw::MissionProgress> callbacks{};
         int32_t last_reached{};
+        MissionState mission_state{MissionState::Unknown};
     } _mission_progress{};
 
     struct {