Heading from 3D magnetometer

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

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

Join diydrones

Email me when people reply –

Replies

This reply was deleted.

Activity

DIY Robocars via Twitter
RT @gclue_akira: 柏の葉で走行させてるjetracerの中身 #instantNeRF #jetracer https://t.co/giVvuE4hP7
Jul 4
DIY Robocars via Twitter
Cool web-based self-driving simulator. Click save when the AI does the right thing https://github.com/pncsoares/self-driving-car
Jul 4
DIY Robocars via Twitter
RT @donkey_car: Human-scale Donkey Car! Hope this makes it to a @diyrobocars race https://www.youtube.com/watch?v=ZMaf031U8jg
Jun 25
DIY Robocars via Twitter
Jun 25
DIY Robocars via Twitter
Jun 16
DIY Robocars via Twitter
RT @GrantEMoe: I won my first @diyrobocars @donkey_car virtual race! Many thanks to @chr1sa @EllerbachMaxime @tawnkramer and everyone who m…
Jun 13
DIY Robocars via Twitter
RT @gclue_akira: JetRacerで自動走行したコースを、InstantNeRFで再構築。データセットは別々に収集 #jetracer #instantNeRT https://t.co/T8zjg3MFyO
Jun 13
DIY Robocars via Twitter
RT @SmallpixelCar: SPC 3.0 Now the motor also works. This car is doable. I just need to design a deck to mount my compute and sensors. http…
Jun 13
DIY Robocars via Twitter
RT @SmallpixelCar: My new car SPC 3.0. https://t.co/CKtkZOxeNQ
Jun 7
DIY Robocars via Twitter
RT @SmallpixelCar: High speed at @diyrobocars thanks @EdwardM26321707 for sharing the video https://t.co/o4317Y2U1S
Jun 7
DIY Robocars via Twitter
RT @SmallpixelCar: Today at @RAMS_RC_Club for @diyrobocars. Used @emlid RTK GPS and @adafruit @BoschGlobal IMU. Lap time 28s https://t.co/R…
May 28
DIY Robocars via Twitter
May 15
DIY Robocars via Twitter
May 14
DIY Robocars via Twitter
May 13
DIY Robocars via Twitter
RT @f1tenth: Say hi to our newest #F1TENTH creation for @ieee_ras_icra next week in Philly. It’s going to be huge! 😎 🔥 @AutowareFdn @PennEn…
May 13
DIY Robocars via Twitter
May 11
More…