Two Stage Climb First RTH

[MrD-RC]

Overview

Climb first can be a pretty inefficient part of the RTH sequence. The problem is that you are using energy spiralling up to altitude, or worse, flying away from home while gaining height. However, turning off climb first may not be a valid option, depending on the flying environment. This setting gives pilots more options with climb first.

So, what I'm adding, a two stage climb first. It's a really simple concept, and something that I think will be really useful. There are two new CLI commands:

• nav_rth_climb_first_stage_mode: This setting is similar to nav_rth_mode, in that it lets you decide how you want to use the first climb stage altitude. Settings are AT_LEAST and EXTRA.
• nav_rth_climb_first_stage_altitude: Allows you to set an altitude for the first climb stage. The default, 0, means the feature is disabled.

Why do we need this?

Other than the obvious energy benefits from turning towards home sooner. This also means that there should be no worries with nav_rth_abort_threshold when using a linear climb. Climb first is usually used because you're flying in an environment with obstacles. The two stage setting means that you can set the first stage to clear obstacles, before heading up to your chosen RTH Altitude.

So how does it work?

To be honest, pretty much as you expect it to. Once you select RTH, the model will start climbing (linear or spiral) up until the first stage target is met. Then it turns towards home and flies in that direction. If more altitude is needed to reach your target RTH altitude, it will climb on the way home. If the target altitude is met during the first climb stage, it will just fly home. Nice and simple, and much more energy efficient.

nav_rth_climb_first_stage_mode = AT_LEAST

This setting works in the same vein as the main RTH modes. Your target altitude for the first stage climb will be what you have set in nav_rth_climb_first_stage_altitude. If you are below the first climb stage altitude, it will climb to it. If not, it will turn to home and either fly home, or climb on the way home if your main RTH altitude target has not been reached. If the RTH Altitude is reached in the first stage, it will immediately turn towards home.

nav_rth_climb_first_stage_mode = EXTRA

Again, this setting works just like the main RTH modes. The target altitude for the first stage climb will be your current altitude plus the value you have set in nav_rth_climb_first_stage_altitude. If you are below the first climb stage altitude, it will climb to it. If not, it will turn to home and either fly home, or climb on the way home if your main RTH altitude target has not been reached. If the RTH Altitude is reached in the first stage, it will immediately turn towards home.

Here is a video showing regular RTHs, RTHs using two stage climb first and the AT_LEAST setting, and RTHs using two stage climb first and the EXTRA setting. The only difference between AT_LEAST and EXTRA in the code is the altitude number. So I split testing between the two modes. With AT_LEAST, I tested activating below the RTH Altitude and above. With EXTRA I tested activating well below the RTH Altitude, and just below, so that it would pass the RTH Altitude during the first stage.

https://youtu.be/yIO6CDxu1uM

Other notes

I have created more of those images. So will update the Wiki (if I can) to demonstrate the different climb first options, as that's not really covered in any documentation.

[avsaase]

Great idea! And I love the images.

How does this work on quads?

[MrD-RC]

@avsaase it has no effect on quads. It’s fixed wing only.

I imagine a quad with climb first would just go vertically up to the RTH altitude.

Alex, is this something that a quad could find useful? If so, it's just a case of changing the if statement in navigation.c:2473 to remove (STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) &&

[JulioCesarMatias]

I liked that! And I also liked the pictures, so cute.

[MrD-RC]

I’ve updated the WiKi to cover the climb first modes. I’ve left stuff from this PR out until it gets merged.

[MrD-RC]

Updated from current master and resolved conflicts.

[MrD-RC]

Another update to resolve conflicts. @DzikuVx can we merge it, I know three times is the charm 🤣