Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Magnetometer.cpp@46:fd5a62296b12, 2015-05-27 (annotated)

Committer:
pvaibhav
Date:
Wed May 27 13:01:43 2015 +0000
Revision:
46:fd5a62296b12
Parent:
40:8e852115fe55 
Code reformatted

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 4:e759b8c756da 1 #include "Magnetometer.h"
pvaibhav 4:e759b8c756da 2 #define DEBUG "BMX055-Mag"
pvaibhav 4:e759b8c756da 3 #include "Logger.h"
pvaibhav 4:e759b8c756da 4
pvaibhav 11:d21275e60ebb 5 #include "Vector3.h"
pvaibhav 11:d21275e60ebb 6
pvaibhav 32:d37447aec6b4 7 Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), int1(p5), int2(p27)
pvaibhav 11:d21275e60ebb 8 {
pvaibhav 11:d21275e60ebb 9 if (powerOn()) {
pvaibhav 14:d9fbb3ccd482 10 readCalibrationData(); // temperature calibration
pvaibhav 46:fd5a62296b12 11
pvaibhav 14:d9fbb3ccd482 12 // Initialise hard-iron and soft-iron correction. The minima and maxima values were measured
pvaibhav 34:01dec68de3ed 13 // smartplane2 prototype gives a starting point for the background calibrator.
pvaibhav 34:01dec68de3ed 14 float minimums[] = { -1536.0f, -2701.0f, -2112.0f };
pvaibhav 34:01dec68de3ed 15 float maximums[] = { 187, 1665, 39 };
pvaibhav 14:d9fbb3ccd482 16 calibrator.setExtremes(minimums, maximums);
pvaibhav 46:fd5a62296b12 17
pvaibhav 11:d21275e60ebb 18 INFO("Bosch Sensortec BMX055-Magneto found");
pvaibhav 11:d21275e60ebb 19 powerOff();
pvaibhav 4:e759b8c756da 20 } else {
pvaibhav 11:d21275e60ebb 21 WARN("Bosch Sensortec BMX055-Magneto not found");
pvaibhav 4:e759b8c756da 22 }
pvaibhav 4:e759b8c756da 23 }
pvaibhav 4:e759b8c756da 24
pvaibhav 11:d21275e60ebb 25 bool Magnetometer::powerOn()
pvaibhav 11:d21275e60ebb 26 {
pvaibhav 11:d21275e60ebb 27 write_reg(0x4B, 0x82); // softreset with power off
pvaibhav 11:d21275e60ebb 28 wait_ms(3); // page 18
pvaibhav 11:d21275e60ebb 29 write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134.
pvaibhav 11:d21275e60ebb 30 wait_ms(3);
pvaibhav 11:d21275e60ebb 31 return read_reg(0x40) == 0x32; // verify chip ID
pvaibhav 11:d21275e60ebb 32 }
pvaibhav 11:d21275e60ebb 33
pvaibhav 11:d21275e60ebb 34 void Magnetometer::powerOff()
pvaibhav 11:d21275e60ebb 35 {
pvaibhav 11:d21275e60ebb 36 write_reg(0x4B, 0x82); // softreset and stay powered off
pvaibhav 11:d21275e60ebb 37 LOG("powered off");
pvaibhav 11:d21275e60ebb 38 }
pvaibhav 11:d21275e60ebb 39
pvaibhav 12:1632d7391453 40 bool Magnetometer::performSelfTest()
pvaibhav 12:1632d7391453 41 {
pvaibhav 12:1632d7391453 42 write_reg(0x4C, 0x06); // go to sleep mode
pvaibhav 12:1632d7391453 43 write_reg(0x4C, 0x07); // start self test
pvaibhav 12:1632d7391453 44 wait_ms(50);
pvaibhav 12:1632d7391453 45
pvaibhav 12:1632d7391453 46 const bool done = (read_reg(0x4C) & 0x01) == 0;
pvaibhav 12:1632d7391453 47 const bool x_passed = read_reg(0x42) & 0x01;
pvaibhav 12:1632d7391453 48 const bool y_passed = read_reg(0x44) & 0x01;
pvaibhav 12:1632d7391453 49 const bool z_passed = read_reg(0x46) & 0x01;
pvaibhav 12:1632d7391453 50
pvaibhav 12:1632d7391453 51 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");
pvaibhav 12:1632d7391453 52
pvaibhav 12:1632d7391453 53 return done && x_passed && y_passed && z_passed;
pvaibhav 12:1632d7391453 54 }
pvaibhav 12:1632d7391453 55
pvaibhav 11:d21275e60ebb 56 void Magnetometer::start()
pvaibhav 11:d21275e60ebb 57 {
pvaibhav 11:d21275e60ebb 58 // configure for "high accuracy preset" pg. 122
pvaibhav 11:d21275e60ebb 59 write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis
pvaibhav 11:d21275e60ebb 60 write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis
pvaibhav 11:d21275e60ebb 61 write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start
pvaibhav 11:d21275e60ebb 62 }
pvaibhav 11:d21275e60ebb 63
pvaibhav 11:d21275e60ebb 64 void Magnetometer::stop()
pvaibhav 11:d21275e60ebb 65 {
pvaibhav 11:d21275e60ebb 66 write_reg(0x4C, 0x06); // 10 Hz but sleep mode
pvaibhav 11:d21275e60ebb 67 }
pvaibhav 11:d21275e60ebb 68
pvaibhav 11:d21275e60ebb 69 Vector3 Magnetometer::read()
pvaibhav 11:d21275e60ebb 70 {
pvaibhav 11:d21275e60ebb 71 // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790
pvaibhav 12:1632d7391453 72 uint8_t buffer[8];
pvaibhav 40:8e852115fe55 73 /*
pvaibhav 12:1632d7391453 74 for (size_t i = 0; i < sizeof buffer; i++)
pvaibhav 12:1632d7391453 75 buffer[i] = read_reg(0x42 + i);
pvaibhav 40:8e852115fe55 76 */
pvaibhav 40:8e852115fe55 77 read_reg(0x42, buffer, sizeof buffer);
pvaibhav 12:1632d7391453 78
pvaibhav 12:1632d7391453 79 // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!!
pvaibhav 15:4488660e1a3b 80 const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / 8;
pvaibhav 12:1632d7391453 81 const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8;
pvaibhav 12:1632d7391453 82 const int16_t mdata_z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 2;
pvaibhav 12:1632d7391453 83 const uint16_t data_r = *(reinterpret_cast<const uint16_t*>(buffer + 6)) / 4;
pvaibhav 14:d9fbb3ccd482 84 float input[3];
pvaibhav 11:d21275e60ebb 85
pvaibhav 14:d9fbb3ccd482 86 // calculate temperature compensated 16-bit magnetic fields.
pvaibhav 14:d9fbb3ccd482 87 // The "highly optimised" algorithm comes from Bosch, optimised further by Prashant (temp2)
pvaibhav 14:d9fbb3ccd482 88 const int16_t temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
pvaibhav 14:d9fbb3ccd482 89 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);
pvaibhav 14:d9fbb3ccd482 90 input[0] = ((int16_t)((((int32_t)mdata_x) *
pvaibhav 46:fd5a62296b12 91 (((temp2 +
pvaibhav 46:fd5a62296b12 92 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_x1) << 3);
pvaibhav 11:d21275e60ebb 93
pvaibhav 14:d9fbb3ccd482 94 input[1] = ((int16_t)((((int32_t)mdata_y) *
pvaibhav 46:fd5a62296b12 95 (((temp2 +
pvaibhav 46:fd5a62296b12 96 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_y1) << 3);
pvaibhav 11:d21275e60ebb 97
pvaibhav 14:d9fbb3ccd482 98 input[2] = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
pvaibhav 46:fd5a62296b12 99 ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16))));
pvaibhav 46:fd5a62296b12 100
pvaibhav 14:d9fbb3ccd482 101 float output[3];
pvaibhav 14:d9fbb3ccd482 102 calibrator.run(input, output);
pvaibhav 12:1632d7391453 103
pvaibhav 15:4488660e1a3b 104 return Vector3(-output[0], output[1], output[2]);
pvaibhav 11:d21275e60ebb 105 }
pvaibhav 11:d21275e60ebb 106
pvaibhav 34:01dec68de3ed 107 void Magnetometer::getCalibration(Vector3 &mins, Vector3 &maxs)
pvaibhav 34:01dec68de3ed 108 {
pvaibhav 34:01dec68de3ed 109 float mi[3], ma[3];
pvaibhav 34:01dec68de3ed 110 calibrator.getExtremes(mi, ma);
pvaibhav 34:01dec68de3ed 111 mins.x = mi[0];
pvaibhav 34:01dec68de3ed 112 mins.y = mi[1];
pvaibhav 34:01dec68de3ed 113 mins.z = mi[2];
pvaibhav 34:01dec68de3ed 114 maxs.x = ma[0];
pvaibhav 34:01dec68de3ed 115 maxs.y = ma[1];
pvaibhav 34:01dec68de3ed 116 maxs.z = ma[2];
pvaibhav 34:01dec68de3ed 117 }
pvaibhav 34:01dec68de3ed 118
pvaibhav 11:d21275e60ebb 119 void Magnetometer::readCalibrationData()
pvaibhav 11:d21275e60ebb 120 {
pvaibhav 11:d21275e60ebb 121 // trying to read in serial order of address
pvaibhav 11:d21275e60ebb 122 dig_x1 = read_reg(0x5D);
pvaibhav 11:d21275e60ebb 123 dig_y1 = read_reg(0x5E);
pvaibhav 11:d21275e60ebb 124 dig_z4 = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8));
pvaibhav 11:d21275e60ebb 125 dig_x2 = read_reg(0x64);
pvaibhav 11:d21275e60ebb 126 dig_y2 = read_reg(0x65);
pvaibhav 11:d21275e60ebb 127 dig_z2 = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8));
pvaibhav 11:d21275e60ebb 128 dig_z1 = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8));
pvaibhav 11:d21275e60ebb 129 dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8));
pvaibhav 11:d21275e60ebb 130 dig_z3 = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8));
pvaibhav 11:d21275e60ebb 131 dig_xy2 = read_reg(0x70);
pvaibhav 11:d21275e60ebb 132 dig_xy1 = read_reg(0x71);
pvaibhav 11:d21275e60ebb 133 LOG("calibration data loaded");
pvaibhav 11:d21275e60ebb 134 }