Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Magnetometer.cpp
- Committer:
- pvaibhav
- Date:
- 2015-05-26
- Revision:
- 40:8e852115fe55
- Parent:
- 34:01dec68de3ed
- Child:
- 46:fd5a62296b12
File content as of revision 40:8e852115fe55:
#include "Magnetometer.h" #define DEBUG "BMX055-Mag" #include "Logger.h" #include "Vector3.h" Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), int1(p5), int2(p27) { if (powerOn()) { readCalibrationData(); // temperature calibration // Initialise hard-iron and soft-iron correction. The minima and maxima values were measured // smartplane2 prototype gives a starting point for the background calibrator. float minimums[] = { -1536.0f, -2701.0f, -2112.0f }; float maximums[] = { 187, 1665, 39 }; calibrator.setExtremes(minimums, maximums); INFO("Bosch Sensortec BMX055-Magneto found"); powerOff(); } else { WARN("Bosch Sensortec BMX055-Magneto not found"); } } bool Magnetometer::powerOn() { write_reg(0x4B, 0x82); // softreset with power off wait_ms(3); // page 18 write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134. wait_ms(3); return read_reg(0x40) == 0x32; // verify chip ID } void Magnetometer::powerOff() { write_reg(0x4B, 0x82); // softreset and stay powered off LOG("powered off"); } bool Magnetometer::performSelfTest() { write_reg(0x4C, 0x06); // go to sleep mode write_reg(0x4C, 0x07); // start self test wait_ms(50); const bool done = (read_reg(0x4C) & 0x01) == 0; const bool x_passed = read_reg(0x42) & 0x01; const bool y_passed = read_reg(0x44) & 0x01; const bool z_passed = read_reg(0x46) & 0x01; 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"); return done && x_passed && y_passed && z_passed; } void Magnetometer::start() { // configure for "high accuracy preset" pg. 122 write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start } void Magnetometer::stop() { write_reg(0x4C, 0x06); // 10 Hz but sleep mode } Vector3 Magnetometer::read() { // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790 uint8_t buffer[8]; /* for (size_t i = 0; i < sizeof buffer; i++) buffer[i] = read_reg(0x42 + i); */ read_reg(0x42, buffer, sizeof buffer); // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!! const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / 8; const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8; const int16_t mdata_z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 2; const uint16_t data_r = *(reinterpret_cast<const uint16_t*>(buffer + 6)) / 4; float input[3]; // calculate temperature compensated 16-bit magnetic fields. // The "highly optimised" algorithm comes from Bosch, optimised further by Prashant (temp2) const int16_t temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); 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); input[0] = ((int16_t)((((int32_t)mdata_x) * (((temp2 + ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_x1) << 3); input[1] = ((int16_t)((((int32_t)mdata_y) * (((temp2 + ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + (((int16_t)dig_y1) << 3); input[2] = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) - ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16)))); float output[3]; calibrator.run(input, output); return Vector3(-output[0], output[1], output[2]); } void Magnetometer::getCalibration(Vector3 &mins, Vector3 &maxs) { float mi[3], ma[3]; calibrator.getExtremes(mi, ma); mins.x = mi[0]; mins.y = mi[1]; mins.z = mi[2]; maxs.x = ma[0]; maxs.y = ma[1]; maxs.z = ma[2]; } void Magnetometer::readCalibrationData() { // trying to read in serial order of address dig_x1 = read_reg(0x5D); dig_y1 = read_reg(0x5E); dig_z4 = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8)); dig_x2 = read_reg(0x64); dig_y2 = read_reg(0x65); dig_z2 = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8)); dig_z1 = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8)); dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8)); dig_z3 = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8)); dig_xy2 = read_reg(0x70); dig_xy1 = read_reg(0x71); LOG("calibration data loaded"); }