Unable to read data on arduino serial monitor

MAVLink
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