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?