Magnetometer.cpp

00001 #include "Magnetometer.h"
00002 #define DEBUG "BMX055-Mag"
00003 #include "Logger.h"
00004 
00005 #include "Vector3.h"
00006 
00007 Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), int1(p5), int2(p27)
00008 {
00009     if (powerOn()) {
00010         readCalibrationData(); // temperature calibration
00011 
00012         // Initialise hard-iron and soft-iron correction. The minima and maxima values were measured
00013         // smartplane2 prototype gives a starting point for the background calibrator.
00014         float minimums[] = { -1536.0f, -2701.0f, -2112.0f };
00015         float maximums[] = { 187, 1665, 39 };
00016         calibrator.setExtremes(minimums, maximums);
00017 
00018         INFO("Bosch Sensortec BMX055-Magneto found");
00019         powerOff();
00020     } else {
00021         WARN("Bosch Sensortec BMX055-Magneto not found");
00022     }
00023 }
00024 
00025 bool Magnetometer::powerOn()
00026 {
00027     write_reg(0x4B, 0x82); // softreset with power off
00028     wait_ms(3); // page 18
00029     write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134.
00030     wait_ms(3);
00031     return read_reg(0x40) == 0x32; // verify chip ID
00032 }
00033 
00034 void Magnetometer::powerOff()
00035 {
00036     write_reg(0x4B, 0x82); // softreset and stay powered off
00037     LOG("powered off");
00038 }
00039 
00040 bool Magnetometer::performSelfTest()
00041 {
00042     write_reg(0x4C, 0x06); // go to sleep mode
00043     write_reg(0x4C, 0x07); // start self test
00044     wait_ms(50);
00045 
00046     const bool done = (read_reg(0x4C) & 0x01) == 0;
00047     const bool x_passed = read_reg(0x42) & 0x01;
00048     const bool y_passed = read_reg(0x44) & 0x01;
00049     const bool z_passed = read_reg(0x46) & 0x01;
00050 
00051     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");
00052 
00053     return done && x_passed && y_passed && z_passed;
00054 }
00055 
00056 void Magnetometer::start()
00057 {
00058     // configure for "high accuracy preset" pg. 122
00059     write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis
00060     write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis
00061     write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start
00062 }
00063 
00064 void Magnetometer::stop()
00065 {
00066     write_reg(0x4C, 0x06); // 10 Hz but sleep mode
00067 }
00068 
00069 Vector3 Magnetometer::read()
00070 {
00071     // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790
00072     uint8_t buffer[8];
00073     /*
00074     for (size_t i = 0; i < sizeof buffer; i++)
00075         buffer[i] = read_reg(0x42 + i);
00076     */
00077     read_reg(0x42, buffer, sizeof buffer);
00078 
00079     // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!!
00080     const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / 8;
00081     const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8;
00082     const int16_t mdata_z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 2;
00083     const uint16_t data_r = *(reinterpret_cast<const uint16_t*>(buffer + 6)) / 4;
00084     float input[3];
00085 
00086     // calculate temperature compensated 16-bit magnetic fields.
00087     // The "highly optimised" algorithm comes from Bosch, optimised further by Prashant (temp2)
00088     const int16_t temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
00089     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);
00090     input[0] = ((int16_t)((((int32_t)mdata_x) *
00091                            (((temp2 +
00092                               ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_x1) << 3);
00093 
00094     input[1] = ((int16_t)((((int32_t)mdata_y) *
00095                            (((temp2 +
00096                               ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_y1) << 3);
00097 
00098     input[2] = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
00099                  ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16))));
00100 
00101     float output[3];
00102     calibrator.run(input, output);
00103 
00104     return Vector3(-output[0], output[1], output[2]);
00105 }
00106 
00107 void Magnetometer::getCalibration(Vector3 &mins, Vector3 &maxs)
00108 {
00109     float mi[3], ma[3];
00110     calibrator.getExtremes(mi, ma);
00111     mins.x = mi[0];
00112     mins.y = mi[1];
00113     mins.z = mi[2];
00114     maxs.x = ma[0];
00115     maxs.y = ma[1];
00116     maxs.z = ma[2];
00117 }
00118 
00119 void Magnetometer::readCalibrationData()
00120 {
00121     // trying to read in serial order of address
00122     dig_x1  = read_reg(0x5D);
00123     dig_y1  = read_reg(0x5E);
00124     dig_z4   = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8));
00125     dig_x2  = read_reg(0x64);
00126     dig_y2  = read_reg(0x65);
00127     dig_z2   = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8));
00128     dig_z1   = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8));
00129     dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8));
00130     dig_z3   = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8));
00131     dig_xy2 = read_reg(0x70);
00132     dig_xy1 = read_reg(0x71);
00133     LOG("calibration data loaded");
00134 }