Hi,
I´m working on an autonomous sailing drone. I´ve connected my Arduino to my pixhawk and tried to collect GPS and attitude data via Mavlink. After a long time I´ve managed to finally get the GPS lat and long data but struggle with yaw, roll and pitch. Serial.print shows me yaw and pitch: nan (not a number) and roll: 0.00.
Thats my MavlLink test code:
#include <SPI.h>
#include <SD.h>
#include <Wire.h>
#include <RTClib.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <NMEA2000_CAN.h>
#include <N2kMessages.h>
#include <N2kMessagesEnumToStr.h>
#include <mavlink.h>
//#include “mavlink_msg_distance_sensor.h”
void request_datastream() { //Request data from pixhawk
uint8_t _system_id = 2; //ID of computer which is sending the command (ground control software has id of 255)
uint8_t _component_id = 2; //Any number except sysID
uint8_t _target_system = 1; //ID of pixhawk
uint8_t _target_component = 0; //Target component, 0 = all
uint8_t _req_stream_id = MAV_DATA_STREAM_ALL; //Message ID
uint16_t _req_message_rate = 0x32; //Number of times per second to request the data in hex
uint8_t _start_stop = 1; //1 = start, 0 = stop
mavlink_message_t msg; //Initialize the required buffers
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_request_data_stream_pack(_system_id, //Pack the message
_component_id,
&msg,
_target_system,
_target_component,
_req_stream_id,
_req_message_rate,
_start_stop);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); //Send the message (write sends as bytes)
Serial2.write(buf, len); //Write data to serial port
}
void MavLink_receive() { //Read MavLink message from flightcontroller
mavlink_message_t msg;
mavlink_status_t status;
while(Serial2.available()) {
uint8_t c= Serial2.read();
if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
switch(msg.msgid) { //Handle new message
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(&msg, &packet);
Serial.print("Lat: ");
Serial.print(packet.lat);
Serial.print(" Long: ");
Serial.println(packet.lon);
Serial.print("Alt: ");
Serial.println(packet.alt);
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_message_t msg;
mavlink_attitude_t packet;
Serial.print("Yaw: "); Serial.println(packet.yaw);
Serial.print("Roll: "); Serial.println(packet.roll);
Serial.print("Pitch: "); Serial.println(packet.pitch);
}
break;
}
}
}
}
void setup() {
SPI.begin(); //Initialize SPI communication
Wire.begin(); //Initialize I2C communication
Serial.begin(115200); //Initialize Debug serial
Serial2.begin(115200); //Initialize MavLink serial
Serial3.begin(115200); //Initialize bluetooth serial
}
void loop() {
MavLink_receive(); //Read MAVLink data
}
And the serial monitor output:
18:37:11.144 → Lat: 0 Long: 0
18:37:11.144 → Alt: -17000
18:37:11.144 → Yaw: nan
18:37:11.144 → Roll: 0.00
18:37:11.144 → Pitch: nan