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 #92 from LETS-BEE/upstream_code

Browse Source 
MPU9250: Fix DMP Sample Rate
This commit is contained in:
Eiren Rain
2022-02-05 17:24:58 +02:00
committed by GitHub
parent 002131d2f6 cc1d6e83fb
commit 6596a42966
3 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
lib/mpu9250/MPU9250_6Axis_MotionApps_V6_12.cpp
View File

@@ -359,7 +359,7 @@ uint8_t MPU9250::dmpInitialize() { // Lets get it over with fast Write everythin
I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g
I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read
I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro
I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x02)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat
if (!writeProgMemoryBlock(dmpMemory, MPU9250_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU9250 Memory // Should Never Fail
I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address

8
src/sensors/mpu9250sensor.cpp
View File

@@ -65,7 +65,7 @@ void MPU9250Sensor::motionSetup() {
if(az>0 && 10.0*(ax*ax+ay*ay)<az*az)
startCalibration(0);
}
#ifndef _MAHONY_H_
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
devStatus = imu.dmpInitialize();
if(devStatus == 0){
for(int i = 0; i < 5; ++i) {
@@ -106,9 +106,6 @@ void MPU9250Sensor::motionSetup() {
void MPU9250Sensor::motionLoop() {
unsigned long now = micros();
unsigned long deltat = now - last; //seconds since last update
last = now;
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
// Update quaternion
if(!dmpReady)
@@ -135,6 +132,9 @@ void MPU9250Sensor::motionLoop() {
}else skipCalcMag--;
quaternion=correction*quat;
#else
unsigned long now = micros();
unsigned long deltat = now - last; //seconds since last update
last = now;
getMPUScaled();
mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
// madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);

1
src/sensors/mpu9250sensor.h
View File

@@ -33,7 +33,6 @@ public:
void motionLoop() override final;
void startCalibration(int calibrationType) override final;
void getMPUScaled();
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
private:
MPU9250 imu{};