Diff: Magnetometer.cpp

Revision:

11:d21275e60ebb

Parent:

4:e759b8c756da

Child:

12:1632d7391453

--- a/Magnetometer.cpp	Tue Feb 24 10:10:14 2015 +0000
+++ b/Magnetometer.cpp	Thu Mar 12 10:16:01 2015 +0000
@@ -2,15 +2,107 @@
 #define DEBUG "BMX055-Mag"
 #include "Logger.h"
 
-Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */) {
-    write_reg(0x4B, 0x83); // softreset and powermode=suspend
-    wait_ms(3); // page 18
-    
-    const uint8_t chip_id = read_reg(0x40);
-    if (chip_id == 0x32) {
-        INFO("Bosch Sensortec BMX055-Magneto ready");
+#include "Vector3.h"
+
+Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0)
+{
+    if (powerOn()) {
+        readCalibrationData();
+        INFO("Bosch Sensortec BMX055-Magneto found");
+        powerOff();
     } else {
-        WARN("Bosch Sensortec BMX055-Magneto not found (chip ID=0x%02x, expected=0x32)", chip_id);
+        WARN("Bosch Sensortec BMX055-Magneto not found");
     }
 }
 
+bool Magnetometer::powerOn()
+{
+    write_reg(0x4B, 0x82); // softreset with power off
+    wait_ms(3); // page 18
+    write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134.
+    wait_ms(3);
+    return read_reg(0x40) == 0x32; // verify chip ID
+}
+
+void Magnetometer::powerOff()
+{
+    write_reg(0x4B, 0x82); // softreset and stay powered off
+    LOG("powered off");
+}
+
+void Magnetometer::start()
+{
+    // configure for "high accuracy preset" pg. 122
+    write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis
+    write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis
+    write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start
+}
+
+void Magnetometer::stop()
+{
+    write_reg(0x4C, 0x06); // 10 Hz but sleep mode
+}
+
+Vector3 Magnetometer::read()
+{
+    // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790
+    union {
+        uint8_t bytes[8];
+        struct {
+            uint16_t hall;
+            int16_t z;
+            int16_t y;
+            int16_t x;
+        } val;
+    } buffer;
+    
+    size_t i = 8;
+    while (i --> 0) {
+        buffer.bytes[i] = read_reg(0x42 + i);
+    };
+    
+    int16_t mdata_x = buffer.val.x / 8;
+    int16_t mdata_y = buffer.val.y / 8;
+    int16_t mdata_z = buffer.val.z / 2;
+    uint16_t data_r = buffer.val.hall / 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)));
+
+    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);
+
+    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);
+
+    magData.z = (((((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))));
+    
+    return magData;
+}
+
+void Magnetometer::readCalibrationData()
+{
+    // trying to read in serial order of address
+    dig_x1  = read_reg(0x5D);
+    dig_y1  = read_reg(0x5E);
+    dig_z4   = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8));
+    dig_x2  = read_reg(0x64);
+    dig_y2  = read_reg(0x65);
+    dig_z2   = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8));
+    dig_z1   = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8));
+    dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8));
+    dig_z3   = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8));
+    dig_xy2 = read_reg(0x70);
+    dig_xy1 = read_reg(0x71);
+    LOG("calibration data loaded");
+}
\ No newline at end of file