CODEWITHSUNDEEP
arduinocompass-geolocationmagnetometer
TheLastGimbus
TheLastGimbus

Reputation: 631

How to calculate azimuth from X Y Z values from magnetometer?

I want to make compass with Arduino and QMC5883. Now, the magnetometer outputs me only X Y Z values, and I have to calculate the rest myself. So far, I've used this:

float azimuth = atan2(x, y) * 180.0/PI;

But it's pretty buggy, and vulnerable to tilting in any direction. Is there any better algorythm that - for example - phone manufactures use? I could use accelerometer for help, if it would be needed.

Upvotes: 1

Views: 3397

Answers (2)

James
James

Reputation: 1

I couldn't get the above to work for me, it was missing couple of -ve signs. Edited version below;

Sample3D cs = this->getSample(NORTH_EAST_DOWN);
float x = (float) cs.x;
float y = (float) cs.y;
float z = (float) cs.z;   

float sinRoll = sin(roll_rads);
float cosRoll = cos(roll_rads);
float sinPitch = sin(pitch_rads);
float cosPitch = cos(pitch_rads);

float LHS = -x*cosPitch + y*sinPitch*sinRoll + z*sinPitch*cosRoll;
float RHS = z*sinRoll - y*cosRoll;

float bearing = atan2(-LHS, RHS) * 57.3;
bearing += 90;
if(bearing < 0) bearing += 360;

This returns tilt compensated now.

Upvotes: 0

KevinJWalters
KevinJWalters

Reputation: 193

The BBC micro:bit's device abstraction layer (DAL) includes this code to do tilt adjustment based on angles derived from accelerometer data. From https://github.com/lancaster-university/microbit-dal/blob/master/source/drivers/MicroBitCompass.cpp

/**
 * Calculates a tilt compensated bearing of the device, using the accelerometer.
 */
int MicroBitCompass::tiltCompensatedBearing()
{
    // Precompute the tilt compensation parameters to improve readability.
    float phi = accelerometer->getRollRadians();
    float theta = accelerometer->getPitchRadians();

    // Convert to floating point to reduce rounding errors
    Sample3D cs = this->getSample(NORTH_EAST_DOWN);
    float x = (float) cs.x;
    float y = (float) cs.y;
    float z = (float) cs.z;

    // Precompute cos and sin of pitch and roll angles to make the calculation a little more efficient.
    float sinPhi = sin(phi);
    float cosPhi = cos(phi);
    float sinTheta = sin(theta);
    float cosTheta = cos(theta);

    // Calculate the tilt compensated bearing, and convert to degrees.
    float bearing = (360*atan2(x*cosTheta + y*sinTheta*sinPhi + z*sinTheta*cosPhi, z*sinPhi - y*cosPhi)) / (2*PI);

    // Handle the 90 degree offset caused by the NORTH_EAST_DOWN based calculation.
    bearing = 90 - bearing;

    // Ensure the calculated bearing is in the 0..359 degree range.
    if (bearing < 0)
        bearing += 360.0f;

    return (int) (bearing);
}

Upvotes: 1

Related Questions