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

Using Mahony filter with DMP

Browse Source 
Using Accelerometer, Gyroscope with DMP and magnetomter.
Be Careful! This code is not stable.
This commit is contained in:
LETBBI
2022-01-19 21:29:00 +09:00
parent e578dd2f43
commit 57e3c5efa8
7 changed files with 216 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
README.md
View File

@@ -18,8 +18,9 @@ The following IMUs and their corresponding `IMU` values are supported by the fir
* BNO055 (IMU_BNO055)
* Should be roughly equal BNO080, but cheaper. Not tested thoroughly, please report your results on Discord if you're willing to try.
* MPU-9250 (IMU_MPU9250)
* Using Mahony sensor fusion of Gyroscope, Magnetometer and Accelerometer, requires good magnetic environment.
* NOTE: Currently can only be used as MPU-6050 without magnetometer. To use it as MPU-6050, specify `IMU_MPU6050` in your `defines.h`.
* Using Mahony sensor fusion of Gyroscope, Accelerometer with DMP and magnetometer, requires good magnetic environment.
* NOTE: Require wiring interrupt pin.
* NOTE: Currently can only be used as MPU-6050 without magnetometer. To use it as MPU-6050, specify `IMU_MPU6500` in your `defines.h`.
* MPU-6500 (IMU_MPU6500) & MPU-6050 (IMU_MPU6050)
* Using internal DMP to fuse Gyroscope and Accelerometer. Can drift substantially.
* NOTE: Currently the MPU will auto calibrate when powered on. You *must* place it on the ground and *DO NOT* move it until calibration is complete (for a few seconds). **LED on the ESP will blink 5 times after calibration is over.**

5
lib/math/helper_3dmath.cpp
View File

@@ -31,6 +31,11 @@ float invSqrt(float x) {
return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
}
float vector_dot(float a[3], float b[3])
{
return (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]);
}
void vector_normalize(float a[3])
{
float norm = invSqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);

3
lib/math/helper_3dmath.h
View File

@@ -215,7 +215,8 @@ class VectorFloat {
}
};
void vector_normalize(float a[3]);
float invSqrt(float x);
float vector_dot(float a[3], float b[3]);
void vector_normalize(float a[3]);
#endif /* _HELPER_3DMATH_H_ */

2
lib/mpu9250/MPU9250.cpp
View File

@@ -103,7 +103,7 @@ void MPU9250::initialize(uint8_t address) {
// Set up magnetometer as slave 0 for reading
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_ADDR, MPU9250_RA_MAG_ADDRESS|0x80);
// Start reading from HXL register
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_REG, MPU9250_RA_MAG_WHOAMI);
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_REG, MPU9250_RA_MAG_XOUT_L);
// Read 7 bytes (until ST2 register), group LSB and MSB
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_CTRL, 0x97);
delay(10);

8
lib/mpu9250/MPU9250MotionApps.cpp
View File

@@ -47,7 +47,7 @@ THE SOFTWARE.
#include <avr/pgmspace.h>
#else
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
#ifndef __PGMSPACE_H_
#if not defined(__PGMSPACE_H_) && not defined(PGMSPACE_INCLUDE )
#define __PGMSPACE_H_ 1
#include <inttypes.h>
@@ -522,7 +522,7 @@ uint8_t MPU9250::dmpInitialize() {
// setup AK8963 (0x0C) as Slave 0 in read mode
DEBUG_PRINTLN(F("Setting up AK8963 read slave 0..."));
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_ADDR, MPU9250_RA_MAG_ADDRESS|0x80);
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_REG, MPU9250_RA_MAG_XOUT_L);
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_REG, MPU9250_RA_MAG_WHOAMI);
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_CTRL, 0xDA);
// setup AK8963 (0x0C) as Slave 2 in write mode
@@ -534,7 +534,9 @@ uint8_t MPU9250::dmpInitialize() {
// setup I2C timing/delay control
DEBUG_PRINTLN(F("Setting up slave access delay..."));
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV4_CTRL, 0x18);
// I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV4_CTRL, 0x18);
// samplerate, DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV4_CTRL, 0x01);
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_MST_DELAY_CTRL, 0x05);
// enable interrupts

229
src/mpu9250sensor.cpp
View File

@@ -28,15 +28,21 @@
#include <i2cscan.h>
#include "calibration.h"
#include "magneto1.4.h"
// #include "mahony.h"
#ifndef _MAHONY_H_
#include "mahony.h"
#define _DMP_
#ifdef _DMP_
#include "dmpmag.h"
constexpr float ascale = (4.f / 32768.f);
constexpr float gscale = (875.f / 32768.f); // DMP gyro 875 is more exeptable.
constexpr float mscale = 0.15; // (4912.f / 8190.f) 0.6 | (4912.f / 32760.f) 0.15
#elif
constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB per d/s -> rad/s
#endif
constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB per d/s -> rad/s
#define SKIP_CALC_MAG_INTERVAL 10
#define MAG_CORR_RATIO 0.2
#define SKIP_CALC_MAG_INTERVAL 10
#define MAG_CORR_RATIO 0.02
namespace {
void signalAssert() {
@@ -73,8 +79,11 @@ void MPU9250Sensor::motionSetup() {
Serial.println(imu.getDeviceID(), HEX);
return;
}
int16_t ax,ay,az,dumb;
imu.getMagnetometerAdjustments(magAdjustments);
#ifndef _DMP_
int16_t ax,ay,az,dumb;
// turn on while flip back to calibrate. then, flip again after 5 seconds.
// TODO: Move calibration invoke after calibrate button on slimeVR server available
imu.getMotion6(&ax, &ay, &az, &dumb, &dumb, &dumb);
@@ -87,8 +96,7 @@ void MPU9250Sensor::motionSetup() {
if(az>0 && 10.0*(ax*ax+ay*ay)<az*az)
startCalibration(0);
}
imu.getMagnetometerAdjustments(adjustments);
#ifndef _MAHONY_H_
#else // _DMP_
devStatus = imu.dmpInitialize();
if(devStatus == 0){
for(int i = 0; i < 5; ++i) {
@@ -102,10 +110,11 @@ void MPU9250Sensor::motionSetup() {
Serial.println(F("[NOTICE] Enabling DMP..."));
imu.setDMPEnabled(true);
// enable interrupt detection
Serial.println(F("[NOTICE] Enabling interrupt detection..."));
pinMode(PIN_IMU_INT_2, INPUT);
attachInterrupt(digitalPinToInterrupt(PIN_IMU_INT_2), dmpDataReady, RISING);
pinMode(PIN_IMU_INT, INPUT);
attachInterrupt(digitalPinToInterrupt(PIN_IMU_INT), dmpDataReady, RISING);
mpuIntStatus = imu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
@@ -127,6 +136,24 @@ void MPU9250Sensor::motionSetup() {
Serial.print(devStatus);
Serial.println(F(")"));
}
// turn on while flip back to calibrate. then, flip again after 5 seconds.
// TODO: Move calibration invoke after calibrate button on slimeVR server available
while(!getDMPscaled());
if(rawAcc[2]<0 && 10.0*(rawAcc[0]*rawAcc[0]+rawAcc[1]*rawAcc[1])<rawAcc[2]*rawAcc[2]) {
digitalWrite(CALIBRATING_LED, HIGH);
Serial.println("Calling Calibration... Flip front to confirm start calibration.");
delay(5000);
digitalWrite(CALIBRATING_LED, LOW);
while(!getDMPscaled());
if(rawAcc[2]>0 && 10.0*(rawAcc[0]*rawAcc[0]+rawAcc[1]*rawAcc[1])<rawAcc[2]*rawAcc[2]) {
startCalibration(0);
}
}
#endif
}
@@ -139,39 +166,48 @@ void MPU9250Sensor::motionLoop() {
delayMicroseconds(samplingRateInMillis*1000-deltat);
deltat = samplingRateInMillis*1000;
}
#ifndef _MAHONY_H_
#ifdef _DMP_
// Update quaternion
if(!dmpReady)
return;
Quaternion rawQuat{};
if(!imu.GetCurrentFIFOPacket(fifoBuffer,imu.dmpPacketSize)) return;
imu.dmpGetQuaternion(&rawQuat, fifoBuffer);
Quat quat(-rawQuat.y,rawQuat.x,rawQuat.z,rawQuat.w);
if(!skipCalcMag){
getMPUScaled();
if(Mxyz[0]==0.0f && Mxyz[1]==0.0f && Mxyz[2]==0.0f) return;
skipCalcMag=SKIP_CALC_MAG_INTERVAL;
if(correction.length_squared()==0.0f) {
correction=getCorrection(Axyz,Mxyz,quat);
if(isSecond) skipCalcMag=SKIP_CALC_MAG_INTERVAL/2;
if (getDMPscaled()) {
if(skipCalcMag == 0 && vector_dot(Axyz, Axyz) > 0.0f && vector_dot(Mxyz, Mxyz) > 0.0f){
vector_normalize(Axyz);
vector_normalize(Mxyz);
mahonyQuaternionUpdate(q,
Axyz[0], Axyz[1], Axyz[2],
Gxyz[0], Gxyz[1], Gxyz[2],
Mxyz[1], Mxyz[0], -Mxyz[2],
deltat * 1.0e-6
);
} else {
skipCalcMag = skipCalcMag == 0 ? SKIP_CALC_MAG_INTERVAL : skipCalcMag - 1;
mahonyQuaternionUpdate(q,
Axyz[0], Axyz[1], Axyz[2],
Gxyz[0], Gxyz[1], Gxyz[2],
deltat * 1.0e-6
);
}
else correction = correction.slerp(getCorrection(Axyz,Mxyz,quat),MAG_CORR_RATIO);
}else skipCalcMag--;
quaternion=correction*quat;
quaternion.set(-q[2], q[1], q[3], q[0]);
quaternion *= sensorOffset;
if(!lastQuatSent.equalsWithEpsilon(quaternion)) {
newData = true;
lastQuatSent = quaternion;
}
}
#else
getMPUScaled();
// 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
mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[1], Mxyz[0], -Mxyz[2], deltat * 1.0e-6);
quaternion.set(-q[1], -q[2], -q[0], q[3]);
#endif
quaternion.set(-q[2], q[1], q[3], q[0]);
quaternion *= sensorOffset;
if(!lastQuatSent.equalsWithEpsilon(quaternion)) {
newData = true;
lastQuatSent = quaternion;
}
#endif
}
void MPU9250Sensor::sendData() {
@@ -210,9 +246,9 @@ void MPU9250Sensor::getMPUScaled()
vector_normalize(Axyz);
// Apply correction for 16-bit mode and factory sensitivity adjustments
Mxyz[0] = (float)mx * 0.15f * adjustments[0];
Mxyz[1] = (float)my * 0.15f * adjustments[1];
Mxyz[2] = (float)mz * 0.15f * adjustments[2];
Mxyz[0] = (float)mx * 0.15f * magAdjustments[0];
Mxyz[1] = (float)my * 0.15f * magAdjustments[1];
Mxyz[2] = (float)mz * 0.15f * magAdjustments[2];
//apply offsets and scale factors from Magneto
#if useFullCalibrationMatrix == true
for (i = 0; i < 3; i++)
@@ -227,11 +263,133 @@ void MPU9250Sensor::getMPUScaled()
vector_normalize(Mxyz);
}
bool MPU9250Sensor::getDMPscaled() {
// if programming failed, don't try to do anything
if (!dmpReady) return false;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (!isWiFiConnected())
wifiUpkeep();
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = imu.getIntStatus();
// get current FIFO count
fifoCount = imu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
imu.resetFIFO();
Serial.println(F("FIFO overflow!"));
return false;
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
// read a packet from FIFO
imu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
imu.dmpGetAccel(rawAcc, fifoBuffer);
imu.dmpGetGyro(rawGyro, fifoBuffer);
Axyz[0] = (float)rawAcc[0] * ascale;
Axyz[1] = (float)rawAcc[1] * ascale;
Axyz[2] = (float)rawAcc[2] * ascale;
Gxyz[0] = (float)rawGyro[0] * gscale;
Gxyz[1] = (float)rawGyro[1] * gscale;
Gxyz[2] = (float)rawGyro[2] * gscale;
#if (IMU_HAS_MAG == true)
imu.dmpGetMag(rawMag, fifoBuffer);
Mxyz[0] = (float)rawMag[0] * mscale * magAdjustments[0];
Mxyz[1] = (float)rawMag[1] * mscale * magAdjustments[1];
Mxyz[2] = (float)rawMag[2] * mscale * magAdjustments[2];
float temp[3]{ 0 };
for (int i = 0; i < 3; i++)
temp[i] = (Mxyz[i] - calibration->M_B[i]);
Mxyz[0] = calibration->M_Ainv[0][0] * temp[0] + calibration->M_Ainv[0][1] * temp[1] + calibration->M_Ainv[0][2] * temp[2];
Mxyz[1] = calibration->M_Ainv[1][0] * temp[0] + calibration->M_Ainv[1][1] * temp[1] + calibration->M_Ainv[1][2] * temp[2];
Mxyz[2] = calibration->M_Ainv[2][0] * temp[0] + calibration->M_Ainv[2][1] * temp[1] + calibration->M_Ainv[2][2] * temp[2];
#endif
return true;
}
return false;
}
void MPU9250Sensor::startCalibration(int calibrationType) {
digitalWrite(CALIBRATING_LED, LOW);
Serial.println("[NOTICE] Gathering raw data for device calibration...");
constexpr int calibrationSamples = 300;
DeviceConfig *config = getConfigPtr();
#ifdef _DMP_
Serial.println("[NOTICE] Gyro and Acc data is calibrated by DMP");
Serial.println("[NOTICE] Gathering mag data for device calibration...");
// Blink calibrating led before user should rotate the sensor
Serial.println("[NOTICE] After 3seconds, Gently rotate the device while it's gathering and magnetometer data");
digitalWrite(CALIBRATING_LED, LOW);
delay(1500);
digitalWrite(CALIBRATING_LED, HIGH);
delay(1500);
Serial.println("[NOTICE] Gathering magnetometer data start!!");
float *calibrationDataMag = (float*)malloc(calibrationSamples * 3 * sizeof(float));
for (int i = 0; i < calibrationSamples; i++)
{
digitalWrite(CALIBRATING_LED, LOW);
while(!getDMPscaled());
calibrationDataMag[i * 3 + 0] = (float)rawMag[0] * mscale * magAdjustments[0];
calibrationDataMag[i * 3 + 1] = (float)rawMag[1] * mscale * magAdjustments[1];
calibrationDataMag[i * 3 + 2] = (float)rawMag[2] * mscale * magAdjustments[2];
digitalWrite(CALIBRATING_LED, HIGH);
delay(80);
}
Serial.println("[NOTICE] Calibration data gathered");
digitalWrite(CALIBRATING_LED, HIGH);
Serial.println("[NOTICE] Now Calculate Calibration data");
float M_BAinv[4][3];
CalculateCalibration(calibrationDataMag, calibrationSamples, M_BAinv);
free(calibrationDataMag);
Serial.println("[NOTICE] Finished Calculate Calibration data");
Serial.println("[NOTICE] Now Saving EEPROM");
for (int i = 0; i < 4; i++)
{
Serial.print("M_BAinv[");Serial.print(i);Serial.print("]\t");
Serial.print(M_BAinv[i][0]);
Serial.print("\t");
Serial.print(M_BAinv[i][1]);
Serial.print("\t");
Serial.println(M_BAinv[i][2]);
}
for (int i = 0; i < 3; i++)
{
config->calibration[isSecond?1:0].M_B[i] = M_BAinv[0][i];
config->calibration[isSecond?1:0].M_Ainv[0][i] = M_BAinv[1][i];
config->calibration[isSecond?1:0].M_Ainv[1][i] = M_BAinv[2][i];
config->calibration[isSecond?1:0].M_Ainv[2][i] = M_BAinv[3][i];
}
setConfig(*config);
Serial.println("[NOTICE] Finished Saving EEPROM");
delay(1000);
#elif
Serial.println("[NOTICE] Gathering raw data for device calibration...");
// Reset values
Gxyz[0] = 0;
Gxyz[1] = 0;
@@ -313,4 +471,5 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0, PACKET_RAW_CALIBRATION_DATA);
Serial.println("[NOTICE] Finished Saving EEPROM");
delay(4000);
#endif
}

19
src/sensor.h
View File

@@ -32,7 +32,6 @@
#include "configuration.h"
#include <Adafruit_BNO055.h>
#include "defines.h"
#include <helper_3dmath.h>
#define DATA_TYPE_NORMAL 1
#define DATA_TYPE_CORRECTION 2
@@ -145,27 +144,21 @@ class MPU9250Sensor : public MPU6050Sensor {
void sendData() override final;
void startCalibration(int calibrationType) override final;
void getMPUScaled();
bool getDMPscaled();
void internalCalibration();
MPU9250 imu {};
//raw data and scaled as vector
int16_t rawAcc[3] {};
int16_t rawGyro[3] {};
int16_t rawMag[3] {};
float Axyz[3] {};
float Gxyz[3] {};
float Mxyz[3] {};
int skipCalcMag = 0;
Quat correction{0,0,0,0};
float adjustments[3] {};
Quat correction{};
float magAdjustments[3] {};
unsigned long last = 0; //micros() timers
CalibrationConfig *calibration;
bool newData{false};
Quaternion rawQuat {};
// MPU dmp control/status vars
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)
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
};
#endif // SLIMEVR_SENSOR_H_