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 #51 from deiteris/mpu-fifo

Browse Source 
Improve DMP FIFO packet reader
This commit is contained in:
Eiren Rain
2021-12-16 19:46:58 +03:00
committed by GitHub
parent 4aea9fd2d7 9503ab9763
commit ea775b59e0
1 changed files with 33 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
lib/mpu6050/MPU6050.cpp
View File

@@ -2745,40 +2745,41 @@ void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
/** Get latest byte from FIFO buffer no matter how much time has passed.
* === GetCurrentFIFOPacket ===
* ================================================================
* Returns 1) when nothing special was done
* 2) when recovering from overflow
* Returns 1) when data was successfully read
* 2) when recovering from overflow, only the earliest packet is read
* 0) when no valid data is available
* ================================================================ */
int8_t MPU6050::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
int16_t fifoC;
// This section of code is for when we allowed more than 1 packet to be acquired
uint32_t BreakTimer = micros();
do {
if ((fifoC = getFIFOCount()) > length) {
if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to reset the buffer and wait for the next to arrive
resetFIFO(); // Fixes any overflow corruption
fifoC = 0;
while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (11000))); // Get Next New Packet
} else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
uint8_t Trash[BUFFER_LENGTH];
while ((fifoC = getFIFOCount()) > length) { // Test each time just in case the MPU is writing to the FIFO Buffer
fifoC = fifoC - length; // Save the last packet
uint16_t RemoveBytes;
while (fifoC) { // fifo count will reach zero so this is safe
RemoveBytes = min((int)fifoC, BUFFER_LENGTH); // Buffer Length is different than the packet length this will efficiently clear the buffer
getFIFOBytes(Trash, (uint8_t)RemoveBytes);
fifoC -= RemoveBytes;
}
}
}
}
if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
// We have 1 packet
if ((micros() - BreakTimer) > (11000)) return 0;
} while (fifoC != length);
getFIFOBytes(data, length); //Get 1 packet
return 1;
int8_t MPU6050::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
uint16_t fifoCounter = getFIFOCount();
if (fifoCounter < length) { // If FIFO counter is smaller than packet size - there's nothing to read yet
return 0;
} else if (fifoCounter > 192) { // If FIFO counter exceeds our buffer size - read the first packet to keep it smooth and reset FIFO
getFIFOBytes(data, length);
resetFIFO();
return 2;
} else if (fifoCounter > length) { // If FIFO counter exceeds a size of one packet - read full packets and get the latest packet
uint8_t fifoBuf[192] = {0};
uint8_t bytesCounter = 0;
// Count only full packets and read them into buffer
fifoCounter = length * (fifoCounter / length);
do {
uint8_t i2cBuf[BUFFER_LENGTH] = {0};
uint8_t readBytes = min(fifoCounter, (uint16_t)BUFFER_LENGTH);
getFIFOBytes(i2cBuf, readBytes);
for (uint8_t i = 0; i < readBytes; i++) {
fifoBuf[i + bytesCounter] = i2cBuf[i];
}
fifoCounter -= readBytes;
bytesCounter += readBytes;
} while (fifoCounter);
// Read the last packet
for (uint8_t i = 0, start = bytesCounter - length; i < length; i++) {
data[i] = fifoBuf[start + i];
}
return 1;
}
getFIFOBytes(data, length); // If FIFO counter is exactly the packet size - read the packet
return 1;
}