void mpu6000Decode(void) {
volatile uint8_t *d = mpu6000Data.rxBuf;
int32_t acc[3], temp, gyo[3];
float divisor;
int i;
for (i = 0; i < 3; i++) {
acc[i] = 0;
gyo[i] = 0;
}
temp = 0;
divisor = (float)MPU6000_SLOTS;
for (i = 0; i < MPU6000_SLOTS; i++) {
int j = i*MPU6000_SLOT_SIZE;
// check if we are in the middle of a transaction for this slot
if (i == mpu6000Data.slot && mpu6000Data.spiFlag == 0) {
divisor -= 1.0f;
}
else {
acc[0] += (int16_t)__rev16(*(uint16_t *)&d[j+1]);
acc[1] += (int16_t)__rev16(*(uint16_t *)&d[j+3]);
acc[2] += (int16_t)__rev16(*(uint16_t *)&d[j+5]);
temp += (int16_t)__rev16(*(uint16_t *)&d[j+7]);
gyo[0] += (int16_t)__rev16(*(uint16_t *)&d[j+9]);
gyo[1] += (int16_t)__rev16(*(uint16_t *)&d[j+11]);
gyo[2] += (int16_t)__rev16(*(uint16_t *)&d[j+13]);
}
}
divisor = 1.0f / divisor;
mpu6000Data.rawTemp = temp * divisor * (1.0f / 340.0f) + 36.53f;
mpu6000Data.temp = utilFilter(&mpu6000Data.tempFilter, mpu6000Data.rawTemp);
mpu6000ScaleAcc(acc, mpu6000Data.rawAcc, divisor);
mpu6000CalibAcc(mpu6000Data.rawAcc, mpu6000Data.acc);
mpu6000ScaleGyo(gyo, mpu6000Data.rawGyo, divisor);
mpu6000CalibGyo(mpu6000Data.rawGyo, mpu6000Data.gyo);
mpu6000Data.lastUpdate = timerMicros();
}
void mpu6000DrateDecode(void) {
volatile uint8_t *d = mpu6000Data.rxBuf;
int32_t gyo[3];
float divisor;
int s, i;
if (mpu6000Data.enabled) {
for (i = 0; i < 3; i++) {
gyo[i] = 0;
}
divisor = (float)MPU6000_DRATE_SLOTS;
s = mpu6000Data.slot - 1;
if (s < 0) {
s = MPU6000_SLOTS - 1;
}
for (i = 0; i < MPU6000_DRATE_SLOTS; i++) {
int j = s*MPU6000_SLOT_SIZE;
// check if we are in the middle of a transaction for this slot
if (s == mpu6000Data.slot && mpu6000Data.spiFlag == 0) {
divisor -= 1.0f;
} else {
gyo[0] += (int16_t)__rev16(*(uint16_t *)&d[j+9]);
gyo[1] += (int16_t)__rev16(*(uint16_t *)&d[j+11]);
gyo[2] += (int16_t)__rev16(*(uint16_t *)&d[j+13]);
}
if (--s < 0) {
s = MPU6000_SLOTS - 1;
}
}
divisor = 1.0f / divisor;
mpu6000ScaleGyo(gyo, mpu6000Data.dRateRawGyo, divisor);
mpu6000CalibGyo(mpu6000Data.dRateRawGyo, mpu6000Data.dRateGyo);
}
}