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.

Sign up for GitHub

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

Jump to bottom

Fixed althold issue in surface mode for multirotors #10314

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+14 −2
Open

Fixed althold issue in surface mode for multirotors #10314

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
6 changes: 6 additions & 0 deletions src/main/fc/settings.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions src/main/navigation/navigation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 6 additions & 1 deletion src/main/navigation/navigation_pos_estimator.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,

Expand Down Expand Up @@ -385,7 +386,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
}
else {
/* Update acceleration weight based on vibration levels and clipping */
updateIMUEstimationWeight(dt);
Copy link
Collaborator

Choose a reason for hiding this comment

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

This is changing behavior when not in surface mode.

if (positionEstimationConfig()->acc_weight == 0.0f) {
updateIMUEstimationWeight(dt);
} else {
posEstimator.imu.accWeightFactor = positionEstimationConfig()->acc_weight;
}

fpVector3_t accelBF;

Expand Down
2 changes: 1 addition & 1 deletion src/main/navigation/navigation_pos_estimator_agl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Collaborator

Choose a reason for hiding this comment

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

This feels wrong to me. In AGL mode, the sensor that measures distance to ground should have higher gains than accelerometer. This feels like it was done like this on purpose.

Copy link
Contributor Author

@ultrazar ultrazar Aug 27, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

Let me explain.
Yes, they did it on purpose, but by the time they wrote this code (5 years ago), the way the accelerometer weight worked was different.

inav/src/main/navigation/navigation_pos_estimator.c

Lines 483 to 509 in e64753b

static void estimationPredict(estimationContext_t * ctx)
{
const float accWeight = navGetAccelerometerWeight();
/* Prediction step: Z-axis */
if ((ctx->newFlags & EST_Z_VALID)) {
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
}
/* Prediction step: XY-axis */
if ((ctx->newFlags & EST_XY_VALID)) {
// Predict based on known velocity
posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt;
posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt;
// If heading is valid, accelNEU is valid as well. Account for acceleration
if (navIsHeadingUsable() && navIsAccelerationUsable()) {
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
}
}
}

Originally, its value was fixed and there was a setting for that. Therefore, the weight was squared for vel estimations. But when they changed the way the accelerometer weight works, forgot to update this file. Now the problem is that the accWeight can go down to 0.3 automatically and squared is 0.3*0.3=0.09 which is too low. Some rangefinders like the one in the mtf-01 is too slow taking measurements which makes the navigation very unstable.

posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * accWeight;

// Apply correction
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
Expand Down
Loading