Diff: Magnetometer.cpp

Revision:

12:1632d7391453

Parent:

11:d21275e60ebb

Child:

13:bc20290eaff2

--- a/Magnetometer.cpp	Thu Mar 12 10:16:01 2015 +0000
+++ b/Magnetometer.cpp	Fri Mar 13 09:12:56 2015 +0000
@@ -30,6 +30,22 @@
     LOG("powered off");
 }
 
+bool Magnetometer::performSelfTest()
+{
+    write_reg(0x4C, 0x06); // go to sleep mode
+    write_reg(0x4C, 0x07); // start self test
+    wait_ms(50);
+
+    const bool done = (read_reg(0x4C) & 0x01) == 0;
+    const bool x_passed = read_reg(0x42) & 0x01;
+    const bool y_passed = read_reg(0x44) & 0x01;
+    const bool z_passed = read_reg(0x46) & 0x01;
+
+    INFO("Self test done=%s x=%s y=%s z=%s", done?"pass":"fail", x_passed?"pass":"fail", y_passed?"pass":"fail", z_passed?"pass":"fail");
+
+    return done && x_passed && y_passed && z_passed;
+}
+
 void Magnetometer::start()
 {
     // configure for "high accuracy preset" pg. 122
@@ -46,25 +62,18 @@
 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;
+    uint8_t buffer[8];
+
+    for (size_t i = 0; i < sizeof buffer; i++)
+        buffer[i] = read_reg(0x42 + i);
+
+    //read_reg(0x40, buffer, 4);
+
+    // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!!
+    const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8;
+    const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / -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;
 
@@ -86,8 +95,8 @@
 
     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;
+
+    return magData - Vector3(-720, 1481, -2830);
 }
 
 void Magnetometer::readCalibrationData()