Inav: 3D cruise altitude setpoint not tracking after waypoint

[wilco1967]

INAV/MATEKF722 2.0.0 Jul 14 2018 / 18:30:04 (c54f11317)
Custom compiled latest development version

Behavior

Mini Talon plane

Problem is, when switching from a WP mission, during a change in altitude, and I switch back to 3CRS, the plane tries to go back to the earlier WP altitude....
While it is climbing (or dropping), it doesn't respond to elevator stick command (or it tries to override it).... once it has reached its (faulty) target altitude, the elevator stick works normal again.
It only seems to happen from WP (during an WP commanded altitude change) to 3CRS.

Seems like the target altitude for 3CRS is not properly tracking the current altitude (at least, that's what I think should happen), while switching from WP mode to 3CRS (and possible any althold mode ?).

A quick switch to level mode, and back to 3CRS resets the altitude target setpoint, and all works fine again.

This MAY have be an old problem, which only becomes obvious since today the defaults nav_fw_pos_z_p / i / d where changed. Could be unrelated however....
set nav_fw_pos_z_p = 40
set nav_fw_pos_z_i = 5
set nav_fw_pos_z_d = 10

prior I had 50 / 0 / 0 (old defaults_)

Tomorrow I'll try with old settings (50/0/0), and see if it is gone.
Never experienced this behaviour before.

BTW: with the new settings, it holds altitude pretty good (but so did the old values), as long as it has the right setpoint.... ;-)
The althold controller itself works just fine.... it's only the target setpoint thats wrong sometimes.

dump:
iNav 2.0.0 - MatekF722 - 20180714-tune.txt

blackbox log
https://drive.google.com/open?id=1Goeu1y1PXcxb8c2EzB82suB68G2QwUYl
interesting part starts after aprox 21 minutes in the blackbox log.

[stronnag]

Mode transitions

$ inav_modes.rb ~/dl/LOG00002altholdSP.TXT
iNav version = 2.0.0 (states eq 2.0.0)
Time(s) Elapsed(s)  State
 236.0  (   0.0)    nav_state_launch_wait (26)
 239.1  (   3.2)    nav_state_launch_in_progress (28)
 243.3  (   7.4)    nav_state_idle (1)
 247.9  (  11.9)    nav_state_waypoint_in_progress (17)
 297.5  (  61.5)    nav_state_poshold_3d_in_progress (7)
 297.5  (  61.6)    nav_state_rth_head_home (10)
 328.8  (  92.9)    nav_state_idle (1)
 330.8  (  94.9)    nav_state_cruise_3d_in_progress (33)
 331.5  (  95.6)    nav_state_waypoint_in_progress (17)
1515.6  (1279.7)    nav_state_cruise_3d_in_progress (33)
1533.6  (1297.6)    nav_state_cruise_3d_adjusting (34)
1540.8  (1304.8)    nav_state_cruise_3d_in_progress (33)
1583.3  (1347.3)    nav_state_waypoint_in_progress (17)
1588.2  (1352.2)    nav_state_cruise_3d_in_progress (33)
1595.1  (1359.2)    nav_state_waypoint_in_progress (17)
1598.8  (1362.8)    nav_state_cruise_3d_in_progress (33)
1615.3  (1379.3)    nav_state_cruise_3d_adjusting (34)
1616.3  (1380.4)    nav_state_cruise_3d_in_progress (33)
1629.4  (1393.5)    nav_state_idle (1)
1630.8  (1394.8)    nav_state_cruise_3d_in_progress (33)
1635.4  (1399.4)    nav_state_cruise_3d_adjusting (34)
1638.8  (1402.8)    nav_state_cruise_3d_in_progress (33)
1640.3  (1404.4)    nav_state_cruise_3d_adjusting (34)
1641.6  (1405.6)    nav_state_cruise_3d_in_progress (33)
1655.1  (1419.1)    nav_state_poshold_3d_in_progress (7)
1655.1  (1419.1)    nav_state_rth_head_home (10)
1661.2  (1425.3)    nav_state_poshold_3d_in_progress (7)
1661.3  (1425.3)    nav_state_cruise_3d_in_progress (33)
1666.9  (1430.9)    nav_state_cruise_3d_adjusting (34)
1673.0  (1437.1)    nav_state_cruise_3d_in_progress (33)
1679.9  (1443.9)    nav_state_waypoint_in_progress (17)
1686.2  (1450.2)    nav_state_cruise_3d_in_progress (33)
1688.6  (1452.7)    nav_state_cruise_3d_adjusting (34)
1688.9  (1453.0)    nav_state_cruise_3d_in_progress (33)
1689.0  (1453.1)    nav_state_cruise_3d_adjusting (34)
1689.1  (1453.2)    nav_state_cruise_3d_in_progress (33)
1734.1  (1498.2)    nav_state_cruise_3d_adjusting (34)
1738.0  (1502.0)    nav_state_cruise_3d_in_progress (33)
1754.3  (1518.4)    nav_state_rth_head_home (10)
1762.1  (1526.1)    nav_state_poshold_3d_in_progress (7)
1762.1  (1526.2)    nav_state_cruise_3d_in_progress (33)
1770.6  (1534.6)    nav_state_waypoint_in_progress (17)
1775.8  (1539.8)    nav_state_cruise_3d_in_progress (33)
1803.5  (1567.6)    nav_state_waypoint_in_progress (17)
1815.9  (1579.9)    nav_state_cruise_3d_in_progress (33)
1822.2  (1586.2)    nav_state_cruise_3d_adjusting (34)
1822.3  (1586.3)    nav_state_cruise_3d_in_progress (33)
1835.1  (1599.1)    nav_state_waypoint_in_progress (17)
1838.1  (1602.2)    nav_state_cruise_3d_in_progress (33)
1838.6  (1602.7)    nav_state_idle (1)
1839.6  (1603.6)    nav_state_cruise_3d_in_progress (33)
1843.7  (1607.7)    nav_state_cruise_3d_adjusting (34)
1849.8  (1613.8)    nav_state_cruise_3d_in_progress (33)
1852.1  (1616.1)    nav_state_idle (1)
1860.9  (1624.9)    nav_state_cruise_3d_in_progress (33)
1875.2  (1639.3)    nav_state_idle (1)
1886.7  (1650.7)    nav_state_cruise_3d_in_progress (33)
1938.8  (1702.9)    nav_state_waypoint_in_progress (17)
1953.2  (1717.2)    nav_state_cruise_3d_in_progress (33)
1955.7  (1719.8)    nav_state_cruise_3d_adjusting (34)
1955.8  (1719.8)    nav_state_cruise_3d_in_progress (33)
1973.0  (1737.0)    nav_state_cruise_3d_adjusting (34)
1973.9  (1737.9)    nav_state_cruise_3d_in_progress (33)
1975.1  (1739.2)    nav_state_cruise_3d_adjusting (34)
1975.3  (1739.3)    nav_state_cruise_3d_in_progress (33)
1978.2  (1742.3)    nav_state_cruise_3d_adjusting (34)
1978.7  (1742.7)    nav_state_cruise_3d_in_progress (33)
1986.9  (1750.9)    nav_state_idle (1)
1987.6  (1751.7)    nav_state_cruise_3d_in_progress (33)
1990.2  (1754.2)    nav_state_cruise_3d_adjusting (34)
1990.3  (1754.4)    nav_state_cruise_3d_in_progress (33)
1996.1  (1760.2)    nav_state_cruise_3d_adjusting (34)
1996.6  (1760.6)    nav_state_cruise_3d_in_progress (33)
2014.4  (1778.4)    nav_state_cruise_3d_adjusting (34)
2014.7  (1778.8)    nav_state_cruise_3d_in_progress (33)
2015.0  (1779.1)    nav_state_cruise_3d_adjusting (34)
2022.1  (1786.2)    nav_state_cruise_3d_in_progress (33)
2068.6  (1832.7)    nav_state_waypoint_in_progress (17)
2076.7  (1840.7)    nav_state_cruise_3d_in_progress (33)
2078.2  (1842.3)    nav_state_idle (1)
2078.8  (1842.9)    nav_state_cruise_3d_in_progress (33)
2090.6  (1854.6)    nav_state_cruise_3d_adjusting (34)
2091.2  (1855.3)    nav_state_cruise_3d_in_progress (33)
2092.7  (1856.7)    nav_state_idle (1)
2168.3  (1932.3)    nav_state_cruise_3d_in_progress (33)
2173.4  (1937.4)    nav_state_idle (1)
2178.4  (1942.4)    nav_state_cruise_3d_in_progress (33)
2230.1  (1994.1)    nav_state_cruise_3d_adjusting (34)
2235.8  (1999.8)    nav_state_cruise_3d_in_progress (33)
2250.9  (2015.0)    nav_state_cruise_3d_adjusting (34)
2254.1  (2018.1)    nav_state_cruise_3d_in_progress (33)
2255.4  (2019.4)    nav_state_cruise_3d_adjusting (34)
2257.0  (2021.1)    nav_state_cruise_3d_in_progress (33)
2258.2  (2022.3)    nav_state_cruise_3d_adjusting (34)
2261.1  (2025.2)    nav_state_cruise_3d_in_progress (33)
2318.0  (2082.0)    nav_state_waypoint_in_progress (17)
2499.4  (2263.5)    nav_state_cruise_3d_in_progress (33)
2506.1  (2270.1)    nav_state_waypoint_in_progress (17)
2560.5  (2324.6)    nav_state_cruise_3d_in_progress (33)
2582.4  (2346.4)    nav_state_idle (1)
2582.7  (2346.7)    nav_state_cruise_3d_in_progress (33)
2587.5  (2351.6)    nav_state_cruise_3d_adjusting (34)
2591.5  (2355.6)    nav_state_cruise_3d_in_progress (33)
2592.7  (2356.7)    nav_state_rth_head_home (10)
2651.0  (2415.0)    nav_state_idle (1)

[stronnag]

Glorious technicolor (fast forward mode replay).
https://youtu.be/VHuoDPTL4S0

[wilco1967]

alright, flew again today with 'official' RC4, INAV/MATEKF722 2.0.0 Jul 14 2018 / 22:39:19 (f2cf315ad)
As far as I can see, there has been no code change related to althold between yesterday and today version.

changed the following back to previous (old) defaults:
set nav_fw_pos_z_p = 50
set nav_fw_pos_z_i = 0
set nav_fw_pos_z_d = 0

With those settings, everythings works normal again.... No strange trying to recover 'old' waypoint altitude setpoint when switching from WP to 3CRS.... it just stays at the current altitude, and lets me change using the elevator stick.... no problem, as it's supposed to do....

So I assume (if this is indeed a classic PID controller) the underlying problem was always there, but in only shows up with Integral (z_i) > 0 (as far as my understanding goes.... ;-)

Flew two missions today.... farthest was 31.5 km out / 65 km travelled.... new personal record.
Thanks to iNav (and QCZEK LRS)

If there is interest, I can post the blackbox log as well....

[digitalentity]

I'm moving to 2.1 since this doesn't seem to be a blocker

[digitalentity]

In fact this might be related to https://github.com/iNavFlight/inav/issues/3761#issuecomment-413976947

[wilco1967]

yep, Fix #3762 also seems to have fix this issue....
INAV/MATEKF722 2.0.0 Aug 19 2018 / 12:31:32 (1ca4ae3d4)

tried with new default (before troublesome) settings
set nav_fw_pos_z_p = 40
set nav_fw_pos_z_i = 5
set nav_fw_pos_z_d = 10

works just fine now....

Thanks a lot !

[digitalentity]

Great, moving to 2.0 milestone then

[wilco1967]

Guess I spoke too soon .....
Official 2.0.0...
Still, the target altitude doesn't seem to track the current altitude when climbing / dropping in waypoint mode. As soon as I switch from WP back to 3crs, it attempts to go back to the same altitude as prior to switching to WP.....
The elevator stick now works correct, which it didn't do earlier (prior to the partly fix)..... If I just give a small correction, it is ok again

Not a big problem.... Just a little strange.....
I'll try to make a video / BB log next time....

[wilco1967]

Alright.... tried some more.....
Still official 2.0.0

See attached blacklog and video file.....

LOG00019.TXT
https://drive.google.com/file/d/1Nkn-8-summCu5WLjx3iSxQu_PQ0WqmG6/view?usp=sharing

Still mostly the same behaviour as before.... still surprised, how I believed it worked, because it clearly doesn't

for example... I take off in auto-lauch, and switch to WP directly after take-off.
let it climb in waypoint to 1:07 minutes, 486 meter alt, when I switch to 3CRS.
It immediately begins to dive, which I didn't command.... I would have expected it to remain at steady altitude of 486 (when I changed from WP to 3CRS).
at 1:20 minutes, while still going down, I try two times up elevator, but no response.
at 1:23, after I already returned the stick to neutral, the motor speeds up, and it starts to maintain altitude at 361 meter (over a hunderd meters lower than it was supposed to do).
but it doesn't last long... at 1:29, it starts again to dive, uncommanded.... At about 1:32, it finally settles out at 355 meters.... the rest is commanded by me....

There are other examples of similar behaviour.

for example, at 4:44, I switch from waypoint into RTH.
It immediately takes a dive, and drops to about 300 meters (from 356), before leveling out, and starting to climb again to 340m. This is even though it is supposed to remain at the current altitude (which was 356), until reaching home (where it should go to 250 meters)

I interrupt it and landed manually.

Not sure if the setpoint tracking is the problem, or it is an issue with integral wind-up (that's kind of how it feels, if this is really a classic PID control algorithm).
However, once it's found its altitude, althold works really good, so I doubt it's an integral thing....
On the other hand, if it was a setpoint tracking issue, I would have expected it to go the what it thought was the requested alt setpoint, and level out there, without further dropping / climbing....
When flying waypoint, it perfectly changes altitude, as programmed, without any overshoot.

BTW, I made another flight, with baro disabled (not it this log).... Without baro, it held altitude pretty good once steady (not as good as with baro though), but suffers from the same issue...

Next time, I'll try
set nav_fw_pos_z_p = 50
set nav_fw_pos_z_i = 0
set nav_fw_pos_z_d = 0
again, but I'm pretty sure, it will behave normally again.....

No problem if that is the proper solution, but I'm kind of surprised, no-one else seems to have noticed this behaviour, using the new default settings for pos_z_p/i/d

[giacomo892]

It's not a PID tuning issue, there is something we need to track down in code to prevent this from happening.
Can you share WP list used in that flight?

@stronnag do you mind posting the navigation machine state transition list / little plot ?

[wilco1967]

Here's the WP list I THINK I used during this test...

I interupted somewhere between WP 1 and 2.... didn't finish it this time (but did on other flights, and it never failed)....

Converted to a .txt file, so it would upload to gitub

woold fx61 crash.mission.txt

BTW, the name suggests FX61, but it was flown using a mini talon.... It just follows the same route my FX-61 made when it crashed some time ago.... (I did find it again... ;-)

[giacomo892]

@wilco1967 most of the WP have 10m as altitude. That might explain why it dives down..

[stronnag]

The altitude is in metres in the XML definition.

https://github.com/stronnag/mwptools/blob/master/samples/mw-mission.xsd

[wilco1967]

Nope.... 1000 meters (not cm)....
Flew it a few times already, and it goes to 1000

Only first WP is 200 meters....
So whenever I enable WP again, it will go back to 200 meters.. that part works correct.... Problem is only when switching from WP to 3crs or RTH...