MAVlink Crashing Arduino

Using Arduino Networking, Protocols, and Devices
1

Hi all,

Does anyone have experience with MAVlink on Arduino.

There is one function in the standard code that seems to just completely crash the chip. I'm using the standard tutorial:
http://qgroundcontrol.org/dev/mavlink_arduino_integration_tutorial

The function in question is mavlink_parse_char(..)

I can receive a message, when it has the whole message it prints that it has succeeded, then crashes as if it's in an infinite while loop, bit there isn't one in the code!

My code:

#include <inttypes.h>
//#include <SoftwareSerial.h>
#include "/home/simon/Desktop/arduino-1.0/libraries/mavlink/include/mavlink.h"
//#include "../mavlink/include/mavlink.h"        // Mavlink interface

uint8_t             buf[MAVLINK_MAX_PACKET_LEN];
uint16_t            len;


void mavlink();
void comm_receive();


void setup() { 
  // Initialise Serial & baud rate
  Serial.begin(115200);
  Serial.println("Booting...");

  // Set ADC reference voltage to 3.3v
  analogReference(EXTERNAL);

}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
  while(true){
    mavlink();
    delay(1000);
  }
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void mavlink(){
  // Define the system type (see mavlink_types.h for list of possible types) 
  int system_type = MAV_GROUND;
  int autopilot_type = MAV_AUTOPILOT_GENERIC;

  // Initialize the required buffers 
  mavlink_message_t   msg;
  //uint8_t             buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  // mavlink_message_heartbeat_pack(system id, component id, message container, system type, MAV_AUTOPILOT_GENERIC)
  mavlink_msg_heartbeat_pack(79, 1, &msg, system_type, autopilot_type);

  // Copy the message to send buffer 
  //uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  len = mavlink_msg_to_send_buffer(buf, &msg);

  // Send the message (.write sends as bytes)
  Serial.print("Sent ");
  Serial.print(len,DEC);
  Serial.print(" mavlink chars = ");
  Serial.write(buf,len);
  Serial.println(" ");

  comm_receive();
}

void comm_receive() { 
  static int packet_drops = 0;
  static int mode = MAV_MODE_UNINIT;

  mavlink_message_t msg; 
  mavlink_status_t status;

  Serial.print("buf = ");
  Serial.println((char)buf[0]);
  Serial.print("len = ");
  Serial.println(len,DEC);

  static char y = 0;
  bool msg_received = false;

  //receive data over serial 
  for (char x=0 ; x<len ; x++) { 
    uint8_t c = *(buf+x);
    Serial.print("Receiving Mavlink ASCII# = ");
    //Serial.println(static_cast<char>(c));  // ASCII
    Serial.println(c);                     // Number

    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)){
      Serial.print("MAVlink message received!");
      break;
    }
  }
}
///////////////////////////////////////////////////////////////////////////

Any ideas?

Cheers, & merry xmas.

Simon

The MAVlink function code:

/**
 * This is a convenience function which handles the complete MAVLink parsing.
 * the function will parse one byte at a time and return the complete packet once
 * it could be successfully decoded. Checksum and other failures will be silently
 * ignored.
 *
 * @param chan     ID of the current channel. This allows to parse different channels with this function.
 *                 a channel is not a physical message channel like a serial port, but a logic partition of
 *                 the communication streams in this case. COMM_NB is the limit for the number of channels
 *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
 * @param c        The char to barse
 *
 * @param returnMsg NULL if no message could be decoded, the message data else
 * @return 0 if no message could be decoded, 1 else
 *
 * A typical use scenario of this function call is:
 *
 * @code
 * #include <inttypes.h> // For fixed-width uint8_t type
 *
 * mavlink_message_t msg;
 * int chan = 0;
 *
 *
 * while(serial.bytesAvailable > 0)
 * {
 *   uint8_t byte = serial.getNextByte();
 *   if (mavlink_parse_char(chan, byte, &msg))
 *     {
 *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
 *     }
 * }
 *
 *
 * @endcode
 */
static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{
	static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];

	// Initializes only once, values keep unchanged after first initialization
	mavlink_parse_state_initialize(mavlink_get_channel_status(chan));

	mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
	mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
	int bufferIndex = 0;

	status->msg_received = 0;

	switch (status->parse_state)
	{
	case MAVLINK_PARSE_STATE_UNINIT:
	case MAVLINK_PARSE_STATE_IDLE:
		if (c == MAVLINK_STX)
		{
			status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
			mavlink_start_checksum(rxmsg);
		}
		break;

	case MAVLINK_PARSE_STATE_GOT_STX:
		if (status->msg_received)
		{
			status->buffer_overrun++;
			status->parse_error++;
			status->msg_received = 0;
			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
		}
		else
		{
			// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
			rxmsg->len = c;
			status->packet_idx = 0;
			mavlink_update_checksum(rxmsg, c);
			status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
		}
		break;

	case MAVLINK_PARSE_STATE_GOT_LENGTH:
		rxmsg->seq = c;
		mavlink_update_checksum(rxmsg, c);
		status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
		break;

	case MAVLINK_PARSE_STATE_GOT_SEQ:
		rxmsg->sysid = c;
		mavlink_update_checksum(rxmsg, c);
		status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
		break;

	case MAVLINK_PARSE_STATE_GOT_SYSID:
		rxmsg->compid = c;
		mavlink_update_checksum(rxmsg, c);
		status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
		break;

	case MAVLINK_PARSE_STATE_GOT_COMPID:
		rxmsg->msgid = c;
		mavlink_update_checksum(rxmsg, c);
		if (rxmsg->len == 0)
		{
			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
		}
		else
		{
			status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
		}
		break;

	case MAVLINK_PARSE_STATE_GOT_MSGID:
		rxmsg->payload[status->packet_idx++] = c;
		mavlink_update_checksum(rxmsg, c);
		if (status->packet_idx == rxmsg->len)
		{
			status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
		}
		break;

	case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
		if (c != rxmsg->ck_a)
		{
			// Check first checksum byte
			status->parse_error++;
			status->msg_received = 0;
			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
			if (c == MAVLINK_STX)
			{
				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
				mavlink_start_checksum(rxmsg);
			}
		}
		else
		{
			status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
		}
		break;

	case MAVLINK_PARSE_STATE_GOT_CRC1:
		if (c != rxmsg->ck_b)
		{// Check second checksum byte
			status->parse_error++;
			status->msg_received = 0;
			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
			if (c == MAVLINK_STX)
			{
				status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
				mavlink_start_checksum(rxmsg);
			}
		}
		else
		{
			// Successfully got message
			status->msg_received = 1;
			status->parse_state = MAVLINK_PARSE_STATE_IDLE;
			memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
		}
		break;
	}

	bufferIndex++;
	// If a message has been sucessfully decoded, check index
	if (status->msg_received == 1)
	{
		//while(status->current_seq != rxmsg->seq)
		//{
		//	status->packet_rx_drop_count++;
		//               status->current_seq++;
		//}
		status->current_rx_seq = rxmsg->seq;
		// Initial condition: If no packet has been received so far, drop count is undefined
		if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
		// Count this packet as received
		status->packet_rx_success_count++;
	}

	r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
	r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
	r_mavlink_status->packet_rx_drop_count = status->parse_error;
	status->parse_error = 0;
	return status->msg_received;
}
2 
void loop() {
  while(true){
    mavlink();
    delay(1000);
  }
}

I'm curious why you are running an infinite loop inside an infinite loop.

I can receive a message, when it has the whole message it prints that it has succeeded, then crashes as if it's in an infinite while loop, bit there isn't one in the code!

A crash and an infinite loop are not the same thing.

Add some Serial.print() statements to the function to see where it is having problems.

We have no clue what is sending the data to the Arduino, how that device is connected, or what the data looks like. If you shared that information, you might get more help.

3

No data is sent here. If you see the mavlink() function just fills a buffer, which the comm_receive() function then reads from.

Yes, I understand a crash & inf loop arent the same, typo.

I have many Serial.prints in the code and have discovered that when the mavlink_parse_char function returns true, the if loop exits, it prints the final line in the code which I have added as "success", and then the whole thing just stops.

Iit is definitely the mavlink_parse_char function...or more like how I am using it since I know that function is good code.

My confusion is to why the if exits, "success" is printed, then it stops, leading me to think that possibly things arent being executed in the same order as theyre written in the code? Is this possible?

4

We have no clue what is sending the data to the Arduino, how that device is connected, or what the data looks like. If you shared that information, you might get more help.

It is the Nano board, not connected to anything but a power supply. All the code is in the post above.

5

I have many Serial.prints in the code

Not near enough, apparently. Does the loop() function get called again? Does the infinite loop in the loop() function (you haven't explained why it's there) iterate? Does setup() get called again?

What is the value of status->parse_state? Which case gets executed?

What output are you seeing on the serial monitor? I'm not all that fond of guessing games. Show some data!

6

For simplicity, the main loop is now:

void loop() {
  while(true){
    mavlink();
    Serial.println("END OF LOOP");
    delay(1000);
  }
}

All the serial.prints display what they are supposed to, including the END OF LOOP which only gets triggered when a message has been received and the mavlink_parse_char() returns true, indicated by "MAVlink message received!" being displayed.

After this, nothing happens.

While(){} inside loop(){} does nothing, I just prefer coding like this as arduino doesnt use main(){}. It has no effect on anything bar maybe using another byte or two of ROM.

7