WP linear climb dive

[hali9]

#5219
Not tested yet.

[stronnag]

Tested (flying wing, dev branch + #5112 + #5226)
Brilliant patch. Flew the same mission on standard (dev) firmware then the patched firmware, back to back flights (re-flashed in field). Difference is day and night.

• Std firmware: Altitude changes are abrupt, with large throttle changes for turn + max climb rate.
• Patched firmware. Altitude changes are smooth, smoother turns and no loud throttle changes.

The mission was designed to be easily and audibly observable with visually obvious and achievable altitude changes (WPs at 20m, 35m and 50m with c. 120m legs).

Excellent.

[stronnag]

Here's some graphs of time v baro. The area in the thin red box is the time under WP navigation. You can probably work out which flight matches which plot.

On reflection, it appears that there is a possibly a difference in behaviour for the last part (which was RTH), the elevation at the last WP being 35m, as is the RTH altitude. Logs, mission file and diff available if useful.

[digitalentity]

Not very clean, but we can clean it up later if necessary.

[digitalentity]

Well done @hali9 !

[hali9]

@stronnag

Logs, mission file and diff available if useful

I would like to take a look out of curiosity. And CLI wp please.

Did Your hex contain #4930 ?

[wilco1967]

Just tested last version from development branch, to try the linear WP missions, but didn't go so well....
version INAV/MATEKF722 2.3.0 Nov 30 2019 / 13:03:43 (ca46d7a)
Mini Talon plane.

Very unstable altitude hold in WP / RTH / 3CRS mode. Some slow oscillation, very poor alt hold, while it flew perfectly stable using the same settings before.
More worrying is it sometimes has some strange behaviour going up/down in WP mode, which definitely isn't configured in the mission, but it seems like it is somehow commanded (definitely not some oscillation). Seems like it's getting wrong altitude setpoints.

Still something very wrong in the software.....

Repeated the flight with current official 2.3.0 release, with the exact same mission, and the exact same diff. Now it flew stable again (ok, no linear WP missions, but otherwise fine again).

Still hoping for the linear WP missions, but it isn't working yet....

[stronnag]

@stronnag

Logs, mission file and diff available if useful

I would like to take a look out of curiosity. And CLI wp please.

http://seyrsnys.myzen.co.uk/wp-linear/wp-linear-files.tar.gz XMLmission, cli diff, BBL (one without, two with).

Did Your hex contain #4930 ?

No, I don't think it has been merged yet.

[hali9]

@stronnag
I see in Your logs that the problem is in updating the altitude during the final WPRTH,
but this does not apply to PR because the problem occurs in both LOG14048.TXT and LOG14049.TXT.

And I think I found the reason.
In navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS is

setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);

When rthGetHomeTargetPosition return:

posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));

So generaly alt is set up to value between rthInitialAltitude and rthFinalAltitude, but where do these values come from?
They are set up in updateDesiredRTHAltitude which is call all time, also when WPRTH is executed:

case NAV_RTH_AT_LEAST_ALT:  // Climb to at least some predefined altitude above home
    posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
    posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;

In Your case nav_rth_alt_mode = AT_LEAST, so rthInitialAltitude = MAX(35m, current alt) and rthFinalAltitude = rthInitialAltitude.
And this is problem, because rthGetHomeTargetPosition will always return MAX(35m, current alt).
So at the beginning the desired alt is set to 35m, the plane reaches it and slightly exceeds say, by 0.1m, then the desired alt is set to 35.1m, the plane reaches it and slightly exceeds it ... and it climb all the time until it reaches home.

[stronnag]

It was just an observation; I'm content. In hindsight, it was a poorly defined test to coincidentally have the final WP altitude the same as the RTH AT_LEAST altitude.

[hali9]

Yes, but if the last WP is 35m, the plane should fly at this altitude when doing WPRTH. Instead of rising or falling.
updateDesiredRTHAltitude() should not be called when RTH, FS-RTH, WP-RTH is performed.
This is quite a serious bug and applies to all rth_alt_control_mode except NAV_RTH_CONST_ALT.
I think in NAV_RTH_NO_ALT can also dive.
So maybe in updateDesiredRTHAltitude add condition in if:

        if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH
          || (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {

[OlivierC-FR]

Tested, works great !
Just the throttle was "pumping up and down", but must double-check the settings for this plane, might not be optimized.
https://drive.google.com/file/d/1-esUA2t-PxltkJIUlnWVhpbQpHlwphY0/view

[wilco1967]

Tried today in very windy conditions with my old (T)rusty Tricopter, using the branch from OlivierC with the OSD-HUD (oc_hud_waypoints2)
INAV/MATEKF405 2.3.0 Dec 14 2019 / 16:22:27 (865b32e)

Seems to work just fine on the Tricopter, and very nice to see the waypoints in the HUD.

Now need to try again with the MiniTalon, once weather allows.
Before, on an earlier version, had some random(?) altitude changes (which I assume is fixed now) and very unstable throttle response (which it wasn't doing on the normal branch)..

BTW, @OlivierC-FR
Is there a way, to define one OSD screen with horizon + WP's shown, and another screen with horizon, but without the WP's ?.

Hope this makes it into the development branch, and ultimately, iNav master.

Thanks a lot !

[OlivierC-FR]

BTW, @OlivierC-FR
Is there a way, to define one OSD screen with horizon + WP's shown, and another screen with horizon, but without the WP's ?

To keep it simple I made all hud-things part of the crosshair, so it's not possible to hide only the WP, but what you can do on the 2nd screen is disable the crosshair, this will also turn off the WPs (but also the 3D marker for the home point).

[wilco1967]

Finally, the weather allowed for a test flight with the Mini Talon.
Using the branch from OlivierC with the OSD-HUD (oc_hud_waypoints2)
INAV/MATEKF722 2.3.0 Dec 15 2019 / 17:39:10 (865b32e)

OSD HUD elements worked perfect. Thanks Olivier !

However, WP lineair climb / dive way points still doesn't work as intended.

Was very windy / bumpy today, so cannot say 100% sure, but still, altitude hold in 3CRS feels very different from standard firmware. It simply is not very stable in 3CRS / WP / RTH. Perhaps it is better than what I tried 3 weeks ago, but difficult to say in this wind.
It still manages to get a reasonable linear ascent, and in the beginning all seems to work.

However, on the way to the 3rd WP, which is about 3 km out (the 1st two were only a few hundred meters away from home), after completing about 650 meters (from previous WP), it made a sudden dive. It was initially climbing as expected towards the 500 meter alt target of WP3. It had reached an alt of 220 meters, before it started to dive.

On the first try, I interrupted before it went too low and went into RTH.
On the 2nd attempt to fly the mission, it did exactly the same thing, at the same spot, at the same alt. I let it continiue to dive some more. It stopped diving at 100 meters, and then it slowly began climbing again, as if it was climbing to the WP3 altitude (500m).

After another 1200 meters flight, (Still before reaching WP3) the same thing happened, and it started to dive again. By that time, it was over 2 km away from home, and going too low, behind some obstacles, so I couldn't allow it to go any lower, without completely loosing video, so I had to interrupt.
Looking back at the video, It again made the dive, when hitting 220 meters alt....

I've attached the blackbox log. Hopefully it helps to explain what went wrong.
Also attached, the mission file, which it never completed.

If I run the same mission file in the 'official' 2.3.0 iNav, it executes it perfectly, and also is a lot more solid in alt-hold.

Perhaps there is some kind of overflow in the calculations, that makes it behave like this, and makes it not show up the issue, if only flying close to home ?

A distance of 650 meters (65000 cm) from WP2, could be like some 64K limit overflow (but could just be coincedence... I don't know enough of programming to find out.... )

Or has the 220 meter (256 GPS) altitude something to do with it ? All three times, it dove at pretty much when reaching 220 meters alt AGL (we are at approx 35m ASL here, so that would be 255 GPS alt).

The testflight with the tricopter, that went ok last week, never went that far away from home.

Thanks a lot for all your efforts ! Much appreciated.
Providing feedback like this is the least I can do....
Really would love to see this feature working at some time....

LinWP_OSD.TXT
WP linear test.mission.txt

[hali9]

In math.c in scaleRange function

long int a = ((long int) destMax - (long int) destMin) * ((long int) x - (long int) srcMin);
But int and long int is same range and is overflowed when calculating variable a.
Maybe should be "long long" ?

(signed) int	32	4 (word-aligned)	–2,147,483,648 to 2,147,483,647
unsigned int	32	4 (word-aligned)	0 to 4,294,967,295
(signed) long	32	4 (word-aligned)	–2,147,483,648 to 2,147,483,647
unsigned long	32	4 (word-aligned)	0 to 4,294,967,295

Anyway #5287 should be fix ths.

[wilco1967]

Thanks a lot !
compiled and downloaded already.
Will give it a try as soon as the weather allows (doesn't look good for the next few days).
Will report back asap.

[wilco1967]

Hi @hali9

Just completed a small WP test mission, and with those last changes, it works perfectly !
No more unexpected altitude changes. Alt hold also is stable. No more oscillations, or other strange behavior. All perfect !

THANK YOU !

@OlivierC-FR
Tested it using your oc_hud_waypoints2 branch, with the last changes manually edited in navigation.c.
Also hud shows nicely. Just a few minor cleanup issues.
At one time, it showed 9/111 waypoints, while there were only 11 in my test mission.
Also, it seems to have difficulty showing waypoint above 10. I guess when we require 2 digits, it goes a little strange....
Sadly, I forgot to insert the SD card in the goggles, so no video with OSD overlay....

Anyway, I call this a succes !!!!....
Hope both the OSD hud waypoint and the linear climb/dive changes make it in the development version, and into official iNav 2.4

I'll prepare some more test missions, with 'impossible' alt changes, 180 deg course reversal, etc, just to confirm it doesn't do anything strange, before sending it out again on long WP missions out of radio / video range.

Thanks a lot again !

and a Merry Christmas !

[wilco1967]

Just to confirm:
Made another test mission, in very poor visibility, with some 'impossible' WP actions, like steep climbs, steep dives, reverseal of route, very close WP (unable to make that sharp turn), etc.

No strange behaviour. It flew it as best as it could, without any problems.

I'm confident enough I can sent it out on far missions, even out of radio range, without worries.

Thanks again for a great addition !