Discussions

Hello,

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.)

void
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);
}