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 [url]http://code.tinyclr.com/project/227/sparkfun-stick-imu-driver/[/url] 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:
[url]http://www.tinyclr.com/forum/1/720/[/url]
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
}
}