Adding support for ArduPilot
JonasVautherin opened this issue · 40 comments
Context
A while ago, @peterbarker worked on adding support for APM in the SDK (here). More recently, @jinchengde (and others) started looking into that, again. The goal of this issue is to discuss the high-level requirements and architecture.
Benefits for APM
The project is architectured in such a way that everything relies on a C++ MAVLink implementation. Making this implementation compatible with APM means that APM will also get the language bindings (Python, Swift, Java and future ones like Javascript).
The interface common to all our bindings is defined here and is used to auto-generate language-specific APIs (e.g. using RxSwift in Swift, RxJava in Java, and asyncio in Python).
SDK architecture (overview)
The SDK can been seen as composed from 4 parts:
- The interface definition (protobuf files), common to all the languages (C++, Swift, Java, Python). Each proto file is considered a "plugin", which is a group of (MAVLink) features.
- The C++ MAVLink implementation, which actually maintains a connection to the drone and does the MAVLink logic.
- The backend, which exposes the C++ implementation of (2) as a messaging server through the interface of (1).
- The language bindings (Python, Swift, Java), which implement (1) to communicate with the backend (3).
At the lowest level, each plugin (defined by one proto file) is a C++ library. The backend uses those libraries, as can been seen here.
I can see different ways to achieve that (and I'm looking for more thoughts). I believe that for each feature, there can be 3 situations:
- The feature has (can have) the same interface between APM and PX4, but not the same MAVLink implementation
- The feature does not have the same interface between APM and PX4, and therefore not the same MAVLink implementation
- The feature is compatible between APM and PX4
Because features are grouped into plugins, that applies to plugins as well. The following scenarios could then occur:
- We could keep the same interface (public headers) and build a different library that could be "swapped": for instance
libdronecode_sdk_apm_missionandlibdronecode_sdk_px4_mission, in case mission shares the interface but not the implementation. If the differences are really small, we could also imagine building the same library, but having an interface to decide whether it should start as PX4 or as APM (seems to me that it is what @peterbarker started here). - In this case, we could possibly have a separate plugin definition (e.g.
mission_px4.protoandmission_apm.proto), and therefore a separate implementation. - There is nothing to do here, it should just work
The questions I have would therefore be:
- Do we have plugins that are completely compatible between APM and PX4 (e.g. Action)?
- What are the features that are not compatible?
- For the features that are not compatible, could we share the higher-level interface (at the proto file level) and hide the difference as an implementation detail, or do we have to expose a different interface?
Great contributions would be to test the current SDK against an APM SITL and list the features/plugins that are working or not, and possibly why.
I suggest use the same interface but different implementation what peter had done before. because the different between APM and PX4 is not too much bust just some detail in MAVLINK, therefor we could implement takeoff and land example in APM sitl first.
Could we start a list of differences we know? I've heard that there are "not so many" differences quite a few times, but I still have no clue what that means and what they are :-).
@auturgy and @peterbarker probably the best people for this. Generally speaking the systems use the same messages to achieve the same things, and will often use the same "main" message sequence. They may however use different sequences for edge cases, or use things that are not clearly defined in the spec.
The ones with highest impact are parameter protocol, mission protocol and command protocol.
- Param protocol high level differences here: https://mavlink.io/en/services/parameter.html#implementations
- Mission protocol high level differences: https://mavlink.io/en/services/mission.html#implementations
- Command protocol differences are not documented yet (so I don't know what they are). The main difference is that I believe ArduPilot does not use the LONG form of commands, so suffers rounding errors for positional messages.
In addition, ArduPilot uses different modes - ie Guided vs Offboard and may require different sending rates when in these modes.
I expect you'll discover lots more. Let me know - then I can document them :-)
Also, for testing: is there a SITL that can run headless (e.g. on a CI server) for APM? We will need something like that to run integration tests.
Hi, has there been any movement/development on this?
Unfortunaly, not that I am aware of. On my end, I would still like to have an idea of the differences between PX4 and APM, in order to open a discussion also at the MAVLink level. IMO MAVSDK should implement the "standard" set of messages (presumably common.xml), and allow for "unofficial" plugins supporting dialects.
APM does implement parts of common.xml, and therefore parts of MAVSDK should already work with it. It would be awesome to try to make a proof of concept of that before thinking about how to deal with the differences.
What do you think @fnoop? Could you help with that?
@JonasVautherin Just FYI, ArduPilot differs in (at least) Mission and Parameter protocols. Probably also in the command protocol, but I haven't documented that properly, so no detail.
EDITED: Deleted, just realised I posted most of this above already: #728 (comment)
@fnoop FWIW While it will be much easier to add ArduPilot-specific plugins, long term it will be better for the standard and for ArduPilot maintenance to work on making the protocol implementations of the common protocols "more compliant".
Hi All,
I'm looking to help out with this issue. It seems that a compatibility check between ArduPilot and MAVSDK (in its current form) needs to be undertaken. Ideally we can come up with a set of functionality tests to track development...
These are my current thoughts / action plan:
- Setup a CI server with MAVSDK and ArduPilot SITL.
- Write tests to determine current MAVSDK functionality against Ardupilot.
- Write plugins (or modify ArduPilot, or another approach TBD) for MAVSDK to bridge the gap where items in 2 are failing.
Initial testing of MAVSDK examples and integration tests result in a segfault occurring in the ardupilot SITL application.
Looking into why this might occur...
[New Thread 0x7ffffab80700 (LWP 8519)]
[New Thread 0x7ffffa370700 (LWP 8520)]
[New Thread 0x7ffff9b60700 (LWP 8521)]
[05:50:50|Debug] Ignoring msg 30 (system_impl.cpp:179)
[05:50:50|Debug] Ignoring msg 33 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 1 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 125 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 62 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 42 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 74 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 36 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 65 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 27 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 116 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 129 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 29 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 24 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 2 (system_impl.cpp:179)
[05:50:50|Debug] Ignoring msg 136 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 32 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 241 (system_impl.cpp:179) [05:50:50|Debug] Ignoring msg 147 (system_impl.cpp:179) [05:50:50|Debug] Forwarding msg 0 to 140737287032224 (system_impl.cpp:157)
gdb is not being kind to me but it looks like ardupilot is bombing out when the forwarded message is received by ardupilot.
I will have to add some debugging on the ardupilot side to see why the heartbeat (msgid = 0) is causing this issue.
I will have to add some debugging on the ardupilot side to see why the heartbeat (msgid = 0) is causing this issue.
Heartbeat from MAVSDK?
Yeah, looks that way. If you can think of anything that may be responsible for a segfault in a heartbeat packet... It seems unlikely but certainly the ArduPilot SITL is exiting as soon as that heartbeat is sent.
It seems that one aspect on which MAVSDK relies and where PX4 and APM differ is the flight modes. Flight modes are not part of the MAVLink specification, and MAVSDK currently uses the PX4 definitions (because it is developed and tested against PX4). However, there has been work towards specifying common flight modes (see e.g. here).
The current state, as far as I can tell, is that the PX4 community is open to working on a flight modes specification and updating PX4 to comply with it, but this cannot be done unilaterally. Anybody wanting to work on that part on the ArduPilot side would be more than welcome to the discussion 😊.
I identified a bug where Ardupilot throws an error when using MAVSDK NaN blanked command_long
I have testied this with a naive patch to resolve it and and MAVSDK seems to identify Ardupilot autopilot. However actions (arm, takeoff + telemetry) are pretty broken as they wait for specific state Ardupilot never delivers (PX4 params?)
Following from this I would like to see an abstraction API put into MAVSDK so autopilot implementation quirks be handled, alternatively I would like to see MAVLink standard define a lot more of the implementation so these implementation quirks don't exist in the first place :)
Hi All, I'm looking to help out with this issue. It seems that a compatibility check between ArduPilot and MAVSDK (in its current form) needs to be undertaken. Ideally we can come up with a set of functionality tests to track development...
These are my current thoughts / action plan:
1. Setup a CI server with MAVSDK and ArduPilot SITL. 2. Write tests to determine current MAVSDK functionality against Ardupilot. 3. Write plugins (or modify ArduPilot, or another approach TBD) for MAVSDK to bridge the gap where items in 2 are failing.
Hi,
is Pt. 1 already done? If yes where I can see it? If not then I would like to contribute on this. I have created a docker image containing the ardupilot sitl (Only for the Rover Firmware at the moment but easily expandable.).
How would you prefer to integrate it into the MAVSDK CI tests? Should I provide the docker file like the PX4 sitl at MAVSDK Github Actions PX4-SITL or simply the docker hub tags are sufficient.
The github repo to the current development is at: Ardupilot SITL Headless. The project was inspired by another such development from radarku. The main difference is the multi-stage docker build approach to bring the image size from 4GB to 700MB.
An Ardupilot MAVSDK integration testing suite does not exist at the moment for MAVSDK and would be much appreciated.
I have used the radarku SITL image for my personal testing and it has worked well. If you are happy to contribute your docker file that would be useful.
Then we can set up some integration tests to work with GitHub actions and the new Ardupilot dockerfile
Happy to contribute. Need some guidance on next steps though. Should I create a pull request with the addition of docker files for Ardupilot SITL and changes in the github action file to include the Ardu-SITL jobs as well?
Hi,
Is there any progress to this topic? I would like to know the status and how can I contribute.
Hi @prokas-nikos thanks for writing. So, in general, we want to try to minimize specific hacks and move towards a spec that both flight stacks are compatible with. These discussions and work is generally done in the MAVLink call every two week.
One thing that is being standardized is common flight modes, so one goal would be to implement that for ArduPilot, PX4, MAVSDK, etc..
Apart from that, in the meantime, you can always test specific functionality that you need/use with MAVSDK and check whether it works, and if not, fix it ;).
@JonasVautherin hi, I'm dealing with this at the moment connecting MAVSDK with Ardupilot SITL. So far I've found Ardupilot does not support MAVLINK_MSG_ID_PING which is used in system_impl.cpp line 279. I get intermittent disconnections from the ardupilot side (After disabling disconnections from the MAVSDK side in tcp_connections). I still get Ardupilot resetting the connection, but like I said, sometimes it works, sometimes it doesn't. By the way, I'm only using the command and mavlink passthrough plugins from MAVSDK.
Hello all,
I am noticing some actions about this topic. I can see that in Action Class there is a mapping between Ardupilot Modes and PX4 Modes. I am trying to send mission commands to ArduPilot but I fail. I just want to ask if there is a release notes document where we can track the changes regarding the topic of Ardupilot functionalities in MAVSDK and know if I have to implement something on my own or I am misusing the MAVSDK api.
@prokas-nikos I would suggest you write a small example showing what you are trying to do, open a new issue with it, and explain what you expect and what happens instead 😊. That will help people understand your problem better, and possibly help you 👍.
I will tell you that I run Ardupilot SITL missions using MAVSDK
There is some support for Ardupilot now and we even run some integration tests against Ardupilot in the CI.
I'd say we can close this one. PRs are welcome when needed or if contributors want to enable more integration tests in the CI.
What is the "some support" any doc about this? I tried gotoLocation and mission, both are not working for Ardupilot. But same code are working well on px4.
@LiangHuangBC I found out that for ardupilot the working lib was missionraw
@LiangHuangBC A Document describing the support does not exist yet. I did make an excel sheet describing the current implementation status for APM the link to this is here:
GoToLocation is not supported yet. For Mission, you should use MissionRaw plugin and please Follow the sequence to start the mission-> Arm(), Takeoff() and then StartMission() as done in the following Integration test:
Mission Integration Test for APM
Actually, the best way to check support for APM is to simply go through the tests starting with APM (test only meant to run with APM) or NOT starting with PX4 (test meant to work with both PX4 and APM.)
Hope that helped :)
GoToLocation is not supported yet. For Mission, you should use MissionRaw plugin and please Follow the sequence to start the mission-> Arm(), Takeoff() and then StartMission() as done in the following Integration test:
@ykhedar thanks for linking these tests. I ran them in conjunction with ArduPilot SITL which connects (use udp port 14550). Uploading a mission does not however work and I get an error saying that the sequence is invalid, as below:
[12:37:39|Debug] Component Autopilot (1) added. (system_impl.cpp:377)
[12:37:39|Warn ] Vehicle type changed (new type: 2, old type: 0) (system_impl.cpp:225)
[12:37:39|Debug] Discovered 1 component(s) (system_impl.cpp:578)
Waiting for system to be ready
Waiting for system to be ready
[12:37:41|Warn ] Invalid sequence for ArduPilot items (mavlink_mission_transfer.cpp:249)
/home/oliver/SwarmCore/Tests/px4sitl.cpp:143: Failure
Expected equality of these values:
mission_raw.upload_mission(mission_plan)
Which is: Invalid Argument
MissionRaw::Result::Success
Which is: Success
[12:37:41|Warn ] sending again, retries to do: 3 (MIS_TAKEOFF_ALT). (mavlink_parameters.cpp:1340)
[12:37:42|Warn ] sending again, retries to do: 2 (MIS_TAKEOFF_ALT). (mavlink_parameters.cpp:1340)
[12:37:42|Warn ] sending again, retries to do: 1 (MIS_TAKEOFF_ALT). (mavlink_parameters.cpp:1340)
[12:37:43|Error] Error: Retrying failed get param busy timeout: MIS_TAKEOFF_ALT (mavlink_parameters.cpp:1358)
I did some digging into the Python DroneKit sdk to see what format they use and it looks like they have a different structureas linked. Shown below as well:
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,-34.364114, 149.166022, 30)
:param target_system: This can be set to any value
(DroneKit changes the value to the MAVLink ID of the connected vehicle before the command is sent).
:param target_component: The component id if the message is intended for a particular component within the target system
(for example, the camera). Set to zero (broadcast) in most cases.
:param seq: The sequence number within the mission (the autopilot will reject messages sent out of sequence).
This should be set to zero as the API will automatically set the correct value when uploading a mission.
:param frame: The frame of reference used for the location parameters (x, y, z). In most cases this will be
``mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT``, which uses the WGS84 global coordinate system for latitude and longitude, but sets altitude
as relative to the home position in metres (home altitude = 0). For more information `see the wiki here
<http://planner.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/#frames_of_reference>`_.
:param command: The specific mission command (e.g. ``mavutil.mavlink.MAV_CMD_NAV_WAYPOINT``). The supported commands (and command parameters
are listed `on the wiki <http://planner.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/>`_.
:param current: Set to zero (not supported).
:param autocontinue: Set to zero (not supported).
:param param1: Command specific parameter (depends on specific `Mission Command (MAV_CMD) <http://planner.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/>`_).
:param param2: Command specific parameter.
:param param3: Command specific parameter.
:param param4: Command specific parameter.
:param x: (param5) Command specific parameter used for latitude (if relevant to command).
:param y: (param6) Command specific parameter used for longitude (if relevant to command).
:param z: (param7) Command specific parameter used for altitude (if relevant). The reference frame for altitude depends on the ``frame``.
Looking at MavSDK's approach, I see that we are missing the first two (Target System and Target Component) and we have an extra parameter at the end (Mission Type). Perhaps this issue has arisen due to ArduPilot developments since your original post?
I started modifying the MissionItem struct, building the SDK etc. but it didn't seem as straight forward as I was hoping!
Perhaps someone in the dev team can check this one out? Happy to test the code in a branch with ArduPilot SITL too.
Thanks in advance!
Hi @OliverHeilmann,
target system and component is something which is added by mavlink sender automatically before sending actual command and we don't need to add that in the mission item.
Looking closer at the warning, it is caused by this for loop:
Now lot of ifs: This seems to be added few months before or at least after September 2022 which is the last time I checked the mission raw worked fine for Ardupilot and use the server built at that commit version which continues to work for me. If you are using the newest mavsdk server it should contain this check which basically checks if the index of the mission item list is always one greater than the value of seq in that mission item. It seems reasonable and should not cause problem as long as you are using the exact same code to create the mission plan. So I would ask you to check the following:
- Are you using the exact same example code to upload the mission?
- Are you able to disable this check, build the server yourself and upload the mission successfully? If yes I would request you to print the mission items in the abovementioned for loop to see what is going on.
My only guess is somehow while marshal/unmarshal of the mission item, the seq value at mission item 1 is dropped since protobuf likes to not send over default values and we are not handling it correctly in the check mentioned above. I might able to have a look at it in a few days or sooner, until then if you can identify the problem would be nice ;)
Best,
khedar
Alright so i did have a look and to me the check for ardupilot mission seems not to be correct. ArduPilot Mission Plan sequence starts at 0 just like PX4, its just that the item 0 should be home position and item 1 should be takeoff and then follows other mission commands.
@julianoes since you implemented this check way back, just wanted to ask if I am missing something here? If not then I would simply remove the if/else blocks and check if seq is starting at 0 and increments monotonically.
Ok, so that's not matching what I was told when I first asked about this. And it did sound very odd to me.
I'm happy to change it if it's really wrong. It would be nice to get some sort of confirmation or proof though.
I followed your suggestions @ykhedar and commented out the check you linked, built the project again and then ran the test that you have posted above.
Looks like the mission/ flight plan is uploaded correctly with the fix (I added a comment with lots of ! to clearly mark the point). Arm then fails but this is a gyro inconsistency issue which does not seem code related - will investigate further and post anything relevant.
04:53:19|Info ] MAVSDK version: v1.4.12-dirty (mavsdk_impl.cpp:20)
[04:53:19|Info ] New system on: 127.0.0.1:40465 (with sysid: 1) (udp_connection.cpp:192)
[04:53:19|Debug] New: System ID: 1 Comp ID: 1 (mavsdk_impl.cpp:496)
[04:53:19|Debug] Component Autopilot (1) added. (system_impl.cpp:377)
[04:53:20|Warn ] Vehicle type changed (new type: 2, old type: 0) (system_impl.cpp:225)
[04:53:20|Debug] Discovered 1 component(s) (system_impl.cpp:578)
Waiting for system to be ready
Waiting for system to be ready
!!!!!!DISABLED MISSION CHECK!!!!!!
[04:53:21|Debug] MAVLink: info: Flight plan received (system_impl.cpp:250)
/home/oliver/.../Tests/px4sitl.cpp:144: Failure
Expected equality of these values:
action.arm()
Which is: Command Denied
Action::Result::Success
Which is: Success
[04:53:21|Debug] MAVLink: critical: Arm: Gyros inconsistent (system_impl.cpp:250)
@julianoes thanks for looking into this as well. I see you merged and posted says the below. What does this mean in reference to a fix going forward? Thanks!
I was wrong. The sequence just increases normally from 0, even if 0 is the home position.
EDIT!
I just ran a cut down version of the mission integration tests using ArduRover SITL software and the mission elements
worked (with the commented out for loop).
I did also have to cut out the ASSERT_EQ line for setting takeoff altitude as this test fails consistently with the following error:
[12:56:49|Warn ] sending again, retries to do: 3 (MIS_TAKEOFF_ALT). (mavlink_parameters.cpp:1340)
[12:56:49|Warn ] sending again, retries to do: 2 (MIS_TAKEOFF_ALT). (mavlink_parameters.cpp:1340)
[12:56:50|Warn ] sending again, retries to do: 1 (MIS_TAKEOFF_ALT). (mavlink_parameters.cpp:1340)
[12:56:50|Error] Error: Retrying failed get param busy timeout: MIS_TAKEOFF_ALT (mavlink_parameters.cpp:1358)
/home/oliver/.../Tests/px4sitl.cpp:142: Failure
Expected equality of these values:
action.set_takeoff_altitude(10.0f)
Which is: Parameter Error
Action::Result::Success
Which is: Success
Hi, @OliverHeilmann, the PR #2064 was the fix to the problem and it was merged, so you should be able to compile the latest master branch and should not see the problem.
Regarding the set_takeoff_issue, i guess in rover that command is not supported i guess :D Since rover cannot change takeoff altitude. So its fine to comment that out.
Also its better to open a new issue if you face further problems, rather than discussing in an old and closed issue ;)
This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there: