Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Magnetometer.cpp@32:d37447aec6b4, 2015-05-04 (annotated)

Committer:
pvaibhav
Date:
Mon May 04 15:16:57 2015 +0000
Revision:
32:d37447aec6b4
Parent:
15:4488660e1a3b
Child:
34:01dec68de3ed
Interrupt lines added to all sensors and motor driver, power aware I2C subclass added (currently doesn't do anything)

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 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 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 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 }