Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Magnetometer.cpp@14:d9fbb3ccd482, 2015-03-17 (annotated)
- Committer:
- pvaibhav
- Date:
- Tue Mar 17 15:50:41 2015 +0000
- Revision:
- 14:d9fbb3ccd482
- Parent:
- 13:bc20290eaff2
- Child:
- 15:4488660e1a3b
Magnetometer now has automatic background calibration for hard-iron and soft-iron effects
Who changed what in which revision?
User | Revision | Line number | New 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 | 14:d9fbb3ccd482 | 7 | Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */) |
pvaibhav | 11:d21275e60ebb | 8 | { |
pvaibhav | 11:d21275e60ebb | 9 | if (powerOn()) { |
pvaibhav | 14:d9fbb3ccd482 | 10 | readCalibrationData(); // temperature calibration |
pvaibhav | 14:d9fbb3ccd482 | 11 | |
pvaibhav | 14:d9fbb3ccd482 | 12 | // Initialise hard-iron and soft-iron correction. The minima and maxima values were measured |
pvaibhav | 14:d9fbb3ccd482 | 13 | // on my desk and give a starting point for the background calibrator. |
pvaibhav | 14:d9fbb3ccd482 | 14 | float minimums[] = { -1800.0f, -80.0f, -1800.0f }; |
pvaibhav | 14:d9fbb3ccd482 | 15 | float maximums[] = { - 350, 1200, 4 }; |
pvaibhav | 14:d9fbb3ccd482 | 16 | calibrator.setExtremes(minimums, maximums); |
pvaibhav | 14:d9fbb3ccd482 | 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 | 12:1632d7391453 | 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 | 12:1632d7391453 | 76 | |
pvaibhav | 12:1632d7391453 | 77 | //read_reg(0x40, buffer, 4); |
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 | 13:bc20290eaff2 | 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 | 14:d9fbb3ccd482 | 91 | (((temp2 + |
pvaibhav | 14:d9fbb3ccd482 | 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 | 14:d9fbb3ccd482 | 95 | (((temp2 + |
pvaibhav | 14:d9fbb3ccd482 | 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 | 11:d21275e60ebb | 99 | ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16)))); |
pvaibhav | 14:d9fbb3ccd482 | 100 | |
pvaibhav | 14:d9fbb3ccd482 | 101 | float output[3]; |
pvaibhav | 14:d9fbb3ccd482 | 102 | calibrator.run(input, output); |
pvaibhav | 12:1632d7391453 | 103 | |
pvaibhav | 14:d9fbb3ccd482 | 104 | return Vector3(output[0], output[1], output[2]); |
pvaibhav | 11:d21275e60ebb | 105 | } |
pvaibhav | 11:d21275e60ebb | 106 | |
pvaibhav | 11:d21275e60ebb | 107 | void Magnetometer::readCalibrationData() |
pvaibhav | 11:d21275e60ebb | 108 | { |
pvaibhav | 11:d21275e60ebb | 109 | // trying to read in serial order of address |
pvaibhav | 11:d21275e60ebb | 110 | dig_x1 = read_reg(0x5D); |
pvaibhav | 11:d21275e60ebb | 111 | dig_y1 = read_reg(0x5E); |
pvaibhav | 11:d21275e60ebb | 112 | dig_z4 = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8)); |
pvaibhav | 11:d21275e60ebb | 113 | dig_x2 = read_reg(0x64); |
pvaibhav | 11:d21275e60ebb | 114 | dig_y2 = read_reg(0x65); |
pvaibhav | 11:d21275e60ebb | 115 | dig_z2 = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8)); |
pvaibhav | 11:d21275e60ebb | 116 | dig_z1 = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8)); |
pvaibhav | 11:d21275e60ebb | 117 | dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8)); |
pvaibhav | 11:d21275e60ebb | 118 | dig_z3 = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8)); |
pvaibhav | 11:d21275e60ebb | 119 | dig_xy2 = read_reg(0x70); |
pvaibhav | 11:d21275e60ebb | 120 | dig_xy1 = read_reg(0x71); |
pvaibhav | 11:d21275e60ebb | 121 | LOG("calibration data loaded"); |
pvaibhav | 11:d21275e60ebb | 122 | } |