ArduPlane: use Location::AltFrame for guided_state target frame references enabling ABOVE_TERRAIN

[timtuxworth]

This PR changes the c++ code in Plane Guided mode to use Location:AltFrame for internal references to target Frame.

This only applies for AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED - effectively guided mode commands received via MAVLink, which could also be from Lua using the command_int Lua interface.

A very welcome side effect of this change is that it enables plane MAVLink CMD_GUIDED_CHANGE_ALTITUDE to work for terrain altitudes in addition to the existing GLOBAL/Absolute and RELATIVE/Home altitudes.

Extracted from #27909 as requested by @IamPete1 to fix guided_state.target_alt_frame being a uint8_t whereas it actually stores AltFrame values from Location::AltFrame.

Similar to changes proposed for Sub in #27767

Mavlink alt frame values (probably) needed for altitude navigation:

   MAV_FRAME_GLOBAL=0, /* Global (WGS84) coordinate frame + altitude relative to mean sea level (MSL). | */
   MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* 
          Global (WGS84) coordinate frame + altitude relative to the home position.
         | */
   MAV_FRAME_GLOBAL_TERRAIN_ALT=10, /* Global (WGS84) coordinate frame with AGL altitude (altitude at ground level). | */

whereas the internal Location:AltFrame values are

    enum class AltFrame {
        ABSOLUTE = 0,
        ABOVE_HOME = 1,
        ABOVE_ORIGIN = 2,
        ABOVE_TERRAIN = 3
    };

So as you can see ABSOLUTE is co-incidentally the same (zero) but none of the other values match. Even worse MAV_FRAME_GLOBAL_RELATIVE_ALT=3 == AltFrame::ABOVE_TERRAIN=3.

So the MAVLink values need to be translated by GCS_Mavlink.cpp to values that can be used by Location and all of the other internal code that uses Location::ALtFrame.

[IamPete1]

This has broken CI. It also changes the logged value from being the MAVLink enum to the AP one.

[timtuxworth]

This has broken CI. It also changes the logged value from being the MAVLink enum to the AP one.

I think that is a bug. Why would plane.guided_state internally use the MAVLink enum for the target_alt_frame associated with the internal target_alt value it sends to TECS?

[timtuxworth]

This has broken CI. It also changes the logged value from being the MAVLink enum to the AP one.

Ok found it autotest/arduplane.py MAV_CMD_GUIDED_CHANGE_ALTITUDE test. I'll take a look.

[timtuxworth]

This has broken CI. It also changes the logged value from being the MAVLink enum to the AP one.

I'm wondering if the test is valid? I think that switching out of GUIDED to LOITER and back to GUIDED should reset the plane.guided_state.target_alt back to -1, so I'm not sure why the test is expecting the altitude to be 70 +/-3 - can you explain that to me?

[IamPete1]

I'm wondering if the test is valid? I think that switching out of GUIDED to LOITER and back to GUIDED should reset the plane.guided_state.target_alt back to -1, so I'm not sure why the test is expecting the altitude to be 70 +/-3 - can you explain that to me?

It was circling at 70m in guided. Changing to loiter should maintain that 70m altitude. Then changing back to guided again should still maintain 70m altitude. Until a command is received it should stay at the altitude at which the mode was entered.

[timtuxworth]

I'm wondering if the test is valid? I think that switching out of GUIDED to LOITER and back to GUIDED should reset the plane.guided_state.target_alt back to -1, so I'm not sure why the test is expecting the altitude to be 70 +/-3 - can you explain that to me?

It was circling at 70m in guided. Changing to loiter should maintain that 70m altitude. Then changing back to guided again should still maintain 70m altitude. Until a command is received it should stay at the altitude at which the mode was entered.

Fixed - see my new change to mode_guided.cpp

[IamPete1]

Fixed - see my new change to mode_guided.cpp

I don't understand why we need to fix something that wasn't broken? Maybe I'm missing some case where we get it wrong currently?

[timtuxworth]

Fixed - see my new change to mode_guided.cpp

I don't understand why we need to fix something that wasn't broken? Maybe I'm missing some case where we get it wrong currently?

I think it was broken, which caused the CI fail. It was using last_target_alt without a frame. The way I see it, it was purely coincidence that the sequence of events in the CI test would pass.

By adding a frame i.e. saving last_target_alt_frame and then restoring it when last_target_alt is restored, this passes the Plane.MAV_CMD_GUIDED_CHANGE_ALTITUDE test

[timtuxworth]

It also changes the logged value from being the MAVLink enum to the AP one.

How do you think I should deal with this? I would argue it's a bug that it had been logging the MAVLink enum since that's not what the code is using, so it was probably not very useful.

Update: I've reverted the AltF value to log the incoming MAVLink frame value and added a new AltL value to log the internal Location::AltFrame value it is translated to.

[amilcarlucas]

Are there static asserts that the AP internal frame defenition enums have the same values as the mavlink external frame defenition enums?

[timtuxworth]

Are there static asserts that the AP internal frame defenition enums have the same values as the mavlink external frame defenition enums?

They don't have the same values. That's the fundamental problem. See the first post on this PR, I lay out the mismatch.

[clydemcqueen]

GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame is handy, and used by Copter and Sub.

ardupilot/libraries/GCS_MAVLink/GCS_Common.cpp

Line 6880 in 1439aeb

bool GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, Location::AltFrame &frame)

[timtuxworth]

GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame is handy, and used by Copter and Sub.

ardupilot/libraries/GCS_MAVLink/GCS_Common.cpp

Line 6880 in 1439aeb

bool GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, Location::AltFrame &frame)

Oh that's awesome, I didn't know that existed! Oh and as I change my code to use it, I've found (another) bug - the existing code never compared that the incoming frame was the same as the current frame, so would treat 100m above home the same as 100m ASML (or vice versa), return true but do nothing. A bit of an edge case, but still a problem, especially when flying near actual sea level.

[timtuxworth]

Thanks for looking at this @peterbarker - I've just make a whole bunch of changes which cleans it up a lot. Thanks for your questions/suggestions they lead to a lot of improvements. I'm pretty sure I fixed a couple of bugs in the original code.
@IamPete1 it would be great if you could take another look. I've now added autotests for ABOVE_TERRAIN which works quite nicely.

[IamPete1]

This seems to now add support for a new alt frame? Wasn't that going to be in #27909?

[timtuxworth]

This seems to now add support for a new alt frame? Wasn't that going to be in #27909?

It was, but after adding mavlink_coordinate_frame_to_location_alt_frame() there doesn't seem much point. This gives us ABOVE_TERRAIN and I'd actually have to add code to remove it again.

[IamPete1]

Quite a bit of the existing code looks suspect to me.....

[timtuxworth]

Tested on a real plane and found one bug in the c++ code and the lua example needed some major rework.

[timtuxworth]

Rebased and passed CI. I think this is "ready" - what do you think @IamPete1 ?

[timtuxworth]

Rebased

[timtuxworth]

Only 56 bytes on plane!

Binary Name      Text [B]       Data [B]     BSS (B)      Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  -------------  -----------  -----------  ----------------------------  -------------------------
antennatracker   0 (0.0000%)    0 (0.0000%)  0 (0.0000%)  0 (0.0000%)                                      621548
arducopter       0 (0.0000%)    0 (0.0000%)  0 (0.0000%)  0 (0.0000%)                                      151208
ardusub          0 (0.0000%)    0 (0.0000%)  0 (0.0000%)  0 (0.0000%)                                      360236
blimp            0 (0.0000%)    0 (0.0000%)  0 (0.0000%)  0 (0.0000%)                                      599496
arduplane        56 (+0.0031%)  0 (0.0000%)  0 (0.0000%)  56 (+0.0031%)                                    153736
ardurover        0 (0.0000%)    0 (0.0000%)  0 (0.0000%)  0 (0.0000%)                                      302576
arducopter-heli  0 (0.0000%)    0 (0.0000%)  0 (0.0000%)  0 (0.0000%)                                      152168