Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Magnetometer.cpp
- Revision:
- 14:d9fbb3ccd482
- Parent:
- 13:bc20290eaff2
- Child:
- 15:4488660e1a3b
--- a/Magnetometer.cpp Mon Mar 16 12:39:09 2015 +0000 +++ b/Magnetometer.cpp Tue Mar 17 15:50:41 2015 +0000 @@ -4,10 +4,17 @@ #include "Vector3.h" -Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0) +Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */) { if (powerOn()) { - readCalibrationData(); + readCalibrationData(); // temperature calibration + + // Initialise hard-iron and soft-iron correction. The minima and maxima values were measured + // on my desk and give a starting point for the background calibrator. + float minimums[] = { -1800.0f, -80.0f, -1800.0f }; + float maximums[] = { - 350, 1200, 4 }; + calibrator.setExtremes(minimums, maximums); + INFO("Bosch Sensortec BMX055-Magneto found"); powerOff(); } else { @@ -74,29 +81,27 @@ 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; - int16_t temp; // temporary - Vector3 magData; - - // calculate temperature compensated 16-bit magnetic fields - temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); + float input[3]; - magData.x = ((int16_t)((((int32_t)mdata_x) * - ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + - (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + - ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + - (((int16_t)dig_x1) << 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); - temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); - magData.y = ((int16_t)((((int32_t)mdata_y) * - ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + - (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + - ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + - (((int16_t)dig_y1) << 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); - magData.z = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) - + 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 magData - Vector3(-720, 1481, -2830); + return Vector3(output[0], output[1], output[2]); } void Magnetometer::readCalibrationData()