Problem starting multiple mavlink streams to access GPS, altitude and compass data

Hey guys,

i am new here. I am trying to set up a mavlink connection to my pixhawk.
I am trying to receive GPS data by starting the MAV_DATA_STREAM_POSITION stream. That actually worked great. I get a heartbeat, then the GPS location, heartbeat, GPS,… and so on.

Now, additionatly to the GPS data I want to get the Attitute and Compass data. So after I start the MAV_DATA_STREAM_POSITION stream, I also start the MAV_DATA_STREAM_EXTRA1 and MAV_DATA_STREAM_EXTRA2 stream.

This is the point where I get problems. All 3 streams are working individually but when i try to start them all one after another, I get mixed data e.g.:


I do not understand why. Where is my mistake? Really appreciate any kind of help.

I think it’s to be expected that the different messages are arriving interleaved.

Instead of dealing with low level MAVLink I can always suggest to use MAVSDK instead.

Hey Julian, thanks for the quick response.

I will have a look at MAVSDK and yes it is expected that the messages are arriving interleaved. However, when I use multiple streams as described the time between getting the GPS position can be very high. I will try to show you an example.

I have a switch case that prints the message type that I receive. I start the stream MAV_DATA_STREAM_POSITION with 1 Hz message rate. The output looks like the following:

MavLink configuration: successful
Status: Heartbeat: 50
Status: Heartbeat: 52
Status: Heartbeat: 54
Status: Heartbeat: 56
Status: Heartbeat: 58
Status: Heartbeat: 60
Status: Heartbeat: 64
Status: Heartbeat: 66
Status: Heartbeat: 69
Status: Heartbeat: 71
Status: Heartbeat: 73

The GPS data is received in a continious time interval. Now I start the MAV_DATA_STREAM_POSITION, MAV_DATA_STREAM_EXTRA1 and MAV_DATA_STREAM_EXTRA2 stream. The output looks like this:

MavLink configuration: successful
Status: Heartbeat: 77
Status: Heartbeat: 79
Status: Heartbeat: 85
Status: Heartbeat: 91
Status: Heartbeat: 97
Status: Heartbeat: 104
Status: Heartbeat: 110
Status: Heartbeat: 116
Status: Heartbeat: 122
Status: Heartbeat: 128
Status: Heartbeat: 135
Status: Heartbeat: 142
Status: Heartbeat: 148
Status: Heartbeat: 154
Status: Heartbeat: 162
Status: Heartbeat: 168
Status: Heartbeat: 174
Status: Heartbeat: 180
Status: Heartbeat: 186
Status: Heartbeat: 192
Status: Heartbeat: 199
Status: Heartbeat: 205
Status: Heartbeat: 212

Now I receive the different messages very inconsistently. Sometimes I dont get GPS or Compass data for like 10 seconds.

Is it not intended to use multiple streams at once? Do I have to stop the stream to get messages from a different stream?

Hey @JulianOes

I had a look at MAVSDK which is really great. I tried to build my old example with MAVSDK and got a problem.

I need to get the GPS, roll, pitch and yaw information. To receive attitude data with 1 Hz I do:

const Telemetry::Result set_rate_result_attitude = telemetry->set_rate_attitude(1.0);                 // Attitude
if (set_rate_result_attitude != Telemetry::Result::SUCCESS) {
    std::cout << ERROR_CONSOLE_TEXT
        << "Attitude->Setting rate failed:" << Telemetry::result_str(set_rate_result_attitude)
        << NORMAL_CONSOLE_TEXT << std::endl;

and after that I create a callback for the attitude.

telemetry->attitude_euler_angle_async([](Telemetry::EulerAngle euler_angle) {
    std::cout << "Angle Information" << std::endl
        << "\t Pitch: " << euler_angle.pitch_deg << std::endl
        << "\t Roll: " << euler_angle.roll_deg << std::endl
        << "\t Yaw: " << euler_angle.yaw_deg << std::endl;

The problem is that the callback is never hitted. The program alwasy jumps in if and prints:

Attitude->Setting rate failed:Command denied

I have my PixHawk connected with my computer over USB. Receiving its GPS position works fine. But why is the set attitute comannd denied?

Just figured out that when I start my code after I ran QGroundControl (without powering off the PixHawk) I get the attitude values. So what does QGC activates that I dont?

I really appreciate your help.

Hm, that’s odd. Can you share your whole MAVSDK script that you are using?

Hey @JulianOes,

here is the code I am using to get the attitute:

#include <chrono>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <thread>

using namespace mavsdk;
using namespace std::this_thread;
using namespace std::chrono;

void component_discovered(ComponentType component_type) {
std::cout << "Discovered a component with type " << unsigned(component_type) << std::endl;

int main(int argc, char** argv) {
Mavsdk dc;

bool discovered_system = false;
//ConnectionResult connection_result = dc.add_udp_connection("udp://"); 
ConnectionResult connection_result = dc.add_serial_connection("COM7");

// Wait for the system to connect via heartbeat
while (!dc.is_connected()) {

System& system = dc.system();

dc.register_on_discover([&discovered_system](uint64_t uuid) {
    std::cout << "Discovered system with UUID: " << uuid << std::endl;
    discovered_system = true;

if (!discovered_system) {
    std::cout << "No system found, exiting." << std::endl;
    return 1;

// Register a callback so we get told when components (camera, gimbal) etc
// are found.

auto telemetry = std::make_shared<Telemetry>(system);

// Set attitute rate to default rate
const Telemetry::Result set_rate_result_attitude = telemetry->set_rate_attitude(0);   
if (set_rate_result_attitude != Telemetry::Result::SUCCESS) {
    std::cout << "Attitude->Setting rate failed:" << Telemetry::result_str(set_rate_result_attitude) << std::endl;

// Set up callback to monitor Attitude
telemetry->attitude_euler_angle_async([](Telemetry::EulerAngle euler_angle) {
    std::cout << "Angle Information" << std::endl
        << "\t Pitch: " << euler_angle.roll_deg << std::endl
        << "\t Roll: " << euler_angle.pitch_deg << std::endl
        << "\t Yaw: " << euler_angle.yaw_deg << std::endl;

// Sleep
while (true) {
return 0;

The console output looks like this:

There you can see that in line 8 it says “Command denied”. But when I let my code run after I was connected to MissionPlanner or QGroundStation (without powering off my Pixhawk) the console output looks like this:

Can you tell me what I am doing wrong or what I am missing to do? Thanks!

Aha, you are using MissionPlanner? So this would mean you are actually using ArduPilot and not PX4. MAVSDK is not compatible with ArduPilot (yet).

If this is a question for ArduPilot, I would ask over in their forums:

Thanks @JulianOes, that solved the problem :smiley:

Now I have PX4 on the pixhawk and receiving the attitute is working great. However, I dont get GPS infos anymore. Despite I have up to 9 satellites visible.

This should be the code I need to receive GPS, isnt it?

const Telemetry::Result set_rate_result_position = telemetry->set_rate_position(1.0);               // Position
if (set_rate_result_position != Telemetry::Result::SUCCESS) {
    std::cout << ERROR_CONSOLE_TEXT
        << "Position: Setting rate failed:" << Telemetry::result_str(set_rate_result_position)
        << NORMAL_CONSOLE_TEXT << std::endl;
    return 1;

    telemetry->position_async([](Telemetry::Position position) {                                        // GPS
        std::cout   << "Position Information"  << std::endl
                    << "\t Altitude: " << position.absolute_altitude_m << std::endl
                    << "\t Latitude: " << position.latitude_deg << std::endl
                    << "\t Longitude: " << position.longitude_deg << std::endl;  

Health check tells me that everything is looking fine despite the home_position. But that shoudnt be the problem, should it? How can i set home position?

This looks correct. However, the question is not if you have GPS but whether you have a position estimate, so whether ekf2 was able to initialize the position given the GPS data. Do you see the position information in QGC?

Yes, I do see the correct GPS position in QGC.

In the screenshot it says home_position_ok: 0 which would indicate that the position estimate is not good enough yet and home has not yet been initialized.

Can you check in QGC MAVLink inspector if you receive the message GLOBAL_POSITION?

Are there any other messages, warnings appearing in QGC? And are you able to arm in POSCL?

I have the feeling that home_position_ok never gets 1 :D:D it is frustrating. I have 12 sattelites visible I do see the right GPS coords in QGC. I can see that the GLOBAL_POSITION is updating in QGC but I do not get the GPS messages.

I have uploaded ArduPilot and tried it with Mavlink MAV_CMD_SET_MESSAGE_INTERVAL. This is working. I really dont know why it is not working with MAVSDK. Might it be that my Pixhawk is too old for MAVSDK?

However, thanks for your help anyway.

MAVSDK is not compatible with ArduPilot (yet). That would explain it, right?