diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index fc048b33d7..19d3a36cbd 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -738,14 +738,19 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
     case MSP_RAW_IMU:
         {
             // Hack scale due to choice of units for sensor data in multiwii
-            const uint8_t scale = acc.dev.acc_1G / 512;
+
+            uint8_t scale;
+
+            if (acc.dev.acc_1G > 512*4) {
+                scale = 8;
+            } else if (acc.dev.acc_1G >= 512) {
+                scale = 4;
+            } else {
+                scale = 1;
+            }
 
             for (int i = 0; i < 3; i++) {
-                if (scale) {
-                    sbufWriteU16(dst, acc.accSmooth[i] / scale);
-                } else {
-                    sbufWriteU16(dst, acc.accSmooth[i] * 2);
-                }
+                sbufWriteU16(dst, acc.accSmooth[i] / scale);
             }
             for (int i = 0; i < 3; i++) {
                 sbufWriteU16(dst, gyroRateDps(i));