inav7.1.1 with optflow senser（micoair MTF-01） can't hold altitude on althold mode

[KONG8691]

Current Behavior

blackbox_log_2024-05-12_214254.TXT

test1.TXT

test2.txt

the drone hover above 3-4 meters, when i switch on altthold mode ,the drone decrease altitude until it almost touch the ground,and it throttles up and increases altitude to 6-7meter, and than decrease slowly.
I check the blackbox file ,when I switch on althold mode, navTgtPos is set 0.63m, when I switch off althold mode ,navTgtPos is set at real altitude.

the drone hover above 17-18 meters,althold mode works will.

when the drone is auto landing on rth mode,it decrease altitude to 17 meters,and it hover and do not land continuously.

Steps to Reproduce

1.hover above 3-4 meters
2.swith on angle/nav poshold/nav althold/heading hold/surface mode

Expected behavior

the drone can hover smoothly

Suggested solution(s)

Additional context

INAV_7.1.1_cli_ERDOG_10_20240513_001143.txt

---

INAV/HAKRCF722V2 7.1.1 May 6 2024 / 11:32:50 (dd91a87)

GCC-10.3.1 20210824 (release)

• FC Board name and vendor: HAKRC F722V2
• INAV version string: 7.1.1

[sensei-hacker]

What do you have max_surface_altitude set to?

[KONG8691]

What do you have max_surface_altitude set to?
I set inav_max_surface_altitude = 200

[breadoven]

I can't help but think there's something not quite right with the nav altitude estimations. I had problems the other day with Poshold (no surface) with it sinking to the ground even after you'd added a lot of extra throttle, wasn't stable at all. The log showed navPos[2] and navVel[2] moving in opposite directions for way too long, i.e. Pos increasing whilst Vel was -ve and vice versa, which makes no sense. In fact navVel[2] was indicating a consistent climb of almost 1m/s even before arming according to the OSD DVR.

The following screenshot from Test2 above shows navPos[2] really doesn't align well with the baro altitude or the raw rangefinder reading, they both indicate a climb but navPos[2] barely increases at all. In fact the Rangefinder drops out with navPos[2] still only reading a little over a meter.

In the next screenshot from the same test navPos[2] suddenly shoots up from 3.4m to over 6m in 0.2s even though the max navVel[2] was only 1.3 m/s. The baro barely changes when this happens. This looks pretty odd. There doesn't appear to be any issue with vibration either to explain this.

[sensei-hacker]

Okay so you have it set to ignore the rangefinder above 2 meters. If you want to fly 3-4 meters above the ground, you may want to allow the rangefinder to work at that altitude. Perhaps set that to about 600 or 650.

[breadoven]

@KONG8691 Try setting inav_use_gps_no_baro to OFF and test again.

[KONG8691]

@KONG8691 Try setting inav_use_gps_no_baro to OFF and test again.

I had try this setting,it is not work

[ultrazar]

So I think I found the problem in the althold when using the rangefinder in the latest INAV firmware.
Lately I've been trying to use the surface mode with the mtf oplfow sensor and I had the same issue, failing at maintaining the altitude. But I solved it by disabling the dynamic accelerometer weight and setting it to fixed 1.0 value.
The problem @KONG8691 is facing is actually due to high vibration levels on his quads ( I see more than 8000 measured accVib ). This causes the dynamic accelerometer weight to go down to 0.3 (which is the minimum).

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

Then, in the navigation_pos_estimator_agl.c this value is squared, so it goes down to 0.3*0.3=0.09 which is an extremely low weight. This means that the accelerometer is practically unused when calculating the altitude. Therefore, only the rangefinder is used to calculate z estimations, but then it seems that the MTF-01 has a significant delay in making measurements, so it means that when the drone realizes it is going up it's too late because it went out of the maximum altitude (200cm).

inav/src/main/navigation/navigation_pos_estimator_agl.c

Line 153 in b5e8b2b

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

Here you can see how my drone started to increase the throttle but the sensor takes more than 1 second to realize it is going up ( because it is using neither the accelerometer data nor barometer data).

I'm not an INAV developer and I'm not used to this repo but I'll probably make a PR later on with the changes I made to solve this issue. 🙂

[ultrazar]

Take a look at the end of this #10308 PR where I explain how the dynamic accelerometer weight just messes up altitude estimations in some quads