I want to get telemetry RAW_IMU and ATTITUDE from pixhawk (telem1) by arduino. In mission planner or QGC, I can put stream rate in intervals from 0 to 10 Hz. In actual I can get 5-8 Hz from telem1. I wish to increase the stream rate. I try to increase data rate by arduino function mavlink_msg_request_data_stream_pack(… , uint16_t req_message_rate, …). The function changes rate value in a list from mission planner, but the real value is not changed.
#include <mavlink.h>
#include <SoftwareSerial.h>
SoftwareSerial _MavLinkSerial(10, 11);
// Mavlink variables
unsigned long previousMillisMAVLink = 0; // will store last time MAVLink was transmitted and listened
unsigned long next_interval_MAVLink = 1000; // next interval to count
const int num_hbs = 60; // # of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int num_hbs_pasados = num_hbs;
void Mav_Request_Data()
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// To be setup according to the needed information to be requested from the Pixhawk
const int maxStreams = 1;
const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_ALL};
const uint16_t MAVRates[maxStreams] = {0x0F};
for (int i = 0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_pack(6, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_MavLinkSerial.write(buf, len);
}
}
receive_imu()
{
mavlink_message_t msg;
mavlink_status_t status;
while (_MavLinkSerial.available() > 0)
{
Serial.println(msg.msgid);
float c = _MavLinkSerial.read();
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
/* if(msg.msgid==30) // #30: ATTITUDE
{
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&msg, &attitude);
}
*/
if(msg.msgid==27) // #27: MAVLINK_MSG_ID_RAW_IMU
{
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg, &raw_imu);
Serial.println(raw_imu.zacc);
}
}
}
}
void setup() {
_MavLinkSerial.begin(57600);
Serial.begin(57600);
Mav_Request_Data();
}
void loop() {
receive_imu();
}