Inav: nav_rth_home_altitude ignored on RTH after WP Mission

[peteoz]

Current Behavior

when RTH is enabled after last WP is reached in WP MIssion mode, it will not descend to nav_rth_home_altitude, instead it keeps loitering at nav_rth_altitude

Steps to Reproduce

1. load mission with RTH enabled after last WP
2. execute and wait for mission to complete
3. watch RTH altitude
4. after reaching home point check that it doesn't descend to nav_rth_home_altitude

Expected behavior

model should descend to nav_rth_home_altitude after reaching home point

Suggested solution(s)

model should descend to nav_rth_home_altitude after reaching home point

Additional context

wp

wp 11 valid

wp 0 1 -248767500 1338253000 10000 2250 0
wp 1 1 -248760000 1338250000 10000 2250 0
wp 2 1 -248716799 1338319100 10000 2250 0
wp 3 1 -248650900 1338312800 10000 2250 0
wp 4 1 -248604799 1338353100 10000 2250 0
wp 5 1 -248642100 1338368800 10000 2250 0
wp 6 1 -248647304 1338304348 10000 2250 0
wp 7 1 -248721095 1338322002 10000 2250 0
wp 8 1 -248797326 1338256159 10000 2250 0
wp 9 1 -248772767 1338243837 10000 2250 0
wp 10 4 0 0 0 0 165

set nav_rth_altitude = 10000
set nav_rth_home_altitude = 7000

---

version

INAV/MATEKF405SE 2.2.0 May 26 2019 / 19:56:35 (a713b63e4)

GCC-7.3.1 20180622 (release) [ARM/embedded-7-branch revision 261907]

[issue-label-bot[bot]]

Issue-Label Bot is automatically applying the label BUG to this issue, with a confidence of 0.97. Please mark this comment with :thumbsup: or :thumbsdown: to give our bot feedback!

Links: app homepage, dashboard and code for this bot.

[hali9]

Not bug, new feature rather, because this is implement only for RTH in NAV_STATE_RTH_HOVER_ABOVE_HOME and not in WPRTH.
NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME should be added.

[Airwide]

I'm working on this feature.