This entry's latest version is outdated and must be revised. Please see the documentation for the latest API.

Tilt compensated bearing from LSM303DLHC (a $3 part) by untitled

Nov. 19, 2013   |   Snippet   |   Licensed as Apache 2.0   |   2155 views

This code calculates a tilt compensated bearing from the inexpensive LSM303DLHC IC via I2C interface. This is not the same as the LSM303, the LSM303DLHC is better.

Calibrate is required before valid data will be returned. Calibration is simple. Run the Calibrate routine in a loop with a 100ms delay. While the routine is running move the IC along each axis. The debug console will output your calibration values. When they stop changing, you're done. This should only take 10 seconds or so.

I'd like to add temp compensation. Maybe someone else can help with that? The LSM303DLHC has a temp sensor.

Comments or questions?   Discuss on the forum.



Author Version Date
untitled 1 11/19 '13 at 03:23pm
1 — Source
  1. using System;
  2. using Microsoft.SPOT;
  3. using Microsoft.SPOT.Hardware;
  4.  
  5. namespace YourNameSpaceForHardwareAbstracts
  6. {
  7.  
  8. class Compass
  9. {
  10. #region I2C specific
  11.  
  12. static I2CDevice I2CPort = null;
  13.  
  14. static I2CDevice.Configuration accelerometerConfig = null;
  15.  
  16. static I2CDevice.Configuration magnetometerConfig = null;
  17.  
  18. //Stores raw data from the accelerometer and magnetometer.
  19. static byte[] data = new byte[6];
  20.  
  21. public static byte Read(I2CDevice.Configuration aConfig, byte aRegister)
  22. {
  23. byte[] writeCommand = new byte[1] { aRegister };
  24. byte[] readCommand = new byte[1];
  25.  
  26. I2CDevice.I2CTransaction[] writeRequest = new Microsoft.SPOT.Hardware.I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(writeCommand), I2CDevice.CreateReadTransaction(readCommand) };
  27.  
  28. I2CPort.Config = aConfig;
  29. I2CPort.Execute(writeRequest, 3000);
  30.  
  31. return readCommand[0];
  32. }
  33.  
  34. #endregion
  35.  
  36. #region Constants for IC addresses and registers
  37.  
  38. //See IC datasheet for register info
  39. static byte MAGNETOMETER_ADDRESS = 0x1E;
  40. static byte ACCELEROMETER_ADDRESS = 0x19;
  41.  
  42. //See IC datasheet for register info
  43. static byte ACCELEROMETER_DATA_RATE_CONFIGURATION = 0x20;
  44.  
  45. //See IC datasheet for register info
  46. static byte MAGNOMETER_OPERATING_MODE = 0x02;
  47.  
  48. //See IC datasheet for register info
  49. static byte ACCELEROMETER_X_LOW = 0x28;
  50. static byte ACCELEROMETER_X_HIGH = 0x29;
  51. static byte ACCELEROMETER_Y_LOW = 0x2A;
  52. static byte ACCELEROMETER_Y_HIGH = 0x2B;
  53. static byte ACCELEROMETER_Z_LOW = 0x2C;
  54. static byte ACCELEROMETER_Z_HIGH = 0x2D;
  55.  
  56. //See IC datasheet for register info
  57. static byte MAGNETOMETER_X_HIGH = 0x03;
  58. static byte MAGNETOMETER_X_LOW = 0x04;
  59. static byte MAGNETOMETER_Z_HIGH = 0x05;
  60. static byte MAGNETOMETER_Z_LOW = 0x06;
  61. static byte MAGNETOMETER_Y_HIGH = 0x07;
  62. static byte MAGNETOMETER_Y_LOW = 0x08;
  63.  
  64. #endregion
  65.  
  66. #region Variables used to calculate a tilt compensated bearing
  67.  
  68. //Stores processed data from the accelerometer and magnetometer.
  69. static Vector accelerometerVector = new Vector();
  70. static Vector magnetometerVector = new Vector();
  71.  
  72. //Used to process magnetometer data.
  73. static Vector tempAccelerometerVector = new Vector();
  74. static Vector east = new Vector();
  75. static Vector north = new Vector();
  76. static Vector originVector = new Vector(0f, -1f, 0f);
  77.  
  78. static Vector magnometer_Max_Values = new Vector();
  79. static Vector magnometer_Min_Values = new Vector();
  80.  
  81. //Used to avoid allocating new objects, returned from GetBearing
  82. static double bearing = 0;
  83.  
  84. #endregion
  85.  
  86. #region Vector abstract and math
  87.  
  88. //Stores a point in 3D space
  89. struct Vector
  90. {
  91. public Vector(float aX, float aY, float aZ)
  92. {
  93. X = aX;
  94. Y = aY;
  95. Z = aZ;
  96. }
  97.  
  98. public void Reset()
  99. {
  100. X = 0;
  101. Y = 0;
  102. Z = 0;
  103. }
  104.  
  105. public float X, Y, Z;
  106. }
  107.  
  108.  
  109. //Note: Result passed by reference to avoid creating new objects.
  110. //http://en.wikipedia.org/wiki/Cross_product
  111. static void CrossProduct(Vector aFirst, Vector aSecond, ref Vector aResult)
  112. {
  113. aResult.X = aFirst.Y * aSecond.Z - aFirst.Z * aSecond.Y;
  114. aResult.Y = aFirst.Z * aSecond.X - aFirst.X * aSecond.Z;
  115. aResult.Z = aFirst.X * aSecond.Y - aFirst.Y * aSecond.X;
  116. }
  117.  
  118. //http://en.wikipedia.org/wiki/Dot_product
  119. static float DotProduct(Vector a, Vector b)
  120. {
  121. return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
  122. }
  123.  
  124. //Note: Result passed by reference to avoid creating new objects.
  125. //http://en.wikipedia.org/wiki/Normalized_vector
  126. static void UnitVector(ref Vector a)
  127. {
  128. float mag = (float)System.Math.Sqrt(DotProduct(a, a));
  129. a.X /= mag;
  130. a.Y /= mag;
  131. a.Z /= mag;
  132. }
  133.  
  134. #endregion
  135.  
  136. /**
  137.   * Enables the IC with a usable configuration (could be optimized for specific tasks). This method
  138.   * must be called before Calibrate or GetBearing. When calibrating aCalibrationMode should be true.
  139.   */
  140. public static void Enable(bool aCalibrationMode)
  141. {
  142. I2CDevice.I2CWriteTransaction[] writeRequest;
  143. byte[] writeCommand;
  144. int response;
  145.  
  146. if (accelerometerConfig == null)
  147. {
  148. accelerometerConfig = new I2CDevice.Configuration(ACCELEROMETER_ADDRESS, 100);
  149. }
  150.  
  151. if (I2CPort == null)
  152. {
  153. I2CPort = new I2CDevice(accelerometerConfig);
  154. }
  155.  
  156. // Enable Accelerometer
  157. // 0x27 = 0b00100111
  158. // Normal power mode, all axes enabled
  159. writeCommand = new byte[2] { ACCELEROMETER_DATA_RATE_CONFIGURATION, 0x27 };
  160. writeRequest = new Microsoft.SPOT.Hardware.I2CDevice.I2CWriteTransaction[] { I2CDevice.CreateWriteTransaction(writeCommand) };
  161. response = I2CPort.Execute(writeRequest, 3000);
  162.  
  163. if (response != writeCommand.Length)
  164. {
  165. throw new Exception("Error setting accelerometer configuration.");
  166. }
  167.  
  168. if (magnetometerConfig == null)
  169. {
  170. magnetometerConfig = new I2CDevice.Configuration(MAGNETOMETER_ADDRESS, 100);
  171. }
  172.  
  173. I2CPort.Config = magnetometerConfig;
  174.  
  175. // Enable Magnetometer
  176. // 0x00 = 0b00000000
  177. // Continuous conversion mode
  178. writeCommand = new byte[2] { MAGNOMETER_OPERATING_MODE, 0x00 };
  179. writeRequest = new Microsoft.SPOT.Hardware.I2CDevice.I2CWriteTransaction[] { I2CDevice.CreateWriteTransaction(writeCommand) };
  180. response = I2CPort.Execute(writeRequest, 3000);
  181.  
  182. if (response != writeCommand.Length)
  183. {
  184. throw new Exception("Error setting magnometer configuration.");
  185. }
  186.  
  187. if (aCalibrationMode)
  188. {
  189. magnometer_Max_Values = new Vector(float.MinValue, float.MinValue, float.MinValue);
  190. magnometer_Min_Values = new Vector(float.MaxValue, float.MaxValue, float.MaxValue);
  191. }
  192. else
  193. {
  194. /*
  195.   * These were my calibration values. Yours will be different. You should first calibrate the
  196.   * unit using the Calibrate() routine. Normally these values would be read/written to flash, etc.
  197.   * but for the sake of this example they are not.
  198.   */
  199. magnometer_Max_Values.X = 618;
  200. magnometer_Max_Values.Y = 746;
  201. magnometer_Max_Values.Z = 553;
  202.  
  203. magnometer_Min_Values.X = -680;
  204. magnometer_Min_Values.Y = -465;
  205. magnometer_Min_Values.Z = -584;
  206. }
  207.  
  208. }
  209.  
  210. /**
  211.   * This should be called in a loop with at least a 100ms pause. While the code is running you should
  212.   * move the IC around on its axis. The idea is to find the highest and lowest values for each axis.
  213.   * Once you have these numbers they should be places in the Enable method.
  214.   */
  215. public static void Calibrate()
  216. {
  217. data[0] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Y_HIGH);
  218. data[1] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Y_LOW);
  219. data[2] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Z_HIGH);
  220. data[3] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Z_LOW);
  221. data[4] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_X_HIGH);
  222. data[5] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_X_LOW);
  223.  
  224. magnetometerVector.Y = ((short)((byte)~data[0] << 8 | (byte)~data[1]) + 1f);
  225. magnetometerVector.Z = ((short)((byte)~data[2] << 8 | (byte)~data[3]) + 1f);
  226. magnetometerVector.X = ((short)((byte)~data[4] << 8 | (byte)~data[5]) + 1f);
  227.  
  228. if (magnetometerVector.X < magnometer_Min_Values.X)
  229. {
  230. magnometer_Min_Values.X = magnetometerVector.X;
  231. }
  232. if (magnetometerVector.X > magnometer_Max_Values.X)
  233. {
  234. magnometer_Max_Values.X = magnetometerVector.X;
  235. }
  236. if (magnetometerVector.Y < magnometer_Min_Values.Y)
  237. {
  238. magnometer_Min_Values.Y = magnetometerVector.Y;
  239. }
  240. if (magnetometerVector.Y > magnometer_Max_Values.Y)
  241. {
  242. magnometer_Max_Values.Y = magnetometerVector.Y;
  243. }
  244. if (magnetometerVector.Z < magnometer_Min_Values.Z)
  245. {
  246. magnometer_Min_Values.Z = magnetometerVector.Z;
  247. }
  248. if (magnetometerVector.Z > magnometer_Max_Values.Z)
  249. {
  250. magnometer_Max_Values.Z = magnetometerVector.Z;
  251. }
  252.  
  253. Debug.Print("X max:" + magnometer_Max_Values.X + " | X min:" + magnometer_Min_Values.X + " | " +
  254. "Y min:" + magnometer_Max_Values.Y + " | Y min:" + magnometer_Min_Values.Y + " | " +
  255. "Z max:" + magnometer_Max_Values.Z + " | Z min:" + magnometer_Min_Values.Z + " | ");
  256. }
  257.  
  258. /**
  259.   * Returns a tilt compensated compass bearing
  260.   */
  261. public static double GetBearing()
  262. {
  263. //Reads magnetometer data
  264. data[0] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Y_HIGH);
  265. data[1] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Y_LOW);
  266. data[2] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Z_HIGH);
  267. data[3] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_Z_LOW);
  268. data[4] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_X_HIGH);
  269. data[5] = Compass.Read(Compass.magnetometerConfig, Compass.MAGNETOMETER_X_LOW);
  270.  
  271. //Converts bytes to numbers (note that 2's compliment is used)
  272. magnetometerVector.Y = ((short)((byte)~data[0] << 8 | (byte)~data[1]) + 1f);
  273. magnetometerVector.Z = ((short)((byte)~data[2] << 8 | (byte)~data[3]) + 1f);
  274. magnetometerVector.X = ((short)((byte)~data[4] << 8 | (byte)~data[5]) + 1f);
  275.  
  276. //Applies calibration values
  277. magnetometerVector.X = (magnetometerVector.X - magnometer_Min_Values.X) / (magnometer_Max_Values.X - magnometer_Min_Values.X) * 2f - 1.0f;
  278. magnetometerVector.Y = (magnetometerVector.Y - magnometer_Min_Values.Y) / (magnometer_Max_Values.Y - magnometer_Min_Values.Y) * 2f - 1.0f;
  279. magnetometerVector.Z = (magnetometerVector.Z - magnometer_Min_Values.Z) / (magnometer_Max_Values.Z - magnometer_Min_Values.Z) * 2f - 1.0f;
  280.  
  281. //Reads accelerometer data
  282. data[0] = Compass.Read(Compass.accelerometerConfig, Compass.ACCELEROMETER_Y_HIGH);
  283. data[1] = Compass.Read(Compass.accelerometerConfig, Compass.ACCELEROMETER_Y_LOW);
  284. data[2] = Compass.Read(Compass.accelerometerConfig, Compass.ACCELEROMETER_Z_HIGH);
  285. data[3] = Compass.Read(Compass.accelerometerConfig, Compass.ACCELEROMETER_Z_LOW);
  286. data[4] = Compass.Read(Compass.accelerometerConfig, Compass.ACCELEROMETER_X_HIGH);
  287. data[5] = Compass.Read(Compass.accelerometerConfig, Compass.ACCELEROMETER_X_LOW);
  288.  
  289. //Converts bytes to numbers (note that 2's compliment is used)
  290. accelerometerVector.Y = ((short)((byte)~data[0] << 8 | (byte)~data[1]) >> 4) + 1;
  291. accelerometerVector.Z = ((short)((byte)~data[2] << 8 | (byte)~data[3]) >> 4) + 1;
  292. accelerometerVector.X = ((short)((byte)~data[4] << 8 | (byte)~data[5]) >> 4) + 1;
  293.  
  294. //We use this temp variable so we don't change the actual accelerometer vector
  295. tempAccelerometerVector = accelerometerVector;
  296.  
  297. UnitVector(ref tempAccelerometerVector);
  298.  
  299. east.Reset();
  300. north.Reset();
  301.  
  302. CrossProduct(magnetometerVector, tempAccelerometerVector, ref east);
  303.  
  304. UnitVector(ref east);
  305.  
  306. CrossProduct(tempAccelerometerVector, east, ref north);
  307.  
  308. bearing = System.Math.Round(System.Math.Atan2(DotProduct(east, originVector), DotProduct(north, originVector)) * 180f / System.Math.PI);
  309.  
  310. if (bearing < 0) bearing += 360;
  311.  
  312. //Adjusts compass so North is 0 degrees, by default West is.
  313. bearing += 90;
  314. if (bearing > 360)
  315. {
  316. bearing -= 360;
  317. }
  318.  
  319. return bearing;
  320. }
  321.  
  322. }
  323. }