Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

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?

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