refactoring

implemented beeper muting in simulator mode
implemented debug output in simulator mode
adjusted MSP_SIMULATOR command id
implemented battery emulation in simulator mode

src/main/drivers/sound_beeper.c

@@ -27,6 +27,7 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_output.h"
 #include "fc/config.h"
+#include "fc/runtime_config.h"
 
 #include "sound_beeper.h"
 
@@ -45,6 +46,10 @@ void systemBeep(bool onoff)
     UNUSED(onoff);
 #else
 
+	if (simulatorData.flags & SIMU_MUTE_BEEPER) {
+		onoff = false;
+	}
+
     if (beeperConfig()->pwmMode) {
         pwmWriteBeeper(onoff);
         beeperState = onoff;

src/main/fc/fc_msp.c

@@ -3421,61 +3421,73 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		if (tmp_u8 != 1) break;
 
 		tmp_u8 = sbufReadU8(src);
-        if (tmp_u8 & 1) {
+        if ((tmp_u8 & SIMU_ENABLE) == 0) {
-            if (!ARMING_FLAG(SIMULATOR_MODE)) {
+			DISABLE_ARMING_FLAG(SIMULATOR_MODE);
-                baroStartCalibration(); // just once
+			baroStartCalibration(); // just once
+			simulatorData.flags = 0;
+		}
+		else {
+			simulatorData.flags = tmp_u8;
+
+			if (!ARMING_FLAG(SIMULATOR_MODE)) {
+				baroStartCalibration(); // just once
 				ENABLE_ARMING_FLAG(SIMULATOR_MODE);
+				LOG_D(SYSTEM, "Simulator enabled");
 			}
-        } else {
-            DISABLE_ARMING_FLAG(SIMULATOR_MODE);
-            baroStartCalibration(); // just once
-        }
-        if (dataSize >= 14) {
-#ifdef USE_GPS
-            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 = sbufReadU16(src);
-            gpsSol.groundCourse = 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();
-#endif
-            attitude.values.roll = sbufReadU16(src)-1800;
-            attitude.values.pitch = sbufReadU16(src)-1800;
-            attitude.values.yaw = sbufReadU16(src);
 
-            acc.accADCf[X] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
+			if (dataSize >= 14) {
-            acc.accADCf[Y] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
+				gpsSol.fixType = sbufReadU8(src);
-            acc.accADCf[Z] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
+				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 = sbufReadU16(src);
+				gpsSol.groundCourse = 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();
 
-            gyro.gyroADCf[X] = (float)sbufReadU16(src) / 100 - 320;
+				attitude.values.roll = sbufReadU16(src) - 1800;
-            gyro.gyroADCf[Y] = (float)sbufReadU16(src) / 100 - 320;
+				attitude.values.pitch = sbufReadU16(src) - 1800;
-            gyro.gyroADCf[Z] = (float)sbufReadU16(src) / 100 - 320;
+				attitude.values.yaw = sbufReadU16(src);
 
-            baro.baroPressure = sbufReadU32(src);
+				acc.accADCf[X] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
-        } else {
+				acc.accADCf[Y] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
-            DISABLE_STATE(GPS_FIX);
+				acc.accADCf[Z] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
-        }
 
-        sbufWriteU16(dst, input[INPUT_STABILIZED_ROLL] + 500);
+				gyro.gyroADCf[X] = (float)sbufReadU16(src) / 100 - 320;
-        sbufWriteU16(dst, input[INPUT_STABILIZED_PITCH] + 500);
+				gyro.gyroADCf[Y] = (float)sbufReadU16(src) / 100 - 320;
-        sbufWriteU16(dst, input[INPUT_STABILIZED_YAW] + 500);
+				gyro.gyroADCf[Z] = (float)sbufReadU16(src) / 100 - 320;
-        sbufWriteU16(dst, input[INPUT_STABILIZED_THROTTLE] + 500);
 
-		mspWriteSimulatorOSD(dst);
+				baro.baroPressure = sbufReadU32(src);
+			}
+			else {
+				DISABLE_STATE(GPS_FIX);
+			}
 
+			sbufWriteU16(dst, input[INPUT_STABILIZED_ROLL] + 500);
+			sbufWriteU16(dst, input[INPUT_STABILIZED_PITCH] + 500);
+			sbufWriteU16(dst, input[INPUT_STABILIZED_YAW] + 500);
+			sbufWriteU16(dst, input[INPUT_STABILIZED_THROTTLE] + 500);
+
+			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:

src/main/fc/runtime_config.c

@@ -176,3 +176,5 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
 
     return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
 }
+
+simulatorData_t simulatorData = { flags: 0, debugIndex: 0 };

src/main/fc/runtime_config.h

@@ -165,6 +165,20 @@ typedef enum {
 
 flightModeForTelemetry_e getFlightModeForTelemetry(void);
 
+
+typedef enum {
+	SIMU_ENABLE				= (1 << 0),
+	SIMU_SIMULATE_BATTERY	= (1 << 1),
+	SIMU_MUTE_BEEPER		= (1 << 2)
+} simulatorFlags_t;
+
+typedef struct {
+	simulatorFlags_t flags;
+	uint8_t debugIndex;
+} simulatorData_t;
+
+extern simulatorData_t simulatorData;
+
 uint32_t enableFlightMode(flightModeFlags_e mask);
 uint32_t disableFlightMode(flightModeFlags_e mask);
 

src/main/msp/msp_protocol.h

@@ -288,7 +288,6 @@
 #define MSP_DEBUGMSG             253    //out message         debug string buffer
 #define MSP_DEBUG                254    //out message         debug1,debug2,debug3,debug4
 #define MSP_V2_FRAME             255    //MSPv2 payload indicator
-#define MSP_SIMULATOR            256    //inout               Set simulator state
 
 // Additional commands that are not compatible with MultiWii
 #define MSP_STATUS_EX            150    //out message         cycletime, errors_count, CPU load, sensor present etc

src/main/msp/msp_protocol_v2_inav.h

@@ -53,6 +53,8 @@
 #define MSP2_INAV_SET_TEMP_SENSOR_CONFIG        0x201D
 #define MSP2_INAV_TEMPERATURES                  0x201E
 
+#define MSP_SIMULATOR							0x201F
+
 #define MSP2_INAV_SERVO_MIXER                   0x2020
 #define MSP2_INAV_SET_SERVO_MIXER               0x2021
 #define MSP2_INAV_LOGIC_CONDITIONS              0x2022

src/main/sensors/battery.c

@@ -291,6 +291,12 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
             break;
     }
 
+	if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
+		vbat = 1260;
+		batteryFullVoltage = 1210;
+		batteryWarningVoltage = 1020;
+		batteryCriticalVoltage = 960;
+	}
     if (justConnected) {
         pt1FilterReset(&vbatFilterState, vbat);
     } else {