Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Magnetometer.cpp@34:01dec68de3ed, 2015-05-06 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed May 06 07:50:02 2015 +0000
- Revision:
- 34:01dec68de3ed
- Parent:
- 32:d37447aec6b4
- Child:
- 40:8e852115fe55
motor voltage under 0.5V supported, magneto calibration initial values changed for smartplane2, complementary filter added to sensor fusion (default off), dt now sent with fusion parameters.
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 | 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 | 14:d9fbb3ccd482 | 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 | 34:01dec68de3ed | 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 | 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 | 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 | 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 | } |