Linear climb and dive for waypoint missions

[danarrib]

Current Behavior

When running a mission with different altitude waypoints, INAV will try to climb or dive to the next waypoint altitude as soon as it can, constrained by nav_auto_climb_rate, nav_fw_climb_angle and nav_fw_dive_angle variables.

Desired Behavior

INAV should save energy by climbing or diving linearly, and not spend a lot of energy to reach that altitude quickly just to do the remaining travel on the waypoint altitude.

Suggested Solution

If next waypoint is too far, there's no need to make a hard climb or dive to reach the desired altitude as fast as possible. INAV can climb or dive in a linear manner so it will reach the waypoint altitude only when it reaches the waypoint itself. Just like the new RTH_LINEAR_DESCENT mode.

Who does this impact? Who is this for?

Every INAV user, Fixed Wing or Multirotor, who wants to fly missions with more efficiency.

[issue-label-bot]

Issue-Label Bot is automatically applying the label Feature request to this issue, with a confidence of 0.88. Please mark this comment with 👍 or 👎 to give our bot feedback!

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

[hali9]

I think about something like this:

Before starting a new waypoint, we remember the last achieve point and the current altitude.
We can't use the altitude from the last achieve waypoint because it may not be reached.
If the active waypoint is first (enable waypoint mode) then the "last achieve waypoint" will remember the position when the waypoint mode was started.

[hali9]

I will agree with You, that's what I mean.
Lets:
The height at which the plane is when you start WP mode = 10m
Distance to WP1 = 50m
WP1 altitude = 40m
Distance from WP1 to WP2 = 100m
WP2 altitude = 65m

The plane reaches WP1 at say 35m, i.e. it climbed 25m (using maximum climb).
WP1 has been reached (but the altitude of WP1 is not), so we must remember these 35m.
Now the plane flies to WP2, has a distance of 100m and must climb 30m (65m-35m) and it succeeds after about 60m distance. And this is not needed because it can climb slower, but how to calculate this.
We need altitude when WP1 was reached (35m) and not WP1 altitude (40m).

And now from a simple calculation at what altitude the plane should be at each place between WP1 and WP2. So after 60m distance it should be at height: 35m + 60m/100m * (65m-35m) = 53m.
As You see the WP1 altitude (40m) is not need for this calculation,
but the altitude when WP1 was reached (35m) is necessary.

[hali9]

The distance between WP's is only calculated when loading the active waypoint.
The distance to the active WP what's left is calculated anyway, so we can use it.
We remember how altitude the plane was when loading the WP.
From these 3 variables we can calculate at what altitude the plane should be now.
We set this altitude as desired, the rest will be done by the altitude PID controller.

[danarrib]

Ok, sorry... Shame on me! You probably know INAV better than me.
I'll remove my unnecessary comments to keep this topic in the main subject.

Then you could maybe PR your code change and make the mission execution better? :-)

[danarrib]

Ok, just to give a feedback. I built and tested the #5292 and #5226 last weekend, on both quadcopter and wing. Working just fine. Flying missions is a lot better now. Thanks a lot @hali9 !

[DzikuVx]

I belive we can close it