implemented magnethometer simulation

src/main/fc/fc_msp.c

@@ -3418,7 +3418,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         break;
     case MSP_SIMULATOR:
 		tmp_u8 = sbufReadU8(src); //MSP_SIMULATOR version
-		if (tmp_u8 != 1) break;
+		if (tmp_u8 != 2) break;
 
 		simulatorData.flags = sbufReadU8(src);
         if ((simulatorData.flags & SIMU_ENABLE) == 0) {
@@ -3428,6 +3428,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
 				baroStartCalibration();
 
+				DISABLE_STATE(COMPASS_CALIBRATED);
+				compassInit();
+
 				simulatorData.flags = 0;
 				//review: many states were affected. reboot?
 
@@ -3437,12 +3440,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		else if (!areSensorsCalibrating()) {
 			if (!ARMING_FLAG(SIMULATOR_MODE)) { // just once
 				baroStartCalibration(); 
-
+				
-				//todo: magnetometer emulation support
+				if (compassConfig()->mag_hardware != 0){
-				sensorsClear(SENSOR_MAG);
+					sensorsSet(SENSOR_MAG);
-				mag.magADC[X] = 0;
+					ENABLE_STATE(COMPASS_CALIBRATED);
-				mag.magADC[Y] = 0;
+					DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
-				mag.magADC[Z] = 0;
+					mag.magADC[X] = 800;
+					mag.magADC[Y] = 0;
+					mag.magADC[Z] = 0;
+				}
 
 				ENABLE_ARMING_FLAG(SIMULATOR_MODE);
 				LOG_D(SYSTEM, "Simulator enabled");
@@ -3550,6 +3556,16 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				else {
 					sbufAdvance(src,4);
 				}
+
+				if (sensors(SENSOR_MAG))
+				{
+					mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  //16000/20 = 800uT
+					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;
+					mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
+				}
+				else {
+					sbufAdvance(src, 2*3);
+				}
 			}
 			else {
 				DISABLE_STATE(GPS_FIX);
@@ -3561,6 +3577,27 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_YAW]);
 		sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? input[INPUT_STABILIZED_THROTTLE] : -500));
 
+		/*
+		debug[0] = mag.magADC[X];
+		debug[1] = mag.magADC[Y];
+		debug[2] = mag.magADC[Z];
+		//debug[3] = sensors(SENSOR_MAG) ? 1 : 0;
+		//debug[4] = STATE(COMPASS_CALIBRATED) ? 1 : 0;
+        fpVector3_t vMag;
+        fpVector3_t magBF;
+		magBF.x = mag.magADC[X];
+		magBF.y = mag.magADC[Y];
+		magBF.z = mag.magADC[Z];
+		quaternionRotateVectorInv(&vMag, &magBF, &orientation);    // BF -> EF
+		debug[3] = vMag.x;
+		debug[4] = vMag.y;
+		debug[5] = vMag.z;
+		
+		debug[6] = getvCorrectedMagNorth().x*1000;
+		debug[7] = getvCorrectedMagNorth().y*1000;
+		*/
+
+
 		simulatorData.debugIndex++;
 		if (simulatorData.debugIndex == 8){
 			simulatorData.debugIndex = 0;

src/main/flight/imu.c

@@ -327,6 +327,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 // Normalize to unit vector
                 vectorNormalize(&vMag, &vMag);
 
+				if (ARMING_FLAG(SIMULATOR_MODE)) {
+					imuSetMagneticDeclination(0);
+				}
+
                 // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
                 // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
                 vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);

src/main/sensors/compass.c

@@ -369,6 +369,11 @@ bool compassIsCalibrationComplete(void)
 
 void compassUpdate(timeUs_t currentTimeUs)
 {
+	if (ARMING_FLAG(SIMULATOR_MODE)) {
+		magUpdatedAtLeastOnce = 1;
+		return;
+	}
+
     static sensorCalibrationState_t calState;
     static timeUs_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];

src/main/sensors/diagnostics.c

@@ -63,6 +63,11 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
 
 hardwareSensorStatus_e getHwCompassStatus(void)
 {
+	if (ARMING_FLAG(SIMULATOR_MODE)) {
+		return HW_SENSOR_OK;
+		return;
+	}
+
 #if defined(USE_MAG)
     if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
         if (compassIsHealthy()) {