adjusted sensors usage in simulator mode

src/main/drivers/display.c

@@ -180,10 +180,10 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 
 	if (ARMING_FLAG(SIMULATOR_MODE)) {
 		//some FCs do not power max7456 from USB power 
-		//driver can not read front metadata 
+		//driver can not read font metadata 
 		//chip assumed to not support second bank of font
-		//artifical horizon, variometer and home direction are not drawn ( displat.c: displayUpdateMaxChar())
+		//artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar())
-		//return dummy metadata so OSD work in simulator mode
+		//return dummy metadata to let all OSD elements to work in simulator mode
 		instance->maxChar = 512;
 	}
 

src/main/fc/fc_msp.c

@@ -3420,45 +3420,91 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		tmp_u8 = sbufReadU8(src); //MSP_SIMULATOR version
 		if (tmp_u8 != 1) break;
 
-		tmp_u8 = sbufReadU8(src);
+		simulatorData.flags = sbufReadU8(src);
-        if ((tmp_u8 & SIMU_ENABLE) == 0) {
+        if ((simulatorData.flags & SIMU_ENABLE) == 0) {
-			DISABLE_ARMING_FLAG(SIMULATOR_MODE);
+
-			baroStartCalibration(); // just once
+			if (ARMING_FLAG(SIMULATOR_MODE)) { // just once
-			simulatorData.flags = 0;
+				DISABLE_ARMING_FLAG(SIMULATOR_MODE);
-		}
+
-		else {
+				baroStartCalibration();
-			simulatorData.flags = tmp_u8;
+
+				simulatorData.flags = 0;
+				//review: many states were affected. reboot?
+			}
+		}
+		else if (!areSensorsCalibrating()) {
+			if (!ARMING_FLAG(SIMULATOR_MODE)) { // just once
+				baroStartCalibration(); 
+
+				//todo: magnetometer emulation support
+				sensorsClear(SENSOR_MAG);
+				mag.magADC[X] = 0;
+				mag.magADC[Y] = 0;
+				mag.magADC[Z] = 0;
 
-			if (!ARMING_FLAG(SIMULATOR_MODE)) {
-				baroStartCalibration(); // just once
 				ENABLE_ARMING_FLAG(SIMULATOR_MODE);
 				LOG_D(SYSTEM, "Simulator enabled");
 			}
 
-			if (dataSize >= 14 && isImuReady()) {
+			if (dataSize >= 14) {
-				gpsSol.fixType = sbufReadU8(src);
-				gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
-				gpsSol.flags.hasNewData = true;
-				gpsSol.flags.validVelNE = 0;
-				gpsSol.flags.validVelD = 0;
-				gpsSol.flags.validEPE = 0;
-				gpsSol.numSat = sbufReadU8(src);
-				gpsSol.llh.lat = sbufReadU32(src);
-				gpsSol.llh.lon = sbufReadU32(src);
-				gpsSol.llh.alt = sbufReadU32(src);
-				gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
-				gpsSol.groundCourse = (int16_t)sbufReadU16(src);
-				gpsSol.velNED[X] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
-				gpsSol.velNED[Y] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
-				gpsSol.velNED[Z] = 0;
-				gpsSol.eph = 100;
-				gpsSol.epv = 100;
-				// Feed data to navigation
-				gpsProcessNewSolutionData();
 
-				attitude.values.roll = (int16_t)sbufReadU16(src);
+				if (feature(FEATURE_GPS)) {
-				attitude.values.pitch = (int16_t)sbufReadU16(src);
+					gpsSol.fixType = sbufReadU8(src);
-				attitude.values.yaw = (int16_t)sbufReadU16(src);
+					gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
+					gpsSol.flags.hasNewData = true;
+					gpsSol.flags.validVelNE = 0;
+					gpsSol.flags.validVelD = 0;
+					gpsSol.flags.validEPE = 0;
+					gpsSol.numSat = sbufReadU8(src);
+
+					if (gpsSol.fixType != GPS_NO_FIX) {
+						gpsSol.llh.lat = sbufReadU32(src);
+						gpsSol.llh.lon = sbufReadU32(src);
+						gpsSol.llh.alt = sbufReadU32(src);
+						gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
+						gpsSol.groundCourse = (int16_t)sbufReadU16(src);
+						gpsSol.velNED[X] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
+						gpsSol.velNED[Y] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
+						gpsSol.velNED[Z] = 0;
+						gpsSol.eph = 100;
+						gpsSol.epv = 100;
+
+						ENABLE_STATE(GPS_FIX);
+
+						// Feed data to navigation
+						gpsProcessNewSolutionData();
+					}
+					else {
+						sbufReadU32(src);
+						sbufReadU32(src);
+						sbufReadU32(src);
+						sbufReadU16(src);
+						sbufReadU16(src);
+
+						DISABLE_STATE(GPS_FIX);
+					}
+				}
+				else {
+					sbufReadU8(src);
+					sbufReadU8(src);
+					sbufReadU32(src);
+					sbufReadU32(src);
+					sbufReadU32(src);
+					sbufReadU16(src);
+					sbufReadU16(src);
+				}
+
+				if ((simulatorData.flags & SIMU_USE_SENSORS) == 0) {
+					attitude.values.roll = (int16_t)sbufReadU16(src);
+					attitude.values.pitch = (int16_t)sbufReadU16(src);
+					attitude.values.yaw = (int16_t)sbufReadU16(src);
+				}
+				else
+				{
+					sbufReadU16(src);
+					sbufReadU16(src);
+					sbufReadU16(src);
+				}
 
 				acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units
 				acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
@@ -3471,8 +3517,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
 				gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
 
-				baro.baroPressure = sbufReadU32(src);
+				if (sensors(SENSOR_BARO))
-				baro.baroTemperature = 2500;
+				{
+					baro.baroPressure = sbufReadU32(src);
+					baro.baroTemperature = 2500;
+				}
+				else {
+					baro.baroPressure = sbufReadU32(src);
+				}
+
 			}
 			else {
 				DISABLE_STATE(GPS_FIX);
@@ -3482,12 +3535,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		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]);
+		sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? input[INPUT_STABILIZED_THROTTLE] : -500));
 
 		simulatorData.debugIndex++;
 		if (simulatorData.debugIndex == 8){
 			simulatorData.debugIndex = 0;
 		}
+
+		simulatorData.debugIndex |= (mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0;
 		sbufWriteU8(dst, simulatorData.debugIndex);
 		sbufWriteU32(dst, debug[simulatorData.debugIndex]);
 

src/main/fc/runtime_config.h

@@ -169,7 +169,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);
 typedef enum {
 	SIMU_ENABLE				= (1 << 0),
 	SIMU_SIMULATE_BATTERY	= (1 << 1),
-	SIMU_MUTE_BEEPER		= (1 << 2)
+	SIMU_MUTE_BEEPER		= (1 << 2),
+	SIMU_USE_SENSORS		= (1 << 3)
 } simulatorFlags_t;
 
 typedef struct {

src/main/flight/imu.c

@@ -573,8 +573,7 @@ static void imuCalculateEstimatedAttitude(float dT)
     const float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeight(dT);
     const bool useAcc = (accWeight > 0.001f);
 
-    if (ARMING_FLAG(SIMULATOR_MODE)) {
+    if (ARMING_FLAG(SIMULATOR_MODE) && ((simulatorData.flags & SIMU_USE_SENSORS) == 0 )) {
-        // todo: after imuMahonyAHRSupdate, ground course is divided by 10 =(
         return;
     }
 

src/main/io/gps.c

@@ -362,7 +362,8 @@ bool gpsUpdate(void)
     if (ARMING_FLAG(SIMULATOR_MODE)) {
         gpsUpdateTime();
         gpsSetState(GPS_RUNNING);
-        return gpsSol.flags.hasNewData;
+		sensorsSet(SENSOR_GPS);
+		return gpsSol.flags.hasNewData;
     }
 #ifdef USE_FAKE_GPS
     return gpsFakeGPSUpdate();