From 82f547859de2d8f42c9455d6a9f3b04310721da6 Mon Sep 17 00:00:00 2001 From: ultrazar Date: Sun, 18 Aug 2024 01:51:17 +0200 Subject: [PATCH] Solved my skyrocket issue by adding 2 new features - Now you can disable the dynamic accelerometer weight to set it to stable 0.3 - Added a temperature compensation for the accelerometer using the baro temperature sensor. (This drastically improved the altHold performance on my quad) --- src/main/fc/settings.yaml | 18 +++++++++++++++ src/main/navigation/navigation.h | 4 ++++ .../navigation/navigation_pos_estimator.c | 23 +++++++++++++++++-- 3 files changed, 43 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c16ce93b21..e9a83d29bcd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2461,6 +2461,24 @@ groups: field: baro_epv min: 0 max: 9999 + - name: dynamic_acc_weight + description: "You can disable or enable the dynamic accelerometer weight ( calculated based on accelerometer vibrations and clipping)" + default_value: ON + field: dynamic_acc_weight + type: bool + - name: temp_correction_a + description: "Some accelerometers may experience drifting caused by changing temperatures, you can use this parameter and temp_correction_b to establish the linear relationship between the accelerometer drifting and the temperature (the baro needs to be in the FC, where the accelerometer is located) the formula is f(x) = ax + b where f(x) is the calculated offset for x temperature in degrees" + default_value: 0 + field: temp_correction_a + min: -900 + max: 900 + - name: temp_correction_b + description: "See temp_correction_a" + default_value: 0 + field: temp_correction_b + min: -900 + max: 900 + - name: PG_NAV_CONFIG type: navConfig_t diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b13..d2a899c4e1b 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -259,6 +259,10 @@ typedef struct positionEstimationConfig_s { float max_eph_epv; // Max estimated position error acceptable for estimation (cm) float baro_epv; // Baro position error + + bool dynamic_acc_weight; // To enable/disable the dynamic accelerometer weighting (relative to vibrations and clipping) + float temp_correction_a; + float temp_correction_b; #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d5a342173d3..902a82368e7 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -64,6 +64,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT, .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) + .dynamic_acc_weight = SETTING_DYNAMIC_ACC_WEIGHT_DEFAULT, + .temp_correction_a = SETTING_TEMP_CORRECTION_A_DEFAULT, + .temp_correction_b = SETTING_TEMP_CORRECTION_B_DEFAULT, .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, @@ -385,7 +388,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs) } else { /* Update acceleration weight based on vibration levels and clipping */ - updateIMUEstimationWeight(dt); + if (positionEstimationConfig()->dynamic_acc_weight) { + updateIMUEstimationWeight(dt); + } else { + posEstimator.imu.accWeightFactor = 0.3f; + } fpVector3_t accelBF; @@ -405,7 +412,19 @@ static void updateIMUTopic(timeUs_t currentTimeUs) /* Read acceleration data in NEU frame from IMU */ posEstimator.imu.accelNEU.x = accelBF.x; posEstimator.imu.accelNEU.y = accelBF.y; - posEstimator.imu.accelNEU.z = accelBF.z; + const float baroTemperature = baroGetTemperature(); + /* Some accelerometers may experience drifting caused by changing temperatures, + you can use these 2 parameters to establish the linear relationship between the accelerometer drifting and the temperature + (the baro needs to be in the FC, where the accelerometer is located) + The formula is the following: f(x) = ax + b + where f(x) is the calculated offset, a is how the offset varies for each degree, x is the barometer temperature in celsius and b would be the offset for 0ºC scenario + What works well for me is: + temp_correction_a = 4.834 + temp_correction_b = -217.53 + */ + const float acc_z_offset = ( positionEstimationConfig()->temp_correction_a * ( baroTemperature / 10.0f ) ) + positionEstimationConfig()->temp_correction_b; // f(x) = ax + b + posEstimator.imu.accelNEU.z = accelBF.z + acc_z_offset; + /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ if (gyroConfig()->init_gyro_cal_enabled) {