Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Magnetometer.cpp
- Revision:
- 11:d21275e60ebb
- Parent:
- 4:e759b8c756da
- Child:
- 12:1632d7391453
--- a/Magnetometer.cpp Tue Feb 24 10:10:14 2015 +0000 +++ b/Magnetometer.cpp Thu Mar 12 10:16:01 2015 +0000 @@ -2,15 +2,107 @@ #define DEBUG "BMX055-Mag" #include "Logger.h" -Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */) { - write_reg(0x4B, 0x83); // softreset and powermode=suspend - wait_ms(3); // page 18 - - const uint8_t chip_id = read_reg(0x40); - if (chip_id == 0x32) { - INFO("Bosch Sensortec BMX055-Magneto ready"); +#include "Vector3.h" + +Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0) +{ + if (powerOn()) { + readCalibrationData(); + INFO("Bosch Sensortec BMX055-Magneto found"); + powerOff(); } else { - WARN("Bosch Sensortec BMX055-Magneto not found (chip ID=0x%02x, expected=0x32)", chip_id); + 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"); +} + +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 + union { + uint8_t bytes[8]; + struct { + uint16_t hall; + int16_t z; + int16_t y; + int16_t x; + } val; + } buffer; + + size_t i = 8; + while (i --> 0) { + buffer.bytes[i] = read_reg(0x42 + i); + }; + + int16_t mdata_x = buffer.val.x / 8; + int16_t mdata_y = buffer.val.y / 8; + int16_t mdata_z = buffer.val.z / 2; + uint16_t data_r = buffer.val.hall / 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))); + + 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); + + 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); + + magData.z = (((((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)))); + + return magData; +} + +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"); +} \ No newline at end of file