Merge pull request #2222 from mavlink/pr-fix-ardupilot-mission

julianoes · web-flow · commit afc6cec88a3d · 2024-02-20T13:32:29.000+13:00
ArduPilot mission_raw fixes
diff --git a/src/mavsdk/core/autopilot.h b/src/mavsdk/core/autopilot.h
@@ -8,4 +8,4 @@ enum class Autopilot {
     ArduPilot,
 };
 
-} // namespace mavsdk
+} // namespace mavsdk
diff --git a/src/mavsdk/core/autopilot_callback.h b/src/mavsdk/core/autopilot_callback.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "autopilot.h"
+#include <functional>
+
+namespace mavsdk {
+
+using AutopilotCallback = std::function<Autopilot()>;
+
+} // namespace mavsdk
diff --git a/src/mavsdk/core/mavlink_mission_transfer_client.cpp b/src/mavsdk/core/mavlink_mission_transfer_client.cpp
@@ -9,11 +9,13 @@ MavlinkMissionTransferClient::MavlinkMissionTransferClient(
     Sender& sender,
     MavlinkMessageHandler& message_handler,
     TimeoutHandler& timeout_handler,
-    TimeoutSCallback timeout_s_callback) :
+    TimeoutSCallback timeout_s_callback,
+    AutopilotCallback autopilot_callback) :
     _sender(sender),
     _message_handler(message_handler),
     _timeout_handler(timeout_handler),
-    _timeout_s_callback(std::move(timeout_s_callback))
+    _timeout_s_callback(std::move(timeout_s_callback)),
+    _autopilot_callback(std::move(autopilot_callback))
 {
     if (const char* env_p = std::getenv("MAVSDK_MISSION_TRANSFER_DEBUGGING")) {
         if (std::string(env_p) == "1") {
@@ -49,7 +51,8 @@ MavlinkMissionTransferClient::upload_items_async(
         callback,
         progress_callback,
         _debugging,
-        target_system_id);
+        target_system_id,
+        _autopilot_callback());
 
     _work_queue.push_back(ptr);
 
@@ -181,12 +184,14 @@ MavlinkMissionTransferClient::UploadWorkItem::UploadWorkItem(
     ResultCallback callback,
     ProgressCallback progress_callback,
     bool debugging,
-    uint8_t target_system_id) :
+    uint8_t target_system_id,
+    Autopilot autopilot) :
     WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging),
     _items(items),
     _callback(callback),
     _progress_callback(progress_callback),
-    _target_system_id(target_system_id)
+    _target_system_id(target_system_id),
+    _autopilot(autopilot)
 {
     _message_handler.register_one(
         MAVLINK_MSG_ID_MISSION_REQUEST,
@@ -322,7 +327,7 @@ void MavlinkMissionTransferClient::UploadWorkItem::process_mission_request(
     mavlink_mission_request_t request;
     mavlink_msg_mission_request_decode(&request_message, &request);
 
-    if (_sender.autopilot() == Autopilot::ArduPilot) {
+    if (_autopilot == Autopilot::ArduPilot) {
         // ArduCopter 3.6 sends MISSION_REQUEST (not _INT) but actually accepts ITEM_INT in reply
 
         // FIXME: this will mess with the sequence number.
diff --git a/src/mavsdk/core/mavlink_mission_transfer_client.h b/src/mavsdk/core/mavlink_mission_transfer_client.h
@@ -6,6 +6,8 @@
 #include <memory>
 #include <mutex>
 #include <vector>
+#include "autopilot.h"
+#include "autopilot_callback.h"
 #include "mavlink_address.h"
 #include "mavlink_include.h"
 #include "mavlink_message_handler.h"
@@ -110,7 +112,8 @@ class MavlinkMissionTransferClient {
             ResultCallback callback,
             ProgressCallback progress_callback,
             bool debugging,
-            uint8_t target_system_id);
+            uint8_t target_system_id,
+            Autopilot autopilot);
 
         ~UploadWorkItem() override;
         void start() override;
@@ -147,6 +150,7 @@ class MavlinkMissionTransferClient {
         unsigned _retries_done{0};
 
         uint8_t _target_system_id;
+        Autopilot _autopilot;
     };
 
     class DownloadWorkItem : public WorkItem {
@@ -272,7 +276,8 @@ class MavlinkMissionTransferClient {
         Sender& sender,
         MavlinkMessageHandler& message_handler,
         TimeoutHandler& timeout_handler,
-        TimeoutSCallback get_timeout_s_callback);
+        TimeoutSCallback get_timeout_s_callback,
+        AutopilotCallback autopilot_callback);
 
     ~MavlinkMissionTransferClient() = default;
 
@@ -307,6 +312,7 @@ class MavlinkMissionTransferClient {
     MavlinkMessageHandler& _message_handler;
     TimeoutHandler& _timeout_handler;
     TimeoutSCallback _timeout_s_callback;
+    AutopilotCallback _autopilot_callback;
 
     LockedQueue<WorkItem> _work_queue{};
 
diff --git a/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp
@@ -34,7 +34,12 @@ class MavlinkMissionTransferClientTest : public ::testing::Test {
     MavlinkMissionTransferClientTest() :
         ::testing::Test(),
         timeout_handler(time),
-        mmt(mock_sender, message_handler, timeout_handler, []() { return timeout_s; })
+        mmt(
+            mock_sender,
+            message_handler,
+            timeout_handler,
+            []() { return timeout_s; },
+            []() { return Autopilot::Px4; })
     {}
 
     void SetUp() override
diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp
@@ -15,13 +15,15 @@ MavlinkParameterClient::MavlinkParameterClient(
     MavlinkMessageHandler& message_handler,
     TimeoutHandler& timeout_handler,
     TimeoutSCallback timeout_s_callback,
+    AutopilotCallback autopilot_callback,
     uint8_t target_system_id,
     uint8_t target_component_id,
     bool use_extended) :
     _sender(sender),
     _message_handler(message_handler),
     _timeout_handler(timeout_handler),
     _timeout_s_callback(std::move(timeout_s_callback)),
+    _autopilot_callback(std::move(autopilot_callback)),
     _target_system_id(target_system_id),
     _target_component_id(target_component_id),
     _use_extended(use_extended)
@@ -115,7 +117,7 @@ void MavlinkParameterClient::set_param_int_async(
 
     // PX4 only uses int32_t, so we can be sure and don't need to check the exact type first
     // by getting the param, or checking the cache.
-    if (_sender.autopilot() == Autopilot::Px4) {
+    if (_autopilot_callback() == Autopilot::Px4) {
         ParamValue value_to_set;
         value_to_set.set(static_cast<int32_t>(value));
         set_param_async(name, value_to_set, callback, cookie);
@@ -499,7 +501,7 @@ bool MavlinkParameterClient::send_set_param_message(WorkItemSet& work_item)
             return message;
         });
     } else {
-        const float value_set = (_sender.autopilot() == Autopilot::ArduPilot) ?
+        const float value_set = (_autopilot_callback() == Autopilot::ArduPilot) ?
                                     work_item.param_value.get_4_float_bytes_cast() :
                                     work_item.param_value.get_4_float_bytes_bytewise();
 
@@ -632,8 +634,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag
     ParamValue received_value;
     const bool set_value_success = received_value.set_from_mavlink_param_value(
         param_value,
-        (_sender.autopilot() == Autopilot::ArduPilot) ? ParamValue::Conversion::Cast :
-                                                        ParamValue::Conversion::Bitwise);
+        (_autopilot_callback() == Autopilot::ArduPilot) ? ParamValue::Conversion::Cast :
+                                                          ParamValue::Conversion::Bitwise);
     if (!set_value_success) {
         LogWarn() << "Got ill-formed param_ext_value message (param_type unknown)";
         return;
diff --git a/src/mavsdk/core/mavlink_parameter_client.h b/src/mavsdk/core/mavlink_parameter_client.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "autopilot_callback.h"
 #include "log.h"
 #include "mavlink_include.h"
 #include "timeout_s_callback.h"
@@ -34,6 +35,7 @@ class MavlinkParameterClient : public MavlinkParameterSubscription {
         MavlinkMessageHandler& message_handler,
         TimeoutHandler& timeout_handler,
         TimeoutSCallback timeout_s_callback,
+        AutopilotCallback autopilot_callback,
         uint8_t target_system_id,
         uint8_t target_component_id = MAV_COMP_ID_AUTOPILOT1,
         bool use_extended_protocol = false);
@@ -195,6 +197,7 @@ class MavlinkParameterClient : public MavlinkParameterSubscription {
     MavlinkMessageHandler& _message_handler;
     TimeoutHandler& _timeout_handler;
     TimeoutSCallback _timeout_s_callback;
+    AutopilotCallback _autopilot_callback;
     uint8_t _target_system_id = 0;
     uint8_t _target_component_id = MAV_COMP_ID_AUTOPILOT1;
     bool _use_extended = false;
diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp
@@ -30,7 +30,8 @@ SystemImpl::SystemImpl(MavsdkImpl& mavsdk_impl) :
         _mavsdk_impl.default_server_component_impl().sender(),
         _mavlink_message_handler,
         _mavsdk_impl.timeout_handler,
-        [this]() { return timeout_s(); }),
+        [this]() { return timeout_s(); },
+        [this]() { return autopilot(); }),
     _request_message(
         *this, _command_sender, _mavlink_message_handler, _mavsdk_impl.timeout_handler),
     _mavlink_ftp_client(*this)
@@ -1348,6 +1349,7 @@ MavlinkParameterClient* SystemImpl::param_sender(uint8_t component_id, bool exte
              _mavlink_message_handler,
              _mavsdk_impl.timeout_handler,
              [this]() { return timeout_s(); },
+             [this]() { return autopilot(); },
              get_system_id(),
              component_id,
              extended),
diff --git a/src/mavsdk/core/timeout_s_callback.h b/src/mavsdk/core/timeout_s_callback.h
@@ -1,3 +1,9 @@
 #pragma once
 
+#include <functional>
+
+namespace mavsdk {
+
 using TimeoutSCallback = std::function<double()>;
+
+} // namespace mavsdk
diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp
@@ -112,12 +112,6 @@ MissionImport::import_mission(const Json::Value& root, Autopilot autopilot)
         mission_items[0].current = 1;
     }
 
-    // Don't forget sequence number
-    unsigned sequence = 0;
-    for (auto& mission_item : mission_items) {
-        mission_item.seq = sequence++;
-    }
-
     // Add home position at 0 for ArduPilot
     if (autopilot == Autopilot::ArduPilot) {
         const auto home = mission["plannedHomePosition"];
@@ -146,6 +140,12 @@ MissionImport::import_mission(const Json::Value& root, Autopilot autopilot)
         }
     }
 
+    // Don't forget sequence number
+    unsigned sequence = 0;
+    for (auto& mission_item : mission_items) {
+        mission_item.seq = sequence++;
+    }
+
     // Returning an empty vector is ok here if there were really no mission items.
     return {mission_items};
 }
diff --git a/src/mavsdk/plugins/mission_raw/mission_import_test.cpp b/src/mavsdk/plugins/mission_raw/mission_import_test.cpp
@@ -1,3 +1,4 @@
+#include <algorithm>
 #include <atomic>
 #include <cmath>
 #include <fstream> // for `std::ifstream`
@@ -406,6 +407,11 @@ TEST(MissionRaw, ImportSamplePlanWithArduPilot)
 {
     auto reference_items = create_reference_items();
 
+    // We need to adjust the sequence plus one because we're about to add in home
+    // at 0.
+
+    std::for_each(reference_items.begin(), reference_items.end(), [](auto& item) { ++item.seq; });
+
     auto home_item = MissionRaw::MissionItem{
         0,
         MAV_FRAME_GLOBAL_INT,
@@ -423,9 +429,6 @@ TEST(MissionRaw, ImportSamplePlanWithArduPilot)
 
     reference_items.insert(reference_items.begin(), home_item);
 
-    // There is no need to increment the sequence for all items, the sequence
-    // still starts at 0 after home.
-
     std::ifstream file{path_prefix("qgroundcontrol_sample.plan")};
     ASSERT_TRUE(file);
 

Original file line number	Diff line number	Diff line change
`@@ -112,12 +112,6 @@ MissionImport::import_mission(const Json::Value& root, Autopilot autopilot)`
`112`	`112`	`mission_items[0].current = 1;`
`113`	`113`	`}`
`114`	`114`
`115`		`- // Don't forget sequence number`
`116`		`- unsigned sequence = 0;`
`117`		`- for (auto& mission_item : mission_items) {`
`118`		`- mission_item.seq = sequence++;`
`119`		`- }`
`120`		`-`
`121`	`115`	`// Add home position at 0 for ArduPilot`
`122`	`116`	`if (autopilot == Autopilot::ArduPilot) {`
`123`	`117`	`const auto home = mission["plannedHomePosition"];`
`@@ -146,6 +140,12 @@ MissionImport::import_mission(const Json::Value& root, Autopilot autopilot)`
`146`	`140`	`}`
`147`	`141`	`}`
`148`	`142`
	`143`	`+ // Don't forget sequence number`
	`144`	`+ unsigned sequence = 0;`
	`145`	`+ for (auto& mission_item : mission_items) {`
	`146`	`+ mission_item.seq = sequence++;`
	`147`	`+ }`
	`148`	`+`
`149`	`149`	`// Returning an empty vector is ok here if there were really no mission items.`
`150`	`150`	`return {mission_items};`
`151`	`151`	`}`