github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Make MPU9250 use the FIFO (#192)

Browse Source
This commit is contained in:
Kitlith
2022-11-21 17:37:57 -08:00
committed by GitHub
parent 83550d21ef
commit fd3e463a9c
2 changed files with 190 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
src/sensors/mpu9250sensor.cpp
View File

@@ -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
@@ -95,7 +95,7 @@ void MPU9250Sensor::motionSetup() {
}
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
devStatus = imu.dmpInitialize();
uint8_t devStatus = imu.dmpInitialize();
if(devStatus == 0){
ledManager.pattern(50, 50, 5);
@@ -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
{
@@ -144,11 +157,14 @@ void MPU9250Sensor::motionLoop() {
if(!dmpReady)
return;
Quaternion rawQuat{};
if(!imu.GetCurrentFIFOPacket(fifoBuffer,imu.dmpGetFIFOPacketSize())) return;
if(imu.dmpGetQuaternion(&rawQuat, fifoBuffer)) return; // FIFO CORRUPTED
uint8_t dmpPacket[packetSize];
if(!imu.GetCurrentFIFOPacket(dmpPacket, packetSize)) return;
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;
@@ -178,7 +194,7 @@ void MPU9250Sensor::motionLoop() {
grav.y *= 2;
grav.z *= 2;
this->imu.dmpGetAccel(&this->rawAccel, fifoBuffer);
this->imu.dmpGetAccel(&this->rawAccel, dmpPacket);
this->imu.dmpGetLinearAccel(&this->rawAccel, &this->rawAccel, &grav);
// convert acceleration to m/s^2 (implicitly casts to float)
@@ -190,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
@@ -217,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_))
@@ -320,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;
@@ -337,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];
@@ -346,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;
@@ -356,8 +339,9 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
calibrationDataMag[i * 3 + 0] = my;
calibrationDataMag[i * 3 + 1] = mx;
calibrationDataMag[i * 3 + 2] = -mz;
Network::sendRawCalibrationData(calibrationDataAcc, CALIBRATION_TYPE_EXTERNAL_ACCEL, 0);
Network::sendRawCalibrationData(calibrationDataMag, CALIBRATION_TYPE_EXTERNAL_MAG, 0);
// Thought: make the server run magneto for us, don't store samples in memory?
Network::sendRawCalibrationData(calibrationDataAcc + i * 3, CALIBRATION_TYPE_EXTERNAL_ACCEL, 0);
Network::sendRawCalibrationData(calibrationDataMag + i * 3, CALIBRATION_TYPE_EXTERNAL_MAG, 0);
ledManager.off();
delay(250);
}
@@ -406,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;
}

37
src/sensors/mpu9250sensor.h
View File

@@ -42,24 +42,47 @@ public:
private:
MPU9250 imu{};
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
// TODO: actually check interrupt status
// uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]{}; // FIFO storage buffer
// raw data and scaled as vector
float q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter
float Axyz[3]{};
float Gxyz[3]{};
float Mxyz[3]{};
float rawMag[3]{};
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