Fixed althold issues in surface mode

iNavFlight · Aug 21, 2024 · 79fa8f6 · 79fa8f6
1 parent 6a20ed4
commit 79fa8f6
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 2 deletions.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2461,6 +2461,12 @@ groups:
         field: baro_epv
         min: 0
         max: 9999
+      - name: acc_weight
+        description: "Determines the accelerometer weight. If set to zero, then its value will be automatically set according to vibration levels and clipping (ranging from 0.3 to 1.0)"
+        default_value: 0.0
+        field: acc_weight
+        min: 0
+        max: 10
 
   - name: PG_NAV_CONFIG
     type: navConfig_t

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -260,6 +260,7 @@ typedef struct positionEstimationConfig_s {
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     float baro_epv;     // Baro position error
 
+    float acc_weight;   // Sets the fixed accelerometer weight. If set to 0, it's value will be determined automatically according to vibrations and clipping
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
 #endif

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -65,6 +65,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT,
         .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT,  // 5 cm/s/s calibration error accepted (0.5% of gravity)
         .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT,
+        .acc_weight = SETTING_ACC_WEIGHT_DEFAULT,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
@@ -385,7 +386,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
     }
     else {
         /* Update acceleration weight based on vibration levels and clipping */
-        updateIMUEstimationWeight(dt);
+        if (positionEstimationConfig()->acc_weight == 0.0f) {
+            updateIMUEstimationWeight(dt);
+        } else {
+            posEstimator.imu.accWeightFactor = positionEstimationConfig()->acc_weight;
+        }
 
         fpVector3_t accelBF;
 

diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c
@@ -150,7 +150,7 @@ void estimationCalculateAGL(estimationContext_t * ctx)
         const float accWeight = navGetAccelerometerWeight();
         posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
         posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
-        posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
+        posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * accWeight;
 
         // Apply correction
         if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {