Heading from 3D magnetometer



I am trying to understand how to obtain a heading angle from 3D magnetometer like HMC5843. When I look at compass.cpp in AP_Compass library, I can't get how roll and pitch angles are used to compensate compass measurements.

Is there anyone who can explain the following code in Compass.cpp? (Especially the lines where headX and headY are first computed.)


Compass::calculate(float roll, float pitch)
    float headX;
    float headY;
    float cos_roll;
    float sin_roll;
    float cos_pitch;
    float sin_pitch;

    cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
    sin_roll = 1  - (cos_roll * cos_roll);
    cos_pitch = cos(pitch);
    sin_pitch = 1  - (cos_pitch * cos_pitch);

    // Tilt compensated magnetic field X component:
    headX = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch;
    // Tilt compensated magnetic field Y component:
    headY = mag_y*cos_roll-mag_z*sin_roll;
    // magnetic heading
    heading = atan2(-headY,headX);

    // Declination correction (if supplied)
    if( fabs(_declination) > 0.0 )
        heading = heading + _declination;
        if (heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
            heading -= (2.0 * M_PI);
        else if (heading < -M_PI)
            heading += (2.0 * M_PI);

    // Optimization for external DCM use. Calculate normalized components
    heading_x = cos(heading);
    heading_y = sin(heading);

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


This reply was deleted.


Jørn Ramnæs liked VIDEO LINK's profile
May 28
Mark Harrison left a comment on Moderators
"Wow, Ning2 looks really great on mobile. I had sort of drifted away as my browse-time went more on my phone, so this is great."
May 28
Hank Deucker left a comment on PIXHAWK
"I am sure most will say not again, Pixhawk telemetry. I do need help and all can be reassured that I have spent many hours scouring the net for useful information. I am not new at quadcopter flying, I started in 2012 and have progressed through many…"
May 28
Hank Deucker left a comment on PIXHAWK
"Tomorrow, I am thinking about the wording at the moment."
May 28