diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp
index 0f4003b620..5768f6759c 100644
--- a/src/core/system_impl.cpp
+++ b/src/core/system_impl.cpp
@@ -85,8 +85,6 @@ void SystemImpl::register_mavlink_message_handler(
 
     MAVLinkHandlerTableEntry entry = {msg_id, callback, cookie};
     _mavlink_handler_table.push_back(entry);
-    // Push back can move the std::vector and therefore invalidate the iterator.
-    _iterator_invalidated = true;
 }
 
 void SystemImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie)
@@ -97,7 +95,6 @@ void SystemImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void*
          /* no ++it */) {
         if (it->msg_id == msg_id && it->cookie == cookie) {
             it = _mavlink_handler_table.erase(it);
-            _iterator_invalidated = true;
         } else {
             ++it;
         }
@@ -112,7 +109,6 @@ void SystemImpl::unregister_all_mavlink_message_handlers(const void* cookie)
          /* no ++it */) {
         if (it->cookie == cookie) {
             it = _mavlink_handler_table.erase(it);
-            _iterator_invalidated = true;
         } else {
             ++it;
         }
@@ -147,33 +143,20 @@ void SystemImpl::process_mavlink_message(mavlink_message_t& message)
         }
     }
 
-    _mavlink_handler_table_mutex.lock();
+    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);
 
 #if MESSAGE_DEBUGGING == 1
     bool forwarded = false;
 #endif
-    for (auto it = _mavlink_handler_table.begin(); it != _mavlink_handler_table.end(); /* ++it */) {
+    for (auto it = _mavlink_handler_table.begin(); it != _mavlink_handler_table.end(); ++it) {
         if (it->msg_id == message.msgid) {
 #if MESSAGE_DEBUGGING == 1
             LogDebug() << "Forwarding msg " << int(message.msgid) << " to " << size_t(it->cookie);
             forwarded = true;
 #endif
-            _mavlink_handler_table_mutex.unlock();
             it->callback(message);
-            _mavlink_handler_table_mutex.lock();
-        }
-
-        if (_iterator_invalidated) {
-            // Someone messed with the map while we were doing the callback.
-            // We need to start over. This means that we might call something twice but that's
-            // probably better than to drop the message.
-            it = _mavlink_handler_table.begin();
-            _iterator_invalidated = false;
-        } else {
-            ++it;
         }
     }
-    _mavlink_handler_table_mutex.unlock();
 
 #if MESSAGE_DEBUGGING == 1
     if (!forwarded) {
diff --git a/src/core/system_impl.h b/src/core/system_impl.h
index f5f484eeee..8903edc916 100644
--- a/src/core/system_impl.h
+++ b/src/core/system_impl.h
@@ -311,8 +311,6 @@ class SystemImpl {
 
     ThreadPool _thread_pool{3};
 
-    bool _iterator_invalidated{false};
-
     std::mutex _param_changed_callbacks_mutex{};
     std::map<const void*, param_changed_callback_t> _param_changed_callbacks{};