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

Merge pull request #135 from LETS-BEE/main

Browse Source 
MPU9250: Calibration Routine Optimization
This commit is contained in:
Eiren Rain
2022-03-31 16:54:34 +03:00
committed by GitHub
parent a72bfc8ee5 66f82aba5a
commit 44a9c55413
3 changed files with 124 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
lib/mpu9250/MPU9250.cpp
View File

@@ -82,19 +82,19 @@ bool MPU9250_Base::testConnection() {
* the MPU-6000, which does not have a VLOGIC pin.
* @return I2C supply voltage level (0=VLOGIC, 1=VDD)
*/
uint8_t MPU9250_Base::getAuxVDDIOLevel() {
I2Cdev::readBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, buffer);
return buffer[0];
}
// uint8_t MPU9250_Base::getAuxVDDIOLevel() {
// I2Cdev::readBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, buffer);
// return buffer[0];
// }
/** Set the auxiliary I2C supply voltage level.
* When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
* 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
* the MPU-6000, which does not have a VLOGIC pin.
* @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
*/
void MPU9250_Base::setAuxVDDIOLevel(uint8_t level) {
I2Cdev::writeBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, level);
}
// void MPU9250_Base::setAuxVDDIOLevel(uint8_t level) {
// I2Cdev::writeBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_PWR_MODE_BIT, level);
// }
// SMPLRT_DIV register
@@ -2786,30 +2786,58 @@ void MPU9250_Base::setZFineGain(int8_t gain) {
// XA_OFFS_* registers
int16_t MPU9250_Base::getXAccelOffset() {
I2Cdev::readBytes(devAddr, MPU9250_RA_XA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
I2Cdev::readByte(devAddr, MPU9250_RA_XA_OFFS_TC, buffer);
return buffer[0];
}
void MPU9250_Base::setXAccelOffset(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU9250_RA_XA_OFFS_H, offset);
I2Cdev::writeByte(devAddr, MPU9250_RA_XA_OFFS_TC, offset);
}
// YA_OFFS_* register
int16_t MPU9250_Base::getYAccelOffset() {
I2Cdev::readBytes(devAddr, MPU9250_RA_YA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
I2Cdev::readByte(devAddr, MPU9250_RA_YA_OFFS_TC, buffer);
return buffer[0];
}
void MPU9250_Base::setYAccelOffset(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU9250_RA_YA_OFFS_H, offset);
I2Cdev::writeByte(devAddr, MPU9250_RA_YA_OFFS_TC, offset);
}
// ZA_OFFS_* register
int16_t MPU9250_Base::getZAccelOffset() {
I2Cdev::readBytes(devAddr, MPU9250_RA_ZA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
I2Cdev::readByte(devAddr, MPU9250_RA_ZA_OFFS_TC, buffer);
return buffer[0];
}
void MPU9250_Base::setZAccelOffset(int16_t offset) {
I2Cdev::writeByte(devAddr, MPU9250_RA_ZA_OFFS_TC, offset);
}
int16_t MPU9250_Base::getXAccelOffsetUser() {
I2Cdev::readBytes(devAddr, MPU9250_RA_XA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 7) | (buffer[1] >> 1);
}
void MPU9250_Base::setXAccelOffsetUser(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU9250_RA_XA_OFFS_H, offset);
}
// YA_OFFS_* register
int16_t MPU9250_Base::getYAccelOffsetUser() {
I2Cdev::readBytes(devAddr, MPU9250_RA_YA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 7) | (buffer[1] >> 1);
}
void MPU9250_Base::setYAccelOffsetUser(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU9250_RA_YA_OFFS_H, offset);
}
// ZA_OFFS_* register
int16_t MPU9250_Base::getZAccelOffsetUser() {
I2Cdev::readBytes(devAddr, MPU9250_RA_ZA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 7) | (buffer[1] >> 1);
}
void MPU9250_Base::setZAccelOffsetUser(int16_t offset) {
I2Cdev::writeWord(devAddr, MPU9250_RA_ZA_OFFS_H, offset);
}
@@ -3397,7 +3425,7 @@ bool MPU9250_Base::testConnectionMagnetometer() {
* @see MPU9250_RA_MAG_ADDRESS
* @see MPU9250_RA_MAG_WHOAMI
*/
int8_t MPU9250_Base::getMagnetometerDeviceID() {
uint8_t MPU9250_Base::getMagnetometerDeviceID() {
// Set up magnetometer as slave 0 for reading
I2Cdev::writeByte(devAddr, MPU9250_RA_I2C_SLV0_ADDR, MPU9250_RA_MAG_ADDRESS|0x80);
// Start reading from WHO_AM_I register

46
lib/mpu9250/MPU9250.h
View File

@@ -48,7 +48,8 @@ THE SOFTWARE.
//Magnetometer Registers, AK8963
#define MPU9250_RA_MAG_ADDRESS 0x0C
#define MPU9250_RA_MAG_WHOAMI 0x01
#define MPU9250_RA_MAG_WHOAMI 0x00
#define MPU9250_RA_MAG_INFO 0x01
#define MPU9250_RA_MAG_ST1 0x02
#define MPU9250_RA_MAG_XOUT_L 0x03
#define MPU9250_RA_MAG_XOUT_H 0x04
@@ -95,7 +96,7 @@ THE SOFTWARE.
#define MPU9250_MAG_CNTL2_SRST_BIT 0
//MPU6500
#define MPU9250_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
#define MPU9250_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
#define MPU9250_DEFAULT_ADDRESS MPU9250_ADDRESS_AD0_LOW
@@ -106,28 +107,15 @@ THE SOFTWARE.
#define MPU9250_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU9250_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU9250_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU9250_RA_XA_OFFS_H 0x77 //[15:0] XA_OFFS
#define MPU9250_RA_XA_OFFS_L_TC 0x78
#define MPU9250_RA_YA_OFFS_H 0x7A //[15:0] YA_OFFS
#define MPU9250_RA_YA_OFFS_L_TC 0x7B
#define MPU9250_RA_ZA_OFFS_H 0x7D //[15:0] ZA_OFFS
#define MPU9250_RA_ZA_OFFS_L_TC 0x7E
// #define MPU9250_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
// #define MPU9250_RA_XA_OFFS_L_TC 0x07
// #define MPU9250_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
// #define MPU9250_RA_YA_OFFS_L_TC 0x09
// #define MPU9250_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
// #define MPU9250_RA_ZA_OFFS_L_TC 0x0B
#define MPU9250_RA_XA_OFFS_TC 0x0D
#define MPU9250_RA_YA_OFFS_TC 0x0E
#define MPU9250_RA_ZA_OFFS_TC 0x0F
#define MPU9250_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU9250_RA_XG_OFFS_USRL 0x14
#define MPU9250_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU9250_RA_YG_OFFS_USRL 0x16
#define MPU9250_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU9250_RA_ZG_OFFS_USRL 0x18
#define MPU9250_RA_SMPLRT_DIV 0x19
#define MPU9250_RA_CONFIG 0x1A
#define MPU9250_RA_GYRO_CONFIG 0x1B
@@ -220,6 +208,12 @@ THE SOFTWARE.
#define MPU9250_RA_FIFO_COUNTL 0x73
#define MPU9250_RA_FIFO_R_W 0x74
#define MPU9250_RA_WHO_AM_I 0x75
#define MPU9250_RA_XA_OFFS_H 0x77 //[14:7] XA_OFFS
#define MPU9250_RA_XA_OFFS_L 0x78 //[7:1]
#define MPU9250_RA_YA_OFFS_H 0x7A //[14:7] YA_OFFS
#define MPU9250_RA_YA_OFFS_L 0x7B //[7:1]
#define MPU9250_RA_ZA_OFFS_H 0x7D //[14:7] ZA_OFFS
#define MPU9250_RA_ZA_OFFS_L 0x7E //[7:1]
#define MPU9250_TC_PWR_MODE_BIT 7
#define MPU9250_TC_OFFSET_BIT 6
@@ -456,7 +450,7 @@ THE SOFTWARE.
#define MPU9250_WHO_AM_I_BIT 8
#define MPU9250_WHO_AM_I_LENGTH 8
#define MPU9250_DMP_MEMORY_BANKS 8
// #define MPU9250_DMP_MEMORY_BANKS 8
#define MPU9250_DMP_MEMORY_BANK_SIZE 256
#define MPU9250_DMP_MEMORY_CHUNK_SIZE 16
@@ -783,6 +777,18 @@ class MPU9250_Base {
int16_t getZAccelOffset();
void setZAccelOffset(int16_t offset);
// XA_OFFS_* registers
int16_t getXAccelOffsetUser();
void setXAccelOffsetUser(int16_t offset);
// YA_OFFS_* register
int16_t getYAccelOffsetUser();
void setYAccelOffsetUser(int16_t offset);
// ZA_OFFS_* register
int16_t getZAccelOffsetUser();
void setZAccelOffsetUser(int16_t offset);
// XG_OFFS_USR* registers
int16_t getXGyroOffsetUser();
void setXGyroOffsetUser(int16_t offset);
@@ -851,7 +857,7 @@ class MPU9250_Base {
// AK8963 Magnetomter Functions
void initilaizeMagnetometer();
bool testConnectionMagnetometer();
int8_t getMagnetometerDeviceID();
uint8_t getMagnetometerDeviceID();
void getMagnetometerAdjustments(float *adjustments);
void getMagnetometer(int16_t* x, int16_t* y, int16_t* z);
int16_t getMagnetometerX();

67
src/sensors/mpu9250sensor.cpp
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain, S.J. Remington
Copyright (c) 2021 Eiren Rain, S.J. Remington, SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
@@ -63,8 +63,7 @@ void MPU9250Sensor::motionSetup() {
imu.getAcceleration(&ax, &ay, &az);
g_az = (float)az / 16384;
if(g_az > 0.75f)
{
if(g_az > 0.75f) {
m_Logger.debug("Starting calibration...");
startCalibration(0);
}
@@ -167,15 +166,13 @@ void MPU9250Sensor::getMPUScaled()
{
float temp[3];
int 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 - calibration->G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
// Gxyz[1] = ((float)gy - calibration->G_off[1]) * gscale;
// Gxyz[2] = ((float)gz - calibration->G_off[2]) * gscale;
Gxyz[0] = (float)gx * gscale; //250 LSB(d/s) default to radians/s
Gxyz[1] = (float)gy * gscale;
Gxyz[2] = (float)gz * gscale;
#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 - calibration->G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
Gxyz[1] = ((float)gy - calibration->G_off[1]) * gscale;
Gxyz[2] = ((float)gz - calibration->G_off[2]) * gscale;
Axyz[0] = (float)ax;
Axyz[1] = (float)ay;
@@ -193,6 +190,12 @@ void MPU9250Sensor::getMPUScaled()
Axyz[i] = (Axyz[i] - 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
@@ -214,6 +217,43 @@ void MPU9250Sensor::getMPUScaled()
void MPU9250Sensor::startCalibration(int calibrationType) {
ledManager.on();
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
// with DMP, we just need mag data
constexpr int calibrationSamples = 300;
DeviceConfig *config = getConfigPtr();
// Blink calibrating led before user should rotate the sensor
m_Logger.info("Gently rotate the device while it's gathering magnetometer data");
ledManager.pattern(15, 300, 3000/310);
float *calibrationDataMag = (float*)malloc(calibrationSamples * 3 * sizeof(float));
for (int i = 0; i < calibrationSamples; i++) {
ledManager.on();
int16_t mx,my,mz;
imu.getMagnetometer(&mx, &my, &mz);
calibrationDataMag[i * 3 + 0] = my;
calibrationDataMag[i * 3 + 1] = mx;
calibrationDataMag[i * 3 + 2] = -mz;
Network::sendRawCalibrationData(calibrationDataMag, CALIBRATION_TYPE_EXTERNAL_MAG, 0);
ledManager.off();
delay(250);
}
m_Logger.debug("Calculating calibration data...");
float M_BAinv[4][3];
CalculateCalibration(calibrationDataMag, calibrationSamples, M_BAinv);
free(calibrationDataMag);
m_Logger.debug("[INFO] Magnetometer calibration matrix:");
m_Logger.debug("{");
for (int i = 0; i < 3; i++) {
config->calibration[sensorId].M_B[i] = M_BAinv[0][i];
config->calibration[sensorId].M_Ainv[0][i] = M_BAinv[1][i];
config->calibration[sensorId].M_Ainv[1][i] = M_BAinv[2][i];
config->calibration[sensorId].M_Ainv[2][i] = M_BAinv[3][i];
m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]);
}
m_Logger.debug("}");
#elif
m_Logger.debug("Gathering raw data for device calibration...");
constexpr int calibrationSamples = 300;
@@ -252,8 +292,7 @@ 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));
for (int i = 0; i < calibrationSamples; i++)
{
for (int i = 0; i < calibrationSamples; i++) {
ledManager.on();
int16_t ax,ay,az,gx,gy,gz,mx,my,mz;
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
@@ -298,6 +337,8 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]);
}
m_Logger.debug("}");
#endif
m_Logger.debug("Now Saving EEPROM");
setConfig(*config);
ledManager.off();