Add pure mode

src/mavsdk/core/flight_mode.cpp

@@ -8,9 +8,9 @@
 namespace mavsdk {
 
 FlightMode to_flight_mode_from_custom_mode(
-    Sender::Autopilot autopilot, MAV_TYPE mav_type, uint32_t custom_mode)
+    System::CompatibilityMode compatibility_mode, MAV_TYPE mav_type, uint32_t custom_mode)
 {
-    if (autopilot == Sender::Autopilot::ArduPilot) {
+    if (compatibility_mode == System::CompatibilityMode::Ardupilot) {
         switch (mav_type) {
             case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
             case MAV_TYPE::MAV_TYPE_GROUND_ROVER:

src/mavsdk/core/flight_mode.h

@@ -27,7 +27,7 @@ enum class FlightMode {
 };
 
 FlightMode to_flight_mode_from_custom_mode(
-    Sender::Autopilot autopilot, MAV_TYPE mav_type, uint32_t custom_mode);
+    System::CompatibilityMode compatibility_mode, MAV_TYPE mav_type, uint32_t custom_mode);
 FlightMode to_flight_mode_from_px4_mode(uint32_t custom_mode);
 FlightMode to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode);
 FlightMode to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode);

src/mavsdk/core/include/mavsdk/system.h

@@ -183,6 +183,26 @@ class System {
      */
     void enable_timesync();
 
+    /**
+     * @brief Possible compatibility modes for this system.
+     */
+    enum class CompatibilityMode {
+        Unknown, // Unknown, not set or detected yet.
+        Pure, // No compatibility, everything up to common spec.
+        Px4, // Compatibility with PX4.
+        Ardupilot, // Compatibility with ArduPilot.
+    };
+
+    friend std::ostream& operator<<(std::ostream& str, CompatibilityMode const& compatibility_mode);
+
+    /**
+     * @brief Set compatibility mode.
+     *
+     * This disables auto-detection based on the heartbeat and sticks to
+     * set compatibility mode.
+     */
+    void set_compatibility_mode(CompatibilityMode compatibility_mode);
+
     /**
      * @brief Copy constructor (object is not copyable).
      */

src/mavsdk/core/mavlink_command_sender.cpp

@@ -471,7 +471,7 @@ float MavlinkCommandSender::maybe_reserved(const std::optional<float>& maybe_par
         return maybe_param.value();
 
     } else {
-        if (_parent.autopilot() == SystemImpl::Autopilot::ArduPilot) {
+        if (_parent.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
             return 0.0f;
         } else {
             return NAN;

src/mavsdk/core/mavlink_mission_transfer.cpp

@@ -241,7 +241,8 @@ void MavlinkMissionTransfer::UploadWorkItem::start()
     // item 0, and the sequence starts counting at 0 from 1.
     // This is only for missions, rally points, and geofence items are normal.
     const bool is_ardupilot_mission =
-        _sender.autopilot() == Sender::Autopilot::ArduPilot && _type == MAV_MISSION_TYPE_MISSION;
+        _sender.compatibility_mode() == System::CompatibilityMode::Ardupilot &&
+        _type == MAV_MISSION_TYPE_MISSION;
 
     if (is_ardupilot_mission) {
         for (unsigned i = 1; i < _items.size(); ++i) {
@@ -346,7 +347,7 @@ void MavlinkMissionTransfer::UploadWorkItem::send_cancel_and_finish()
 void MavlinkMissionTransfer::UploadWorkItem::process_mission_request(
     const mavlink_message_t& request_message)
 {
-    if (_sender.autopilot() == Sender::Autopilot::ArduPilot) {
+    if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
         // ArduCopter 3.6 sends MISSION_REQUEST (not _INT) but actually accepts ITEM_INT in reply
         mavlink_mission_request_t request;
         mavlink_msg_mission_request_decode(&request_message, &request);

src/mavsdk/core/mavlink_mission_transfer.h

@@ -12,10 +12,21 @@
 #include "timeout_handler.h"
 #include "timeout_s_callback.h"
 #include "locked_queue.h"
-#include "sender.h"
+#include "mavsdk.h"
 
 namespace mavsdk {
 
+class Sender {
+public:
+    Sender() = default;
+    virtual ~Sender() = default;
+    virtual bool send_message(mavlink_message_t& message) = 0;
+    [[nodiscard]] virtual uint8_t get_own_system_id() const = 0;
+    [[nodiscard]] virtual uint8_t get_own_component_id() const = 0;
+    [[nodiscard]] virtual uint8_t get_system_id() const = 0;
+    [[nodiscard]] virtual System::CompatibilityMode compatibility_mode() const = 0;
+};
+
 class MavlinkMissionTransfer {
 public:
     enum class Result {

src/mavsdk/core/mavlink_mission_transfer_test.cpp

@@ -42,7 +42,8 @@ class MavlinkMissionTransferTest : public ::testing::Test {
         ON_CALL(mock_sender, get_own_component_id())
             .WillByDefault(Return(own_address.component_id));
         ON_CALL(mock_sender, get_system_id()).WillByDefault(Return(target_address.system_id));
-        ON_CALL(mock_sender, autopilot()).WillByDefault(Return(Sender::Autopilot::Px4));
+        ON_CALL(mock_sender, compatibility_mode())
+            .WillByDefault(Return(System::CompatibilityMode::Px4));
     }
 
     MockSender mock_sender;

src/mavsdk/core/mavlink_parameters.cpp

@@ -193,7 +193,8 @@ void MAVLinkParameters::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.
-    const bool exact_int_type_known = (_sender.autopilot() == SystemImpl::Autopilot::Px4);
+    const bool exact_int_type_known =
+        (_sender.compatibility_mode() == System::CompatibilityMode::Px4);
 
     const auto set_step = [=]() {
         auto new_work = std::make_shared<WorkItem>(_timeout_s_callback());
@@ -806,9 +807,10 @@ void MAVLinkParameters::do_work()
                     param_value_buf.data(),
                     work->param_value.get_mav_param_ext_type());
             } else {
-                float value_set = (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) ?
-                                      work->param_value.get_4_float_bytes_cast() :
-                                      work->param_value.get_4_float_bytes_bytewise();
+                float value_set =
+                    (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) ?
+                        work->param_value.get_4_float_bytes_cast() :
+                        work->param_value.get_4_float_bytes_bytewise();
 
                 mavlink_msg_param_set_pack(
                     _sender.get_own_system_id(),
@@ -914,7 +916,7 @@ void MAVLinkParameters::do_work()
                     work->param_index);
             } else {
                 float param_value;
-                if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) {
+                if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
                     param_value = work->param_value.get_4_float_bytes_cast();
                 } else {
                     param_value = work->param_value.get_4_float_bytes_bytewise();
@@ -975,7 +977,7 @@ void MAVLinkParameters::process_param_value(const mavlink_message_t& message)
     }
 
     ParamValue received_value;
-    if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) {
+    if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
         received_value.set_from_mavlink_param_value_cast(param_value);
     } else {
         received_value.set_from_mavlink_param_value_bytewise(param_value);
@@ -1098,7 +1100,7 @@ void MAVLinkParameters::notify_param_subscriptions(const mavlink_param_value_t&
 
         ParamValue value;
 
-        if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) {
+        if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) {
             value.set_from_mavlink_param_value_cast(param_value);
         } else {
             value.set_from_mavlink_param_value_bytewise(param_value);

src/mavsdk/core/mocks/sender_mock.h

@@ -11,7 +11,7 @@ class MockSender : public Sender {
     MOCK_METHOD(uint8_t, get_own_system_id, (), (const, override));
     MOCK_METHOD(uint8_t, get_own_component_id, (), (const, override));
     MOCK_METHOD(uint8_t, get_system_id, (), (const, override));
-    MOCK_METHOD(Autopilot, autopilot, (), (const, override));
+    MOCK_METHOD(System::CompatibilityMode, compatibility_mode, (), (const, override));
 };
 
 } // namespace testing

src/mavsdk/core/server_component_impl.cpp

@@ -382,10 +382,10 @@ uint8_t ServerComponentImpl::OurSender::get_system_id() const
     return current_target_system_id;
 }
 
-Sender::Autopilot ServerComponentImpl::OurSender::autopilot() const
+System::CompatibilityMode ServerComponentImpl::OurSender::compatibility_mode() const
 {
     // FIXME: hard-coded to PX4 for now to avoid the dependency into mavsdk_impl.
-    return Sender::Autopilot::Px4;
+    return System::CompatibilityMode::Px4;
 }
 
 } // namespace mavsdk

src/mavsdk/core/server_component_impl.h

@@ -34,7 +34,7 @@ class ServerComponentImpl {
         [[nodiscard]] uint8_t get_own_system_id() const override;
         [[nodiscard]] uint8_t get_own_component_id() const override;
         [[nodiscard]] uint8_t get_system_id() const override;
-        [[nodiscard]] Autopilot autopilot() const override;
+        [[nodiscard]] System::CompatibilityMode compatibility_mode() const override;
 
         uint8_t current_target_system_id{0};
 

src/mavsdk/core/system.cpp

@@ -92,4 +92,25 @@ void System::enable_timesync()
     _system_impl->enable_timesync();
 }
 
+void System::set_compatibility_mode(CompatibilityMode compatibility_mode)
+{
+    _system_impl->set_compatibility_mode(compatibility_mode);
+}
+
+std::ostream& operator<<(std::ostream& str, const System::CompatibilityMode& compatibility_mode)
+{
+    switch (compatibility_mode) {
+        default:
+            // Passthrough
+        case System::CompatibilityMode::Unknown:
+            return str << "Unknown";
+        case System::CompatibilityMode::Pure:
+            return str << "Pure";
+        case System::CompatibilityMode::Px4:
+            return str << "PX4";
+        case System::CompatibilityMode::Ardupilot:
+            return str << "ArduPilot";
+    }
+}
+
 } // namespace mavsdk

src/mavsdk/core/system_impl.cpp

@@ -214,10 +214,17 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
     mavlink_heartbeat_t heartbeat;
     mavlink_msg_heartbeat_decode(&message, &heartbeat);
 
-    if (heartbeat.autopilot == MAV_AUTOPILOT_PX4) {
-        _autopilot = Autopilot::Px4;
-    } else if (heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) {
-        _autopilot = Autopilot::ArduPilot;
+    {
+        std::lock_guard<std::mutex> lock(_statustext_handler_callbacks_mutex);
+        if (!_compatibility_mode_fixed) {
+            if (heartbeat.autopilot == MAV_AUTOPILOT_PX4) {
+                _compatibility_mode = System::CompatibilityMode::Px4;
+                LogDebug() << "Auto-detected " << _compatibility_mode;
+            } else if (heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+                _compatibility_mode = System::CompatibilityMode::Ardupilot;
+                LogDebug() << "Auto-detected " << _compatibility_mode;
+            }
+        }
     }
 
     // Only set the vehicle type if the heartbeat is from an autopilot component
@@ -240,8 +247,8 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
         _hitl_enabled = (heartbeat.base_mode & MAV_MODE_FLAG_HIL_ENABLED) != 0;
     }
     if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-        _flight_mode =
-            to_flight_mode_from_custom_mode(_autopilot, _vehicle_type, heartbeat.custom_mode);
+        _flight_mode = to_flight_mode_from_custom_mode(
+            _compatibility_mode, _vehicle_type, heartbeat.custom_mode);
     }
 
     set_connected();
@@ -812,7 +819,7 @@ void SystemImpl::set_flight_mode_async(
 std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
 SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_id)
 {
-    if (_autopilot == Autopilot::ArduPilot) {
+    if (compatibility_mode() == System::CompatibilityMode::Ardupilot) {
         return make_command_ardupilot_mode(flight_mode, component_id);
     } else {
         return make_command_px4_mode(flight_mode, component_id);
@@ -1306,4 +1313,18 @@ void SystemImpl::subscribe_param_custom(
     _params.subscribe_param_custom_changed(name, callback, cookie);
 }
 
+void SystemImpl::set_compatibility_mode(System::CompatibilityMode compatibility_mode)
+{
+    std::lock_guard<std::mutex> lock(_compatibility_mode_mutex);
+    _compatibility_mode = compatibility_mode;
+
+    if (compatibility_mode == System::CompatibilityMode::Unknown) {
+        LogDebug() << "Compatibility reset to Unknown/automatic";
+        _compatibility_mode_fixed = false;
+    } else {
+        LogDebug() << "Compatibility fixed to " << compatibility_mode;
+        _compatibility_mode_fixed = true;
+    }
+}
+
 } // namespace mavsdk

src/mavsdk/core/system_impl.h

@@ -77,7 +77,7 @@ class SystemImpl : public Sender {
 
     bool send_message(mavlink_message_t& message) override;
 
-    Autopilot autopilot() const override { return _autopilot; };
+    System::CompatibilityMode compatibility_mode() const override { return _compatibility_mode; };
 
     using CommandResultCallback = MavlinkCommandSender::CommandResultCallback;
 
@@ -274,12 +274,14 @@ class SystemImpl : public Sender {
 
     RequestMessage& request_message() { return _request_message; };
 
+    double timeout_s() const;
+
+    void set_compatibility_mode(System::CompatibilityMode compatibility_mode);
+
     // Non-copyable
     SystemImpl(const SystemImpl&) = delete;
     const SystemImpl& operator=(const SystemImpl&) = delete;
 
-    double timeout_s() const;
-
 private:
     static bool is_autopilot(uint8_t comp_id);
     static bool is_camera(uint8_t comp_id);
@@ -346,8 +348,6 @@ class SystemImpl : public Sender {
     std::atomic<bool> _hitl_enabled{false};
     bool _always_connected{false};
 
-    std::atomic<Autopilot> _autopilot{Autopilot::Unknown};
-
     MavsdkImpl& _parent;
 
     std::thread* _system_thread{nullptr};
@@ -392,6 +392,10 @@ class SystemImpl : public Sender {
 
     bool _old_message_520_supported{true};
     bool _old_message_528_supported{true};
+
+    std::mutex _compatibility_mode_mutex{};
+    System::CompatibilityMode _compatibility_mode{System::CompatibilityMode::Unknown};
+    bool _compatibility_mode_fixed{false};
 };
 
 } // namespace mavsdk