reverted unrelated changes

src/main/fc/fc_msp.c

@@ -21,7 +21,7 @@
 #include <string.h>
 #include <math.h>
 
-#include "common/log.h"
+#include "common/log.h" //for MSP_SIMULATOR
 #include "platform.h"
 
 #include "blackbox/blackbox.h"
@@ -100,8 +100,8 @@
 #include "msp/msp_serial.h"
 
 #include "navigation/navigation.h"
-#include "navigation/navigation_private.h"
+#include "navigation/navigation_private.h" //for MSP_SIMULATOR
-#include "navigation/navigation_pos_estimator_private.h"
+#include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR
 
 #include "rx/rx.h"
 #include "rx/msp.h"
@@ -607,14 +607,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
     case MSP_ALTITUDE:
+ 		sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
+        sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
 #if defined(USE_BARO)
-        sbufWriteU32(dst, baro.BaroAlt);
+        sbufWriteU32(dst, baroGetLatestAltitude());
-        sbufWriteU32(dst, baro.BaroMslAlt);
-        sbufWriteU32(dst, baro.baroPressure);
 #else
         sbufWriteU32(dst, 0);
-        sbufWriteU32(dst, 0);
-        sbufWriteU32(dst, 0);
 #endif
         break;
 
@@ -2480,8 +2478,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_GPS
     case MSP_SET_RAW_GPS:
         if (dataSize == 14) {
-            gpsSol.fixType = sbufReadU8(src);
+			if (sbufReadU8(src)) {
-            if (gpsSol.fixType) {
                 ENABLE_STATE(GPS_FIX);
             } else {
                 DISABLE_STATE(GPS_FIX);
@@ -3188,6 +3185,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
     return true;
 }
 
+#ifdef USE_SIMULATOR
 bool isOSDTypeSupportedBySimulator(void)
 {
 	displayPort_t *osdDisplayPort = osdGetDisplayPort();
@@ -3320,12 +3318,11 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 		sbufWriteU8(dst, 255);
 	}
 }
+#endif
 
 bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret)
 {
     uint8_t tmp_u8;
-//    uint16_t tmp_u16;
-
     const unsigned int dataSize = sbufBytesRemaining(src);
 
     switch (cmdMSP) {
@@ -3423,6 +3420,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
         *ret = MSP_RESULT_ACK;
         break;
+#ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
 		tmp_u8 = sbufReadU8(src); //MSP_SIMULATOR version
 		if (tmp_u8 != 2) break;
@@ -3551,27 +3549,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 		sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_YAW);
 		sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.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;
@@ -3593,6 +3570,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
         *ret = MSP_RESULT_ACK;
         break;
+#endif
+
     default:
         // Not handled
         return false;

src/main/flight/imu.c

@@ -600,12 +600,37 @@ static void imuCalculateEstimatedAttitude(float dT)
     imuUpdateEulerAngles();
 }
 
+#ifdef HIL
+void imuHILUpdate(void)
+{
+    /* Set attitude */
+    attitude.values.roll = hilToFC.rollAngle;
+    attitude.values.pitch = hilToFC.pitchAngle;
+    attitude.values.yaw = hilToFC.yawAngle;
+
+    /* Compute rotation quaternion for future use */
+    imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
+
+    /* Fake accADC readings */
+    accADCf[X] = hilToFC.bodyAccel[X] / GRAVITY_CMSS;
+    accADCf[Y] = hilToFC.bodyAccel[Y] / GRAVITY_CMSS;
+    accADCf[Z] = hilToFC.bodyAccel[Z] / GRAVITY_CMSS;
+}
+#endif
+
 void imuUpdateAccelerometer(void)
 {
+#ifdef HIL
+    if (sensors(SENSOR_ACC) && !hilActive) {
+        accUpdate();
+        isAccelUpdatedAtLeastOnce = true;
+    }
+#else
     if (sensors(SENSOR_ACC)) {
         accUpdate();
         isAccelUpdatedAtLeastOnce = true;
     }
+#endif
 }
 
 void imuCheckVibrationLevels(void)
@@ -630,10 +655,23 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
     previousIMUUpdateTimeUs = currentTimeUs;
 
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
+#ifdef HIL
+        if (!hilActive) {
             gyroGetMeasuredRotationRate(&imuMeasuredRotationBF);    // Calculate gyro rate in body frame in rad/s
             accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
             imuCheckVibrationLevels();
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
+        }
+        else {
+            imuHILUpdate();
+            imuUpdateMeasuredAcceleration();
+        }
+#else
+        gyroGetMeasuredRotationRate(&imuMeasuredRotationBF);    // Calculate gyro rate in body frame in rad/s
+        accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
+        imuCheckVibrationLevels();
+        imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
+#endif
     } else {
         acc.accADCf[X] = 0.0f;
         acc.accADCf[Y] = 0.0f;

src/main/flight/servos.c

@@ -246,7 +246,6 @@ void writeServos(void)
 void servoMixer(float dT)
 {
     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
-
     if (FLIGHT_MODE(MANUAL_MODE)) {
         input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
         input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
@@ -572,9 +571,11 @@ void processContinuousServoAutotrim(const float dT)
 }
 
 void processServoAutotrim(const float dT) {
+#ifdef USE_SIMULATOR
     if (ARMING_FLAG(SIMULATOR_MODE)) {
         return;
     }
+#endif
     if (feature(FEATURE_FW_AUTOTRIM)) {
         processContinuousServoAutotrim(dT);
     } else {

src/main/io/osd_dji_hd.c

@@ -537,8 +537,10 @@ static char * osdArmingDisabledReasonMessage(void)
             FALLTHROUGH;
         case ARMED:
             FALLTHROUGH;
+#ifdef USE_SIMULATOR
         case SIMULATOR_MODE:
             FALLTHROUGH;
+#endif
         case WAS_EVER_ARMED:
             break;
     }

src/main/navigation/navigation_pos_estimator.c

@@ -629,7 +629,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     }
 
     DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
-    DEBUG_SET(DEBUG_ALTITUDE, 2, imuMeasuredAccelBF.z);        // Baro altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
     DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
     DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
     DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
@@ -858,7 +858,6 @@ void initializePositionEstimator(void)
     pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
 }
 
-
 /**
  * Update estimator
  *  Update rate: loop rate (>100Hz)

src/main/sensors/acceleration.c

@@ -552,7 +552,6 @@ void accUpdate(void)
     // Filter acceleration
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
-        DEBUG_SET(DEBUG_ACC, axis, acc.accADCf[axis] * 1000);
     }
 
     if (accelerometerConfig()->acc_notch_hz) {

src/main/sensors/barometer.c

@@ -332,7 +332,7 @@ int32_t baroCalculateAltitude(void)
         }
 #endif
         // calculates height from ground via baro readings
-        baro.BaroMslAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
+		baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
    }
 
     return baro.BaroAlt;

src/main/sensors/battery.c

@@ -291,7 +291,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
             break;
     }
 #ifdef USE_SIMULATOR
-	if (!ARMING_FLAG(SIMULATOR_MODE)) {
+	if (ARMING_FLAG(SIMULATOR_MODE)) {
 		if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
 			vbat = 1260;
 			batteryFullVoltage = 1210;

src/main/target/HGLRCF722/target.c

@@ -41,13 +41,8 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH2, PB5,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S2   UP1-2   D(1, 5, 5)
     DEF_TIM(TIM3, CH3, PB0,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S3   UP1-2   D(1, 7, 5)
     DEF_TIM(TIM3, CH4, PB1,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S4   UP1-2   D(1, 2, 5)
-
+    DEF_TIM(TIM2, CH1, PA15,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5   UP1-7   D(1, 5, 3) - clash with S2
-//    DEF_TIM(TIM2, CH1, PA15,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5   UP1-7   D(1, 5, 3) - clash with S2
+    DEF_TIM(TIM2, CH2, PB3,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6   UP1-7   D(1, 6, 3)
-//    DEF_TIM(TIM2, CH2, PB3,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6   UP1-7   D(1, 6, 3)
-
-    DEF_TIM(TIM2, CH3, PB10,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5   UP1-7   D(1, 5, 3) - clash with S2
-    DEF_TIM(TIM8, CH2, PC7,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6   UP1-7   D(1, 6, 3)
-
     DEF_TIM(TIM4, CH1, PB6,   TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0),   // S7   UP1-6   D(1, 0, 2)
     DEF_TIM(TIM4, CH2, PB7,   TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0),   // S8   UP1-6   D(1, 3, 2)