Fixed althold issue in surface mode for multirotors

[ultrazar]

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 to 0.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

float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s

inav/src/main/navigation/navigation_pos_estimator_agl.c

Line 153 in b5e8b2b

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

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:

[ultrazar]

#10025

[mmosca]

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.

[ultrazar]

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.

[mmosca]

This is changing behavior when not in surface mode.

[mmosca]

This appears to be the same as #10308

[mmosca]

@breadoven what do you think about this one?

[breadoven]

@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.

[marcaz]

Any news regarding merging into master?

[Jetrell]

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.
Its whether the original cause of the problem should be reverted. Because its also affecting position estimation and leading to drift, not just the z axis, but xy as well when GNSS precision is lower. And this variably, depending on accelerometer noise, based on the build quality of the copter.

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 .

Have you by chance also made a build that reverted AccWeight changes, To see the effect ?