diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c16ce93b21..9d36db84f21 100644 --- 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 index 1b7734c8b13..598da52702c 100644 --- 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 index d5a342173d3..bcd8b61619c 100755 --- 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 index 8180b16d65f..62808f54483 100644 --- 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) {