-
Notifications
You must be signed in to change notification settings - Fork 1.5k
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
Fixed althold issue in surface mode for multirotors #10314
base: master
Are you sure you want to change the base?
Conversation
@@ -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); |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
@@ -385,7 +386,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs) | |||
} | |||
else { | |||
/* Update acceleration weight based on vibration levels and clipping */ | |||
updateIMUEstimationWeight(dt); |
There was a problem hiding this comment.
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.
This appears to be the same as #10308 |
@breadoven what do you think about this one? |
Didn't have too good a look at it so can't really be sure at the moment. |
Any news regarding merging into master? |
@ultrazar I think the issue with merging this is not that your commit won't fix the problem for rangefinders. I assumed you've followed the conversations in other PR's surrounding this issue. And you seem to have a good grasp on the problem. Noticing your comments here and here and here . Have you by chance also made a build that reverted AccWeight changes, To see the effect ? |
Some people, including me, were experiencing problems with maintaining altitude in surface mode using mtf-01 sensor.
It seems that the problem was caused by high vibration levels which decreased the accelerometer weight down to 0.3, which is the minimum. But then, this value is squared in
navigation_pos_estimator_agl.c
, which means it goes down to0.3*0.3=0.09
which is so low that the accelerometer almost becomes unused. Therefore, it was only using the rangefinder measurements but they have so much delay that the drone becomes unable to maintain a good altitude.inav/src/main/navigation/navigation_pos_estimator.c
Line 360 in b5e8b2b
inav/src/main/navigation/navigation_pos_estimator_agl.c
Line 153 in b5e8b2b
I fixed this issue primarily by disabling the dynamic accelerometer weight and setting it to fixed 1.0 ( which is the value it should get when there are normal vibration levels )
I added a new setting to do that,
acc_weight
. By default, it is set to zero and that means that its value is determined automatically (as normal). But you can set it to 1.0 and that means the accelerometer weight will always be 1.0.I particularly tested this version and works well.
Surface mode with 0.3 accelerometer weight:
Surface mode with 1.0 accelerometer weight: