Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Magnetometer.cpp@12:1632d7391453, 2015-03-13 (annotated)
- Committer:
- pvaibhav
- Date:
- Fri Mar 13 09:12:56 2015 +0000
- Revision:
- 12:1632d7391453
- Parent:
- 11:d21275e60ebb
- Child:
- 13:bc20290eaff2
Code reformatting
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 | 11:d21275e60ebb | 7 | Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0) |
pvaibhav | 11:d21275e60ebb | 8 | { |
pvaibhav | 11:d21275e60ebb | 9 | if (powerOn()) { |
pvaibhav | 11:d21275e60ebb | 10 | readCalibrationData(); |
pvaibhav | 11:d21275e60ebb | 11 | INFO("Bosch Sensortec BMX055-Magneto found"); |
pvaibhav | 11:d21275e60ebb | 12 | powerOff(); |
pvaibhav | 4:e759b8c756da | 13 | } else { |
pvaibhav | 11:d21275e60ebb | 14 | WARN("Bosch Sensortec BMX055-Magneto not found"); |
pvaibhav | 4:e759b8c756da | 15 | } |
pvaibhav | 4:e759b8c756da | 16 | } |
pvaibhav | 4:e759b8c756da | 17 | |
pvaibhav | 11:d21275e60ebb | 18 | bool Magnetometer::powerOn() |
pvaibhav | 11:d21275e60ebb | 19 | { |
pvaibhav | 11:d21275e60ebb | 20 | write_reg(0x4B, 0x82); // softreset with power off |
pvaibhav | 11:d21275e60ebb | 21 | wait_ms(3); // page 18 |
pvaibhav | 11:d21275e60ebb | 22 | write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134. |
pvaibhav | 11:d21275e60ebb | 23 | wait_ms(3); |
pvaibhav | 11:d21275e60ebb | 24 | return read_reg(0x40) == 0x32; // verify chip ID |
pvaibhav | 11:d21275e60ebb | 25 | } |
pvaibhav | 11:d21275e60ebb | 26 | |
pvaibhav | 11:d21275e60ebb | 27 | void Magnetometer::powerOff() |
pvaibhav | 11:d21275e60ebb | 28 | { |
pvaibhav | 11:d21275e60ebb | 29 | write_reg(0x4B, 0x82); // softreset and stay powered off |
pvaibhav | 11:d21275e60ebb | 30 | LOG("powered off"); |
pvaibhav | 11:d21275e60ebb | 31 | } |
pvaibhav | 11:d21275e60ebb | 32 | |
pvaibhav | 12:1632d7391453 | 33 | bool Magnetometer::performSelfTest() |
pvaibhav | 12:1632d7391453 | 34 | { |
pvaibhav | 12:1632d7391453 | 35 | write_reg(0x4C, 0x06); // go to sleep mode |
pvaibhav | 12:1632d7391453 | 36 | write_reg(0x4C, 0x07); // start self test |
pvaibhav | 12:1632d7391453 | 37 | wait_ms(50); |
pvaibhav | 12:1632d7391453 | 38 | |
pvaibhav | 12:1632d7391453 | 39 | const bool done = (read_reg(0x4C) & 0x01) == 0; |
pvaibhav | 12:1632d7391453 | 40 | const bool x_passed = read_reg(0x42) & 0x01; |
pvaibhav | 12:1632d7391453 | 41 | const bool y_passed = read_reg(0x44) & 0x01; |
pvaibhav | 12:1632d7391453 | 42 | const bool z_passed = read_reg(0x46) & 0x01; |
pvaibhav | 12:1632d7391453 | 43 | |
pvaibhav | 12:1632d7391453 | 44 | 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 | 45 | |
pvaibhav | 12:1632d7391453 | 46 | return done && x_passed && y_passed && z_passed; |
pvaibhav | 12:1632d7391453 | 47 | } |
pvaibhav | 12:1632d7391453 | 48 | |
pvaibhav | 11:d21275e60ebb | 49 | void Magnetometer::start() |
pvaibhav | 11:d21275e60ebb | 50 | { |
pvaibhav | 11:d21275e60ebb | 51 | // configure for "high accuracy preset" pg. 122 |
pvaibhav | 11:d21275e60ebb | 52 | write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis |
pvaibhav | 11:d21275e60ebb | 53 | write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis |
pvaibhav | 11:d21275e60ebb | 54 | write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start |
pvaibhav | 11:d21275e60ebb | 55 | } |
pvaibhav | 11:d21275e60ebb | 56 | |
pvaibhav | 11:d21275e60ebb | 57 | void Magnetometer::stop() |
pvaibhav | 11:d21275e60ebb | 58 | { |
pvaibhav | 11:d21275e60ebb | 59 | write_reg(0x4C, 0x06); // 10 Hz but sleep mode |
pvaibhav | 11:d21275e60ebb | 60 | } |
pvaibhav | 11:d21275e60ebb | 61 | |
pvaibhav | 11:d21275e60ebb | 62 | Vector3 Magnetometer::read() |
pvaibhav | 11:d21275e60ebb | 63 | { |
pvaibhav | 11:d21275e60ebb | 64 | // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790 |
pvaibhav | 12:1632d7391453 | 65 | uint8_t buffer[8]; |
pvaibhav | 12:1632d7391453 | 66 | |
pvaibhav | 12:1632d7391453 | 67 | for (size_t i = 0; i < sizeof buffer; i++) |
pvaibhav | 12:1632d7391453 | 68 | buffer[i] = read_reg(0x42 + i); |
pvaibhav | 12:1632d7391453 | 69 | |
pvaibhav | 12:1632d7391453 | 70 | //read_reg(0x40, buffer, 4); |
pvaibhav | 12:1632d7391453 | 71 | |
pvaibhav | 12:1632d7391453 | 72 | // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!! |
pvaibhav | 12:1632d7391453 | 73 | const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8; |
pvaibhav | 12:1632d7391453 | 74 | const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / -8; |
pvaibhav | 12:1632d7391453 | 75 | const int16_t mdata_z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 2; |
pvaibhav | 12:1632d7391453 | 76 | const uint16_t data_r = *(reinterpret_cast<const uint16_t*>(buffer + 6)) / 4; |
pvaibhav | 11:d21275e60ebb | 77 | int16_t temp; // temporary |
pvaibhav | 11:d21275e60ebb | 78 | Vector3 magData; |
pvaibhav | 11:d21275e60ebb | 79 | |
pvaibhav | 11:d21275e60ebb | 80 | // calculate temperature compensated 16-bit magnetic fields |
pvaibhav | 11:d21275e60ebb | 81 | temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); |
pvaibhav | 11:d21275e60ebb | 82 | |
pvaibhav | 11:d21275e60ebb | 83 | magData.x = ((int16_t)((((int32_t)mdata_x) * |
pvaibhav | 11:d21275e60ebb | 84 | ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + |
pvaibhav | 11:d21275e60ebb | 85 | (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + |
pvaibhav | 11:d21275e60ebb | 86 | ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + |
pvaibhav | 11:d21275e60ebb | 87 | (((int16_t)dig_x1) << 3); |
pvaibhav | 11:d21275e60ebb | 88 | |
pvaibhav | 11:d21275e60ebb | 89 | temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); |
pvaibhav | 11:d21275e60ebb | 90 | magData.y = ((int16_t)((((int32_t)mdata_y) * |
pvaibhav | 11:d21275e60ebb | 91 | ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + |
pvaibhav | 11:d21275e60ebb | 92 | (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + |
pvaibhav | 11:d21275e60ebb | 93 | ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + |
pvaibhav | 11:d21275e60ebb | 94 | (((int16_t)dig_y1) << 3); |
pvaibhav | 11:d21275e60ebb | 95 | |
pvaibhav | 11:d21275e60ebb | 96 | magData.z = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) - |
pvaibhav | 11:d21275e60ebb | 97 | ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16)))); |
pvaibhav | 12:1632d7391453 | 98 | |
pvaibhav | 12:1632d7391453 | 99 | return magData - Vector3(-720, 1481, -2830); |
pvaibhav | 11:d21275e60ebb | 100 | } |
pvaibhav | 11:d21275e60ebb | 101 | |
pvaibhav | 11:d21275e60ebb | 102 | void Magnetometer::readCalibrationData() |
pvaibhav | 11:d21275e60ebb | 103 | { |
pvaibhav | 11:d21275e60ebb | 104 | // trying to read in serial order of address |
pvaibhav | 11:d21275e60ebb | 105 | dig_x1 = read_reg(0x5D); |
pvaibhav | 11:d21275e60ebb | 106 | dig_y1 = read_reg(0x5E); |
pvaibhav | 11:d21275e60ebb | 107 | dig_z4 = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8)); |
pvaibhav | 11:d21275e60ebb | 108 | dig_x2 = read_reg(0x64); |
pvaibhav | 11:d21275e60ebb | 109 | dig_y2 = read_reg(0x65); |
pvaibhav | 11:d21275e60ebb | 110 | dig_z2 = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8)); |
pvaibhav | 11:d21275e60ebb | 111 | dig_z1 = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8)); |
pvaibhav | 11:d21275e60ebb | 112 | dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8)); |
pvaibhav | 11:d21275e60ebb | 113 | dig_z3 = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8)); |
pvaibhav | 11:d21275e60ebb | 114 | dig_xy2 = read_reg(0x70); |
pvaibhav | 11:d21275e60ebb | 115 | dig_xy1 = read_reg(0x71); |
pvaibhav | 11:d21275e60ebb | 116 | LOG("calibration data loaded"); |
pvaibhav | 11:d21275e60ebb | 117 | } |