Uploading a waypoint and interfacing the Pixhawk to an Arduino using Mavlink
I wrote this post to outline the Mavlink Mission Protocol for uploading a new flight plan to the Autopilot using the Arduino Mavlink library.
A good starting point to understanding how to use the files included in the Arduino Mavlink library is provided here:
https://www.locarbftw.com/understanding-the-arduino-mavlink-library/
Understanding the Mavlink Mission Protocol
In order to successfully upload a new flightplan to a Pixhawk using an Arduino one needs to understand the Mavlink Mission Protocol. See: https://mavlink.io/en/services/mission.html#mission_types
In the image above, the mission protocol is explained in visual form.
In more detail (copied from the link above), the sequence of operations are:
- GCS (client) sends MISSION_COUNT including the number of mission items to be uploaded (count)
- A timeout must be started for the GCS to wait on the response from Drone (MISSION_REQUEST_INT) .
- Drone (server) receives the message, and prepares to upload mission items.
- Drone responds with MISSION_REQUEST_INT requesting the first mission item (seq==1).
- GCS receives MISSION_REQUEST_INT and responds with the requested mission item in a MISSION_ITEM_INT message.
- Drone and GCS repeat the MISSION_REQUEST_INT/MISSION_ITEM_INT cycle, iterating seq until all items are uploaded.
- For the last mission item, the drone responds with MISSION_ACK with the result of the operation result: type (MAV_MISSION_RESULT):
- On success, type must be set to MAV_MISSION_ACCEPTED.
- On failure, type must set to MAV_MISSION_ERROR or some other error code.
- GCS receives MISSION_ACK: If MAV_MISSION_ACCEPTED the operation is complete. If an error, the transaction fails but may be retried.
This high level understanding of how the Mavlink Mission Protocol flows is KEY in understanding how the Pixhawk manages a flight plan (or way-point) sent by the Arduino. Without following the steps shown above, the Pixhawk will not successfully accept a new flight plan/way-point.
The more you know…
The implementation status for the PX4 Pixhawk (at time of writing):
- Flight plan missions:
- upload, download, clearing missions, and monitoring progress are supported as defined in this specification.
- partial upload and partial download are not supported.
- Geofence “missions” are supported as defined in this specification.
- Rally point “missions” are not supported on PX4.
- You can interchange MISSION_ITEM_INT with MISSION_ITEM. The only difference is that MISSION_ITEM_INT encodes the latitude and longitude as integers rather than floats.
- It seems that setting the initial HOME way-point in the example below does not seem to affect anything as HOME will be set once the vehicle is armed.
**When first using this library and referencing the Mavlink documentation, MISSION_ITEM_INT was referenced as the way to send way-points to the Pixhawk. However, this file was not included in the Arduino mavlink library. It WAS included in the Mavlink2 version of the library which I then tried to install and work with unsuccessfully. After finding this note in the Mission Protocol documentation, I could have saved myself some time and just used the mavlink_msg_mission_item.h file instead of chasing down a mavlink_msg_mission_item.int file unnecessarily.
Simplifying the protocol
- Arduino sends MISSION_COUNT including the number of mission items to be uploaded.
- Pixhawk receives the message, responds with MISSION_REQUEST requesting the first mission item.
- Arduino receives MISSION_REQUEST and responds with the requested mission item in a MISSION_ITEM message.
- Pixhawk and Arduino repeat the MISSION_REQUEST_INT/MISSION_ITEM_INT cycle until all items are uploaded.
- For the last mission item, the Pixhawk responds with MISSION_ACK with the result of the operation result: type (MAV_MISSION_RESULT):
- On success, type must be set to MAV_MISSION_ACCEPTED (0).
- On failure, type must set to MAV_MISSION_ERROR (1).
- Arduino receives MISSION_ACK: If failed or successful, then do something (example code below not given for this step).
Coding the mission protocol
Arduino Mavlink Library files required (located in common directory):
Files to handle sending of mission variables to Pixhawk
- mavlink_msg_mission_count.h
- mavlink_msg_mission_item.h
Files to handle receiving messages sent by Pixhawk
- mavlink_msg_mission_request.h
- mavlink_msg_mission_ack.h
If we look at the the Mission Protocol, we see that what actually gets the Pixhawk to begin the process of accepting parameters of new way-points is not the mavlink_msg_mission_item.h parameters (which we would expect), but the mavlink_msg_mission_count.h parameters instead. Sending a mavlink_msg_mission_count message lets the Pixhawk know that we want to upload some new way-points and how many to expect. Without initiating the protocol with this Mavlink message, the Pixhawk would not know what to do with the sent mission item parameters.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 |
void mission_count() { //Step #1 of uploading a new waypoint uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software uint8_t _component_id = 2; // component id of sending station 2 works fine uint8_t _target_system = 1; // Pixhawk id uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine) uint16_t count = 2; // How many items to upload (HOME coordinates are always the first way-point) // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_mission_count_pack(_system_id, _component_id, &msg, _target_system, _target_component, count); //uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial1.write(buf, len); } |
The mission_count() function packs the data we specify in variables and sends it to the Pixhawk – letting it know we want to upload two way-points (with HOME being the first way-point, and another way-point being the first real destination). After this message is sent, the Pixhawk will then send a Mission Request message back to the Arduino specifying the sequence of way-points it is expecting. We then use this message as a trigger to begin the next step in the protocol.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
// Step 2 uploading a new waypoint - Check for mission replies case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t missionreq; mavlink_msg_mission_request_decode(&msg, &missionreq); Serial.print("\nMission Req Sequence: ");Serial.println(missionreq.seq); Serial.print("\SysID: ");Serial.println(missionreq.target_system); Serial.print("\CompID: ");Serial.println(missionreq.target_component); if (missionreq.seq == 0) { create_home(); Serial.print("Sent Home: \n"); } if (missionreq.seq == 1) { create_waypoint(); Serial.print("Sent Waypoint: \n"); } } break; |
The code snippet above does not account for timeouts and retries and it certainly isn’t efficient, but it is perfect for illustrating the protocol steps involved. This code snippet above listens for the Mission Request Sequence message sent from the Pixhawk and handles it by looking for the sequence number specified. If the Pixhawk is asking for the first way-point (sequence 0), the code will run the create_home function shown below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 |
void create_home() { //Step 3 of uploading a new waypoint (send HOME coordinates) uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software uint8_t _component_id = 2; // component id of sending station 2 works fine uint8_t _target_system = 1; // Pixhawk id uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine) uint16_t seq = 0; // Sequence number uint8_t frame = 0; // Set target frame to global default uint16_t command = MAV_CMD_NAV_WAYPOINT; // Can be substituted with other commands https://mavlink.io/en/services/mission.html#mission_types uint8_t current = 0; // false:0, true:1 - When downloading, whether the item is the current mission item. uint8_t autocontinue = 0; // Always 0 float param1 = 0; // Loiter time float param2 = 0; // Acceptable range from target - radius in meters float param3 = 0; // Pass through waypoint float param4 = 0; // Desired yaw angle float x = 45.464217; // Latitude - degrees float y = -1.280222; // Longitude - degrees float z = 200; // Altitude - meters // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial1.write(buf, len); } |
The create_home() function above packs the parameters specified in the variables into a Mavlink message which specifies the home coordinates using the MAV_CMD_NAV_WAYPOINT command.
**In the mission protocol documentation, one can find other commands which can be substituted to specify DO (like activating a servo) and CONDITION commands (pausing the mission).**
Once this message is sent to the Pixhawk, the Pixhawk will then send another Mission Request Sequence message to the Arduino containing the sequence number of the way-point it expects next (sequence 1).
Once again, the Mission Request Sequence message is handled by the code snippet in step 2, and executes the create_waypoint() function
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 |
void create_waypoint() { //Step 3 continuation of uploading a new waypoint (send 1st coordinates) uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software uint8_t _component_id = 2; // component id of sending station 2 works fine uint8_t _target_system = 1; // Pixhawk id uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine) uint16_t seq = 1; // Sequence number uint8_t frame = 0; // Set target frame to global default uint16_t command = MAV_CMD_NAV_WAYPOINT; // Can be substituted with other commands https://mavlink.io/en/services/mission.html#mission_types uint8_t current = 1; // false:0, true:1 - When downloading, whether the item is the current mission item. uint8_t autocontinue = 0; // Always 0 float param1 = 0; // Loiter time float param2 = 0; // Acceptable range from target - radius in meters float param3 = 0; // Pass through waypoint float param4 = 0; // Desired yaw angle float x = 15.464217; // Latitude - degrees float y = -11.280222; // Longitude - degrees float z = 200; // Altitude - meters // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial1.write(buf, len); } |
This function is pretty much identical to the create_home() function with a different GPS coordinate and seq number. I also set “current” to 1 to specify it as the current mission item (does it really have an effect if the mission is on auto?). After this way-point is sent and successfully received by the Pixhawk, the Pixhawk is no longer waiting for any further coordinates and sends back a MISSION_ACK message instead. This MISSION_ACK message will contain either a 0 (successful), or 1 (failure) for its TYPE.
In order to handle this message, we use the function below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t missionack; mavlink_msg_mission_ack_decode(&msg, &missionack); Serial.print("\nMission Ack Sequence: ");Serial.println(missionack.type); Serial.print("\SysID: ");Serial.println(missionack.target_system); Serial.print("\CompID: ");Serial.println(missionack.target_component); if (missionack.type == 1) { Serial.print("\nMission upload FAILED: ");Serial.println(missionack.type); } if (missionack.type == 0) { Serial.print("\nMission upload SUCCESSFULL: ");Serial.println(missionack.type); } } break; |
Serial output of all of the above:
Handling the MISSION_ACK message aids in deciding whether or not to retry the entire process in case something has failed (I did not show this in my code examples). This is definitely a function to incorporate into your code as I noticed uploading of way-points will sometimes fail if data-streams are being sent from the Pixhawk at the same time.
Putting it all together
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 |
#include <mavlink.h> //#include <SoftwareSerial.h> //#define RXpin 0 //#define TXpin 1 //SoftwareSerial SerialMAV(RXpin, TXpin); // sets up serial communication on pins 3 and 2 void setup() { Serial1.begin(57600); //RXTX from Pixhawk (Port 19,18 Arduino Mega) Serial.begin(57600); //Main serial port to read the port mission_count(); } void loop() { MavLink_receive(); } //function called by arduino to read any MAVlink messages sent by serial communication from flight controller to arduino void MavLink_receive() { mavlink_message_t msg; mavlink_status_t status; while(Serial1.available()) { uint8_t c= Serial1.read(); //Get new message if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { //Handle new message from autopilot switch(msg.msgid) { // Step 2 uploading a new waypoint - Check for mission replies case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t missionreq; mavlink_msg_mission_request_decode(&msg, &missionreq); Serial.print("\nMission Req Sequence: ");Serial.println(missionreq.seq); Serial.print("\SysID: ");Serial.println(missionreq.target_system); Serial.print("\Compid: ");Serial.println(missionreq.target_component); if (missionreq.seq == 0) { create_home(); Serial.print("Sent Home: \n"); } if (missionreq.seq == 1) { create_waypoint(); Serial.print("Sent Waypoint: \n"); } } break; case MAVLINK_MSG_ID_MISSION_ACK: // Step 4 uploading a new waypoint - Receive Mission Ack Message { mavlink_mission_ack_t missionack; mavlink_msg_mission_ack_decode(&msg, &missionack); Serial.print("\nMission Ack Sequence: ");Serial.println(missionack.type); Serial.print("\SysID: ");Serial.println(missionack.target_system); Serial.print("\CompID: ");Serial.println(missionack.target_component); if (missionack.type == 1) { Serial.print("\nMission upload FAILED: ");Serial.println(missionack.type); } if (missionack.type == 0) { Serial.print("\nMission upload SUCCESSFULL: ");Serial.println(missionack.type); } } break; } } } } void mission_count() { //Step #1 of uploading a new waypoint uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software uint8_t _component_id = 2; // component id of sending station 2 works fine uint8_t _target_system = 1; // Pixhawk id uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine) uint16_t count = 2; // How many items to upload (HOME coordinates are always the first way-point) // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_mission_count_pack(_system_id, _component_id, &msg, _target_system, _target_component, count); //uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial1.write(buf, len); } void create_home() { //Step 3 of uploading a new waypoint (send HOME coordinates) uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software uint8_t _component_id = 2; // component id of sending station 2 works fine uint8_t _target_system = 1; // Pixhawk id uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine) uint16_t seq = 0; // Sequence number uint8_t frame = 0; // Set target frame to global default uint16_t command = MAV_CMD_NAV_WAYPOINT; // Specific command for PX4 uint8_t current = 0; // false:0, true:1 - When downloading, whether the item is the current mission item. uint8_t autocontinue = 0; // Always 0 float param1 = 0; // Loiter time float param2 = 0; // Acceptable range from target - radius in meters float param3 = 0; // Pass through waypoint float param4 = 0; // Desired yaw angle float x = 45.464217; // Latitude - degrees float y = -1.280222; // Longitude - degrees float z = 200; // Altitude - meters // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial1.write(buf, len); } void create_waypoint() { //Step 3 continuation of uploading a new waypoint (send 1st coordinates) uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software uint8_t _component_id = 2; // component id of sending station 2 works fine uint8_t _target_system = 1; // Pixhawk id uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine) uint16_t seq = 1; // Sequence number uint8_t frame = 0; // Set target frame to global default uint16_t command = MAV_CMD_NAV_WAYPOINT; // Specific command for PX4 uint8_t current = 1; // false:0, true:1 - When downloading, whether the item is the current mission item. uint8_t autocontinue = 0; // Always 0 float param1 = 0; // Loiter time float param2 = 0; // Acceptable range from target - radius in meters float param3 = 0; // Pass through waypoint float param4 = 0; // Desired yaw angle float x = 31.0529340; // Latitude - degrees float y = -162.0703125; // Longitude - degrees float z = 200; // Altitude - meters // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial1.write(buf, len); } |
Awesome work!!!
Thank you so much! You are genius. Hope you are safe in this corona time.
Regards
How we can upload waypoint into arduino
Thanks very much.
I upload code on my board and program change way point correctly but home point doesn’t change though I receive “Sent Home:” message on monitor serial.
Any idea for my problem?
My understanding is that it doesn’t really matter. As soon as you arm the autopilot home coordinates are set automatically.
Thanks for answer.
A question,Do I read previous waypoint before that write new way point with arduino? I don’t find any command for this.
Uploading a new mission plan will overwrite any existing mission plan.
Get perfect results in ESP32 verification. TKS!
Can you kindly try to introduce: How to Use ESP32 to coding mavproxy
Hi,
I am trying to new create a waypoint.Althogh I run your code,there is no difference on plan map.
Do you have any suggestions?
Make sure you turn off all other messaging prior to running the code. Also check to make sure communication between pixhawk and Arduino is working first
16:41:41.881 -> Mission Req Sequence: 0
16:41:41.881 -> SysID: 255
16:41:41.881 -> Compid: 2
16:41:41.881 -> Sent Home:
16:41:41.928 ->
16:41:41.928 -> Mission Req Sequence: 1
16:41:41.928 -> SysID: 255
16:41:41.928 -> Compid: 2
16:41:41.928 -> Sent Waypoint:
16:41:41.928 ->
16:41:41.928 -> Mission Ack Sequence: 0
16:41:41.928 -> SysID: 255
16:41:41.928 -> CompID: 2
16:41:41.975 ->
16:41:41.975 -> Mission upload SUCCESSFULL: 0
I get such a response on the com port.
But there is no change on the mission planner map screen. Do I need to do something different for this?
After the mission is uploaded connect mission planner to the pixhawk and under the flight plan section click read wps. It will read the info from the pixhawk and should show the updated waypoint sent via Arduino.
Yes, thank you very much for your answer.It works.I have one more question.For example if a new waypoint comes from arduino to pixhawk , Will the plane go in that direction?
I mean that when a new point comes from arduino, as you said I should read the mission on mission planner to be able to see.
Does the plane need anything like writing or reading to move in that direction with the new incoming command?or just is it enough for the plane to go on the new route?
After a new update you’ll need to reboot the pixhawk as I found it would sometimes not begin the new plan. If you reboot the pixhawk I also found you need to reset the Arduino after the pixhawk reboots in order to regain serial communication. If you have the pixhawk configured for full autonomous operation like listed on my pixhawk settings page, the autopilot should begin working with the new flight plan.
I guess you are talking about this page https://www.locarbftw.com/ardurover-pixhawk-settings-for-autonomous-operation/#
Also, thank you very much for your explanatory and instructive messages.
Yep that’s the one, No problem!
hi
firstly thanks for your nice tutorial
how i can read(download) mission with arduino to be sure about process
The example sketch already contains the answer as “missionack.”
The pixhawk will return a 0 (success) or 1 (failure) depending on the new mission plan upload. Just write a function to reupload the mission if the pixhawk returns a value of “1.”
thanks 🙂
Thanks a lot for the informative post. Could you make a tutorial on how to download the current mission list from the pixhawk to the arduino/rpi board? Thanks!
I found your tutorial very useful. My objective was to get the rc in and servo out data. This worked well. But msgid 35 only gives 8 channels. Msgid 65 gets 16 channels. But how do I call 65 ?
Hey, thanks for the great tutorial. I’m using UAV to measure soil moisture. So, when the UAV lands, soil moisture will be measured at that location. I’m using automatic flight plans with mission planner to navigate between points and land. So, once the payload measures soil moisture, I need it to send a message to pixhawk to go to next location. I’m using arduino mega. Please let me know, what command to use to achieve this.
One solution might be to set the UAV to land at each waypoint then have the Arduino send a set mode (auto) command after each soil sample is taken.
https://www.locarbftw.com/arduino-mavlink-library-changing-flight-modes/
Hey Adrian,
Thanks a lot for the reply and sorry for such a late response. I will implement this and keep you posted. You are a saviour man.
Hello,
I have a small doubt, once the UAV lands, it will disarm the rotors right, so if I set to automode, will automatically arm the rotor. Or should I arm the rotor through my program please let me know.
Once you have the drone land, you should be able to send a mode command through your program to change the mode on the pixhawk. This should allow it to continue on it’s mission. Unfortunately I’m not sure how all that works together in your intended usage so it’s just trial and error really…