diff --git a/src/mavsdk/core/CMakeLists.txt b/src/mavsdk/core/CMakeLists.txt
index 9ce1560c6..4ddc287c3 100644
--- a/src/mavsdk/core/CMakeLists.txt
+++ b/src/mavsdk/core/CMakeLists.txt
@@ -42,6 +42,7 @@ target_sources(mavsdk
     mavlink_parameter_subscription.cpp
     mavlink_parameter_helper.cpp
     mavlink_receiver.cpp
+    mavlink_request_message.cpp
     mavlink_request_message_handler.cpp
     mavlink_statustext_handler.cpp
     mavlink_message_handler.cpp
@@ -60,7 +61,6 @@ target_sources(mavsdk
     log.cpp
     cli_arg.cpp
     geometry.cpp
-    request_message.cpp
     mavsdk_time.cpp
     timesync.cpp
 )
diff --git a/src/mavsdk/core/mavlink_component_metadata.cpp b/src/mavsdk/core/mavlink_component_metadata.cpp
index f4a900178..b69aa7116 100644
--- a/src/mavsdk/core/mavlink_component_metadata.cpp
+++ b/src/mavsdk/core/mavlink_component_metadata.cpp
@@ -61,7 +61,7 @@ void MavlinkComponentMetadata::request_component(uint32_t compid)
     const std::lock_guard lg{_mavlink_components_mutex};
     if (_mavlink_components.find(compid) == _mavlink_components.end()) {
         _mavlink_components[compid] = MavlinkComponent{};
-        _system_impl.request_message().request(
+        _system_impl.mavlink_request_message().request(
             MAVLINK_MSG_ID_COMPONENT_METADATA, compid, [this](auto&& result, auto&& message) {
                 receive_component_metadata(result, message);
             });
diff --git a/src/mavsdk/core/mavlink_message_handler.cpp b/src/mavsdk/core/mavlink_message_handler.cpp
index b37ea0ed6..e4adc7ee1 100644
--- a/src/mavsdk/core/mavlink_message_handler.cpp
+++ b/src/mavsdk/core/mavlink_message_handler.cpp
@@ -17,51 +17,121 @@ MavlinkMessageHandler::MavlinkMessageHandler()
 void MavlinkMessageHandler::register_one(
     uint16_t msg_id, const Callback& callback, const void* cookie)
 {
-    std::lock_guard<std::mutex> lock(_mutex);
-
-    Entry entry = {msg_id, {}, callback, cookie};
-    _table.push_back(entry);
+    register_one_impl(msg_id, {}, callback, cookie);
 }
 
 void MavlinkMessageHandler::register_one_with_component_id(
     uint16_t msg_id, uint8_t component_id, const Callback& callback, const void* cookie)
 {
-    std::lock_guard<std::mutex> lock(_mutex);
+    register_one_impl(msg_id, {component_id}, callback, cookie);
+}
 
-    Entry entry = {msg_id, component_id, callback, cookie};
-    _table.push_back(entry);
+void MavlinkMessageHandler::register_one_impl(
+    uint16_t msg_id,
+    std::optional<uint8_t> maybe_component_id,
+    const Callback& callback,
+    const void* cookie)
+{
+    Entry entry = {msg_id, maybe_component_id, callback, cookie};
+
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (lock.owns_lock()) {
+        _table.push_back(entry);
+    } else {
+        std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);
+        _register_later_table.push_back(entry);
+    }
 }
 
 void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)
 {
-    std::lock_guard<std::mutex> lock(_mutex);
+    unregister_impl({msg_id}, cookie);
+}
 
-    for (auto it = _table.begin(); it != _table.end();
-         /* no ++it */) {
-        if (it->msg_id == msg_id && it->cookie == cookie) {
-            it = _table.erase(it);
-        } else {
-            ++it;
-        }
+void MavlinkMessageHandler::unregister_all(const void* cookie)
+{
+    unregister_impl({}, cookie);
+}
+
+void MavlinkMessageHandler::unregister_impl(
+    std::optional<uint16_t> maybe_msg_id, const void* cookie)
+{
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (lock.owns_lock()) {
+        _table.erase(
+            std::remove_if(
+                _table.begin(),
+                _table.end(),
+                [&](auto& entry) {
+                    if (maybe_msg_id) {
+                        return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);
+                    } else {
+                        return (entry.cookie == cookie);
+                    }
+                }),
+            _table.end());
+    } else {
+        std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);
+        _unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});
     }
 }
 
-void MavlinkMessageHandler::unregister_all(const void* cookie)
+void MavlinkMessageHandler::check_register_later()
 {
-    std::lock_guard<std::mutex> lock(_mutex);
+    std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);
+
+    // We could probably just grab the lock here, but it's safer not to
+    // acquire both locks to avoid deadlocks.
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (!lock.owns_lock()) {
+        // Try again later.
+        return;
+    }
 
-    for (auto it = _table.begin(); it != _table.end();
-         /* no ++it */) {
-        if (it->cookie == cookie) {
-            it = _table.erase(it);
-        } else {
-            ++it;
-        }
+    for (const auto& entry : _register_later_table) {
+        _table.push_back(entry);
+    }
+
+    _register_later_table.clear();
+}
+
+void MavlinkMessageHandler::check_unregister_later()
+{
+    std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);
+
+    // We could probably just grab the lock here, but it's safer not to
+    // acquire both locks to avoid deadlocks.
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (!lock.owns_lock()) {
+        // Try again later.
+        return;
+    }
+
+    for (const auto& unregister_entry : _unregister_later_table) {
+        _table.erase(
+            std::remove_if(
+                _table.begin(),
+                _table.end(),
+                [&](auto& entry) {
+                    if (unregister_entry.maybe_msg_id) {
+                        return (
+                            entry.msg_id == unregister_entry.maybe_msg_id.value() &&
+                            entry.cookie == unregister_entry.cookie);
+                    } else {
+                        return (entry.cookie == unregister_entry.cookie);
+                    }
+                }),
+            _table.end());
     }
+
+    _unregister_later_table.clear();
 }
 
 void MavlinkMessageHandler::process_message(const mavlink_message_t& message)
 {
+    check_register_later();
+    check_unregister_later();
+
     std::lock_guard<std::mutex> lock(_mutex);
 
     bool forwarded = false;
@@ -97,6 +167,9 @@ void MavlinkMessageHandler::process_message(const mavlink_message_t& message)
 void MavlinkMessageHandler::update_component_id(
     uint16_t msg_id, uint8_t component_id, const void* cookie)
 {
+    check_register_later();
+    check_unregister_later();
+
     std::lock_guard<std::mutex> lock(_mutex);
 
     for (auto& entry : _table) {
diff --git a/src/mavsdk/core/mavlink_message_handler.h b/src/mavsdk/core/mavlink_message_handler.h
index b75b79770..c10bd5a5b 100644
--- a/src/mavsdk/core/mavlink_message_handler.h
+++ b/src/mavsdk/core/mavlink_message_handler.h
@@ -31,9 +31,31 @@ class MavlinkMessageHandler {
     void update_component_id(uint16_t msg_id, uint8_t cmp_id, const void* cookie);
 
 private:
+    void register_one_impl(
+        uint16_t msg_id,
+        std::optional<uint8_t> maybe_component_id,
+        const Callback& callback,
+        const void* cookie);
+
+    void unregister_impl(std::optional<uint16_t> maybe_msg_id, const void* cookie);
+
+    void check_register_later();
+    void check_unregister_later();
+
     std::mutex _mutex{};
     std::vector<Entry> _table{};
 
+    std::mutex _register_later_mutex{};
+    std::vector<Entry> _register_later_table{};
+
+    struct UnregisterEntry {
+        std::optional<uint16_t> maybe_msg_id;
+        const void* cookie;
+    };
+
+    std::mutex _unregister_later_mutex{};
+    std::vector<UnregisterEntry> _unregister_later_table{};
+
     bool _debugging{false};
 };
 
diff --git a/src/mavsdk/core/mavlink_request_message.cpp b/src/mavsdk/core/mavlink_request_message.cpp
new file mode 100644
index 000000000..cf1128012
--- /dev/null
+++ b/src/mavsdk/core/mavlink_request_message.cpp
@@ -0,0 +1,299 @@
+
+#include "log.h"
+#include "mavlink_request_message.h"
+#include "system_impl.h"
+#include <cstdint>
+#include <string>
+
+namespace mavsdk {
+
+MavlinkRequestMessage::MavlinkRequestMessage(
+    SystemImpl& system_impl,
+    MavlinkCommandSender& command_sender,
+    MavlinkMessageHandler& message_handler,
+    TimeoutHandler& timeout_handler) :
+    _system_impl(system_impl),
+    _command_sender(command_sender),
+    _message_handler(message_handler),
+    _timeout_handler(timeout_handler)
+{}
+
+void MavlinkRequestMessage::request(
+    uint32_t message_id,
+    uint8_t target_component,
+    MavlinkRequestMessageCallback callback,
+    uint32_t param2)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    // Cleanup previous requests.
+    for (const auto id : _deferred_message_cleanup) {
+        _message_handler.unregister_one(id, this);
+    }
+    _deferred_message_cleanup.clear();
+
+    // Respond with 'Busy' if already in progress.
+    for (auto& item : _work_items) {
+        if (item.message_id == message_id && item.param2 == param2) {
+            lock.unlock();
+            if (callback) {
+                callback(MavlinkCommandSender::Result::Busy, {});
+            }
+            return;
+        }
+    }
+
+    // Otherwise, schedule it.
+    _work_items.emplace_back(WorkItem{message_id, target_component, callback, param2});
+
+    // Register for message
+    _message_handler.register_one(
+        message_id,
+        [this](const mavlink_message_t& message) { handle_any_message(message); },
+        this);
+
+    // And send off command
+    send_request(_work_items.back());
+}
+
+void MavlinkRequestMessage::send_request(WorkItem& item)
+{
+    // Every second time, starting with first retry, we try the old request commands.
+    // If no old commands exist, we of course just send the new one again.
+    if (item.retries % 2 != 0) {
+        if (!try_sending_request_using_old_command(item)) {
+            send_request_using_new_command(item);
+        }
+    } else {
+        send_request_using_new_command(item);
+    }
+}
+
+void MavlinkRequestMessage::send_request_using_new_command(WorkItem& item)
+{
+    if (item.retries > 0) {
+        LogWarn() << "Request message " << item.message_id
+                  << " again using REQUEST_MESSAGE (retries: " << item.retries << ")";
+    } else {
+        LogDebug() << "Request message " << item.message_id << " using REQUEST_MESSAGE";
+    }
+
+    MavlinkCommandSender::CommandLong command_request_message{};
+    command_request_message.command = MAV_CMD_REQUEST_MESSAGE;
+    command_request_message.target_system_id = _system_impl.get_system_id();
+    command_request_message.target_component_id = item.target_component;
+    command_request_message.params.maybe_param1 = {static_cast<float>(item.message_id)};
+    _command_sender.queue_command_async(
+        command_request_message,
+        [this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
+            if (result != MavlinkCommandSender::Result::InProgress) {
+                handle_command_result(message_id, result);
+            }
+        });
+}
+
+bool MavlinkRequestMessage::try_sending_request_using_old_command(WorkItem& item)
+{
+    MavlinkCommandSender::CommandLong command_request_message{};
+    command_request_message.target_system_id = _system_impl.get_system_id();
+    command_request_message.target_component_id = item.target_component;
+
+    std::string command_name;
+
+    switch (item.message_id) {
+        case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
+            command_request_message.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
+            command_request_message.params.maybe_param1 = {1.f};
+            command_name = "REQUEST_AUTOPILOT_CAPABILITIES";
+            break;
+
+        case MAVLINK_MSG_ID_CAMERA_INFORMATION:
+            command_request_message.command = MAV_CMD_REQUEST_CAMERA_INFORMATION;
+            command_request_message.params.maybe_param1 = {1.0f};
+            command_name = "REQUEST_CAMERA_INFORMATION";
+            break;
+
+        case MAVLINK_MSG_ID_CAMERA_SETTINGS:
+            command_request_message.command = MAV_CMD_REQUEST_CAMERA_SETTINGS;
+            command_request_message.params.maybe_param1 = {1.0f};
+            command_name = "REQUEST_CAMERA_SETTINGS";
+
+            break;
+
+        case MAVLINK_MSG_ID_STORAGE_INFORMATION:
+            command_request_message.command = MAV_CMD_REQUEST_STORAGE_INFORMATION;
+            command_request_message.params.maybe_param1 = {
+                static_cast<float>(item.param2)}; // storage ID
+            command_request_message.params.maybe_param2 = {1.0f};
+            command_name = "REQUEST_STORAGE_INFORMATION";
+            break;
+
+        case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
+            command_request_message.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS;
+            command_request_message.params.maybe_param1 = {1.0f};
+            command_name = "REQUEST_CAMERA_CAPTURE_STATUS";
+            break;
+
+        case MAVLINK_MSG_ID_FLIGHT_INFORMATION:
+            command_request_message.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;
+            command_request_message.params.maybe_param1 = {1.0f};
+            command_name = "REQUEST_FLIGHT_INFORMATION";
+            break;
+
+        case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
+            command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_STATUS;
+            command_request_message.params.maybe_param1 = {
+                static_cast<float>(item.param2)}; // stream ID
+            command_name = "REQUEST_VIDEO_STREAM_STATUS";
+            break;
+
+        case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
+            command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION;
+            command_request_message.params.maybe_param1 = {
+                static_cast<float>(item.param2)}; // stream ID
+            command_name = "REQUEST_VIDEO_STREAM_INFORMATION";
+            break;
+
+        case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
+            command_request_message.command = MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE;
+            command_request_message.params.maybe_param1 = {
+                static_cast<float>(item.param2)}; // sequence number
+            command_name = "REQUEST_CAMERA_IMAGE_CAPTURE";
+            break;
+
+        default:
+            // No alternative command for this message.
+            return false;
+    }
+
+    LogWarn() << "Request message " << item.message_id << " again using " << command_name
+              << " (retries: " << item.retries << ")";
+
+    _command_sender.queue_command_async(
+        command_request_message,
+        [this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
+            if (result != MavlinkCommandSender::Result::InProgress) {
+                handle_command_result(message_id, result);
+            }
+        });
+
+    return true;
+}
+
+void MavlinkRequestMessage::handle_any_message(const mavlink_message_t& message)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
+        // Check if we're waiting for this message.
+        // TODO: check if params are correct.
+        if (it->message_id != message.msgid && it->target_component == message.compid) {
+            continue;
+        }
+
+        _timeout_handler.remove(it->timeout_cookie);
+        // We have at least the message which is what we care about most, so we
+        // tell the user about it and call it a day. If we receive the result
+        // of the command later, we ignore it, and we fake the result to be successful
+        // anyway.
+        auto temp_callback = it->callback;
+        // We can now get rid of the entry now.
+        _work_items.erase(it);
+        // We have to clean up later outside this callback, otherwise we lock ourselves out.
+        _deferred_message_cleanup.push_back(message.msgid);
+        lock.unlock();
+
+        if (temp_callback) {
+            temp_callback(MavlinkCommandSender::Result::Success, message);
+        }
+        return;
+    }
+}
+
+void MavlinkRequestMessage::handle_command_result(
+    uint32_t message_id, MavlinkCommandSender::Result result)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
+        // Check if we're waiting for this result
+        if (it->message_id != message_id) {
+            continue;
+        }
+
+        switch (result) {
+            case MavlinkCommandSender::Result::Success:
+                // This is promising, let's hope the message will actually arrive.
+                // We'll set a timeout in case we need to retry.
+                it->timeout_cookie = _timeout_handler.add(
+                    [this, message_id, target_component = it->target_component]() {
+                        handle_timeout(message_id, target_component);
+                    },
+                    1.0);
+                return;
+
+            case MavlinkCommandSender::Result::NoSystem:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::ConnectionError:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::Busy:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::Denied:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::Unsupported:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::Timeout:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::TemporarilyRejected:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::Failed:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::Cancelled:
+                // FALLTHROUGH
+            case MavlinkCommandSender::Result::UnknownError: {
+                // It looks like this did not work, and we can report the error
+                // No need to try again.
+                auto temp_callback = it->callback;
+                _message_handler.unregister_one(it->message_id, this);
+                _work_items.erase(it);
+                lock.unlock();
+                if (temp_callback) {
+                    temp_callback(result, {});
+                }
+                return;
+            }
+
+            case MavlinkCommandSender::Result::InProgress:
+                // Should not happen, as it is already filtered out.
+                return;
+        }
+    }
+}
+
+void MavlinkRequestMessage::handle_timeout(uint32_t message_id, uint8_t target_component)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
+        // Check if we're waiting for this result
+        if (it->message_id != message_id || it->target_component != target_component) {
+            continue;
+        }
+
+        if (++it->retries > 3) {
+            // We have already retried, let's give up.
+            auto temp_callback = it->callback;
+            _message_handler.unregister_one(it->message_id, this);
+            _work_items.erase(it);
+            lock.unlock();
+            if (temp_callback) {
+                temp_callback(MavlinkCommandSender::Result::Timeout, {});
+            }
+            return;
+        }
+
+        send_request(*it);
+    }
+}
+
+} // namespace mavsdk
diff --git a/src/mavsdk/core/request_message.h b/src/mavsdk/core/mavlink_request_message.h
similarity index 76%
rename from src/mavsdk/core/request_message.h
rename to src/mavsdk/core/mavlink_request_message.h
index 579c1659e..bf5dd48fb 100644
--- a/src/mavsdk/core/request_message.h
+++ b/src/mavsdk/core/mavlink_request_message.h
@@ -13,36 +13,38 @@ namespace mavsdk {
 
 class SystemImpl;
 
-class RequestMessage {
+class MavlinkRequestMessage {
 public:
-    RequestMessage(
+    MavlinkRequestMessage(
         SystemImpl& system_impl,
         MavlinkCommandSender& command_sender,
         MavlinkMessageHandler& message_handler,
         TimeoutHandler& timeout_handler);
-    RequestMessage() = delete;
+    MavlinkRequestMessage() = delete;
 
-    using RequestMessageCallback =
+    using MavlinkRequestMessageCallback =
         std::function<void(MavlinkCommandSender::Result, const mavlink_message_t&)>;
 
     void request(
         uint32_t message_id,
         uint8_t target_component,
-        RequestMessageCallback callback,
+        MavlinkRequestMessageCallback callback,
         uint32_t param2 = 0);
 
 private:
     struct WorkItem {
         uint32_t message_id{0};
         uint8_t target_component{0};
-        RequestMessageCallback callback{};
+        MavlinkRequestMessageCallback callback{};
         uint32_t param2{0};
-        std::size_t retries{0};
+        unsigned retries{0};
         TimeoutHandler::Cookie timeout_cookie{};
         std::optional<MavlinkCommandSender::Result> maybe_result{};
     };
 
-    void send_request(uint32_t message_id, uint8_t target_component);
+    void send_request(WorkItem& item);
+    void send_request_using_new_command(WorkItem& item);
+    bool try_sending_request_using_old_command(WorkItem& item);
     void handle_any_message(const mavlink_message_t& message);
     void handle_command_result(uint32_t message_id, MavlinkCommandSender::Result result);
     void handle_timeout(uint32_t message_id, uint8_t target_component);
diff --git a/src/mavsdk/core/request_message.cpp b/src/mavsdk/core/request_message.cpp
deleted file mode 100644
index 31f8eb86a..000000000
--- a/src/mavsdk/core/request_message.cpp
+++ /dev/null
@@ -1,182 +0,0 @@
-
-#include "log.h"
-#include "request_message.h"
-#include "system_impl.h"
-
-namespace mavsdk {
-
-RequestMessage::RequestMessage(
-    SystemImpl& system_impl,
-    MavlinkCommandSender& command_sender,
-    MavlinkMessageHandler& message_handler,
-    TimeoutHandler& timeout_handler) :
-    _system_impl(system_impl),
-    _command_sender(command_sender),
-    _message_handler(message_handler),
-    _timeout_handler(timeout_handler)
-{}
-
-void RequestMessage::request(
-    uint32_t message_id, uint8_t target_component, RequestMessageCallback callback, uint32_t param2)
-{
-    if (!callback) {
-        LogWarn() << "Can't request message without callback";
-        return;
-    }
-    std::unique_lock<std::mutex> lock(_mutex);
-
-    // Cleanup previous requests.
-    for (const auto id : _deferred_message_cleanup) {
-        _message_handler.unregister_one(id, this);
-    }
-    _deferred_message_cleanup.clear();
-
-    // Respond with 'Busy' if already in progress.
-    for (auto& item : _work_items) {
-        if (item.message_id == message_id && item.param2 == param2) {
-            lock.unlock();
-            callback(MavlinkCommandSender::Result::Busy, {});
-            return;
-        }
-    }
-
-    // Otherwise, schedule it.
-    _work_items.emplace_back(WorkItem{message_id, target_component, callback, param2});
-
-    // Register for message
-    _message_handler.register_one(
-        message_id,
-        [this](const mavlink_message_t& message) { handle_any_message(message); },
-        this);
-
-    // And send off command
-    send_request(message_id, target_component);
-}
-
-void RequestMessage::send_request(uint32_t message_id, uint8_t target_component)
-{
-    MavlinkCommandSender::CommandLong command_request_message{};
-    command_request_message.command = MAV_CMD_REQUEST_MESSAGE;
-    command_request_message.target_system_id = _system_impl.get_system_id();
-    command_request_message.target_component_id = target_component;
-    command_request_message.params.maybe_param1 = {static_cast<float>(message_id)};
-    _command_sender.queue_command_async(
-        command_request_message, [this, message_id](MavlinkCommandSender::Result result, float) {
-            if (result != MavlinkCommandSender::Result::InProgress) {
-                handle_command_result(message_id, result);
-            }
-        });
-}
-
-void RequestMessage::handle_any_message(const mavlink_message_t& message)
-{
-    std::unique_lock<std::mutex> lock(_mutex);
-
-    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
-        // Check if we're waiting for this message.
-        // TODO: check if params are correct.
-        if (it->message_id != message.msgid) {
-            continue;
-        }
-
-        _timeout_handler.remove(it->timeout_cookie);
-        // We have at least the message which is what we care about most, so we
-        // tell the user about it and call it a day. If we receive the result
-        // of the command later, we ignore it, and we fake the result to be successful
-        // anyway.
-        auto temp_callback = it->callback;
-        // We can now get rid of the entry now.
-        _work_items.erase(it);
-        // We have to clean up later outside this callback, otherwise we lock ourselves out.
-        _deferred_message_cleanup.push_back(message.msgid);
-        lock.unlock();
-
-        temp_callback(MavlinkCommandSender::Result::Success, message);
-        return;
-    }
-}
-
-void RequestMessage::handle_command_result(uint32_t message_id, MavlinkCommandSender::Result result)
-{
-    std::unique_lock<std::mutex> lock(_mutex);
-
-    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
-        // Check if we're waiting for this result
-        if (it->message_id != message_id) {
-            continue;
-        }
-
-        switch (result) {
-            case MavlinkCommandSender::Result::Success:
-                // This is promising, let's hope the message will actually arrive.
-                // We'll set a timeout in case we need to retry.
-                it->timeout_cookie = _timeout_handler.add(
-                    [this, message_id, target_component = it->target_component]() {
-                        handle_timeout(message_id, target_component);
-                    },
-                    1.0);
-                return;
-
-            case MavlinkCommandSender::Result::NoSystem:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::ConnectionError:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::Busy:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::Denied:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::Unsupported:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::Timeout:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::TemporarilyRejected:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::Failed:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::Cancelled:
-                // FALLTHROUGH
-            case MavlinkCommandSender::Result::UnknownError: {
-                // It looks like this did not work, and we can report the error
-                // No need to try again.
-                auto temp_callback = it->callback;
-                _message_handler.unregister_one(it->message_id, this);
-                _work_items.erase(it);
-                lock.unlock();
-                temp_callback(result, {});
-                return;
-            }
-
-            case MavlinkCommandSender::Result::InProgress:
-                // Should not happen, as it is already filtered out.
-                return;
-        }
-    }
-}
-
-void RequestMessage::handle_timeout(uint32_t message_id, uint8_t target_component)
-{
-    std::unique_lock<std::mutex> lock(_mutex);
-
-    for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
-        // Check if we're waiting for this result
-        if (it->message_id != message_id) {
-            continue;
-        }
-
-        if (it->retries > 2) {
-            // We have already retried, let's give up.
-            auto temp_callback = it->callback;
-            _message_handler.unregister_one(it->message_id, this);
-            _work_items.erase(it);
-            lock.unlock();
-            temp_callback(MavlinkCommandSender::Result::Timeout, {});
-            return;
-        } else {
-            send_request(message_id, target_component);
-            LogWarn() << "Requesting message again (retries: " << it->retries << ")";
-            it->retries += 1;
-        }
-    }
-}
-
-} // namespace mavsdk
diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp
index 73c411407..8d59b198b 100644
--- a/src/mavsdk/core/system_impl.cpp
+++ b/src/mavsdk/core/system_impl.cpp
@@ -6,7 +6,6 @@
 #include "plugin_impl_base.h"
 #include "px4_custom_mode.h"
 #include "ardupilot_custom_mode.h"
-#include "request_message.h"
 #include "callback_list.tpp"
 #include "unused.h"
 #include <cassert>
@@ -32,7 +31,7 @@ SystemImpl::SystemImpl(MavsdkImpl& mavsdk_impl) :
         _mavsdk_impl.timeout_handler,
         [this]() { return timeout_s(); },
         [this]() { return autopilot(); }),
-    _request_message(
+    _mavlink_request_message(
         *this, _command_sender, _mavlink_message_handler, _mavsdk_impl.timeout_handler),
     _mavlink_ftp_client(*this),
     _mavlink_component_metadata(*this)
@@ -472,31 +471,8 @@ bool SystemImpl::queue_message(
 
 void SystemImpl::send_autopilot_version_request()
 {
-    MavlinkCommandSender::CommandLong command{};
-    command.target_component_id = get_autopilot_id();
-
-    if (_old_message_520_supported) {
-        // Note: This MAVLINK message is deprecated and would be removed from MAVSDK in a future
-        // release.
-        command.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
-        command.params.maybe_param1 = 1.0f;
-    } else {
-        command.command = MAV_CMD_REQUEST_MESSAGE;
-        command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_AUTOPILOT_VERSION)};
-    }
-
-    send_command_async(command, [this](MavlinkCommandSender::Result result, float) {
-        receive_autopilot_version_request_ack(result);
-    });
-}
-
-void SystemImpl::receive_autopilot_version_request_ack(MavlinkCommandSender::Result result)
-{
-    if (result == MavlinkCommandSender::Result::Unsupported) {
-        _old_message_520_supported = false;
-        LogWarn()
-            << "Trying alternative command REQUEST_MESSAGE instead of REQUEST_AUTOPILOT_CAPABILITIES next.";
-    }
+    mavlink_request_message().request(
+        MAVLINK_MSG_ID_AUTOPILOT_VERSION, MAV_COMP_ID_AUTOPILOT1, nullptr);
 }
 
 void SystemImpl::set_connected()
diff --git a/src/mavsdk/core/system_impl.h b/src/mavsdk/core/system_impl.h
index dd6a03e1a..5b33b93be 100644
--- a/src/mavsdk/core/system_impl.h
+++ b/src/mavsdk/core/system_impl.h
@@ -9,17 +9,14 @@
 #include "mavlink_include.h"
 #include "mavlink_parameter_client.h"
 #include "mavlink_command_sender.h"
-#include "mavlink_command_receiver.h"
 #include "mavlink_ftp_client.h"
 #include "mavlink_message_handler.h"
 #include "mavlink_mission_transfer_client.h"
-#include "mavlink_request_message_handler.h"
+#include "mavlink_request_message.h"
 #include "mavlink_statustext_handler.h"
-#include "request_message.h"
 #include "ardupilot_custom_mode.h"
 #include "ping.h"
 #include "timeout_handler.h"
-#include "safe_queue.h"
 #include "timesync.h"
 #include "system.h"
 #include <cstdint>
@@ -290,7 +287,7 @@ class SystemImpl {
 
     MavlinkComponentMetadata& component_metadata() { return _mavlink_component_metadata; }
 
-    RequestMessage& request_message() { return _request_message; }
+    MavlinkRequestMessage& mavlink_request_message() { return _mavlink_request_message; }
 
     // Non-copyable
     SystemImpl(const SystemImpl&) = delete;
@@ -309,8 +306,6 @@ class SystemImpl {
     void set_connected();
     void set_disconnected();
 
-    void receive_autopilot_version_request_ack(MavlinkCommandSender::Result result);
-
     static std::string component_name(uint8_t component_id);
     static System::ComponentType component_type(uint8_t component_id);
 
@@ -397,7 +392,7 @@ class SystemImpl {
     Ping _ping;
 
     MavlinkMissionTransferClient _mission_transfer_client;
-    RequestMessage _request_message;
+    MavlinkRequestMessage _mavlink_request_message;
     MavlinkFtpClient _mavlink_ftp_client;
     MavlinkComponentMetadata _mavlink_component_metadata;
 
@@ -416,8 +411,6 @@ class SystemImpl {
 
     std::mutex _mavlink_ftp_files_mutex{};
     std::unordered_map<std::string, std::string> _mavlink_ftp_files{};
-
-    bool _old_message_520_supported{true};
 };
 
 } // namespace mavsdk
diff --git a/src/mavsdk/plugins/camera/camera_impl.cpp b/src/mavsdk/plugins/camera/camera_impl.cpp
index a7189f330..716b73935 100644
--- a/src/mavsdk/plugins/camera/camera_impl.cpp
+++ b/src/mavsdk/plugins/camera/camera_impl.cpp
@@ -265,17 +265,6 @@ Camera::Result CameraImpl::select_camera(const size_t id)
     return Camera::Result::Success;
 }
 
-MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_info()
-{
-    MavlinkCommandSender::CommandLong command_camera_info{};
-
-    command_camera_info.command = MAV_CMD_REQUEST_CAMERA_INFORMATION;
-    command_camera_info.params.maybe_param1 = 1.0f; // Request it
-    command_camera_info.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return command_camera_info;
-}
-
 MavlinkCommandSender::CommandLong
 CameraImpl::make_command_take_photo(float interval_s, float no_of_photos)
 {
@@ -461,54 +450,6 @@ MavlinkCommandSender::CommandLong CameraImpl::make_command_set_camera_mode(float
     return cmd_set_camera_mode;
 }
 
-MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_settings()
-{
-    MavlinkCommandSender::CommandLong cmd_req_camera_settings{};
-
-    cmd_req_camera_settings.command = MAV_CMD_REQUEST_CAMERA_SETTINGS;
-    cmd_req_camera_settings.params.maybe_param1 = 1.f; // Request it
-    cmd_req_camera_settings.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return cmd_req_camera_settings;
-}
-
-MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_capture_status()
-{
-    MavlinkCommandSender::CommandLong cmd_req_camera_cap_stat{};
-
-    cmd_req_camera_cap_stat.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS;
-    cmd_req_camera_cap_stat.params.maybe_param1 = 1.0f; // Request it
-    cmd_req_camera_cap_stat.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return cmd_req_camera_cap_stat;
-}
-
-MavlinkCommandSender::CommandLong
-CameraImpl::make_command_request_camera_image_captured(const size_t photo_id)
-{
-    MavlinkCommandSender::CommandLong cmd_req_camera_image_captured{};
-
-    cmd_req_camera_image_captured.command = MAV_CMD_REQUEST_MESSAGE;
-    cmd_req_camera_image_captured.params.maybe_param1 =
-        static_cast<float>(MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED);
-    cmd_req_camera_image_captured.params.maybe_param2 = static_cast<float>(photo_id);
-    cmd_req_camera_image_captured.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return cmd_req_camera_image_captured;
-}
-
-MavlinkCommandSender::CommandLong CameraImpl::make_command_request_storage_info()
-{
-    MavlinkCommandSender::CommandLong cmd_req_storage_info{};
-
-    cmd_req_storage_info.command = MAV_CMD_REQUEST_STORAGE_INFORMATION;
-    cmd_req_storage_info.params.maybe_param1 = 0.f; // Reserved, set to 0
-    cmd_req_storage_info.params.maybe_param2 = 1.f; // Request it
-    cmd_req_storage_info.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return cmd_req_storage_info;
-}
-
 MavlinkCommandSender::CommandLong CameraImpl::make_command_start_video_streaming(int32_t stream_id)
 {
     MavlinkCommandSender::CommandLong cmd_start_video_streaming{};
@@ -531,28 +472,6 @@ MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_video_streaming(
     return cmd_stop_video_streaming;
 }
 
-MavlinkCommandSender::CommandLong CameraImpl::make_command_request_video_stream_info()
-{
-    MavlinkCommandSender::CommandLong cmd_req_video_stream_info{};
-
-    cmd_req_video_stream_info.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION;
-    cmd_req_video_stream_info.params.maybe_param2 = 1.0f;
-    cmd_req_video_stream_info.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return cmd_req_video_stream_info;
-}
-
-MavlinkCommandSender::CommandLong CameraImpl::make_command_request_video_stream_status()
-{
-    MavlinkCommandSender::CommandLong cmd_req_video_stream_status{};
-
-    cmd_req_video_stream_status.command = MAV_CMD_REQUEST_VIDEO_STREAM_STATUS;
-    cmd_req_video_stream_status.params.maybe_param2 = 1.0f;
-    cmd_req_video_stream_status.target_component_id = _camera_id + MAV_COMP_ID_CAMERA;
-
-    return cmd_req_video_stream_status;
-}
-
 Camera::Result CameraImpl::take_photo()
 {
     // TODO: check whether we are in photo mode.
@@ -993,8 +912,10 @@ Camera::Result CameraImpl::stop_video_streaming(int32_t stream_id)
 
 void CameraImpl::request_video_stream_info()
 {
-    _system_impl->send_command_async(make_command_request_video_stream_info(), nullptr);
-    _system_impl->send_command_async(make_command_request_video_stream_status(), nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, _camera_id + MAV_COMP_ID_CAMERA, nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, _camera_id + MAV_COMP_ID_CAMERA, nullptr);
 }
 
 Camera::VideoStreamInfo CameraImpl::video_stream_info()
@@ -1219,8 +1140,10 @@ bool CameraImpl::interval_valid(float interval_s)
 
 void CameraImpl::request_status()
 {
-    _system_impl->send_command_async(make_command_request_camera_capture_status(), nullptr);
-    _system_impl->send_command_async(make_command_request_storage_info(), nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, _camera_id + MAV_COMP_ID_CAMERA, nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_STORAGE_INFORMATION, _camera_id + MAV_COMP_ID_CAMERA, nullptr);
 }
 
 Camera::StatusHandle CameraImpl::subscribe_status(const Camera::StatusCallback& callback)
@@ -1431,9 +1354,11 @@ void CameraImpl::request_missing_capture_info()
     if (!_capture_info.missing_image_retries.empty()) {
         auto it_lowest_retries = std::min_element(
             _capture_info.missing_image_retries.begin(), _capture_info.missing_image_retries.end());
-        _system_impl->send_command_async(
-            CameraImpl::make_command_request_camera_image_captured(it_lowest_retries->first),
-            nullptr);
+        _system_impl->mavlink_request_message().request(
+            MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED,
+            _camera_id + MAV_COMP_ID_CAMERA,
+            nullptr,
+            it_lowest_retries->first);
         it_lowest_retries->second += 1;
     }
 }
@@ -2304,14 +2229,14 @@ bool CameraImpl::get_option_str(
 
 void CameraImpl::request_camera_settings()
 {
-    auto command_camera_settings = make_command_request_camera_settings();
-    _system_impl->send_command_async(command_camera_settings, nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_CAMERA_SETTINGS, _camera_id + MAV_COMP_ID_CAMERA, nullptr);
 }
 
 void CameraImpl::request_camera_information()
 {
-    auto command_camera_info = make_command_request_camera_info();
-    _system_impl->send_command_async(command_camera_info, nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_CAMERA_INFORMATION, _camera_id + MAV_COMP_ID_CAMERA, nullptr);
 }
 
 Camera::Result CameraImpl::format_storage(int32_t storage_id)
@@ -2474,8 +2399,11 @@ void CameraImpl::list_photos_async(
                         return;
                     }
 
-                    _system_impl->send_command_async(
-                        make_command_request_camera_image_captured(i), nullptr);
+                    _system_impl->mavlink_request_message().request(
+                        MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED,
+                        _camera_id + MAV_COMP_ID_CAMERA,
+                        nullptr,
+                        i);
                     cv_status = _captured_request_cv.wait_for(
                         capture_request_lock, std::chrono::seconds(1));
                 }
diff --git a/src/mavsdk/plugins/camera/camera_impl.h b/src/mavsdk/plugins/camera/camera_impl.h
index 9757fa99e..ed435b215 100644
--- a/src/mavsdk/plugins/camera/camera_impl.h
+++ b/src/mavsdk/plugins/camera/camera_impl.h
@@ -218,22 +218,13 @@ class CameraImpl : public PluginImplBase {
     MavlinkCommandSender::CommandLong make_command_take_photo(float interval_s, float no_of_photos);
     MavlinkCommandSender::CommandLong make_command_stop_photo();
 
-    MavlinkCommandSender::CommandLong make_command_request_camera_info();
     MavlinkCommandSender::CommandLong make_command_set_camera_mode(float mavlink_mode);
-    MavlinkCommandSender::CommandLong make_command_request_camera_settings();
-    MavlinkCommandSender::CommandLong make_command_request_camera_capture_status();
-    MavlinkCommandSender::CommandLong make_command_request_camera_image_captured(size_t photo_id);
-    MavlinkCommandSender::CommandLong make_command_request_storage_info();
-
     MavlinkCommandSender::CommandLong make_command_start_video(float capture_status_rate_hz);
     MavlinkCommandSender::CommandLong make_command_stop_video();
 
     MavlinkCommandSender::CommandLong make_command_start_video_streaming(int32_t stream_id);
     MavlinkCommandSender::CommandLong make_command_stop_video_streaming(int32_t stream_id);
 
-    MavlinkCommandSender::CommandLong make_command_request_video_stream_info();
-    MavlinkCommandSender::CommandLong make_command_request_video_stream_status();
-
     MavlinkCommandSender::CommandLong make_command_zoom_in();
     MavlinkCommandSender::CommandLong make_command_zoom_out();
     MavlinkCommandSender::CommandLong make_command_zoom_stop();
diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp
index 87caf91ab..0cd95f728 100644
--- a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp
+++ b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp
@@ -86,12 +86,8 @@ void GimbalImpl::request_gimbal_manager_information(uint8_t target_component_id)
                    << std::to_string(target_component_id);
     }
 
-    MavlinkCommandSender::CommandLong command{};
-    command.command = MAV_CMD_REQUEST_MESSAGE;
-    command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION)};
-    command.target_system_id = _system_impl->get_system_id();
-    command.target_component_id = target_component_id;
-    _system_impl->send_command_async(command, nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, target_component_id, nullptr);
 }
 
 void GimbalImpl::request_gimbal_device_information(uint8_t target_component_id) const
@@ -102,12 +98,8 @@ void GimbalImpl::request_gimbal_device_information(uint8_t target_component_id)
                    << std::to_string(target_component_id);
     }
 
-    MavlinkCommandSender::CommandLong command{};
-    command.command = MAV_CMD_REQUEST_MESSAGE;
-    command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION)};
-    command.target_system_id = _system_impl->get_system_id();
-    command.target_component_id = target_component_id;
-    _system_impl->send_command_async(command, nullptr);
+    _system_impl->mavlink_request_message().request(
+        MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, target_component_id, nullptr);
 }
 
 void GimbalImpl::process_heartbeat(const mavlink_message_t& message)
diff --git a/src/mavsdk/plugins/info/info_impl.cpp b/src/mavsdk/plugins/info/info_impl.cpp
index 600a6b917..435fa3f9a 100644
--- a/src/mavsdk/plugins/info/info_impl.cpp
+++ b/src/mavsdk/plugins/info/info_impl.cpp
@@ -230,7 +230,7 @@ std::pair<Info::Result, Info::FlightInfo> InfoImpl::get_flight_information()
 {
     std::promise<std::pair<Info::Result, Info::FlightInfo>> prom;
     auto fut = prom.get_future();
-    _system_impl->request_message().request(
+    _system_impl->mavlink_request_message().request(
         MAVLINK_MSG_ID_FLIGHT_INFORMATION,
         MAV_COMP_ID_AUTOPILOT1,
         [&](MavlinkCommandSender::Result result, const mavlink_message_t& message) {
diff --git a/src/mavsdk/plugins/mission/mission_impl.cpp b/src/mavsdk/plugins/mission/mission_impl.cpp
index a632888a4..1f6b840fa 100644
--- a/src/mavsdk/plugins/mission/mission_impl.cpp
+++ b/src/mavsdk/plugins/mission/mission_impl.cpp
@@ -39,34 +39,17 @@ void MissionImpl::init()
         MAVLINK_MSG_ID_MISSION_ITEM_REACHED,
         [this](const mavlink_message_t& message) { process_mission_item_reached(message); },
         this);
-
-    _system_impl->register_mavlink_message_handler(
-        MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION,
-        [this](const mavlink_message_t& message) { process_gimbal_manager_information(message); },
-        this);
 }
 
-void MissionImpl::enable()
-{
-    _gimbal_protocol_cookie =
-        _system_impl->register_timeout_handler([this]() { receive_protocol_timeout(); }, 1.0);
-
-    MavlinkCommandSender::CommandLong command{};
-    command.command = MAV_CMD_REQUEST_MESSAGE;
-    command.params.maybe_param1 = static_cast<float>(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION);
-    command.target_component_id = 0; // any component
-    _system_impl->send_command_async(command, nullptr);
-}
+void MissionImpl::enable() {}
 
 void MissionImpl::disable()
 {
     reset_mission_progress();
-    _gimbal_protocol = GimbalProtocol::Unknown;
 }
 
 void MissionImpl::deinit()
 {
-    _system_impl->unregister_timeout_handler(_gimbal_protocol_cookie);
     _system_impl->unregister_timeout_handler(_timeout_cookie);
     _system_impl->unregister_all_mavlink_message_handlers(this);
 }
@@ -100,36 +83,6 @@ void MissionImpl::process_mission_item_reached(const mavlink_message_t& message)
     report_progress_locked();
 }
 
-void MissionImpl::process_gimbal_manager_information(const mavlink_message_t& message)
-{
-    UNUSED(message);
-    if (_gimbal_protocol == GimbalProtocol::Unknown) {
-        LogDebug() << "Using gimbal protocol v2";
-        _gimbal_protocol = GimbalProtocol::V2;
-        _system_impl->unregister_timeout_handler(_gimbal_protocol_cookie);
-    }
-}
-
-void MissionImpl::wait_for_protocol()
-{
-    while (_gimbal_protocol == GimbalProtocol::Unknown) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
-}
-
-void MissionImpl::wait_for_protocol_async(std::function<void()> callback)
-{
-    wait_for_protocol();
-    callback();
-}
-
-void MissionImpl::receive_protocol_timeout()
-{
-    LogDebug() << "Falling back to gimbal protocol v1";
-    _gimbal_protocol = GimbalProtocol::V1;
-    _gimbal_protocol_cookie = {};
-}
-
 Mission::Result MissionImpl::upload_mission(const Mission::MissionPlan& mission_plan)
 {
     auto prom = std::promise<Mission::Result>();
@@ -153,22 +106,20 @@ void MissionImpl::upload_mission_async(
 
     reset_mission_progress();
 
-    wait_for_protocol_async([callback, mission_plan, this]() {
-        const auto int_items = convert_to_int_items(mission_plan.mission_items);
-
-        _mission_data.last_upload = _system_impl->mission_transfer_client().upload_items_async(
-            MAV_MISSION_TYPE_MISSION,
-            _system_impl->get_system_id(),
-            int_items,
-            [this, callback](MavlinkMissionTransferClient::Result result) {
-                auto converted_result = convert_result(result);
-                _system_impl->call_user_callback([callback, converted_result]() {
-                    if (callback) {
-                        callback(converted_result);
-                    }
-                });
+    const auto int_items = convert_to_int_items(mission_plan.mission_items);
+
+    _mission_data.last_upload = _system_impl->mission_transfer_client().upload_items_async(
+        MAV_MISSION_TYPE_MISSION,
+        _system_impl->get_system_id(),
+        int_items,
+        [this, callback](MavlinkMissionTransferClient::Result result) {
+            auto converted_result = convert_result(result);
+            _system_impl->call_user_callback([callback, converted_result]() {
+                if (callback) {
+                    callback(converted_result);
+                }
             });
-    });
+        });
 }
 
 void MissionImpl::upload_mission_with_progress_async(
@@ -186,29 +137,27 @@ void MissionImpl::upload_mission_with_progress_async(
 
     reset_mission_progress();
 
-    wait_for_protocol_async([callback, mission_plan, this]() {
-        const auto int_items = convert_to_int_items(mission_plan.mission_items);
-
-        _mission_data.last_upload = _system_impl->mission_transfer_client().upload_items_async(
-            MAV_MISSION_TYPE_MISSION,
-            _system_impl->get_system_id(),
-            int_items,
-            [this, callback](MavlinkMissionTransferClient::Result result) {
-                auto converted_result = convert_result(result);
-                _system_impl->call_user_callback([callback, converted_result]() {
-                    if (callback) {
-                        callback(converted_result, Mission::ProgressData{});
-                    }
-                });
-            },
-            [this, callback](float progress) {
-                _system_impl->call_user_callback([callback, progress]() {
-                    if (callback) {
-                        callback(Mission::Result::Next, Mission::ProgressData{progress});
-                    }
-                });
+    const auto int_items = convert_to_int_items(mission_plan.mission_items);
+
+    _mission_data.last_upload = _system_impl->mission_transfer_client().upload_items_async(
+        MAV_MISSION_TYPE_MISSION,
+        _system_impl->get_system_id(),
+        int_items,
+        [this, callback](MavlinkMissionTransferClient::Result result) {
+            auto converted_result = convert_result(result);
+            _system_impl->call_user_callback([callback, converted_result]() {
+                if (callback) {
+                    callback(converted_result, Mission::ProgressData{});
+                }
+            });
+        },
+        [this, callback](float progress) {
+            _system_impl->call_user_callback([callback, progress]() {
+                if (callback) {
+                    callback(Mission::Result::Next, Mission::ProgressData{progress});
+                }
             });
-    });
+        });
 }
 
 Mission::Result MissionImpl::cancel_mission_upload() const
@@ -461,26 +410,11 @@ MissionImpl::convert_to_int_items(const std::vector<MissionItem>& mission_items)
         }
 
         if (std::isfinite(item.gimbal_yaw_deg) || std::isfinite(item.gimbal_pitch_deg)) {
-            const auto temp_gimbal_protocol = _gimbal_protocol.load();
-            switch (temp_gimbal_protocol) {
-                case GimbalProtocol::V1:
-                    add_gimbal_items_v1(
-                        int_items, item_i, item.gimbal_pitch_deg, item.gimbal_yaw_deg);
-                    break;
-
-                case GimbalProtocol::V2:
-                    if (!_mission_data.gimbal_v2_in_control) {
-                        acquire_gimbal_control_v2(int_items, item_i);
-                        _mission_data.gimbal_v2_in_control = true;
-                    }
-                    add_gimbal_items_v2(
-                        int_items, item_i, item.gimbal_pitch_deg, item.gimbal_yaw_deg);
-                    break;
-                case GimbalProtocol::Unknown:
-                    // This should not happen because we wait until we know the protocol version.
-                    LogErr() << "Unknown gimbal protocol, skipping gimbal commands.";
-                    break;
+            if (!_mission_data.gimbal_v2_in_control) {
+                acquire_gimbal_control_v2(int_items, item_i);
+                _mission_data.gimbal_v2_in_control = true;
             }
+            add_gimbal_items_v2(int_items, item_i, item.gimbal_pitch_deg, item.gimbal_yaw_deg);
         }
 
         // A loiter time of NAN is ignored but also a loiter time of 0 doesn't
@@ -750,16 +684,6 @@ std::pair<Mission::Result, Mission::MissionPlan> MissionImpl::convert_to_result_
 
                 have_set_position = true;
 
-            } else if (int_item.command == MAV_CMD_DO_MOUNT_CONTROL) {
-                if (int(int_item.z) != MAV_MOUNT_MODE_MAVLINK_TARGETING) {
-                    LogErr() << "Gimbal mount control mode unsupported";
-                    result_pair.first = Mission::Result::Unsupported;
-                    break;
-                }
-
-                new_mission_item.gimbal_pitch_deg = int_item.param1;
-                new_mission_item.gimbal_yaw_deg = int_item.param3;
-
             } else if (int_item.command == MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) {
                 if (int_item.x !=
                     (GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK)) {
@@ -775,21 +699,6 @@ std::pair<Mission::Result, Mission::MissionPlan> MissionImpl::convert_to_result_
                 // We need to ignore it in order to not throw an "Unsupported" error
                 continue;
 
-            } else if (int_item.command == MAV_CMD_DO_MOUNT_CONFIGURE) {
-                if (int(int_item.param1) != MAV_MOUNT_MODE_MAVLINK_TARGETING) {
-                    LogErr() << "Gimbal mount configure mode unsupported";
-                    result_pair.first = Mission::Result::Unsupported;
-                    break;
-                }
-
-                // FIXME: ultimately param4 doesn't count anymore and
-                //        param7 holds the truth.
-                if (int(int_item.param4) == 1 || int(int_item.z) == 2) {
-                    _enable_absolute_gimbal_yaw_angle = true;
-                } else {
-                    _enable_absolute_gimbal_yaw_angle = false;
-                }
-
             } else if (int_item.command == MAV_CMD_IMAGE_START_CAPTURE) {
                 if (int_item.param2 > 0 && int(int_item.param3) == 0) {
                     new_mission_item.camera_action = CameraAction::StartPhotoInterval;
@@ -1197,66 +1106,6 @@ Mission::Result MissionImpl::convert_result(MavlinkMissionTransferClient::Result
     }
 }
 
-void MissionImpl::add_gimbal_items_v1(
-    std::vector<MavlinkMissionTransferClient::ItemInt>& int_items,
-    unsigned item_i,
-    float pitch_deg,
-    float yaw_deg)
-{
-    if (_enable_absolute_gimbal_yaw_angle) {
-        // We need to configure the gimbal to use an absolute angle.
-
-        // Current is the 0th waypoint
-        uint8_t current = ((int_items.size() == 0) ? 1 : 0);
-
-        uint8_t autocontinue = 1;
-
-        MavlinkMissionTransferClient::ItemInt next_item{
-            static_cast<uint16_t>(int_items.size()),
-            MAV_FRAME_MISSION,
-            MAV_CMD_DO_MOUNT_CONFIGURE,
-            current,
-            autocontinue,
-            MAV_MOUNT_MODE_MAVLINK_TARGETING,
-            0.0f, // stabilize roll
-            0.0f, // stabilize pitch
-            1.0f, // stabilize yaw, FIXME: for now we use this for an absolute yaw angle,
-                  // because it works.
-            0,
-            0,
-            2.0f, // eventually this is the correct flag to set absolute yaw angle.
-            MAV_MISSION_TYPE_MISSION};
-
-        _mission_data.mavlink_mission_item_to_mission_item_indices.push_back(item_i);
-        int_items.push_back(next_item);
-    }
-
-    // The gimbal has changed, we need to add a gimbal command.
-
-    // Current is the 0th waypoint
-    uint8_t current = ((int_items.size() == 0) ? 1 : 0);
-
-    uint8_t autocontinue = 1;
-
-    MavlinkMissionTransferClient::ItemInt next_item{
-        static_cast<uint16_t>(int_items.size()),
-        MAV_FRAME_MISSION,
-        MAV_CMD_DO_MOUNT_CONTROL,
-        current,
-        autocontinue,
-        pitch_deg, // pitch
-        0.0f, // roll (yes it is a weird order)
-        yaw_deg, // yaw
-        NAN,
-        0,
-        0,
-        MAV_MOUNT_MODE_MAVLINK_TARGETING,
-        MAV_MISSION_TYPE_MISSION};
-
-    _mission_data.mavlink_mission_item_to_mission_item_indices.push_back(item_i);
-    int_items.push_back(next_item);
-}
-
 void MissionImpl::add_gimbal_items_v2(
     std::vector<MavlinkMissionTransferClient::ItemInt>& int_items,
     unsigned item_i,
diff --git a/src/mavsdk/plugins/mission/mission_impl.h b/src/mavsdk/plugins/mission/mission_impl.h
index 4d664191f..e16f609ae 100644
--- a/src/mavsdk/plugins/mission/mission_impl.h
+++ b/src/mavsdk/plugins/mission/mission_impl.h
@@ -80,7 +80,6 @@ class MissionImpl : public PluginImplBase {
 
     void process_mission_current(const mavlink_message_t& message);
     void process_mission_item_reached(const mavlink_message_t& message);
-    void process_gimbal_manager_information(const mavlink_message_t& message);
     void receive_protocol_timeout();
     void wait_for_protocol();
     void wait_for_protocol_async(std::function<void()> callback);
@@ -99,18 +98,12 @@ class MissionImpl : public PluginImplBase {
         Mission::ResultCallback callback, MavlinkCommandSender::Result result);
     static Mission::Result command_result_to_mission_result(MavlinkCommandSender::Result result);
 
-    // FIXME: make static
     std::pair<Mission::Result, Mission::MissionPlan> convert_to_result_and_mission_items(
         MavlinkMissionTransferClient::Result result,
         const std::vector<MavlinkMissionTransferClient::ItemInt>& int_items);
 
     static Mission::Result convert_result(MavlinkMissionTransferClient::Result result);
 
-    void add_gimbal_items_v1(
-        std::vector<MavlinkMissionTransferClient::ItemInt>& int_items,
-        unsigned item_i,
-        float pitch_deg,
-        float yaw_deg);
     void add_gimbal_items_v2(
         std::vector<MavlinkMissionTransferClient::ItemInt>& int_items,
         unsigned item_i,
@@ -139,14 +132,6 @@ class MissionImpl : public PluginImplBase {
     TimeoutHandler::Cookie _timeout_cookie{};
 
     bool _enable_return_to_launch_after_mission{false};
-
-    // FIXME: This is hardcoded for now because it is urgently needed for 3DR with Yuneec H520.
-    //        Ultimate it needs a setter.
-    bool _enable_absolute_gimbal_yaw_angle{true};
-
-    TimeoutHandler::Cookie _gimbal_protocol_cookie{};
-    enum class GimbalProtocol { Unknown, V1, V2 };
-    std::atomic<GimbalProtocol> _gimbal_protocol{GimbalProtocol::Unknown};
 };
 
 } // namespace mavsdk
diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp
index 570e223aa..439eab6f3 100644
--- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp
+++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp
@@ -196,18 +196,24 @@ void TelemetryImpl::enable()
 void TelemetryImpl::disable()
 {
     _system_impl->remove_call_every(_homepos_cookie);
+    {
+        std::lock_guard<std::mutex> lock(_health_mutex);
+        _health.is_home_position_ok = false;
+    }
 }
 
 void TelemetryImpl::request_home_position_again()
 {
     {
-        std::lock_guard<std::mutex> lock(_request_home_position_mutex);
+        std::lock_guard<std::mutex> lock(_health_mutex);
         if (_health.is_home_position_ok) {
             _system_impl->remove_call_every(_homepos_cookie);
             return;
         }
+
+        _system_impl->mavlink_request_message().request(
+            MAVLINK_MSG_ID_HOME_POSITION, MAV_COMP_ID_AUTOPILOT1, nullptr);
     }
-    request_home_position_async();
 }
 
 Telemetry::Result TelemetryImpl::set_rate_position_velocity_ned(double rate_hz)
@@ -2471,19 +2477,10 @@ void TelemetryImpl::unsubscribe_altitude(Telemetry::AltitudeHandle handle)
     _altitude_subscriptions.unsubscribe(handle);
 }
 
-void TelemetryImpl::request_home_position_async()
-{
-    MavlinkCommandSender::CommandLong command_request_message{};
-    command_request_message.command = MAV_CMD_REQUEST_MESSAGE;
-    command_request_message.target_component_id = MAV_COMP_ID_AUTOPILOT1;
-    command_request_message.params.maybe_param1 = static_cast<float>(MAVLINK_MSG_ID_HOME_POSITION);
-    _system_impl->send_command_async(command_request_message, nullptr);
-}
-
 void TelemetryImpl::get_gps_global_origin_async(
     const Telemetry::GetGpsGlobalOriginCallback callback)
 {
-    _system_impl->request_message().request(
+    _system_impl->mavlink_request_message().request(
         MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN,
         MAV_COMP_ID_AUTOPILOT1,
         [this, callback](MavlinkCommandSender::Result result, const mavlink_message_t& message) {
@@ -2519,12 +2516,4 @@ std::pair<Telemetry::Result, Telemetry::GpsGlobalOrigin> TelemetryImpl::get_gps_
     return fut.get();
 }
 
-void TelemetryImpl::check_calibration()
-{
-    if (_system_impl->has_autopilot() && _system_impl->autopilot() == Autopilot::ArduPilot) {
-        // We need to ask for the home position from ArduPilot
-        request_home_position_async();
-    }
-}
-
 } // namespace mavsdk
diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.h b/src/mavsdk/plugins/telemetry/telemetry_impl.h
index b1b3a0091..d0ed9383a 100644
--- a/src/mavsdk/plugins/telemetry/telemetry_impl.h
+++ b/src/mavsdk/plugins/telemetry/telemetry_impl.h
@@ -261,9 +261,7 @@ class TelemetryImpl : public PluginImplBase {
 
     void receive_statustext(const MavlinkStatustextHandler::Statustext&);
 
-    void request_home_position_async();
     void request_home_position_again();
-    void check_calibration();
 
     static bool sys_status_present_enabled_health(
         const mavlink_sys_status_t& sys_status, MAV_SYS_STATUS_SENSOR flag);
@@ -370,8 +368,6 @@ class TelemetryImpl : public PluginImplBase {
     mutable std::mutex _altitude_mutex{};
     Telemetry::Altitude _altitude{};
 
-    mutable std::mutex _request_home_position_mutex{};
-
     std::mutex _subscription_mutex{};
     CallbackList<Telemetry::PositionVelocityNed> _position_velocity_ned_subscriptions{};
     CallbackList<Telemetry::Position> _position_subscriptions{};