diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index e34b8000e..938d31f74 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -33,9 +33,9 @@ #include "dmpmag.h" #endif -#if defined(_MAHONY_H_) || defined(_MADGWICK_H_) +//#if defined(_MAHONY_H_) || defined(_MADGWICK_H_) constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB per d/s -> rad/s -#endif +//#endif #define MAG_CORR_RATIO 0.02 @@ -121,12 +121,25 @@ void MPU9250Sensor::motionSetup() { m_Logger.error("DMP Initialization failed (code %d)", devStatus); } #else + // NOTE: could probably combine these into less total writes, but this should work, and isn't time critical. + imu.setAccelFIFOEnabled(true); + imu.setXGyroFIFOEnabled(true); + imu.setYGyroFIFOEnabled(true); + imu.setZGyroFIFOEnabled(true); + imu.setSlave0FIFOEnabled(true); + + // TODO: set a rate we prefer instead of getting the current rate from the device. + deltat = 1.0 / 1000.0 * (1 + imu.getRate()); + //imu.setRate(blah); + + imu.resetFIFO(); + imu.setFIFOEnabled(true); + working = true; configured = true; #endif } - void MPU9250Sensor::motionLoop() { #if ENABLE_INSPECTION { @@ -149,7 +162,9 @@ void MPU9250Sensor::motionLoop() { if(imu.dmpGetQuaternion(&rawQuat, dmpPacket)) return; // FIFO CORRUPTED Quat quat(-rawQuat.y,rawQuat.x,rawQuat.z,rawQuat.w); - getMPUScaled(); + int16_t temp[3]; + imu.getMagnetometer(temp + 0, temp + 1, temp + 2); + parseMagData(temp); if (Mxyz[0] == 0.0f && Mxyz[1] == 0.0f && Mxyz[2] == 0.0f) { return; @@ -191,16 +206,29 @@ void MPU9250Sensor::motionLoop() { quaternion = correction * quat; #else - unsigned long now = micros(); - unsigned long deltat = now - last; //seconds since last update - last = now; - getMPUScaled(); + + union fifo_sample_raw buf; + uint16_t remaining_samples; + // TODO: would it be faster to read multiple samples at once + while (getNextSample (&buf, &remaining_samples)) { + parseAccelData(buf.sample.accel); + parseGyroData(buf.sample.gyro); + parseMagData(buf.sample.mag); + + // TODO: monitor magnetometer status + // buf.sample.mag_status; + // TODO: monitor interrupts + // imu.getIntStatus(); + // TODO: monitor remaining_samples to ensure that the number is going down, not up. + // remaining_samples + + #if defined(_MAHONY_H_) + mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); + #elif defined(_MADGWICK_H_) + madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); + #endif + } - #if defined(_MAHONY_H_) - mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); - #elif defined(_MADGWICK_H_) - madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); - #endif quaternion.set(-q[2], q[1], q[3], q[0]); #endif @@ -218,59 +246,6 @@ void MPU9250Sensor::motionLoop() { } } -void MPU9250Sensor::getMPUScaled() -{ - float temp[3]; - int i; - -#if defined(_MAHONY_H_) || defined(_MADGWICK_H_) - int16_t ax, ay, az, gx, gy, gz, mx, my, mz; - imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - Gxyz[0] = ((float)gx - m_Calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s - Gxyz[1] = ((float)gy - m_Calibration.G_off[1]) * gscale; - Gxyz[2] = ((float)gz - m_Calibration.G_off[2]) * gscale; - - Axyz[0] = (float)ax; - Axyz[1] = (float)ay; - Axyz[2] = (float)az; - - //apply offsets (bias) and scale factors from Magneto - #if useFullCalibrationMatrix == true - for (i = 0; i < 3; i++) - temp[i] = (Axyz[i] - m_Calibration.A_B[i]); - Axyz[0] = m_Calibration.A_Ainv[0][0] * temp[0] + m_Calibration.A_Ainv[0][1] * temp[1] + m_Calibration.A_Ainv[0][2] * temp[2]; - Axyz[1] = m_Calibration.A_Ainv[1][0] * temp[0] + m_Calibration.A_Ainv[1][1] * temp[1] + m_Calibration.A_Ainv[1][2] * temp[2]; - Axyz[2] = m_Calibration.A_Ainv[2][0] * temp[0] + m_Calibration.A_Ainv[2][1] * temp[1] + m_Calibration.A_Ainv[2][2] * temp[2]; - #else - for (i = 0; i < 3; i++) - Axyz[i] = (Axyz[i] - m-Calibration.A_B[i]); - #endif - -#else - int16_t mx, my, mz; - // with DMP, we just need mag data - imu.getMagnetometer(&mx, &my, &mz); -#endif - - // Orientations of axes are set in accordance with the datasheet - // See Section 9.1 Orientation of Axes - // https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf - Mxyz[0] = (float)my; - Mxyz[1] = (float)mx; - Mxyz[2] = -(float)mz; - //apply offsets and scale factors from Magneto - #if useFullCalibrationMatrix == true - for (i = 0; i < 3; i++) - temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); - Mxyz[0] = m_Calibration.M_Ainv[0][0] * temp[0] + m_Calibration.M_Ainv[0][1] * temp[1] + m_Calibration.M_Ainv[0][2] * temp[2]; - Mxyz[1] = m_Calibration.M_Ainv[1][0] * temp[0] + m_Calibration.M_Ainv[1][1] * temp[1] + m_Calibration.M_Ainv[1][2] * temp[2]; - Mxyz[2] = m_Calibration.M_Ainv[2][0] * temp[0] + m_Calibration.M_Ainv[2][1] * temp[1] + m_Calibration.M_Ainv[2][2] * temp[2]; - #else - for (i = 0; i < 3; i++) - Mxyz[i] = (Mxyz[i] - m_Calibration.M_B[i]); - #endif -} - void MPU9250Sensor::startCalibration(int calibrationType) { ledManager.on(); #if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_)) @@ -321,13 +296,17 @@ void MPU9250Sensor::startCalibration(int calibrationType) { // Wait for sensor to calm down before calibration m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); delay(2000); - for (int i = 0; i < calibrationSamples; i++) - { - int16_t ax,ay,az,gx,gy,gz,mx,my,mz; - imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - Gxyz[0] += float(gx); - Gxyz[1] += float(gy); - Gxyz[2] += float(gz); + + union fifo_sample_raw buf; + + imu.resetFIFO(); // fifo is sure to have filled up in the seconds of delay, don't try reading it. + for (int i = 0; i < calibrationSamples; i++) { + // wait for new sample + while (!getNextSample(&buf, nullptr)) { ; } + + Gxyz[0] += float(buf.sample.gyro[0]); + Gxyz[1] += float(buf.sample.gyro[1]); + Gxyz[2] += float(buf.sample.gyro[2]); } Gxyz[0] /= calibrationSamples; Gxyz[1] /= calibrationSamples; @@ -338,6 +317,7 @@ void MPU9250Sensor::startCalibration(int calibrationType) { #endif Network::sendRawCalibrationData(Gxyz, CALIBRATION_TYPE_EXTERNAL_GYRO, 0); + // TODO: use offset registers? m_Calibration.G_off[0] = Gxyz[0]; m_Calibration.G_off[1] = Gxyz[1]; m_Calibration.G_off[2] = Gxyz[2]; @@ -347,6 +327,8 @@ void MPU9250Sensor::startCalibration(int calibrationType) { ledManager.pattern(15, 300, 3000/310); float *calibrationDataAcc = (float*)malloc(calibrationSamples * 3 * sizeof(float)); float *calibrationDataMag = (float*)malloc(calibrationSamples * 3 * sizeof(float)); + + // NOTE: we don't use the FIFO here on *purpose*. This makes the difference between a calibration that takes a second or three and a calibration that takes much longer. for (int i = 0; i < calibrationSamples; i++) { ledManager.on(); int16_t ax,ay,az,gx,gy,gz,mx,my,mz; @@ -408,4 +390,101 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.debug("Saved the calibration data"); m_Logger.info("Calibration data gathered"); + // fifo will certainly have overflown due to magnetometer calibration, reset it. + imu.resetFIFO(); +} + +void MPU9250Sensor::parseMagData(int16_t data[3]) { + // reading *little* endian int16 + Mxyz[0] = (float)data[0]; + Mxyz[1] = (float)data[1]; + Mxyz[2] = -(float)data[2]; + + float temp[3]; + + //apply offsets and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + #if useFullCalibrationMatrix == true + Mxyz[i] = m_Calibration.M_Ainv[i][0] * temp[0] + m_Calibration.M_Ainv[i][1] * temp[1] + m_Calibration.M_Ainv[i][2] * temp[2]; + #else + Mxyz[i] = temp[i]; + #endif + } +} + +void MPU9250Sensor::parseAccelData(int16_t data[3]) { + // reading big endian int16 + Axyz[0] = (float)data[0]; + Axyz[1] = (float)data[1]; + Axyz[2] = (float)data[2]; + + float temp[3]; + + //apply offsets (bias) and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + #if useFullCalibrationMatrix == true + Axyz[i] = m_Calibration.A_Ainv[i][0] * temp[0] + m_Calibration.A_Ainv[i][1] * temp[1] + m_Calibration.A_Ainv[i][2] * temp[2]; + #else + Axyz[i] = temp[i]; + #endif + } +} + +// TODO: refactor so that calibration/conversion to float is only done in one place. +void MPU9250Sensor::parseGyroData(int16_t data[3]) { + // reading big endian int16 + Gxyz[0] = ((float)data[0] - m_Calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s + Gxyz[1] = ((float)data[1] - m_Calibration.G_off[1]) * gscale; + Gxyz[2] = ((float)data[2] - m_Calibration.G_off[2]) * gscale; } + +// really just an implementation detail of getNextSample... +void MPU9250Sensor::swapFifoData(union fifo_sample_raw* sample) { + #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + // byteswap the big endian integers + for (unsigned iii = 0; iii < 12; iii += 2) { + uint8_t tmp = sample->raw[iii + 0]; + sample->raw[iii + 0] = sample->raw[iii + 1]; + sample->raw[iii + 1] = tmp; + } + #elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ + // byteswap the little endian integers + for (unsigned iii = 12; iii < 18; iii += 2) { + uint8_t tmp = sample->raw[iii + 0]; + sample->raw[iii + 0] = sample->raw[iii + 1]; + sample->raw[iii + 1] = tmp; + } + #else + #error "Endian isn't Little endian or big endian, are we not using GCC or is this a PDP?" + #endif + + // compiler hint for the union, should be optimized away for optimization >= -O1 according to compiler explorer + memmove(&sample->sample, sample->raw, sensor_data_len); +} + +// thought experiments: +// is a single burst i2c transaction faster than multiple? by how much? +// how does that compare to the performance of a memcpy? +// how does that compare to the performance of data fusion? +// if we read an extra byte from the magnetometer (or otherwise did something funky) +// we could read into a properly aligned array of fifo_samples (and not require a memcpy?) +// TODO: strict aliasing might not be violated if we just read directly into a fifo_sample*. +// which means the union approach may be overcomplicated. *shrug* +bool MPU9250Sensor::getNextSample(union fifo_sample_raw *buffer, uint16_t *remaining_count) { + uint16_t count = imu.getFIFOCount(); + if (count < sensor_data_len) { + // no samples to read + remaining_count = 0; + return false; + } + + if (remaining_count) { + *remaining_count = (count / sensor_data_len) - 1; + } + + imu.getFIFOBytes(buffer->raw, sensor_data_len); + swapFifoData(buffer); + return true; +} \ No newline at end of file diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index d48946387..e49ab50d7 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -54,10 +54,35 @@ class MPU9250Sensor : public Sensor VectorInt16 rawAccel{}; Quat correction{0, 0, 0, 0}; // Loop timing globals - unsigned long now = 0, last = 0; // micros() timers - float deltat = 0; // loop time in seconds + float deltat = 0; // sample time in seconds SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration; + + // outputs to respective member variables + void parseAccelData(int16_t data[3]); + void parseGyroData(int16_t data[3]); + void parseMagData(int16_t data[3]); + + // 6 bytes for gyro, 6 bytes for accel, 7 bytes for magnetometer + static constexpr uint16_t sensor_data_len = 19; + + struct fifo_sample { + int16_t accel[3]; + int16_t gyro[3]; + int16_t mag[3]; + uint8_t mag_status; + }; + + // acts as a memory space for getNextSample. upon success, can read from the sample member + // TODO: this may be overcomplicated, we may be able to just use fifo_sample and i misunderstood strict aliasing rules. + union fifo_sample_raw { + uint8_t raw[sensor_data_len]; + struct fifo_sample sample; + }; + + // returns true if sample was read, outputs number of waiting samples in remaining_count if not null. + bool getNextSample(union fifo_sample_raw *buffer, uint16_t *remaining_count); + static void swapFifoData(union fifo_sample_raw* sample); }; #endif