Diff: Magnetometer.cpp

Revision:

14:d9fbb3ccd482

Parent:

13:bc20290eaff2

Child:

15:4488660e1a3b

--- a/Magnetometer.cpp	Mon Mar 16 12:39:09 2015 +0000
+++ b/Magnetometer.cpp	Tue Mar 17 15:50:41 2015 +0000
@@ -4,10 +4,17 @@
 
 #include "Vector3.h"
 
-Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0)
+Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */)
 {
     if (powerOn()) {
-        readCalibrationData();
+        readCalibrationData(); // temperature calibration
+        
+        // Initialise hard-iron and soft-iron correction. The minima and maxima values were measured
+        // on my desk and give a starting point for the background calibrator.
+        float minimums[] = { -1800.0f, -80.0f, -1800.0f };
+        float maximums[] = { - 350, 1200, 4 };
+        calibrator.setExtremes(minimums, maximums);
+        
         INFO("Bosch Sensortec BMX055-Magneto found");
         powerOff();
     } else {
@@ -74,29 +81,27 @@
     const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8;
     const int16_t mdata_z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 2;
     const uint16_t data_r = *(reinterpret_cast<const uint16_t*>(buffer + 6)) / 4;
-    int16_t temp; // temporary
-    Vector3 magData;
-
-    // calculate temperature compensated 16-bit magnetic fields
-    temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
+    float input[3];
 
-    magData.x = ((int16_t)((((int32_t)mdata_x) *
-                            ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
-                                 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
-                               ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
-                (((int16_t)dig_x1) << 3);
+    // calculate temperature compensated 16-bit magnetic fields.
+    // The "highly optimised" algorithm comes from Bosch, optimised further by Prashant (temp2)
+    const int16_t temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
+    const int32_t temp2 = (((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9);
+    input[0] = ((int16_t)((((int32_t)mdata_x) *
+                (((temp2 +
+                ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_x1) << 3);
 
-    temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
-    magData.y = ((int16_t)((((int32_t)mdata_y) *
-                            ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
-                                 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
-                               ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
-                (((int16_t)dig_y1) << 3);
+    input[1] = ((int16_t)((((int32_t)mdata_y) *
+                (((temp2 +
+                ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_y1) << 3);
 
-    magData.z = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
+    input[2] = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
                   ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16))));
+                  
+    float output[3];
+    calibrator.run(input, output);
 
-    return magData - Vector3(-720, 1481, -2830);
+    return Vector3(output[0], output[1], output[2]);
 }
 
 void Magnetometer::readCalibrationData()