SparkFun Forums

[webpaul]

When I tilt the device left and right up and down, I see the accelerometer and gyro readings change as I would expect but when hold it flat and rotate it around the compass directions all the numbers seem to stay in about the same range (-650 to -680 or so). Suspicious that the magnetometer wasn't working or not being read correctly I got a fridge magnet and held it at the 3 axis and did see the numbers change to -4096 depending on where it was held for each of the axis so I'm pretty sure I'm getting the data correctly from the device.

Thinking perhaps the numbers were just only valid in a small range I went ahead and tried the following math to get the actual tilt adjusted heading:

Code: Select all

        private double WrapAngle(double angle)
        {
            if(angle > pi)
                angle -= (2*pi);
            else if(angle < -pi)
                angle += (2*pi);
            else if (angle < 0)
                angle += 2*pi;
 
            return angle;
        }
 
        private double GetHeading(int bx, int by, int bz, int phi, int theta)
        {
            var Xh = bx * MathEx.Cos(theta) + by * MathEx.Sin(phi) * MathEx.Sin(theta) + bz * MathEx.Cos(phi) * MathEx.Sin(theta);
            var Yh = by * MathEx.Cos(phi) - bz * MathEx.Sin(phi);
            return WrapAngle((MathEx.Atan2(-Yh, Xh) + variation));
        }
 
        Debug.Print(GetHeading(stickIMU.imuData.MagnetX, stickIMU.imuData.MagnetY, stickIMU.imuData.MagnetZ, stickIMU.imuData.AccelXDegrees, stickIMU.imuData.AccelYDegrees).ToString());
 

But I'm only getting headings in the range of -5 to 5 degrees. I don't have it on top of any metal, in fact it is about 4 or 5 inches above a small micro controller. Any advice or tips on getting better magnetometer readings?

[monstrum]

Not sure if something is wrong with your hardware or software. But what really blow my mind when I first started playing with magnetometers was that earths magnetic field up here in the north is far from parallel with the ground. It is rather almost perpendicular to the ground.

You should calculate the norm of the magnetic vector [ (X^2 + Y^2 + Z^2)^(1/2) ]. This should be constant no matter what the orientation is.

[webpaul]

It was some kind of issue with continuous mode for the magnetometer. I changed it to single reading mode and started getting valid numbers. Here is the math I ended up with that seems to work very well for tilt corrected heading.

Code: Select all

when reading accelerometer
......
                if (imuData.AccelYGs == 0 && imuData.AccelZGs == 0)
                    imuData.PitchRadians = 0;
                else
                    imuData.PitchRadians = MathEx.Atan(imuData.AccelXGs / (MathEx.Sqrt(MathEx.Pow(imuData.AccelYGs, 2) + MathEx.Pow(imuData.AccelZGs, 2))));

                imuData.PitchDegrees = radiansToDegreesMultiplier * imuData.PitchRadians;

                if (imuData.AccelXGs == 0 && imuData.AccelZGs == 0)
                    imuData.RollRadians = 0;
                else
                    imuData.RollRadians = -1 * MathEx.Atan(imuData.AccelYGs / (MathEx.Sqrt(MathEx.Pow(imuData.AccelXGs, 2) + MathEx.Pow(imuData.AccelZGs, 2))));

                imuData.RollDegrees = radiansToDegreesMultiplier * imuData.RollRadians;
......

        const double pi = 3.14159265;
        const double variation = 3.9 * (pi / 180.0); //magnetic declination for dallas, tx

        private double WrapAngle(double angle)
        {
            if (angle > pi)
                angle -= (2 * pi);
            else if (angle < -pi)
                angle += (2 * pi);
            else if (angle < 0)
                angle += 2 * pi;

            return (angle < 0 ? -1 : 1) * angle;
        }
 
        public double GetHeadingDegrees()
        {
            return GetHeadingDegrees(imuData.MagnetX, imuData.MagnetY, imuData.MagnetZ, imuData.PitchRadians, imuData.RollRadians);
        }

        private double GetHeadingDegrees(int magX, int magY, int magZ, double pitch, double roll)
        {
            var Xh = (magX * MathEx.Cos(pitch)) + (magY * MathEx.Sin(pitch) * MathEx.Sin(roll)) - (magZ * MathEx.Cos(roll) * MathEx.Sin(pitch));
            var Yh = magY * MathEx.Cos(roll) + magZ * MathEx.Sin(roll);

            return WrapAngle(MathEx.Atan2(-Yh, Xh) + variation) * radiansToDegreesMultiplier;
        }