I want to get
Roll angel, pitch angle, Yaw angle, Roll angular speed, Pitch angular speed, Yaw angular speed.
There are still many things you are not doing correctly in your code unfortunately. I’d suggest you look at the example I provided and try to get that working in your environment. That might be quicker than trying to fix your code.
Thanks, this has been a great help. Unfortunately I am stuck.
The only messages I seem to get back is HEARTBEAT, despite requesting MAV_DATA_STREAM_ALL. I am running on an ESP32.
I am curious about the component id. It seems from the code that that the system id is 2 and the component id is 200. I thought the companion computer would be 200 and the system id 1 (for the copter).
I am using software serial to talk to the Pixhawk, and the Serial for debugging.
#include <SoftwareSerial.h>
#include <mavlink.h>
SoftwareSerial ardupilotPort;
// Mavlink variables
#define ARDUPILOT_RX 33
#define ARDUPILOT_TX 32
#define CC_SYSID 2
#define CC_COMPID 200
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 setup() {
Serial.begin(115200);
ardupilotPort.begin(57600, SWSERIAL_8N1, ARDUPILOT_RX, ARDUPILOT_TX, false);
if (!ardupilotPort) { // If the object did not initialize, then its configuration is invalid
Serial.println("Invalid SoftwareSerial pin configuration, check config");
while (1) { // Don't continue with invalid configuration
delay (1000);
}
}
}
/***********************************************************************************************************************************/
void loop() {
int type = MAV_TYPE_ONBOARD_CONTROLLER; ///< This system is an Onboard companion controller
// Define the system type, in this case an airplane -> on-board controller
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;
uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
//mavlink_msg_heartbeat_pack(CC_SYSID, CC_COMPID, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_heartbeat_pack(1,0, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message with the standard UART send function
// uart0_send might be named differently depending on
// the individual microcontroller / library in use.
unsigned long currentMillisMAVLink = millis();
if (currentMillisMAVLink - previousMillisMAVLink >= next_interval_MAVLink) {
// Timing variables
previousMillisMAVLink = currentMillisMAVLink;
ardupilotPort.write(buf, len);
num_hbs_pasados++;
if (num_hbs_pasados >= num_hbs) {
// Request streams from Pixhawk
mavlink_Request_Data();
num_hbs_pasados = 0;
}
}
//mavlink_Request_Data();
// Check reception buffer
mavlink_receive();
}
void mavlink_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] = {0x01};
for (int i = 0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_pack(CC_SYSID, CC_COMPID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
ardupilotPort.write(buf, len);
}
}
void mavlink_receive() {
mavlink_message_t msg;
mavlink_status_t status;
while (ardupilotPort.available() > 0) {
uint8_t c = ardupilotPort.read();
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
Serial.println(msg.msgid);
// Handle message
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat
{
// E.g. read GCS heartbeat and go into
// comm lost mode if timer times out
Serial.println("HEARTBEAT");
}
break;
case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS
{
/* Message decoding: PRIMITIVE
mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
*/
//mavlink_message_t* msg;
mavlink_sys_status_t sys_status;
mavlink_msg_sys_status_decode(&msg, &sys_status);
Serial.println("STATUS");
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE
{
/* Message decoding: PRIMITIVE
mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value)
*/
//mavlink_message_t* msg;
mavlink_param_value_t param_value;
mavlink_msg_param_value_decode(&msg, ¶m_value);
}
break;
case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU
{
/* Message decoding: PRIMITIVE
static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu)
*/
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg, &raw_imu);
Serial.println("IMU");
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: // #35 RC_CHANNELS_RAW
{
mavlink_rc_channels_raw_t rc_channels_raw;
mavlink_msg_rc_channels_raw_decode(&msg, &rc_channels_raw);
Serial.println("CHANNEL RAW");
}
break;
case MAVLINK_MSG_ID_ATTITUDE: // #30
{
/* Message decoding: PRIMITIVE
mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
*/
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&msg, &attitude);
Serial.println("ATTITUDE");
}
break;
default:
break;
}
}
}
}
Any help would be most appreciated.
1 Like
I too am struggling with talking between esp32 and pixhawk so I can’t help much with that yet. But I can help you with your serial communications efficiency. All esp32 devices have three hardware serial ports so you rarely need to use software serial (which can be problematic).
Here is a great video which explains how to use the hardware serial ports on the esp32 and assign them to any GPIO port: Tech Note 137 - ESP32 a further insight into using the serial ports - YouTube
Totally agree, always try and use hardware serial ports not Software Serial. There any many catches to SoftwareSerial 
Hi @CarbonMan,
I have a couple of suggestions.
You are using a SYSID of 1 which is ok as I understand it, but the COMPID of 0 may be a problem. On my Orange Cube autopilot, it talks on both sysid=1, compid=1 AND sysid=1, compid=0. So I stay away from the combinations. My Arduino is always 1,100.
The other thing I noticed in your loop() is that you have commented out the function that requests the stream, I’m not sure that was just from some testing, but it would be a problem if not.
//mavlink_Request_Data();
Also within mavlink_Request_Data() you also need to change the line
mavlink_msg_request_data_stream_pack(CC_SYSID, CC_COMPID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
to be
mavlink_msg_request_data_stream_pack(CC_SYSID, CC_COMPID, &msg, 1, 1, MAVStreams[i], MAVRates[i], 1);
so that you are asking the autopilot (sysid=1, compid=1) to send you the stream.
I define these as follows in my code
#define AP_SYS_ID 1 // MAVLink System ID of the autopilot.
#define AP_COMP_ID 1 // MAVLink Component ID of the autopilot.
and if you do the same then the above line becomes;
mavlink_msg_request_data_stream_pack(CC_SYSID, CC_COMPID, &msg, AP_SYS_ID, AP_COMP_ID, MAVStreams[i], MAVRates[i], 1);
See if that all helps
Cheers,
PJ
Hi Jeffrey,
What’s happening with yours, need any help?
By the way I have a whole big MavLink Tester for Arduino at GitHub - pauljeffress/Pulsar-MAVLink-Tester
It’s a menu driven sketch that you can use to test things out.
I know its a bit involved, but if you look through the code it may help.
Good Luck
Cheers,
PJ
Paul, I have actually using been using your tester for a few days but am not not getting very good results. Instead of your SAM based feather I am using an esp32-e firebeetle from DFRobot and instead of the Cube I am using a generic Pixhawk 2.4.8 so before I started asking questions I wanted to do my homework. I’ve tried other people’s programs also and am not getting very good results.
I compiled under platformio and the menu works fine but I never see any messages come back.
I think I see a heartbeat on my logic analyzer but my messages which query the ardupilot pixhawk (running 4.2 ardurover) don’t answer back anything but the initial stream which I think is a heartbeat.
Data sent when I seleted arm from the menu (I think)
Pretty much all I see coming from the TELEM2 port (56K, Mavlink1).
I think I need to set up some of the defines differently for my system which is a ROVER, not a drone. And I haven’t asked for any specific message types which may also be my problem. Also, I freshened the library below your code with the latest from GitHub - mavlink/c_library_v1: MAVLink protocol C/C++ implementation auto-generated from latest protocol specs. but I’m sure I’m missing something major somewhere. Thanks much for any help here.
@Jeffrey_Berezin that is almost exactly my setup as well. I want to build for Rover, but when I ran into this problem I tried loading the Copter firmware but had the same problem.
In my current version I get a HEARTBEAT every second but every minute I will get 14 REQUEST_DATA_STREAM messages in a row, as well
I am actually after MAV_DATA_STREAM_RC_CHANNELS but then I get nothing
@pauljeff That commented line was just an act of desperation. There is another line above that makes the call.
Well, please ignore my previous post. I moved from TELEM2 to TELEM1 and switched cables and am getting more reasonable results. I was still getting Async framing errors on the logic analyzer but recycled power on the pixhawk and those went away. Still need to do a lot more work before I can say anything definitive.
@Jeffrey_Berezin I will go and fire up Mission Planner and check exactly how I have my AutoPilots port configured and share it here.
By the way I like your approach of using the logic sniffer, that should reveal a lot. I do the same, I also just drop a 2nd UART<>USB (FTDI board) in and just connect its RX to either of the two lines between the AP and Arduino to see what’s being sent/received. I use Coolterm on my Mac and just have it open and displaying anything it sees on that RX pin. I put Coolterm in hex mode as well so you see the gibberish text and the hex dump. That helped me a lot…I ended up getting quite deep on MavLink packets for a while there 
I am using Rover 4.0 by the way, I need to update my AP.
Also if you post your full code I’ll try it on an ESP32 and my AP if you like.
1 Like
Thanks a lot. I am first figuring out my basic problem which is that I am seeing data coming in from the pixhawk on the logic analyzer that is not being read in by my esp32e firebeetle. the Serial2.available() function is not seeing the data at times and I think I am seeing a bug that allegedly was fixed years ago by Expressif.
ah ok, yeah sounds like Serial level issues then huh.
I checked my AP setup via Mission Planner and I have my Arduino connected to Serial4/SR4 on the Cube AutoPilot. That port is marked GPS2.
My config is as per below photos.
Also it might be worth putting aside your hardware serial for a sec and just use the dreaded softwareserial just to see if at least you can isolate the problem to serial hw?
The other thing I have read was that the AP will not send any Mavlink over a port until it has seen some MAVLink (heartbeat etc) inbound on that port. I have found this isn’t always the case though, but its something to be aware of.
The doco here is good but you need to read it a few times to understand the nuances.
https://ardupilot.org/dev/docs/mavlink-commands.html
I thought about that but am switching to a different piece of hardware first. I am keeping the software serial in my back pocket as a contingency. The good news is that using the logic analyzer I can see what looks like mavlink packets moving bidirectionally so I think I’m close to at least seeing some message traffic.
Thanks a lot for your help here.
I am now receiving packets. It turned out I have a wiring issue. I now have to get sending packets working again
… But this is progress
nice one, good news.
Paul
Actually, it was working bidirectionally as I was able to DISARM the Pixhawk using the Pulsar-MAVlink-Tester menu. The reason I thought it wasn’t working is that I can’t see the esp32 transmit stream on my logic analyzer for some bizarre reason. I need to figure that one out but I am very happy that communications are up and running.
Thanks for developing this excellent test program. My next step after the logic analyzer debugging is to find which packet contains the motor speed information because I want to control a non-standard hoverboard controller using serial uart control.
.
That’s so good to hear. I really enjoyed learning about mavlink and coding up that tester. I leveraged a lot of great work by others and knowledge gained in this thread. 