adjustments

iNavFlight · Oct 27, 2023 · b911185 · b911185
1 parent eb6634d
commit b911185
Show file tree

Hide file tree

Showing 4 changed files with 4 additions and 21 deletions.
diff --git a/docs/Settings.md b/docs/Settings.md
@@ -1922,16 +1922,6 @@ Decay coefficient for estimated velocity when GPS reference for position is lost
 
 ---
 
-### inav_w_xyz_acc_p
-
-_// TODO_
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 1.0 | 0 | 1 |
-
----
-
 ### inav_w_z_baro_p
 
 Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2366,11 +2366,6 @@ groups:
         field: w_xy_res_v
         min: 0
         max: 10
-      - name: inav_w_xyz_acc_p
-        field: w_xyz_acc_p
-        min: 0
-        max: 1
-        default_value: 1.0
       - name: inav_w_acc_bias
         description: "Weight for accelerometer drift estimation"
         default_value: 0.01

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -203,7 +203,6 @@ typedef struct positionEstimationConfig_s {
     float w_xy_res_v;
 
     float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
-    float w_xyz_acc_p;
 
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     float baro_epv;     // Baro position error

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
-        .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
-
         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
 
         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
@@ -389,7 +387,8 @@ static bool gravityCalibrationComplete(void)
 
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
-
+#define ACC_VIB_FACTOR_S 1.0f
+#define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
 {
     static float acc_clip_factor = 1.0f;
@@ -403,15 +402,15 @@ static void updateIMUEstimationWeight(const float dt)
         acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
     }
     // Update accelerometer weight based on vibration levels and clipping
-    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),1.0f,4.0f),1.0f,2.0f,1.0f,0.3f); // g/s
+    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s
     posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
     // DEBUG_VIBE[0-3] are used in IMU
     DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
 }
 
 float navGetAccelerometerWeight(void)
 {
-    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    const float accWeightScaled = posEstimator.imu.accWeightFactor;
     DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
 
     return accWeightScaled;