Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Magnetometer.cpp
- Revision:
- 12:1632d7391453
- Parent:
- 11:d21275e60ebb
- Child:
- 13:bc20290eaff2
diff -r d21275e60ebb -r 1632d7391453 Magnetometer.cpp --- a/Magnetometer.cpp Thu Mar 12 10:16:01 2015 +0000 +++ b/Magnetometer.cpp Fri Mar 13 09:12:56 2015 +0000 @@ -30,6 +30,22 @@ 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 @@ -46,25 +62,18 @@ 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; + uint8_t buffer[8]; + + for (size_t i = 0; i < sizeof buffer; i++) + buffer[i] = read_reg(0x42 + i); + + //read_reg(0x40, buffer, 4); + + // Datasheet is wrong, BMX055 magneto x and y axis are interchanged and y axis is inverted !!! + const int16_t mdata_x = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 8; + const int16_t mdata_y = *(reinterpret_cast<const int16_t*>(buffer + 0)) / -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; @@ -86,8 +95,8 @@ 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; + + return magData - Vector3(-720, 1481, -2830); } void Magnetometer::readCalibrationData()