diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 0cabce055b..28bbd88133 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -178,6 +178,7 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
         displayUpdateMaxChar(instance);
     }
 
+#ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE)) {
 		//some FCs do not power max7456 from USB power 
 		//driver can not read font metadata 
@@ -186,6 +187,7 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
 		//return dummy metadata to let all OSD elements to work in simulator mode
 		instance->maxChar = 512;
 	}
+#endif
 
     if (c > instance->maxChar) {
         return -1;
diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c
index b52d8bd871..9b2c12d466 100644
--- a/src/main/drivers/sound_beeper.c
+++ b/src/main/drivers/sound_beeper.c
@@ -46,9 +46,13 @@ void systemBeep(bool onoff)
     UNUSED(onoff);
 #else
 
-	if (simulatorData.flags & SIMU_MUTE_BEEPER) {
-		onoff = false;
+#ifdef USE_SIMULATOR
+	if (ARMING_FLAG(SIMULATOR_MODE)) {
+		if (simulatorData.flags & SIMU_MUTE_BEEPER) {
+			onoff = false;
+		}
 	}
+#endif
 
     if (beeperConfig()->pwmMode) {
         pwmWriteBeeper(onoff);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 084bcf8baf..476eb6d317 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1734,19 +1734,6 @@ static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const s
     }
 }
 
-static void cliOutput(char *cmdline)
-{
-    // print out servo output
-    cliPrintHashLine("servo");
-    const char *format = "servo %u %d";
-    for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-        cliPrintLinef(format,
-            i,
-            getServoValue(i)
-        );
-    }
-}
-
 static void cliServo(char *cmdline)
 {
     enum { SERVO_ARGUMENT_COUNT = 5 };
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 9857e28c3b..fa18a97d93 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -942,13 +942,26 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     }
 
     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
-    if (isServoOutputEnabled() && ! ARMING_FLAG(SIMULATOR_MODE)) {
+
+#ifdef USE_SMULATOR
+	if (!ARMING_FLAG(SIMULATOR_MODE)) {
+	    if (isServoOutputEnabled()) {
+	        writeServos();
+	    }
+
+	    if (motorControlEnable) {
+	        writeMotors();
+	    }
+	}
+#else
+    if (isServoOutputEnabled()) {
         writeServos();
     }
 
-    if (motorControlEnable && ! ARMING_FLAG(SIMULATOR_MODE)) {
+    if (motorControlEnable) {
         writeMotors();
     }
+#endif
 
     // Check if landed, FW and MR
     if (STATE(ALTITUDE_CONTROL)) {
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 231df20a12..12ccf9b692 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -3474,10 +3474,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 						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.velNED[X] = (int16_t)sbufReadU16(src);
 						gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
 						gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
@@ -3485,25 +3481,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 						gpsSol.eph = 100;
 						gpsSol.epv = 100;
 
-						/*
-						//------------------------
-						gpsSol.flags.validVelNE = 0;
-						gpsSol.flags.validVelD = 0;
-						gpsSol.flags.validEPE = 0;
-
-						debug[0] = gpsSol.velNED[X];
-						debug[1] = gpsSol.velNED[Y];
-						debug[2] = gpsSol.velNED[Z];
-
-						debug[3] = posEstimator.gps.vel.x;
-						debug[4] = posEstimator.gps.vel.y;
-						debug[5] = posEstimator.gps.vel.z;
-
-						debug[6] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
-						debug[7] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse));
-						//------------------------
-						*/
-
 						ENABLE_STATE(GPS_FIX);
 
 						// Feed data to navigation
@@ -3526,16 +3503,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				else
 				{
 					sbufAdvance(src, 2*3);
-					
-					/*
-					debug[0] = (int16_t)sbufReadU16(src);
-					debug[1] = (int16_t)sbufReadU16(src);
-					debug[2] = (int16_t)sbufReadU16(src);
-
-					debug[3] = attitude.values.roll;
-					debug[4] = attitude.values.pitch;
-					debug[5] = attitude.values.yaw;
-					*/
 				}
 
 				acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units
@@ -3551,7 +3518,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
 				if (sensors(SENSOR_BARO))
 				{
-					baro.baroPressure = sbufReadU32(src);
+					baro.baroPressure = (int32_t)sbufReadU32(src);
 					baro.baroTemperature = 2500;
 				}
 				else {
@@ -3573,10 +3540,10 @@ 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)(ARMING_FLAG(ARMED) ? input[INPUT_STABILIZED_THROTTLE] : -500));
+		sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_ROLL);
+		sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_PITCH);
+		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];
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 9936d2638f..fd306db8f7 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -306,9 +306,13 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
     updatePIDCoefficients();
     dynamicLpfGyroTask();
-    if (! ARMING_FLAG(SIMULATOR_MODE)) {
+#ifdef USE_SIMULATOR
+    if (!ARMING_FLAG(SIMULATOR_MODE)) {
         updateFixedWingLevelTrim(currentTimeUs);
     }
+#else
+	updateFixedWingLevelTrim(currentTimeUs);
+#endif
 }
 
 void fcTasksInit(void)
diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
index 9244eeadcb..d3c5410745 100644
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@@ -177,4 +177,13 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
     return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
 }
 
-simulatorData_t simulatorData = { flags: 0, debugIndex: 0 };
\ No newline at end of file
+#ifdef USE_SIMULATOR
+simulatorData_t simulatorData = { 
+	flags: 0, 
+	debugIndex: 0,
+	INPUT_STABILIZED_ROLL: 0,
+	INPUT_STABILIZED_PITCH: 0,
+	INPUT_STABILIZED_YAW: 0,
+	INPUT_STABILIZED_THROTTLE: 0
+};
+#endif
\ No newline at end of file
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 8ce3f5890b..f586d84ef4 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -165,7 +165,7 @@ typedef enum {
 
 flightModeForTelemetry_e getFlightModeForTelemetry(void);
 
-
+#ifdef USE_SIMULATOR
 typedef enum {
 	SIMU_ENABLE				= (1 << 0),
 	SIMU_SIMULATE_BATTERY	= (1 << 1),
@@ -177,9 +177,14 @@ typedef enum {
 typedef struct {
 	simulatorFlags_t flags;
 	uint8_t debugIndex;
+	int16_t INPUT_STABILIZED_ROLL;
+	int16_t INPUT_STABILIZED_PITCH;
+	int16_t INPUT_STABILIZED_YAW;
+	int16_t INPUT_STABILIZED_THROTTLE;
 } simulatorData_t;
 
 extern simulatorData_t simulatorData;
+#endif
 
 uint32_t enableFlightMode(flightModeFlags_e mask);
 uint32_t disableFlightMode(flightModeFlags_e mask);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index dcd07dacac..ae5d7d04ea 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2476,7 +2476,7 @@ groups:
         description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]"
         default_value: 50
         field: general.land_minalt_vspd
-        min: 0
+        min: 50
         max: 500
       - name: nav_land_maxalt_vspd
         description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]"
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 521b370aee..0f6b2ecff1 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -327,9 +327,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 // Normalize to unit vector
                 vectorNormalize(&vMag, &vMag);
 
+#ifdef USE_SIMULATOR
 				if (ARMING_FLAG(SIMULATOR_MODE)) {
 					imuSetMagneticDeclination(0);
 				}
+#endif
 
                 // 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)
@@ -459,11 +461,13 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
 STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 {
+#ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE) && ((simulatorData.flags & SIMU_USE_SENSORS) == 0)) {
 		imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
 		imuComputeRotationMatrix();
 	}
 	else
+#endif
 	{
 		/* Compute pitch/roll angles */
 		attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 5b828d5671..c68cd4ec0a 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -101,7 +101,6 @@ void pgResetFn_servoParams(servoParam_t *instance)
     }
 }
 
-int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
 int16_t servo[MAX_SUPPORTED_SERVOS];
 
 static uint8_t servoRuleCount = 0;
@@ -246,6 +245,8 @@ 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];
@@ -319,6 +320,13 @@ void servoMixer(float dT)
     input[INPUT_RC_CH16]     = GET_RX_CHANNEL_INPUT(AUX12);
 #undef GET_RX_CHANNEL_INPUT
 
+#ifdef USE_SIMULATOR
+		simulatorData.INPUT_STABILIZED_ROLL = input[INPUT_STABILIZED_ROLL];
+		simulatorData.INPUT_STABILIZED_PITCH = input[INPUT_STABILIZED_PITCH];
+		simulatorData.INPUT_STABILIZED_YAW = input[INPUT_STABILIZED_YAW];
+		simulatorData.INPUT_STABILIZED_THROTTLE = input[INPUT_STABILIZED_THROTTLE];
+#endif
+
     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
         servo[i] = 0;
     }
@@ -588,12 +596,3 @@ bool isMixerUsingServos(void)
 {
     return mixerUsesServos;
 }
-
-int16_t getServoValue(uint32_t n)
-{
-    if (n >= MAX_SUPPORTED_SERVOS) {
-        return;
-    }
-
-    return servo[n];
-}
\ No newline at end of file
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 47a0e04ce3..d2990650a4 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -150,7 +150,6 @@ typedef struct servoMetadata_s {
 } servoMetadata_t;
 
 extern int16_t servo[MAX_SUPPORTED_SERVOS];
-extern int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
 
 bool isServoOutputEnabled(void);
 void setServoOutputEnabled(bool flag);
@@ -160,5 +159,4 @@ void loadCustomServoMixer(void);
 void servoMixer(float dT);
 void servoComputeScalingFactors(uint8_t servoIndex);
 void servosInit(void);
-int getServoCount(void);
-int16_t getServoValue(uint32_t n);
+int getServoCount(void);
\ No newline at end of file
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 2f1b586c29..9ef80af69f 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -344,12 +344,14 @@ bool gpsUpdate(void)
         return false;
     }
 
+#ifdef USE_SIMULATOR
     if (ARMING_FLAG(SIMULATOR_MODE)) {
         gpsUpdateTime();
         gpsSetState(GPS_RUNNING);
 		sensorsSet(SENSOR_GPS);
 		return gpsSol.flags.hasNewData;
     }
+#endif
 #ifdef USE_FAKE_GPS
     return gpsFakeGPSUpdate();
 #else
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 8ab3671e9e..e7dbbd368d 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -846,8 +846,10 @@ static const char * osdArmingDisabledReasonMessage(void)
             FALLTHROUGH;
         case ARMED:
             FALLTHROUGH;
+#ifdef USE_SIMULATOR
         case SIMULATOR_MODE:
             FALLTHROUGH;
+#endif
         case WAS_EVER_ARMED:
             break;
     }
@@ -4410,12 +4412,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
                     tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
                     messages[messageCount++] = messageBuf;
-                } else if (NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE) {
-                    // Countdown display for RTH minutes
-                    char buf[6];
-                    osdFormatDistanceSymbol(buf, GPS_distanceToHome * 60 * 60 * 3.6/ gpsSol.groundSpeed , 0);
-                    tfp_sprintf(messageBuf, "EN ROUTE TO HOME (%s)", buf);
-                    messages[messageCount++] = messageBuf;
                 } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                     if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT);
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index 81dac367aa..9b9b7b08b5 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -257,7 +257,6 @@
 #define MSP_RC_DEADBAND          125    //out message         deadbands for yaw alt pitch roll
 #define MSP_SENSOR_ALIGNMENT     126    //out message         orientation of acc,gyro,mag
 #define MSP_LED_STRIP_MODECOLOR  127    //out message         Get LED strip mode_color settings
-#define MSP_TELEMETRY            128    //out message         Get controller telemetry
 
 #define MSP_SET_RAW_RC           200    //in message          8 rc chan
 #define MSP_SET_RAW_GPS          201    //in message          fix, numsat, lat, lon, alt, speed
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 7693a162de..2c721f4f00 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -146,8 +146,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     //  speedWeight = 1.0 : pitch will only control airspeed and won't control altitude
     //  speedWeight = 0.5 : pitch will be used to control both airspeed and altitude
     //  speedWeight = 0.0 : pitch will only control altitude
-    const float speedWeight = 0.7f; // no speed sensing for now
-//    const float speedWeight = 1.0f - constrainf(fabs(demSPE - estSPE) / GRAVITY_MSS, 0.0f, 1.0f); // no speed sensing for now
+    const float speedWeight = 0.0f; // no speed sensing for now
 
     const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight;
     const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight;
@@ -537,7 +536,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         if (navStateFlags & NAV_CTL_LAND) {
             // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
-            throttleCorrection = minThrottleCorrection;
         } else {
 #endif
             throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 8866e4c476..708cf538c2 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -448,9 +448,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 
         /* If calibration is incomplete - report zero acceleration */
         if (gravityCalibrationComplete()) {
+#ifdef USE_SIMULATOR
             if (ARMING_FLAG(SIMULATOR_MODE)) {
                 posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS;
             }
+#endif
             posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
         }
         else {
@@ -537,11 +539,6 @@ static void estimationPredict(estimationContext_t * ctx)
         posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
         posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
     }
-	/*
-    if (ARMING_FLAG(SIMULATOR_MODE)) {
-        posEstimator.est.pos.z = baro.BaroAlt;
-    }
-	*/
 
     /* Prediction step: XY-axis */
     if ((ctx->newFlags & EST_XY_VALID)) {
@@ -561,15 +558,6 @@ static void estimationPredict(estimationContext_t * ctx)
 
 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, 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
-    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
-    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
-    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
-
     if (ctx->newFlags & EST_BARO_VALID) {
         timeUs_t currentTimeUs = micros();
 
@@ -640,6 +628,15 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         return true;
     }
 
+    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 2, imuMeasuredAccelBF.z);        // 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
+    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
+    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
+    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
+
     return false;
 }
 
@@ -861,7 +858,6 @@ void initializePositionEstimator(void)
     pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
 }
 
-static bool isInitialized = false;
 
 /**
  * Update estimator
@@ -869,8 +865,7 @@ static bool isInitialized = false;
  */
 void updatePositionEstimator(void)
 {
-    // todo: why this const is inside function?
-//    static bool isInitialized = false;
+    static bool isInitialized = false;
 
     if (!isInitialized) {
         initializePositionEstimator();
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index b14ff6e871..3bfa76edb5 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -503,18 +503,19 @@ float accGetMeasuredMaxG(void)
 
 void accUpdate(void)
 {
+#ifdef USE_SIMULATOR
     if (ARMING_FLAG(SIMULATOR_MODE)) {
 		//output: acc.accADCf
 		//unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
         return;
     }
+#endif
     if (!acc.dev.readFn(&acc.dev)) {
         return;
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         accADC[axis] = acc.dev.ADCRaw[axis];
-        DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
     }
 
     performAcclerationCalibration();
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index c174285a3b..fcec44d287 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -276,11 +276,14 @@ uint32_t baroUpdate(void)
             if (baro.dev.start_ut) {
                 baro.dev.start_ut(&baro.dev);
             }
-            if (! ARMING_FLAG(SIMULATOR_MODE)) {
+#ifdef USE_SIMULATOR
+            if (!ARMING_FLAG(SIMULATOR_MODE)) {
 				//output: baro.baroPressure, baro.baroTemperature
                 baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
             }
-
+#else
+                baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
+#endif
             state = BAROMETER_NEEDS_SAMPLES;
             return baro.dev.ut_delay;
         break;
@@ -320,12 +323,16 @@ int32_t baroCalculateAltitude(void)
         }
 
         baro.BaroAlt = 0;
-        baro.BaroMslAlt = 0;
     }
     else {
+#ifdef HIL
+        if (hilActive) {
+            baro.BaroAlt = hilToFC.baroAlt;
+            return baro.BaroAlt;
+        }
+#endif
         // calculates height from ground via baro readings
-        baro.BaroMslAlt = pressureToAltitude(baro.baroPressure);
-        baro.BaroAlt = baro.BaroMslAlt - baroGroundAltitude;
+        baro.BaroMslAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
    }
 
     return baro.BaroAlt;
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 29a5f8ad5f..c2722e7d3c 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -41,9 +41,8 @@ typedef enum {
 typedef struct baro_s {
     baroDev_t dev;
     int32_t BaroAlt;
-    int32_t BaroMslAlt;
     int32_t baroTemperature;            // Use temperature for telemetry
-    uint32_t baroPressure;               // Use pressure for telemetry
+    int32_t baroPressure;               // Use pressure for telemetry
 } baro_t;
 
 extern baro_t baro;
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 662fd53c0c..3f93f4e494 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -290,13 +290,16 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
             vbat = 0;
             break;
     }
-
-	if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
-		vbat = 1260;
-		batteryFullVoltage = 1210;
-		batteryWarningVoltage = 1020;
-		batteryCriticalVoltage = 960;
+#ifdef USE_SIMULATOR
+	if (!ARMING_FLAG(SIMULATOR_MODE)) {
+		if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
+			vbat = 1260;
+			batteryFullVoltage = 1210;
+			batteryWarningVoltage = 1020;
+			batteryCriticalVoltage = 960;
+		}
 	}
+#endif
     if (justConnected) {
         pt1FilterReset(&vbatFilterState, vbat);
     } else {
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index b4d435c9ce..5461841b01 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -356,11 +356,12 @@ bool compassIsCalibrationComplete(void)
 
 void compassUpdate(timeUs_t currentTimeUs)
 {
+#ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE)) {
 		magUpdatedAtLeastOnce = 1;
 		return;
 	}
-
+#endif
     static sensorCalibrationState_t calState;
     static timeUs_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];
diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
index 2a3ac6e9df..d75df94a35 100644
--- a/src/main/sensors/diagnostics.c
+++ b/src/main/sensors/diagnostics.c
@@ -63,11 +63,11 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
 
 hardwareSensorStatus_e getHwCompassStatus(void)
 {
+#ifdef USE_SIMULATOR
 	if (ARMING_FLAG(SIMULATOR_MODE)) {
 		return HW_SENSOR_OK;
-		return;
 	}
-
+#endif
 #if defined(USE_MAG)
     if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
         if (compassIsHealthy()) {
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 77d521d0fa..bb64f0ea16 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -437,12 +437,6 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
 
 void FAST_CODE NOINLINE gyroFilter()
 {
-	/*
-    if (ARMING_FLAG(SIMULATOR_MODE)) {
-        return;
-    }
-	*/
-
     if (!gyro.initialized) {
         return;
     }
@@ -505,12 +499,13 @@ void FAST_CODE NOINLINE gyroFilter()
 
 void FAST_CODE NOINLINE gyroUpdate()
 {
+#ifdef USE_SIMULATOR
     if (ARMING_FLAG(SIMULATOR_MODE)) {
 		//output: gyro.gyroADCf[axis]
 		//unused: dev->gyroADCRaw[], dev->gyroZero[];
         return;
     }
-
+#endif
     if (!gyro.initialized) {
         return;
     }
diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h
index 1fd2a92c73..71da34c814 100644
--- a/src/main/target/HGLRCF722/target.h
+++ b/src/main/target/HGLRCF722/target.h
@@ -127,19 +127,19 @@
 #define UART2_TX_PIN            PA2
 #define UART2_RX_PIN            PA3
 
-//#define USE_UART3
-//#define UART3_TX_PIN            PB10
-//#define UART3_RX_PIN            PB11
+#define USE_UART3
+#define UART3_TX_PIN            PB10
+#define UART3_RX_PIN            PB11
 
 #define USE_UART4
 #define UART4_TX_PIN            PA0
 #define UART4_RX_PIN            PA1
 
-//#define USE_UART6
-//#define UART6_TX_PIN            PC6
-//#define UART6_RX_PIN            PC7
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
      
-#define SERIAL_PORT_COUNT       4
+#define SERIAL_PORT_COUNT       6
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 7c9c282812..d3cfa2d3b3 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -193,4 +193,6 @@
 #define USE_TELEMETRY_HOTT
 #define USE_HOTT_TEXTMODE
 
+#define USE_SIMULATOR
+
 #endif