ardupilot
ardupilot copied to clipboard
ArduPilot
ArduPlane: use Location::AltFrame for guided_state.target_alt_frame
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
The change in mode_guided.cpp is required, as it was ignoring the target_alt_frame which I'm pretty sure it shouldn't.
In the other PR I add a switch statement that also deals with ABOVE_TERRAIN.
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.
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?
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.
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?
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.
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
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?
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
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.
Are there static asserts that the AP internal frame defenition enums have the same values as the mavlink external frame defenition enums?
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.
GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame is handy, and used by Copter and Sub.
https://github.com/ArduPilot/ardupilot/blob/1439aebf94ae388a82f28199aa7dd333c009b98c/libraries/GCS_MAVLink/GCS_Common.cpp#L6880
GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frameis handy, and used by Copter and Sub.https://github.com/ArduPilot/ardupilot/blob/1439aebf94ae388a82f28199aa7dd333c009b98c/libraries/GCS_MAVLink/GCS_Common.cpp#L6880
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.
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.
This seems to now add support for a new alt frame? Wasn't that going to be in https://github.com/ArduPilot/ardupilot/pull/27909?
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.
Tested on a real plane and found one bug in the c++ code and the lua example needed some major rework.
Rebased and passed CI. I think this is "ready" - what do you think @IamPete1 ?
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