AHRS
Dependencies: Eigen
Diff: BMX055.cpp
- Revision:
- 29:cd963a6d31c5
- Parent:
- 28:21dfb161c67c
- Child:
- 30:9b0cd3caf0ec
--- a/BMX055.cpp Mon Jan 20 12:41:13 2020 +0000 +++ b/BMX055.cpp Mon Jan 27 09:23:58 2020 +0000 @@ -52,7 +52,7 @@ dt[0] = 0x02; i2c.write(chip_addr, dt, 1, true); i2c.read(chip_addr, dt, 6, false); - x = dt[1] << 8 | (dt[0] & 0xf0); + x = dt[1] << 8 | (dt[0] & 0xf0); // values have to be divided by 16 y = dt[3] << 8 | (dt[2] & 0xf0); z = dt[5] << 8 | (dt[4] & 0xf0); accX = (float)x * acc_factor; @@ -79,20 +79,22 @@ void BMX055::readMag(void) { - int16_t x,y,z,msb_data; + int16_t x,y,z; + chip_addr = inf_addr.mag_addr; dt[0] = 0x42; i2c.write(chip_addr, dt, 1, true); i2c.read(chip_addr, dt, 6, false); - // folgende Implementierung ist im original verkehrt!!!, verwende von https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino - // Zeile 797 (altb Dez 2019) - x = (int16_t) (((int16_t)dt[1] << 8) | dt[0]) >> 3; // 13-bit signed integer for x-axis field - y = (int16_t) (((int16_t)dt[3] << 8) | dt[2]) >> 3; // 13-bit signed integer for x-axis field - z = (int16_t) (((int16_t)dt[5] << 8) | dt[4]) >> 1; // 13-bit signed integer for x-axis field - magX = (float)y * 0.01f; // MAGNETOMETER x-y are switched, see BMX055 datasheet page 161 - magY = (float)x * 0.01f; // x-y is switched - magZ = (float)z * 0.01f; // z is z + // corrected, pmic 21.01.2020 + x = dt[1] << 8 | (dt[0] & 0xf8); // value has to be divided by 8 + y = dt[3] << 8 | (dt[2] & 0xf8); // value has to be divided by 8 + z = dt[5] << 8 | (dt[4] & 0xfe); // value has to be divided by 2 + + // scaling factor 1/301.5815f if experimentally validated in comparision to pes board -> approx. same scaling of mag values, pmic 21.01.2020 + magX = -(float)y / 8.0f / 301.5815f; // MAGNETOMETER x-y are switched, see BMX055 datasheet page 161, also measurements show change in sign x!!! + magY = (float)x / 8.0f / 301.5815f; // x-y is switched + magZ = -(float)z / 2.0f / 301.5815f; // z is -z } float BMX055::get_chip_temperature() @@ -188,15 +190,15 @@ // MAG chip_addr = inf_addr.mag_addr; dt[0] = 0x4b; // Select Mag register - dt[1] = 0x83; // Soft reset + dt[1] = 0x83; // Soft reset 10000011 i2c.write(chip_addr, dt, 2, false); wait_ms(10); dt[0] = 0x4b; // Select Mag register - dt[1] = 0x01; // Soft reset + dt[1] = 0x01; // Soft reset 00000001 i2c.write(chip_addr, dt, 2, false); wait_ms(10); dt[0] = 0x4c; // Select Mag register - dt[1] = bmx055_parameters.mag_odr; + dt[1] = bmx055_parameters.mag_odr << 3; // corrected, pmic 20.01.2020 i2c.write(chip_addr, dt, 2, false); wait_ms(1); dt[0] = 0x4e; // Select Mag register @@ -204,11 +206,11 @@ i2c.write(chip_addr, dt, 2, false); wait_ms(1); dt[0] = 0x51; // Select Mag register - dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9 + dt[1] = 0x51; // No. of Repetitions for X-Y Axis = 163, fmaxODR = 33.2060 Hz, pmic 21.01.2020 i2c.write(chip_addr, dt, 2, false); wait_ms(1); dt[0] = 0x52; // Select Mag register - dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15 + dt[1] = 0x0a; // No. of Repetitions for Z-Axis = 11, fmaxODR = 33.2060 Hz, pmic 21.01.2020 i2c.write(chip_addr, dt, 2, false); #if 0 // ACC