Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Magnetometer.cpp@11:d21275e60ebb, 2015-03-12 (annotated)
- Committer:
- pvaibhav
- Date:
- Thu Mar 12 10:16:01 2015 +0000
- Revision:
- 11:d21275e60ebb
- Parent:
- 4:e759b8c756da
- Child:
- 12:1632d7391453
Magnetometer driver implemented including hall-compensation. Reading registers for all sensors uses big-endian sequential read.
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 | 11:d21275e60ebb | 33 | void Magnetometer::start() |
pvaibhav | 11:d21275e60ebb | 34 | { |
pvaibhav | 11:d21275e60ebb | 35 | // configure for "high accuracy preset" pg. 122 |
pvaibhav | 11:d21275e60ebb | 36 | write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis |
pvaibhav | 11:d21275e60ebb | 37 | write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis |
pvaibhav | 11:d21275e60ebb | 38 | write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start |
pvaibhav | 11:d21275e60ebb | 39 | } |
pvaibhav | 11:d21275e60ebb | 40 | |
pvaibhav | 11:d21275e60ebb | 41 | void Magnetometer::stop() |
pvaibhav | 11:d21275e60ebb | 42 | { |
pvaibhav | 11:d21275e60ebb | 43 | write_reg(0x4C, 0x06); // 10 Hz but sleep mode |
pvaibhav | 11:d21275e60ebb | 44 | } |
pvaibhav | 11:d21275e60ebb | 45 | |
pvaibhav | 11:d21275e60ebb | 46 | Vector3 Magnetometer::read() |
pvaibhav | 11:d21275e60ebb | 47 | { |
pvaibhav | 11:d21275e60ebb | 48 | // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790 |
pvaibhav | 11:d21275e60ebb | 49 | union { |
pvaibhav | 11:d21275e60ebb | 50 | uint8_t bytes[8]; |
pvaibhav | 11:d21275e60ebb | 51 | struct { |
pvaibhav | 11:d21275e60ebb | 52 | uint16_t hall; |
pvaibhav | 11:d21275e60ebb | 53 | int16_t z; |
pvaibhav | 11:d21275e60ebb | 54 | int16_t y; |
pvaibhav | 11:d21275e60ebb | 55 | int16_t x; |
pvaibhav | 11:d21275e60ebb | 56 | } val; |
pvaibhav | 11:d21275e60ebb | 57 | } buffer; |
pvaibhav | 11:d21275e60ebb | 58 | |
pvaibhav | 11:d21275e60ebb | 59 | size_t i = 8; |
pvaibhav | 11:d21275e60ebb | 60 | while (i --> 0) { |
pvaibhav | 11:d21275e60ebb | 61 | buffer.bytes[i] = read_reg(0x42 + i); |
pvaibhav | 11:d21275e60ebb | 62 | }; |
pvaibhav | 11:d21275e60ebb | 63 | |
pvaibhav | 11:d21275e60ebb | 64 | int16_t mdata_x = buffer.val.x / 8; |
pvaibhav | 11:d21275e60ebb | 65 | int16_t mdata_y = buffer.val.y / 8; |
pvaibhav | 11:d21275e60ebb | 66 | int16_t mdata_z = buffer.val.z / 2; |
pvaibhav | 11:d21275e60ebb | 67 | uint16_t data_r = buffer.val.hall / 4; |
pvaibhav | 11:d21275e60ebb | 68 | int16_t temp; // temporary |
pvaibhav | 11:d21275e60ebb | 69 | Vector3 magData; |
pvaibhav | 11:d21275e60ebb | 70 | |
pvaibhav | 11:d21275e60ebb | 71 | // calculate temperature compensated 16-bit magnetic fields |
pvaibhav | 11:d21275e60ebb | 72 | temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); |
pvaibhav | 11:d21275e60ebb | 73 | |
pvaibhav | 11:d21275e60ebb | 74 | magData.x = ((int16_t)((((int32_t)mdata_x) * |
pvaibhav | 11:d21275e60ebb | 75 | ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + |
pvaibhav | 11:d21275e60ebb | 76 | (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + |
pvaibhav | 11:d21275e60ebb | 77 | ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + |
pvaibhav | 11:d21275e60ebb | 78 | (((int16_t)dig_x1) << 3); |
pvaibhav | 11:d21275e60ebb | 79 | |
pvaibhav | 11:d21275e60ebb | 80 | temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); |
pvaibhav | 11:d21275e60ebb | 81 | magData.y = ((int16_t)((((int32_t)mdata_y) * |
pvaibhav | 11:d21275e60ebb | 82 | ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + |
pvaibhav | 11:d21275e60ebb | 83 | (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + |
pvaibhav | 11:d21275e60ebb | 84 | ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + |
pvaibhav | 11:d21275e60ebb | 85 | (((int16_t)dig_y1) << 3); |
pvaibhav | 11:d21275e60ebb | 86 | |
pvaibhav | 11:d21275e60ebb | 87 | magData.z = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) - |
pvaibhav | 11:d21275e60ebb | 88 | ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16)))); |
pvaibhav | 11:d21275e60ebb | 89 | |
pvaibhav | 11:d21275e60ebb | 90 | return magData; |
pvaibhav | 11:d21275e60ebb | 91 | } |
pvaibhav | 11:d21275e60ebb | 92 | |
pvaibhav | 11:d21275e60ebb | 93 | void Magnetometer::readCalibrationData() |
pvaibhav | 11:d21275e60ebb | 94 | { |
pvaibhav | 11:d21275e60ebb | 95 | // trying to read in serial order of address |
pvaibhav | 11:d21275e60ebb | 96 | dig_x1 = read_reg(0x5D); |
pvaibhav | 11:d21275e60ebb | 97 | dig_y1 = read_reg(0x5E); |
pvaibhav | 11:d21275e60ebb | 98 | dig_z4 = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8)); |
pvaibhav | 11:d21275e60ebb | 99 | dig_x2 = read_reg(0x64); |
pvaibhav | 11:d21275e60ebb | 100 | dig_y2 = read_reg(0x65); |
pvaibhav | 11:d21275e60ebb | 101 | dig_z2 = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8)); |
pvaibhav | 11:d21275e60ebb | 102 | dig_z1 = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8)); |
pvaibhav | 11:d21275e60ebb | 103 | dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8)); |
pvaibhav | 11:d21275e60ebb | 104 | dig_z3 = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8)); |
pvaibhav | 11:d21275e60ebb | 105 | dig_xy2 = read_reg(0x70); |
pvaibhav | 11:d21275e60ebb | 106 | dig_xy1 = read_reg(0x71); |
pvaibhav | 11:d21275e60ebb | 107 | LOG("calibration data loaded"); |
pvaibhav | 11:d21275e60ebb | 108 | } |