ardupilot
ardupilot copied to clipboard
ArduPilot
Plane: Fix climb-before-turn when terrain following
Currently, climb-before-turn works great, except when using terrain following. When NOT using terrain following and are in RTL, plane.next_WP_loc.alt is set to the ALT_HOLD_RTL (e.g. 20000). This works fine, because the plane is also in "altitude from home", e.g. 10000.
However, when terrain following is ENABLED, plane.next_WP_loc.alt is still 20000, but now plane.current_loc.alt is AMSL, e.g. 58100, even though the plane is below ALT_HOLD_RTL. This makes the check true, so the plane starts turning immediately.
In this patch, I tried to convert everything to its proper altitude, and it seems to work in the SITL in both cases, however I am not sure at all about the code. My main source of doubt is that plane.next_WP_loc.alt was 20000 when I was testing the original code, but when I added the check for terrain following, it switched to 58100, even though I am using the exact same variable with terrain following STILL BEING DISABLED.
I would appreciate any feedback on the right way to do this.
Am I doing the conversions the right way? Should I be doing them in the first place? I'm very unsure about how the terrain system works.
I mainly don't understand why, when terrain following is off, the next WP altitude is exactly ALT_HOLD_RTL, while plane.current_loc.alt seems to be AMSL.
OK, this is not correct either. It doesn't properly compensate for the right terrain altitude, it just happened to do that in my tests somehow.
I would guess that plane.current_loc is actually relative home but is set at abs in the location, so when you convert it does the abs to terrain not relative to terrain. Planes altitude frame handling is a massive mess.
@IamPete1 Hm, right... I'm a bit lost as to how I should approach this :/ Do you have any tips? I'm also on Discord.
@timtuxworth I believe you have recent experience with this scenario - could you weigh in here, please?
This is definitely a problem and this seems like a good idea. I've had multiple clients including a partner complain about the lack of terrain awareness in RTL.
@skorokithakis are you still interested in chasing this one? Tim's worked with terrain-following stuff recently, so this is probably still applicable.
The partner I'm working with is also very interested in this one. We'd like to see it merged.