From 37b9bd41ab248b9bc4ea476df622bf83af4258ac Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Sun, 26 Jun 2022 21:15:46 +0300
Subject: [PATCH] no overrides before sensors are calibrated

---
 src/main/fc/fc_msp.c            | 34 ++++++++++++++++++---------------
 src/main/sensors/acceleration.c |  2 ++
 src/main/sensors/gyro.c         |  2 ++
 3 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index f099c0722b..eccc7e9856 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -3435,7 +3435,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				LOG_D(SYSTEM, "Simulator enabled");
 			}
 
-			if (dataSize >= 14) {
+			if (dataSize >= 14 && isImuReady()) {
 				gpsSol.fixType = sbufReadU8(src);
 				gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
 				gpsSol.flags.hasNewData = true;
@@ -3463,6 +3463,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units
 				acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
 				acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
+				acc.accVibeSq[X] = 0;
+				acc.accVibeSq[Y] = 0;
+				acc.accVibeSq[Z] = 0;
 
 				gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
 				gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
@@ -3473,21 +3476,22 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 			else {
 				DISABLE_STATE(GPS_FIX);
 			}
-
-			sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_ROLL]);
-			sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_PITCH]);
-			sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_YAW]);
-			sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_THROTTLE]);
-
-			simulatorData.debugIndex++;
-			if (simulatorData.debugIndex == 8){
-				simulatorData.debugIndex = 0;
-			}
-			sbufWriteU8(dst, simulatorData.debugIndex);
-			sbufWriteU32(dst, debug[simulatorData.debugIndex]);
-
-			mspWriteSimulatorOSD(dst);
 		}
+
+		sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_ROLL]);
+		sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_PITCH]);
+		sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_YAW]);
+		sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_THROTTLE]);
+
+		simulatorData.debugIndex++;
+		if (simulatorData.debugIndex == 8){
+			simulatorData.debugIndex = 0;
+		}
+		sbufWriteU8(dst, simulatorData.debugIndex);
+		sbufWriteU32(dst, debug[simulatorData.debugIndex]);
+
+		mspWriteSimulatorOSD(dst);
+
         *ret = MSP_RESULT_ACK;
         break;
     default:
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 68bca9d6a4..ec0e88bbb9 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -518,6 +518,8 @@ float accGetMeasuredMaxG(void)
 void accUpdate(void)
 {
     if (ARMING_FLAG(SIMULATOR_MODE)) {
+		//output: acc.accADCf
+		//unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
         return;
     }
     if (!acc.dev.readFn(&acc.dev)) {
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 0847b0cec5..97e8f580aa 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -498,6 +498,8 @@ void FAST_CODE NOINLINE gyroFilter()
 void FAST_CODE NOINLINE gyroUpdate()
 {
     if (ARMING_FLAG(SIMULATOR_MODE)) {
+		//output: gyro.gyroADCf[axis]
+		//unused: dev->gyroADCRaw[], dev->gyroZero[];
         return;
     }