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

Increase MPU9250 magnetometer sample rate to configured TPS (#143)

Browse Source
This commit is contained in:
TheDevMinerTV
2022-04-04 15:04:47 +02:00
committed by GitHub
parent 72b1cd247c
commit efe8bb352a
2 changed files with 23 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/sensors/mpu9250sensor.cpp
View File

@@ -35,7 +35,6 @@
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
void MPU9250Sensor::motionSetup() {
@@ -141,23 +140,29 @@ void MPU9250Sensor::motionLoop() {
if(!imu.GetCurrentFIFOPacket(fifoBuffer,imu.dmpGetFIFOPacketSize())) return;
if(imu.dmpGetQuaternion(&rawQuat, fifoBuffer)) return; // FIFO CORRUPTED
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;
VectorFloat grav;
imu.dmpGetGravity(&grav,&rawQuat);
float Grav[3]={grav.x,grav.y,grav.z};
skipCalcMag=SKIP_CALC_MAG_INTERVAL;
if(correction.length_squared()==0.0f) {
correction=getCorrection(Grav,Mxyz,quat);
if(sensorId) skipCalcMag=SKIP_CALC_MAG_INTERVAL/2;
getMPUScaled();
if (Mxyz[0] == 0.0f && Mxyz[1] == 0.0f && Mxyz[2] == 0.0f) {
return;
}
VectorFloat grav;
imu.dmpGetGravity(&grav, &rawQuat);
float Grav[] = {grav.x, grav.y, grav.z};
if (correction.length_squared() == 0.0f) {
correction = getCorrection(Grav, Mxyz, quat);
} else {
Quat newCorr = getCorrection(Grav, Mxyz, quat);
if(!__isnanf(newCorr.w)) {
correction = correction.slerp(newCorr, MAG_CORR_RATIO);
}
else {
Quat newCorr = getCorrection(Grav,Mxyz,quat);
if(!__isnanf(newCorr.w)) correction = correction.slerp(newCorr,MAG_CORR_RATIO);
}
}else skipCalcMag--;
quaternion=correction*quat;
}
quaternion = correction * quat;
#else
unsigned long now = micros();
unsigned long deltat = now - last; //seconds since last update
@@ -371,4 +376,4 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
m_Logger.debug("Saved the calibration data");
m_Logger.info("Calibration data gathered");
}
}

1
src/sensors/mpu9250sensor.h
View File

@@ -48,7 +48,6 @@ private:
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]{}; // FIFO storage buffer
// raw data and scaled as vector
int skipCalcMag = 0;
float q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter
float Axyz[3]{};
float Gxyz[3]{};