From 78c2060093ebbff2fa3e3c1a3a10d0a6d9c9d35b Mon Sep 17 00:00:00 2001
From: Sergey <sevrugin@gmail.com>
Date: Thu, 14 Apr 2022 20:56:37 +0300
Subject: [PATCH] first good version

---
 src/main/fc/fc_msp.c                           |  1 +
 src/main/navigation/navigation_pos_estimator.c | 12 +++++++-----
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 08725d4fb0..18c0ee9cc9 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -21,6 +21,7 @@
 #include <string.h>
 #include <math.h>
 
+#include "common/log.h"
 #include "platform.h"
 
 #include "blackbox/blackbox.h"
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 9b54af6fa3..b86ab0a89e 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -537,6 +537,9 @@ 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)) {
@@ -848,17 +851,16 @@ void initializePositionEstimator(void)
     pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
 }
 
+static bool isInitialized = false;
+
 /**
  * Update estimator
  *  Update rate: loop rate (>100Hz)
  */
 void updatePositionEstimator(void)
 {
-    if (ARMING_FLAG(SIMULATOR_MODE)) {
-        posEstimator.est.pos.z = baro.BaroAlt;
-        //return;
-    }
-    static bool isInitialized = false;
+    // todo: why this const is inside function?
+//    static bool isInitialized = false;
 
     if (!isInitialized) {
         initializePositionEstimator();