Planning missions via Mavlink

ArduCopter Copter 3.2
1

Hi everyone,

I’m struggling with an issue currently. I’m working on a copter (APM2.8), 3.2.1 release. I’m planning to set a mission via mavlink protocol using an Arduino and its serial port. I’ve written an script and some commands are working properly, for example, ARMING the copter (mavlink_msg_command_long_pack(255, 1, &msg, 1, 0, MAV_CMD_COMPONENT_ARM_DISARM, 0, 1,0,0,0,0,0,0)), CLEAR PREVIOUS MISSION (mavlink_msg_mission_clear_all_pack(255, 1, &msg, 1, 0)).

I’m aware of the mission protocol described in the following link: http://qgroundcontrol.org/mavlink/waypoint_protocol

However, I’m not sure about this protocol due to the old terminology (for example: WAYPOINT_ACK should be MISSION_ACK) and because it’s not working. Anyway, I’ve been trying this protocol, I show you my function calls below(I’m not sure about the function parameters) :

//mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count)

mavlink_msg_mission_count_pack(255, 1, &msg, 1, 0, 3);

//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)

mavlink_msg_mission_item_pack(255, 1, &msg, 1, 0, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 25);

//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)

mavlink_msg_mission_item_pack(255, 1, &msg, 1, 0, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 2, 0, 0, 52.464217, -1.280222, 50);

//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)

mavlink_msg_mission_item_pack(255, 1, &msg, 1, 0, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0, 0);

Can anyone help me with this problem? Are the function parameters correct?

I will really appreciate your help.

Thanks in advance.

Sergi

2

A couple issues. First, the MAV_FRAME_GLOBAL should probably be MAV_FRAME_GLOBAL_RELATIVE_ALT unless you happen to be flying exactly at sea level or otherwise account for the flying site offset.

You also don’t send the proper sequence numbers for each MISSION_ITEM packet. They should be incremented by one for each subsequent mission item. Sequence numbers are indexed starting at 1 and goes through your highest waypoint number.

You also need to send the waypoints based on the MISSION_REQUEST messages you receive. So you’ll send the MISSION_COUNT packet, and monitor the incoming telemetry for a MISSION_REQUEST message with a sequence number. You need to reply to that with the MISSION_ITEM with the matching sequence number. That process will be repeated until all the waypoints have been successfully received by the autopilot, at which point you’ll get a MISSION_ACK.

Under a really reliable link, you might be able to fake the transaction with careful delays, but it’s probably not great practice to do so.

In general, you’re really close. The code draft helps a ton.

3

Thank you for your reply jmack!

I’ve corrected those mistakes but it’s still not working… I send the MISSION_COUNT and I receive MISSION REQUEST with the following parameters:

  • seq = 0.
  • target_component = 0.
  • target_system = 0.

I’m answering with:

//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)

mavlink_msg_mission_item_pack(255, 1, &msg, 1, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 25)

But I never receive a MISSION_REQUEST again. Is any parameter not matching?

Thank you so much for your help!

4

I will have to experiment. It looks correct to me. But the reply with all zeroes in seq, component, and system is weird.

What are the target_system and target_component in your received HEARTBEAT messages?

5

Thank you for your reply jmack. I’m sorry for the delay I’ve been out for few days.

I don’t have target_system and target_component in the received HEARTBEAT. Otherwise I received the following parameters:

  • Custom_mode: 0
  • Type: 2
  • Autopilot: 3
  • Base_mode: 81
  • System_status: 3
  • Mavlink_version: 3

I reach to a new doubt, How frequently I need to send heartbeats from the arduino to the APM? I’m sending the HEARTBEAT command with the following parameters:

int system_type = MAV_TYPE_QUADROTOR;
int autopilot_type = MAV_AUTOPILOT_ARDUPILOTMEGA;

//mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
mavlink_msg_heartbeat_pack(255, 200, &msg, system_type, autopilot_type,0x51,0,3);

Are the parameters correct?

I have another problem too. I’m not receiving the MISSION_ACK when I send the MISSION_CLEAR_ALL. But if I store a mission in the APM I can read it with the mission planner. Once I run the MISSION_CLEAR_ALL I read the missions from the mission planner but there’s nothing. It seems to be working, but I’m not getting the MISSION_ACK.

Thank you for your help!

6

I just reviewed a log of a mission via Mission Planner, and there are a few things that look wrong in your code.

I said the wrong fields earlier for the heartbeat messages. You should be receiving heartbeat messages with:

  • system_id = 1
  • component_id = 1

Do these match?

If so, you are not sending to the proper target_system and target_component, as they should match what you’re getting from the HEARTBEAT. In the default case, target_system and target_component should be 1.

You do not need to send heartbeat messages to the autopilot. I would recommend not sending heartbeats for this application because you may inadvertently trigger failsafe.

Here’s the sequence from a log I just grabbed…

mavlink_msg_mission_count_pack(255, 190, &msg, 1, 1, 4);
// Wait for mission_request
mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, 0, 0, 0); // home WP
// Wait for mission_request
mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 1, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 25);
// Wait for mission_request
mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 2, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 2, 0, 0, 52.464217, -1.280222, 50);
// Wait for mission_request
mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 3, MAV_FRAME_GLOBAL, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0, 0);
// Wait for mission_ack
mavlink_msg_mission_ack_pack(255, 190, &msg, 1, 1, 0);
7

Thank you for your reply jmack!

I think it’s about to work! I made the changes you told me and also I implemented the timeouts and now I’m receiving the MISSION_ACK and MISSION_REQUEST commands, but not correctly. I’m getting an strange behaviour, I’ll try to explain it as detailed as possible:

The mission I’m trying to store is the following:

  • Take off
  • Waypoint
  • RTH

And this what I’m sending and receiving:

mavlink_msg_mission_clear_all_pack(255, 190, &msg, 1, 1) //CLEAR ALL

Here I’m getting MISSION_ACK (Type: 0, target_component: 190, target_system: 255)

mavlink_msg_mission_count_pack(255, 190, &msg, 1, 1, 3) //COUNT MISSION ITEMS

Here I’m getting MISSION_REQUEST (seq: 0, target_component: 0, target_system: 0)

mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 25) //TAKE OFF

Here I’m getting again MISSION_REQUEST (seq: 0, target_component: 0, target_system: 0). I don’t understand the seq=0…

mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 1, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 2, 0, 0, 52.088767, 1.952730, 50) //WAYPOINT

Here I’m getting MISSION_REQUEST (seq: 1, target_component: 0, target_system: 0).

mavlink_msg_mission_item_pack(255, 190, &msg, 1, 1, 2, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0, 0) //RTH

Here I’m getting MISSION_REQUEST (seq: 2, target_component: 0, target_system: 0).
Then I receive MISSION_ACK (Type: 13, target_component: 190, target_system: 255) which shows a seq error. But inmediately after I receive MISSION_ACK (Type: 0, target_component: 190, target_system: 255).

I check the mission with mission planner and there’s only the WAYPOINT and the RTH but the TAKEOFF is missing.

Also, if I try to add an extra command it answers me with the seq error MISSION_ACK (Type: 13, target_component: 190, target_system: 255).

I don’t really know how to solve this problem. I think I’m about to achieve it. Please, do you see anything wrong?

Thank you so much for your help. Your experience is really useful for me.

8

seq=0 is your HOME waypoint, and your flight plan starts with seq=1. You’ll need to set the home waypoint along with the others (see my example above). You could monitor the GPS telemetry to grab the home location, maybe? Or you could download the current mission and copy the existing home waypoint.

So, you’ll want to send mission_count with count=4, and include your home waypoint as seq=0 and your mission as seq=1 through seq=3.

9

I read that the home location will be reset to the current location when you arm the copter, so you probably do not need to be really careful to get the home location perfect. But setting it to the current GPS location “just in case” is probably still good practice.

10

IT IS WORKING! Thank you so much for your help!
But it’s not robust. It works one out of five tries more or less. But I’m sure it’s due to a very busy serial port. I think the serial TX and RX buffers are overcrowded.

Once I get the thing working I get new doubts: Is it possible to plan a mission in this way on flight time? Before planning the mission is there any way to check for gps lock? And last but not least, I’ve seen a command to jump mission item, I suppose it works on flight time, how it really works?

Thank you for sharing your experience jmack.

11

I hope you can debug the serial port issue. It should be very reliable with normal telemetry over 57600 baud. Maybe it’s related to the Arduino or wiring.

Do not worry too much about the GPS home. If you set lat/lon/alt to 0/0/0, that will work, because the home position will be re-set when the copter gets armed. But in the rare case that the copter somehow is already armed when your code runs, this would result in a fly-away.

But here’s the easiest way I can think to set a reasonable home position for backup. Monitor for GPS_RAW messages, and wait for the fix_type field to equal GPS_FIX_TYPE_3D_FIX (3). When it does, save that lat/lon/alt to variables to set your home position.

12

Thank you for your reply jmack.

I don’t really understand how GPS_RAW works. Do I need to send the command GPS_RAW_INT and then wait for answer? I would read the variables fix_type, lat,lon and alt. No problem. But For me it doesn’t make any sense to send GPS_RAW_INT with the following parameters:

mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible)

Lat, lon, alt -> I don’t know them, I’m asking for them.
fix_type -> the same, I don’t know, I’m asking for it.

And the same for the others, I find that all the parameters of the function are what I would like to know…I’m sorry if this is such a trivial question…

Apart from that, Is it possible to plan missions in the flight time? Once I’m flying?

Thank you.

13

Either GPS_RAW or GPS_RAW_INT should be in the telemetry stream if your system has a GPS attached. You should be able to wait for the autopilot to send them just like the HEARTBEAT packets. You should not have to send anything new.

I don’t know which one is sent by default, but I think it’s actually GPS_RAW_INT. The difference is one is “interpolated” so it can be sent at a different message rate from the actual GPS data.

Of course, there’s a chance your system has a non-standard configuration and those messages are not being sent by the autopilot. But we can address that later if you’re not seeing either GPS_RAW or GPS_RAW_INT in the autopilot telemetry.

14

So I think it could be the last option. I’m not receiving GPS_RAW_INT. And GPS_RAW doesn’t let me compile the code. It tells me it doesn’t exists. Can I configure the APM to send the GPS_RAW_INT messages?

Thank you!

15

What is the APM’s parameter SR1_EXT_STAT set to? If it’s 0, set it to something like 4 (4 times per second).

16

I got it set to 2… It should be working. Is there an other way to ask for the home position? I don’t want to use it as a home backup. I’m planning to store and compare it with a list of coordinates and make the Copter decide where to go on its own depending on the proximity compared to the home position.

17

You can ask for the home position by downloading the current mission immediately after arming the copter. The home location will be waypoint 0. This is done by sending a MISSION_REQUEST_LIST, waiting for a MISSION_COUNT, and sending a series of MISSION_REQUEST as you receive MISSION_ITEM replies.

It’s much simpler to get GPS_RAW_INT working, so I’d try and debug that. What messages (besides HEARTBEAT) are you getting in the telemetry stream?

18

Yes, it’s so much simpler GPS_RAW_INT method. I’m only listening for HEARTBEAT and GPS_RAW_INT. What other messages I should be receiving?

19

I don’t have the specific list, but there are 20-30 different messages as part of the default telemetry. You might try printing out the msgid of each decoded mavlink packet as they come through.

Are you using the “ardupilotmega” dialect when generating your C library? If not, that might also explain why you get occasional missed messages when sending a mission.

20

I’ve listened for msgid and I’m only receiving 0 (MAVLINK_MSG_ID_HEARTBEAT), 30 (MAVLINK_MSG_ID_ATTITUDE) and 253 (MAVLINK_MSG_ID_STATUSTEXT). There’s no trace of MAVLINK_MSG_ID_GPS_RAW_INT… Is there any parameter to activate it?

Yes, I’m using the “ardupilotmega” to generate the C library.