[RTH] Refactor linear RTH descent; Fix initial altitude bug

[digitalentity]

Refactor linear descent for better readability. Fix the bug with posControl.homeWaypointAbove.pos.z not being updated correctly.

[wilco1967]

Thanks for the quick update.

Just test flew the de_refractor_linear_descent branch.
Works slightly better, but not there yet...
It does kind-of a linear descent, but ONLY if it was flying towards home already,

If flying away from the home point, when enabling RTH, it makes a steep dive at max dive angle,, while turning towards home. After some time, it powers up and climbs again making up a lot of altitude lost during the initial dive. After that, it settles for a nice steady descent back to home.
this is not PID controller oscillation... it clearly gets a wrong setpoint somehow, Alt controller is doing a perfect job otherwise.

If it was flying in the general direction of home already when engaging RTH, it immediately settles in a nice steady descent like it is supposed to do.

Seems like it somehow doesn't like when the distance from home is still increasing when calculating the climb/descent angle / alt setpoint.

here is my dump
iNav 2.2.0dev - MatekF722 - 20190602 RTH descent.txt

and here is the BB log
LOG00001.TXT

Hope this helps to find the issue...

Thanks a lot !

[digitalentity]

@wilco1967 it certainly helps. Fix pending.

[digitalentity]

Interestingly, it shouldn't enter the steep dive - instead it should be climbing above the target altitude, so I'm not sure what's actually happening there 🤔

[digitalentity]

@wilco1967 can you please test the current version as well?

Also, linear descent seems to affect final stages of RTH as well. Needs further work.

[wilco1967]

Is this in any way perhaps related to #3600 ?
There I also saw incorrect altitude control, if using the default values of nav_fw_pos_z_p / i / d.
As far as I'm aware, that issue is still open.

Currently flying with
set nav_fw_pos_z_p = 50
set nav_fw_pos_z_i = 0
set nav_fw_pos_z_d = 0
working perfect with those values in 'normal' RTH / Althold / Cruise / WP, even in 2.2 (except this strange RTH linear descent behaviour).

Haven't tried current defaults (40/5/10) in current version yet....
Hopefully can try tomorrow with latest version and either set of pos_z_p/i/d settings.
For now, rain expected.

Haven't noticed any strange behaviour when reaching home position. It started to circle (or it least, it said so), but I switched off RTH before I really could notice anything strange (my home position is close to some houses, and I don't want it start circling over those houses).

[digitalentity]

@wilco1967 this should now be ready to be tested. Note that I've changed a lot of code and RTH may be totally broken (unlikely, but possible).

[wilco1967]

alright, thanks a lot !
I'll give it a go, probably tomorrow..... I'll let you know my findings a.s.a.p.

Any idea on if the nav_fw_pos_z_p / i / d settings would make any difference ?

Hopefully it doesn't dive below LOS like it did before..... I try higher up / not so far out, just to make sure I don't end up below the radio horizon and loose video if it does a crazy dive again.....
As long as it comes out of RTH when I tell it to, I should even be able to fly it home on 3DCRS on telemetry only.... No worries....

[shellixyz]

I think there is an issue with updateDesiredRTHAltitude() and the NAV_RTH_MAX_ALT, NAV_RTH_AT_LEAST_ALT and NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT altitude modes.

updateDesiredRTHAltitude() is called regularly (10Hz) by updatePositionEstimator() as soon as the position is valid and each time it is called and the aircraft is armed and not executing RTH posControl.rthState.rthInitialAltitude will be increased by navConfig()->general.rth_altitude in the case where posControl.rthState.rthInitialAltitude + navConfig()->general.rth_altitude is greater than posControl.actualState.abs.pos.z so as soon as the aircraft is lower than the set RTH altitude.

posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);

The fix is probably to replace posControl.rthState.rthInitialAltitude in the rvalues by posControl.rthState.homePosition.pos.z

[digitalentity]

Well spotted, thanks!

[wilco1967]

Ok, tesflew latest version edf2ca0, and it's nearly perfect !

on an outgoing flight, when enabling RTH, it makes a turn, keeping more or less the same altitude. perhaps even (very) slightly climbing. Once it completed the turn, it settles in to a nice steady descent towards home. When returning home, it start circling at 150 meters. Perfect

on an incoming flight, when enabling RTH, it immediately settles into a nice descent home. Also Perfect.

When RTH from lower as 150 meter, it makes a climb, and then returns at 150 meters... Again perfect.

when triggering a failsafe, it performs a FS RTH as normal. Perfect.

Doesn't seem to matter if RTH is enabled from cruise, or angle, or manual. Works fine.

Only one slight issue remaining (while we are nitpicking...)
On a FS (radio switched off), it performs a FS/ RTH . When I set the switches on the radio to RTH, switch back on the Tx, and wiggle the sticks to get out of FS, it goes to 'normal' RTH as expected, but when doing so, it starts to climb back to the same altitude as where the FS occured, before descending again on the normal slope. A quick toggle to cruise (or whatever), and back to RTH, resets the altitude setpoint, and from that point it continues descending as expected.

So pretty much perfect ! Thanks a lot !
guess that last little item shouldn't be too difficult to solve ?

Any chance of getting the same functionallity during WP mode ? (or is that reserved for 2.3 ;-)

Thanks !

Ok, here's the BB
LOG00002.TXT

Diff as before.

Oh, one more thing I noticed.... on one occasion, when enabling RTH, it turned right to come home for approx 270 deg, while a turn left would have been only 90 deg.... Perhaps this is because of my switch layout, where I need to switch through Poshold to get to RTH... Most of the times it takes the correct turn left/right. Added a delay in OpenTX so I can switch straight to RTH without commanding poshold even for a brief period. Will find out if that solves it.

[digitalentity]

@wilco1967 I suggest we have a separate bug for the last item you described. It's quite a bit of an edge case. I think we're good enough with linear descent as it is now. Please open a new issue so we can track the FS/RTH bug

[wilco1967]

Ok, done.....
This hasn't made it into RC2 as far as I can see, but I certainly think it is good enough for RC3 / final 2.2

Perhaps a warning can be added to RC2 release notes, that linear descent in RC2 is still faulty...
Someone may have a scare if they try linear descent, and it makes a sudden unexpected dive behind some obstacles, causing loss of video.... The faulty version still performs a RTH, but at RTH_altitude, so not a big problem...

[digitalentity]

For scope - the RTH/Failsafe altitude issue is tracked in #4792

[digitalentity]

I'm merging this. Looks like I didn't break anything critical here. Merging.

[Mckenzie74]

today I had the same behaviour described by @wilco1967 (the 'small issue'). FS_RTH with radio switched off, nice descending path to home, set the switches on the radio to RTH, switch back on the Tx and wiggle the sticks to get out of FS, it went to 'normal' RTH but started climbing back to the same altitude as where the FS occured (a little higher), before descending again on the normal slope. Nothing major, but it's still there. Inav 2.2.1 on a matek 411fw so no logs.