diff --git a/src/mavesp8266_vehicle.cpp b/src/mavesp8266_vehicle.cpp
index a5adba3..81c4ef8 100644
--- a/src/mavesp8266_vehicle.cpp
+++ b/src/mavesp8266_vehicle.cpp
@@ -94,7 +94,8 @@ MavESP8266Vehicle::readMessage()
             //-- Move the pending ones up the queue
             int left = _queue_count - sent;
             for(int i = 0; i < left; i++) {
-                memcpy(&_message[sent+i], &_message[i], sizeof(mavlink_message_t));
+                //memcpy(&_message[sent+i], &_message[i], sizeof(mavlink_message_t));
+                memcpy(&_message[i], &_message[sent+i], sizeof(mavlink_message_t));
             }
             _queue_count = left;
         }