diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 1e6bed12b4efe971332e485d07ceefc1f3914f2a..beefecaa077b7bb8922bcec0a70e9abe5d4c3aef 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -1179,6 +1179,24 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
     }
 }
 
+// Ignore warnings from mavlink headers for both GCC/Clang and MSVC
+#ifdef __GNUC__
+
+#if __GNUC__ > 8
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#elif defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Waddress-of-packed-member"
+#else
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wall"
+#endif
+
+#else
+#pragma warning(push, 0)
+#endif
+
 void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
 {
     mavlink_attitude_target_t attitudeTarget;
@@ -1916,24 +1934,6 @@ void Vehicle::_handleRadioStatus(mavlink_message_t& message)
     }
 }
 
-// Ignore warnings from mavlink headers for both GCC/Clang and MSVC
-#ifdef __GNUC__
-
-#if __GNUC__ > 8
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
-#elif defined(__clang__)
-#pragma clang diagnostic push
-#pragma clang diagnostic ignored "-Waddress-of-packed-member"
-#else
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wall"
-#endif
-
-#else
-#pragma warning(push, 0)
-#endif
-
 void Vehicle::_handleRCChannels(mavlink_message_t& message)
 {
     mavlink_rc_channels_t channels;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index ab6dcc4dfc53ef96534d828e81bab15717f28b0a..b789b7f4b0eb35782e0c8b9ed09b8511dbf3ed63 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -127,6 +127,24 @@ int UAS::getUASID() const
     return uasId;
 }
 
+// Ignore warnings from mavlink headers for both GCC/Clang and MSVC
+#ifdef __GNUC__
+
+#if __GNUC__ > 8
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#elif defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Waddress-of-packed-member"
+#else
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wall"
+#endif
+
+#else
+#pragma warning(push, 0)
+#endif
+
 void UAS::receiveMessage(mavlink_message_t message)
 {
     // Only accept messages from this system (condition 1)
@@ -327,6 +345,17 @@ void UAS::receiveMessage(mavlink_message_t message)
     }
 }
 
+// Pop warnings ignoring for mavlink headers for both GCC/Clang and MSVC
+#ifdef __GNUC__
+    #if defined(__clang__)
+        #pragma clang diagnostic pop
+    #else
+        #pragma GCC diagnostic pop
+    #endif
+#else
+#pragma warning(pop, 0)
+#endif
+
 void UAS::startCalibration(UASInterface::StartCalibrationType calType)
 {
     if (!_vehicle) {