Main Site Documentation

Converting magnetometer readings to a heading


#1

Hi, I’m using the Rador 9dof stick with a Panda II, trying to get a magnetic heading from it. I’m using the driver provided at http://code.tinyclr.com/project/227/sparkfun-stick-imu-driver/ which seems to work pretty well.

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 then I tried using the formulas mentioned at the end of this discussion to get the heading:
http://www.tinyclr.com/forum/1/720/

Here is the code I’m using, it is only returning values between about -5 and 5 degrees no matter where its pointing. I’m making lots of guesses about how this is supposed to work so I’m sure there are some errors in here somewhere.

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

Here is my modified IMU code to include the tilt information in degrees among other things which is more useful than the raw info from the sensors. I’m not terribly sure about the complementary filter part but all of it seems to be pretty accurate except for the magnetometer part.


using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.System;

namespace RadioReporterMicro
{
    public class StickIMUDriver
    {
        #region Declarations

        private I2CDevice i2cBus = new I2CDevice(new I2CDevice.Configuration(0x00, 400));
        private I2CDevice.Configuration accelerometer = new I2CDevice.Configuration(0x53, 400);
        private I2CDevice.Configuration magnetometer = new I2CDevice.Configuration(0x1e, 400);
        private I2CDevice.Configuration gyroscope = new I2CDevice.Configuration(0x68, 400);

        private I2CDevice.I2CTransaction[] writeTransactions;
        private I2CDevice.I2CTransaction[] readTransactions;

        private byte[] data = new byte[6] { 0, 0, 0, 0, 0, 0 };

        DateTime lastMeasure = DateTime.Now;
        DateTime currentMeasure = DateTime.MinValue;

        public IMUData imuData = new IMUData();
        public struct IMUData
        {
            public double TiltXDegrees, TiltYDegrees, TiltZDegrees;

            public double AccelXGs, AccelYGs, AccelZGs;
            public double AccelXDegrees, AccelYDegrees, AccelZDegrees;

            //DPS = degrees per second
            public short GyroXDPS, GyroYDPS, GyroZDPS;
            public double GyroXDegrees, GyroYDegrees, GyroZDegrees;

            public short MagnetX, MagnetY, MagnetZ;
        }

        #endregion

        #region Construction

        public StickIMUDriver()
        {
            writeTransactions = new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(new byte[] { 0, 0 }) };
            readTransactions = new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(new byte[] { 0 }), I2CDevice.CreateReadTransaction(new byte[] { 0, 0, 0, 0, 0, 0 }) };

            // Initialize the sensors...
            accelerometerInit();
            gyroscopeInit();
            magnetometerInit();
        }

        #endregion

        #region Private methods

        /// <summary>
        /// Private function used to write data into the sensor's registers
        /// </summary>
        /// <param name="device">The I2C device to write to</param>
        /// <param name="register">The register address to write to</param>
        /// <param name="value">The value to write</param>
        private void WriteRegister(I2CDevice.Configuration device, byte register, byte value)
        {
            lock (this)
            {
                writeTransactions[0].Buffer[0] = register;
                writeTransactions[0].Buffer[1] = value;

                // Set the bus to use specified sensor
                i2cBus.Config = device;

                // Execute the transation
                i2cBus.Execute(writeTransactions, 10);
            }
        }

        /// <summary>
        /// Private function used to read data from the sensor's registers
        /// </summary>
        /// <param name="device">The I2C device to read from</param>
        /// <param name="register">The register to read from</param>
        /// <param name="length">The length of data to read</param>
        /// <returns>A byte value containing the data read from the sensor's register</returns>
        private byte[] ReadRegister(I2CDevice.Configuration device, byte register, int length)
        {
            if (length != 6)
                throw new Exception("Code optimised for reading a 6 bytes buffer");

            lock (this)
            {
                readTransactions[0].Buffer[0] = register;

                // Set the bus to use our sensor
                i2cBus.Config = device;

                // Execute the transation
                if (i2cBus.Execute(readTransactions, 10) == 7)
                    return readTransactions[1].Buffer;
            }

            return null;
        }

        /// <summary>
        /// Initializing Accelerometer
        /// </summary>
        private void accelerometerInit()
        {
            // Set POWER_CTL register to use measurement mode
            WriteRegister(accelerometer, 0x2d, 0x08);

            // Set DATA_FORMAT register to use full resolution
            WriteRegister(accelerometer, 0x31, 0x08);

            // Set BW_RATE register to 50Hz (25Hz bandwidth)
            WriteRegister(accelerometer, 0x2c, 0x09);
        }

        /// <summary>
        /// Initialize Gyroscope
        /// </summary>
        private void gyroscopeInit()
        {
            // Set Gyroscope to use Full Scale (±2000°/s),
            // internal LPF bandwith to 188Hz and Internal Sampling Rate to 1kHz
            WriteRegister(gyroscope, 0x16, 0x19);
        }

        /// <summary>
        /// Initialize Magnetometer
        /// </summary>
        private void magnetometerInit()
        {
            // Write to mode register to set at continuous mode
            WriteRegister(magnetometer, 0x02, 0x00);

            // Write to config register A to set output rate to 50Hz
            WriteRegister(magnetometer, 0x00, 0x06);
        }

        #endregion

        #region Public methods

        public override string ToString()
        {
            lock (this)
            {
                return //"A: " + imuData.AccelXGs.ToString("N2") + ", " + imuData.AccelYGs.ToString("N2") + ", " + imuData.AccelZGs.ToString("N2") + ", "
//                 + "G: " + imuData.GyroXDPS.ToString() + ", " + imuData.GyroYDPS.ToString() + ", " + imuData.GyroZDPS.ToString() + ", " + 
                 "Tilt: " + imuData.TiltXDegrees.ToString("N2") + ", " + imuData.TiltYDegrees.ToString("N2") + ", " + imuData.TiltZDegrees.ToString("N2") + ", "
                 + "AD: " + imuData.AccelXDegrees.ToString("N2") + ", " + imuData.AccelYDegrees.ToString("N2") + ", " + imuData.AccelZDegrees.ToString("N2") + ", "
                 + "GD: " + imuData.GyroXDegrees.ToString("N2") + ", " + imuData.GyroYDegrees.ToString("N2") + ", " + imuData.GyroZDegrees.ToString("N2") + ", "
                 + "M: " + imuData.MagnetX.ToString() + ", " + imuData.MagnetY.ToString() + ", " + imuData.MagnetZ.ToString();
            }
        }

        private const double radiansToDegreesMultiplier = 57.295779513082320876798154814105;
        private double accelForce = 0;

        public void ReadAll()
        {
            readAccelerometer();
            readGyroscope();
            readMagnetometer();
        }

        /// <summary>
        /// Read Accelerometer Values
        /// </summary>
        private bool readAccelerometer()
        {
            lock (this)
            {
                currentMeasure = DateTime.Now;
                dt = (currentMeasure - lastMeasure).Milliseconds / 1000.0;
                lastMeasure = currentMeasure;

                Array.Copy(ReadRegister(accelerometer, 0x32, 6), data, 6);

                if (data == null)
                    return false;

                //accel data is returned as LSB which is converted to milli-Gs by multiplying by 3.9, / 1000 to get Gs
                imuData.AccelXGs = ((short)((data[1] << 8 | data[0])) * 3.9) / 1000.0; // X axis (internal sensor X axis)
                imuData.AccelYGs = ((short)((data[3] << 8 | data[2])) * 3.9) / 1000.0; // Y axis (internal sensor Y axis)
                imuData.AccelZGs = ((short)((data[5] << 8 | data[4])) * 3.9) / 1000.0; // Z axis (internal sensor Z axis)

                accelForce = MathEx.Sqrt(MathEx.Pow(imuData.AccelXGs, 2) + MathEx.Pow(imuData.AccelYGs, 2) + MathEx.Pow(imuData.AccelZGs, 2));
                imuData.AccelXDegrees = accelForce == 0 ? 0 : MathEx.Acos(imuData.AccelXGs / accelForce) * radiansToDegreesMultiplier;
                imuData.AccelYDegrees = accelForce == 0 ? 0 : MathEx.Acos(imuData.AccelYGs / accelForce) * radiansToDegreesMultiplier;
                imuData.AccelZDegrees = accelForce == 0 ? 0 : MathEx.Acos(imuData.AccelZGs / accelForce) * radiansToDegreesMultiplier;

                return true;
            }
        }

        private double dt;

        /// <summary>
        /// Read Gyroscope Values
        /// </summary>
        private bool readGyroscope()
        {
            lock (this)
            {
                Array.Copy(ReadRegister(gyroscope, 0x1d, 6), data, 6);

                if (data == null)
                    return false;

                imuData.GyroYDPS = (short)(data[0] << 8 | data[1]); // Y (Pitch) axis (internal sensor X axis)
                imuData.GyroXDPS = (short)(data[2] << 8 | data[3]); // X (Roll) axis (internal sensor Y axis)
                imuData.GyroZDPS = (short)(data[4] << 8 | data[5]); // Z (Yaw) axis (internal sensor Z axis)

                imuData.GyroXDegrees = imuData.GyroXDPS * dt;
                imuData.GyroYDegrees = imuData.GyroYDPS * dt;
                imuData.GyroZDegrees = imuData.GyroZDPS * dt;


                //this tends to keep increasing the combined value, not sure how this is supposed to work
                //imuData.TiltXDegrees = 0.98 * (imuData.TiltXDegrees + imuData.GyroXDegrees * dt) + (.02 * imuData.AccelXDegrees);
                //imuData.TiltYDegrees = 0.98 * (imuData.TiltYDegrees + imuData.GyroYDegrees * dt) + (.02 * imuData.AccelYDegrees);
                //imuData.TiltZDegrees = 0.98 * (imuData.TiltZDegrees + imuData.GyroZDegrees * dt) + (.02 * imuData.AccelZDegrees);
                
                //complementary filter
                imuData.TiltXDegrees = 0.98 * (imuData.GyroXDegrees * dt) + (.02 * imuData.AccelXDegrees);
                imuData.TiltYDegrees = 0.98 * (imuData.GyroYDegrees * dt) + (.02 * imuData.AccelYDegrees);
                imuData.TiltZDegrees = 0.98 * (imuData.GyroZDegrees * dt) + (.02 * imuData.AccelZDegrees);

                return true;
            }
        }

        /// <summary>
        /// Read Magnetometer Values
        /// </summary>
        private bool readMagnetometer()
        {
            lock (this)
            {
                Array.Copy(ReadRegister(magnetometer, 0x03, 6), data, 6);

                if (data == null)
                    return false;

                imuData.MagnetX = (short)(data[0] << 8 | data[1]);
                imuData.MagnetY = (short)(data[2] << 8 | data[3]);
                imuData.MagnetZ = (short)(data[4] << 8 | data[5]);

                return true;
            }
        }

        #endregion
    }
}

#2

Changing it to single measurement mode instead of continous gives me good numbers - comment out all of the magnetometerInit and add the below line to the top of readMagnetometer method:

 
              WriteRegister(magnetometer, 0x02, 0x01);


#3

Hello I try also to using the 9dof stick but I have some Problems to understand the Structure of your Code. Can you tell me how I have to construct this to Parts in the Development Studio?

Do you have an alternative Address for the Link?

Sorry for my bad English


#4

Now does the Link in the firs Post still works and I have the Stick still running.

One Question to Paul B.:
In your Code you use Mathematic Operations how I cant find. Which Library have you use for this operations? Cos and Sin are still no Problem but I cant find an Atan function in other Library’s
Greetings