diff --git a/src/mavsdk/core/flight_mode.cpp b/src/mavsdk/core/flight_mode.cpp
index c60bee3ca3..92aee1e93e 100644
--- a/src/mavsdk/core/flight_mode.cpp
+++ b/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:
diff --git a/src/mavsdk/core/flight_mode.h b/src/mavsdk/core/flight_mode.h
index 0b145a221f..5a65cb0a46 100644
--- a/src/mavsdk/core/flight_mode.h
+++ b/src/mavsdk/core/flight_mode.h
@@ -24,7 +24,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);
diff --git a/src/mavsdk/core/mavlink_mission_transfer.h b/src/mavsdk/core/mavlink_mission_transfer.h
index ed66ce24dd..aab2c9b481 100644
--- a/src/mavsdk/core/mavlink_mission_transfer.h
+++ b/src/mavsdk/core/mavlink_mission_transfer.h
@@ -27,7 +27,7 @@ class Sender {
     [[nodiscard]] virtual System::CompatibilityMode compatibility_mode() const = 0;
 };
 
-class MAVLinkMissionTransfer {
+class MavlinkMissionTransfer {
 public:
     enum class Result {
         Success,
diff --git a/src/mavsdk/core/mavlink_mission_transfer_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_test.cpp
index 6689ee1ee4..55fb3eee96 100644
--- a/src/mavsdk/core/mavlink_mission_transfer_test.cpp
+++ b/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;
diff --git a/src/mavsdk/core/mavlink_parameters.cpp b/src/mavsdk/core/mavlink_parameters.cpp
index 5b9ab25aa5..9dc24e01af 100644
--- a/src/mavsdk/core/mavlink_parameters.cpp
+++ b/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);
diff --git a/src/mavsdk/core/mocks/sender_mock.h b/src/mavsdk/core/mocks/sender_mock.h
index da0f4a1fd4..940d447ccb 100644
--- a/src/mavsdk/core/mocks/sender_mock.h
+++ b/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
diff --git a/src/mavsdk/core/server_component_impl.cpp b/src/mavsdk/core/server_component_impl.cpp
index c2728d924c..b42c452f4a 100644
--- a/src/mavsdk/core/server_component_impl.cpp
+++ b/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
diff --git a/src/mavsdk/core/server_component_impl.h b/src/mavsdk/core/server_component_impl.h
index adfad82ba0..c10e607120 100644
--- a/src/mavsdk/core/server_component_impl.h
+++ b/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};
 
diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp
index 3dce7c960e..b6b2309dc1 100644
--- a/src/mavsdk/core/system_impl.cpp
+++ b/src/mavsdk/core/system_impl.cpp
@@ -236,8 +236,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();
@@ -955,105 +955,6 @@ FlightMode SystemImpl::get_flight_mode() const
     return _flight_mode;
 }
 
-SystemImpl::FlightMode SystemImpl::to_flight_mode_from_custom_mode(uint32_t custom_mode)
-{
-    if (compatibility_mode() == System::CompatibilityMode::Ardupilot) {
-        switch (_vehicle_type) {
-            case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
-            case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
-                return to_flight_mode_from_ardupilot_rover_mode(custom_mode);
-            default:
-                return to_flight_mode_from_ardupilot_copter_mode(custom_mode);
-        }
-    } else {
-        return to_flight_mode_from_px4_mode(custom_mode);
-    }
-}
-
-SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode)
-{
-    switch (static_cast<ardupilot::RoverMode>(custom_mode)) {
-        case ardupilot::RoverMode::Auto:
-            return FlightMode::Mission;
-        case ardupilot::RoverMode::Acro:
-            return FlightMode::Acro;
-        case ardupilot::RoverMode::Hold:
-            return FlightMode::Hold;
-        case ardupilot::RoverMode::RTL:
-            return FlightMode::ReturnToLaunch;
-        case ardupilot::RoverMode::Manual:
-            return FlightMode::Manual;
-        case ardupilot::RoverMode::Follow:
-            return FlightMode::FollowMe;
-        default:
-            return FlightMode::Unknown;
-    }
-}
-SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode)
-{
-    switch (static_cast<ardupilot::CopterMode>(custom_mode)) {
-        case ardupilot::CopterMode::Auto:
-            return FlightMode::Mission;
-        case ardupilot::CopterMode::Acro:
-            return FlightMode::Acro;
-        case ardupilot::CopterMode::Alt_Hold:
-        case ardupilot::CopterMode::POS_HOLD:
-        case ardupilot::CopterMode::Flow_Hold:
-            return FlightMode::Hold;
-        case ardupilot::CopterMode::RTL:
-        case ardupilot::CopterMode::Auto_RTL:
-            return FlightMode::ReturnToLaunch;
-        case ardupilot::CopterMode::Land:
-            return FlightMode::Land;
-        default:
-            return FlightMode::Unknown;
-    }
-}
-
-SystemImpl::FlightMode SystemImpl::to_flight_mode_from_px4_mode(uint32_t custom_mode)
-{
-    px4::px4_custom_mode px4_custom_mode;
-    px4_custom_mode.data = custom_mode;
-
-    switch (px4_custom_mode.main_mode) {
-        case px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD:
-            return FlightMode::Offboard;
-        case px4::PX4_CUSTOM_MAIN_MODE_MANUAL:
-            return FlightMode::Manual;
-        case px4::PX4_CUSTOM_MAIN_MODE_POSCTL:
-            return FlightMode::Posctl;
-        case px4::PX4_CUSTOM_MAIN_MODE_ALTCTL:
-            return FlightMode::Altctl;
-        case px4::PX4_CUSTOM_MAIN_MODE_RATTITUDE:
-            return FlightMode::Rattitude;
-        case px4::PX4_CUSTOM_MAIN_MODE_ACRO:
-            return FlightMode::Acro;
-        case px4::PX4_CUSTOM_MAIN_MODE_STABILIZED:
-            return FlightMode::Stabilized;
-        case px4::PX4_CUSTOM_MAIN_MODE_AUTO:
-            switch (px4_custom_mode.sub_mode) {
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_READY:
-                    return FlightMode::Ready;
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
-                    return FlightMode::Takeoff;
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
-                    return FlightMode::Hold;
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
-                    return FlightMode::Mission;
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL:
-                    return FlightMode::ReturnToLaunch;
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND:
-                    return FlightMode::Land;
-                case px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
-                    return FlightMode::FollowMe;
-                default:
-                    return FlightMode::Unknown;
-            }
-        default:
-            return FlightMode::Unknown;
-    }
-}
-
 void SystemImpl::receive_float_param(
     MAVLinkParameters::Result result,
     MAVLinkParameters::ParamValue value,
diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp
index 8b6d48498d..3ccc23dc50 100644
--- a/src/mavsdk/plugins/action/action_impl.cpp
+++ b/src/mavsdk/plugins/action/action_impl.cpp
@@ -364,7 +364,7 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const
 
 void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const
 {
-    if (_parent->autopilot() == SystemImpl::Autopilot::Px4) {
+    if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) {
         takeoff_async_px4(callback);
     } else {
         takeoff_async_apm(callback);
@@ -466,9 +466,9 @@ void ActionImpl::goto_location_async(
 
     if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) {
         // Change to Hold mode first
-        if (_parent->get_flight_mode() != SystemImpl::FlightMode::Hold) {
+        if (_parent->get_flight_mode() != FlightMode::Hold) {
             _parent->set_flight_mode_async(
-                SystemImpl::FlightMode::Hold,
+                FlightMode::Hold,
                 [this, callback, send_do_reposition](MavlinkCommandSender::Result result, float) {
                     Action::Result action_result = action_result_from_command_result(result);
                     if (action_result != Action::Result::Success) {