From 38b90c69fe0c1158c63f355dc43f38e8b3806bf7 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Tue, 1 Aug 2017 14:34:13 +0900
Subject: [PATCH] Per @martinbudden 's formula

---
 src/main/fc/fc_msp.c | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

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));