Unable to read data on arduino serial monitor

#1

Hello, I am new to this and am trying to read data on the serial monitor. I have installed mavlink V1 and have tried different variations of this code. My teensyduino is connected to the PX4 cube through the telem2 port and then the teensyduino is connected to my laptop through the USB connection.

My code is below.

#include <mavlink.h>

void setup() {

  Serial.begin(57600);
  Serial1.begin(57600);

}


void reqStreams() {

  int i;
  mavlink_message_t message;
  mavlink_request_data_stream_t store;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];


  int maxStreams = 1;
  uint8_t MAVStreams[maxStreams] = { MAV_DATA_STREAM_ALL };
  uint16_t MAVRates[maxStreams] = { 0x05 }; //5 times per second
  for (i = 0; i < maxStreams; i++)
  {
    //mavlink_msg_request_data_stream_pack(1, 200, &message, store.target_system, store.target_component, MAVStreams[i], MAVRates[i], 1);//this goes thru so ?????? idk if it does the right thing
    uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
    Serial.print("Request stream "); Serial1.write(buf, len); Serial.println("");
  }
}

void dispIMU() {
  //Pack message
  //mavlink_msg_highres_imu_pack(sysid, compid, &msg, packet.time_usec, packet.xacc, packet.yacc, packet.zacc, packet.xgyro, packet.ygyro, packet.zgyro, packet.xmag, packet.ymag, packet.zmag, packet.abs_pressure, packet.diff_pressure, packet.pressure_alt, packet.temperature, packet.fields_updated);
  mavlink_message_t msg;
  mavlink_highres_imu_t highres;
  mavlink_msg_highres_imu_decode(&msg, &highres);

  uint16_t xacc = highres.xacc;
  uint16_t yacc = highres.yacc;
  uint16_t zacc = highres.zacc ;

  Serial.print("HiRes IMU Msgid: ");  Serial.println(msg.msgid);
  Serial.print("Xacc: "); Serial.println(xacc , 5);
  Serial.print("Yacc: "); Serial.println(yacc, 5);
  Serial.print("Zacc: "); Serial.println(zacc , 5);
  Serial.print("\n");
  delay(1000);

}//void 

void loop() {
  while (Serial.available() > 0) {

    char b = Serial.read(); 

    if (b == '1')
    {
      reqStreams();

      int result = Serial1.read();
      if (result > 0) { 

        Serial.println("result > 0");
        dispIMU();

      }//if result
    }//if b
  }//while
}//void

I am trying to get the IMU values and these either show up as 0 values or random numbers that do not correlate to the information in Qgroundcontrol. The message ID is also wrong.

How do I ask for specific messages (highres imu values) and how do I get new data each loop?

Problems with mavlink_msg_interval_pack
#2

There are a couple of things wrong with your code, so let’s try to fix that:

  1. First of all, don’t try to request the stream because it should already be sent, so you don’t need to do that.
  2. All data is continuously sent over serial, so you just have to read through all bytes and parse them and wait for a HIGHRES_IMU packet which you can then decode and display.
    You do the parsing using mavlink_parse_char which you feed one char at a time until it returns 1. For an example how this is done, have a look here: https://github.com/mavlink/c_uart_interface_example/blob/f4811338373d8ddc0f63cbf104291205a527e6da/serial_port.cpp#L133
  3. Once you have a message parsed and filtered the HIGHRES_IMU message out from it, you need to display it correctly. In your dispIMU function, you are casting the xacc, yacc, zacc values from float to uint16_t, however it would be better to display them as floats directly, otherwise you lose precision. Also uint16_t is unsigned, but the acceleration values can be negative.
    To see the types of the values, you can check the docs here: https://mavlink.io/en/messages/common.html#HIGHRES_IMU
#3

Thanks for the speedy reply! I have tried using mavlink_parse_char before and I am always unable to get it to equate anything other than 0. For example, I used this line to parse the characters and then go into a switch case that dealt with each message.

if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status) { 
 

However, it would never go through the if statement.

I will try again after implementing your other suggestions and update again.

#4

I have tried using mavlink_parse_char before and I am always unable to get it to equate anything other than 0

That means something else must be wrong as well. What mavlink headers are you using? I recommend the mavlink 2 headers autogenerated here: https://github.com/mavlink/c_library_v2/

Also, you need to be sure the settings for the serial device as well as the baudrate are all correct.

#5

I have been using https://github.com/mavlink/c_library_v1 .

After making some changes, I am still unable to do the parsing. How would I make sure that the baudrate is correct? I have tried a few different ones but am unsure of the right one.

Also, in mavlink_parse_char, how do I know which channel to use?

My updated code

#include <mavlink.h>

void setup() {

  Serial.begin(57600); 
  Serial1.begin(57600);

}


void dispIMU() {

  mavlink_message_t msg;
  mavlink_highres_imu_t highres;
  mavlink_msg_highres_imu_decode(&msg, &highres);

  float xacc = highres.xacc;
  float yacc = highres.yacc;
  float zacc = highres.zacc ;

  Serial.print("HiRes IMU Msgid: ");  Serial.println(msg.msgid);
  Serial.print("Xacc: "); Serial.println(xacc , 5);
  Serial.print("Yacc: "); Serial.println(yacc, 5);
  Serial.print("Zacc: "); Serial.println(zacc , 5);
  Serial.print("\n");
  delay(1000);

}//void

void loop() {

  uint8_t msgReceived = false;
  mavlink_message_t message;
  mavlink_status_t status;

  while (Serial.available() > 0) {

    char b = Serial.read();

    if (b == '1')
    {
      Serial.println("B");

      if (Serial1.available() > 0) {
      uint8_t result = Serial1.read();
        if (result > 0) {

          Serial.println("result > 0");

          msgReceived = mavlink_parse_char(MAVLINK_COMM_1, result, &message, &status);

          if (msgReceived) {
            Serial.println("Message received");

            switch (message.msgid) {
              case MAVLINK_MSG_ID_HIGHRES_IMU:
                {
                  dispIMU();
                }
              default:
                {
                  Serial.println(message.msgid);
                }
            }//switch

          }//if msg

        }//if result
      }//if serial 1
    }//if b
  }//while
}//void
#6

Where are you connecting this to and what is this port set to?

Also, in mavlink_parse_char, how do I know which channel to use?

It doesn’t matter as long as you keep using the same one, so 1 is fine.

Also, you need to make sure that your dispIMU() function actually used the mavlink_message_t that you’re getting from mavlink_parse_char, so you need to pass it to dispIMU(). Right now you just create yet another message object in dispIMU() which is not initialized so probably just random memory.

It looks like you’re only reading one byte and then you’re going back to Serial.read(). You should be disabling everything checking for your input for now and just loop and read on Serial1 until a message is parsed.

#7

I am now able to get the message parsed and I can display heartbeat messages and a few other messages. However, the messages that I can display seem to be only a certain few that I do not have control over. How can I display a specific message? For example, I need to display the Highres IMU values.

#8

You can request a message by sending this command: https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL

Alternatively, you can look at the configuration: https://dev.px4.io/en/companion_computer/pixhawk_companion.html and you set the correct mode which includes the message you need. You can see the modes with their messages here: https://github.com/PX4/Firmware/blob/b7b5b89969935acd57133f327f640dc1db657fed/src/modules/mavlink/mavlink_main.cpp#L1746-L1904

#9

To send this command, would I use the command long message like this?

mavlink_msg_command_long_pack(1, 1, &message, 1, 1, MAV_CMD_SET_MESSAGE_INTERVAL, 1, 105, 0, 0, 0, 0, 0, 0);
uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
Serial1.write(buf, len);

I was able to achieve it by changing the mode but I want to explore this option too.

#10

This looks almost right but you need to use a different component ID for your arduino, e.g. 241 (https://mavlink.io/en/messages/common.html#MAV_COMP_ID_UART_BRIDGE).

#11

I have been using this function and after parsing the messages, I can see that the command message is parsed and that the Result=0 (which means that it was accepted and executed). However, the IMU values never get parsed. Strangely, the heartbeat message also never gets parsed. What could be the problem?

void send_command() {
mavlink_message_t message;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_command_long_pack(1, 241, &message, 1, 1, MAV_CMD_SET_MESSAGE_INTERVAL, 1, 105, 0, 0, 0, 0, 0, 0);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
  Serial1.write(buf, len);
}
#12

I’m a bit confused what works and what does not at this point.

You need to post the whole code and say exactly what goes wrong where.

#13

Okay, this is my full code. I am trying to send the request command so that I can read the IMU values (without changing the mode).

#include <mavlink.h>

void setup() {
  Serial.begin(57600);
  Serial1.begin(57600);
}

void loop() {

mavlink_message_t message;
uint8_t msgReceived = false;
mavlink_status_t status;


uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_command_long_pack(1, 241, &message, 1, 1, MAV_CMD_SET_MESSAGE_INTERVAL, 1, 105, 0, 0, 0, 0, 0, 0);
uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
Serial1.write(buf, len);

if (Serial1.available() > 0) {
    uint8_t result = Serial1.read();

    msgReceived = mavlink_parse_char(MAVLINK_COMM_1, result, &message, &status);

    if (msgReceived) {
      switch (message.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:
          {
            mavlink_heartbeat_t hb;
            mavlink_msg_heartbeat_decode(&message, &hb);
            Serial.print("\n");
            Serial.println("Heartbeat Message");
            Serial.print("Msgid: ");  Serial.println(message.msgid);
            Serial.print("Type: ");  Serial.println(hb.type);
            Serial.print("Autopilot type: ");  Serial.println(hb.autopilot);
            Serial.print("System mode: ");  Serial.println(hb.base_mode);
            Serial.print("Custom mode: ");  Serial.println(hb.custom_mode);
            Serial.print("System status: ");  Serial.println(hb.system_status);
            Serial.print("Mavlink version: "); Serial.println(hb.mavlink_version);
            delay(1000);
          }
        case MAVLINK_MSG_ID_HIGHRES_IMU:
          {
            mavlink_highres_imu_t highres;
            mavlink_msg_highres_imu_decode(&message, &highres);

            Serial.print("HiRes IMU Msgid: ");  Serial.println(message.msgid);
            Serial.print("Xacc: "); Serial.println(highres.xacc);
            Serial.print("Yacc: "); Serial.println(highres.yacc);
            Serial.print("Zacc: "); Serial.println(highres.zacc);
            Serial.print("Xgyro: "); Serial.println(highres.xgyro);
            Serial.print("Ygyro: "); Serial.println(highres.ygyro);
            Serial.print("Zgyro: "); Serial.println(highres.zgyro);
            Serial.print("Xmag: "); Serial.println(highres.xmag);
            Serial.print("Ymag: "); Serial.println(highres.ymag);
            Serial.print("Zmag: "); Serial.println(highres.zmag);
            Serial.print("Absolute Pressure: "); Serial.println(highres.abs_pressure);
            Serial.print("Diff Pressure: "); Serial.println(highres.diff_pressure);
            Serial.print("Pressure Alt : "); Serial.println(highres.pressure_alt);
            Serial.print("Temperature : "); Serial.println(highres.temperature);
            Serial.print("\n");
            delay(1000);
          }
        default :
          {
            Serial.println(message.msgid);
          }

      }//switch
    }//if msg
  }//if serial 1
}//loop

However, I cannot get the HIGHRES_IMU message ID to go through the switch case even after sending the command. The switch case goes straight to default and prints some inconsistent message IDs like #77 and #30.

#14

Ok, this looks mostly correct now, two things that you should try:

  1. Remove the delay(1000) because like this you stop the loop and you potentially miss data from serial because you are not reading fast enough.
  2. Looking at the arguments of mavlink_msg_command_long_pack I see that param1 is 105 which is for the IMU_HIGHRES message, so that’s correct, but then param2 is 0 which according to the MALink message definitions stands for:

The interval between two messages, in microseconds. Set to -1 to disable and 0 to request default rate.

You set it to 0 which means default, however, presumably the default is not to send it all, otherwise it would already be sent when you connect anyway. Therefore, I’d try to set it to a certain rate. E.g. if you want 20 Hz, then you would use 1/20s = 50000 us.

#15

I was able to get it to work in the end, and this is the code I ended up with. I also found that changing the mode as in this reply helps with some messages.

#include <mavlink.h>

//mavlink_msg_command_long_pack() function
uint8_t target_system = 1;
uint8_t target_component = 1;
uint16_t CMD_LONG_command = 0;
uint8_t CMD_LONG_confirmation = 0;
float CMD_LONG_param1 = 0;
float CMD_LONG_param2 = 0;
float CMD_LONG_param3 = 0;
float CMD_LONG_param4 = 0;
float CMD_LONG_param5 = 0;
float CMD_LONG_param6 = 0;
float CMD_LONG_param7 = 0;

int currenttime = 0;
int previoustime = 0;

int numberhb = 10;
int numberhbpast = 0;

uint8_t system_id = 1;
uint8_t component_id = 241;

void setup() {
  Serial.begin(57600);
  Serial1.begin(57600);

  send_command();
}

void loop() {
  mavlink_message_t message;

  uint8_t type = MAV_TYPE_QUADROTOR;
  uint8_t system_type = MAV_TYPE_GENERIC;
  uint8_t autopilot = MAV_AUTOPILOT_PX4;
  uint8_t base_mode;
  uint32_t custom_mode;
  uint8_t system_status;

  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_heartbeat_pack(1, 241, &message, type, autopilot, base_mode, custom_mode, system_status);

  uint16_t len = mavlink_msg_to_send_buffer(buf, &message);

  currenttime = millis();
  if (currenttime - previoustime >= 1000) {
    previoustime = currenttime;
    Serial1.write(buf, len);

    numberhbpast++;

    if ( numberhbpast >= numberhb ) {
      send_command();
      Serial.println("Command sent");
      numberhbpast = 0;
    }
  }
  readmessage();
}//loop



void send_command() {
  mavlink_message_t message;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  CMD_LONG_param1 = MAVLINK_MSG_ID_HIGHRES_IMU;
  CMD_LONG_param2 = 10000; //100Hz
  CMD_LONG_command = MAV_CMD_SET_MESSAGE_INTERVAL;

  mavlink_msg_command_long_pack(system_id, component_id, &message, target_system, target_component, CMD_LONG_command, CMD_LONG_confirmation, CMD_LONG_param1, CMD_LONG_param2, CMD_LONG_param3, CMD_LONG_param4, CMD_LONG_param5, CMD_LONG_param6, CMD_LONG_param7);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
  Serial1.write(buf, len);

}

void readmessage() {

  uint8_t msgReceived = false;
  mavlink_message_t message;
  mavlink_status_t status;

  if (Serial1.available() > 0) {
    uint8_t result = Serial1.read();

    msgReceived = mavlink_parse_char(MAVLINK_COMM_1, result, &message, &status);

    if (msgReceived) {
      switch (message.msgid) {
        case MAVLINK_MSG_ID_HIGHRES_IMU:
          {
            mavlink_highres_imu_t highres;
            mavlink_msg_highres_imu_decode(&message, &highres);

            Serial.println("High Res IMU Message");
            Serial.print("Xacc: "); Serial.print(highres.xacc); Serial.println(" m/s/s");
            Serial.print("Yacc: "); Serial.print(highres.yacc); Serial.println(" m/s/s");
            Serial.print("Zacc: "); Serial.print(highres.zacc); Serial.println(" m/s/s");
            Serial.print("Xgyro: "); Serial.print(highres.xgyro); Serial.println(" rad/s");
            Serial.print("Ygyro: "); Serial.print(highres.ygyro); Serial.println(" rad/s");
            Serial.print("Zgyro: "); Serial.print(highres.zgyro); Serial.println(" rad/s");
            Serial.print("Xmag: "); Serial.print(highres.xmag); Serial.println(" gauss");
            Serial.print("Ymag: "); Serial.print(highres.ymag); Serial.println(" gauss");
            Serial.print("Zmag: "); Serial.print(highres.zmag); Serial.println(" gauss");
            Serial.print("Absolute Pressure: "); Serial.print(highres.abs_pressure); Serial.println(" mbar");
            Serial.print("Diff Pressure: "); Serial.print(highres.diff_pressure); Serial.println(" mbar");
            Serial.print("Pressure Alt : "); Serial.println(highres.pressure_alt);
            Serial.print("Temperature : "); Serial.print(highres.temperature); Serial.println(" degC");

            Serial.print("\n");
          }

        default :
          {

          }

      }
    }
  }
}//void
Problems with mavlink_msg_interval_pack