Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Solved my skyrocket issue by adding 2 new features #10308

Closed
wants to merge 1 commit into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions src/main/fc/settings.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 4 additions & 0 deletions src/main/navigation/navigation.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
23 changes: 21 additions & 2 deletions src/main/navigation/navigation_pos_estimator.c
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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) {
Copy link
Collaborator

@mmosca mmosca Aug 18, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure it this is needed. And if it is, it should not be hard coded to 0.3f when off.

updateIMUEstimationWeight(dt);
} else {
posEstimator.imu.accWeightFactor = 0.3f;
}

fpVector3_t accelBF;

Expand All @@ -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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should be using the accelerometer temps, not the baro temps for this. The baro could be very far from the accelerometer and not be exposed to the same local temperature gradients.

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) {
Expand Down