void max21100Decode(void) {
volatile uint8_t *d = max21100Data.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)MAX21100_SLOTS;
for (i = 0; i < MAX21100_SLOTS; i++) {
int j = i*MAX21100_SLOT_SIZE;
// check if we are in the middle of a transaction for this slot
if (i == max21100Data.slot && max21100Data.spiFlag == 0) {
divisor -= 1.0f;
}
else {
gyo[0] += (int16_t)__rev16(*(uint16_t *)&d[j+1]);
gyo[1] += (int16_t)__rev16(*(uint16_t *)&d[j+3]);
gyo[2] += (int16_t)__rev16(*(uint16_t *)&d[j+5]);
acc[0] += (int16_t)__rev16(*(uint16_t *)&d[j+7]);
acc[1] += (int16_t)__rev16(*(uint16_t *)&d[j+9]);
acc[2] += (int16_t)__rev16(*(uint16_t *)&d[j+11]);
temp += (int16_t)__rev16(*(uint16_t *)&d[j+19]);
}
}
divisor = 1.0f / divisor;
max21100Data.rawTemp = temp * divisor * (1.0f / 255.0f);
max21100Data.temp = utilFilter(&max21100Data.tempFilter, max21100Data.rawTemp);
max21100ScaleAcc(acc, max21100Data.rawAcc, divisor);
max21100CalibAcc(max21100Data.rawAcc, max21100Data.acc);
max21100ScaleGyo(gyo, max21100Data.rawGyo, divisor);
max21100CalibGyo(max21100Data.rawGyo, max21100Data.gyo);
max21100Data.lastUpdate = timerMicros();
}
float utilFilter3(utilFilter_t *f, float signal) {
return utilFilter(&f[0], utilFilter(&f[1], utilFilter(&f[2], signal)));
}
// calculate radio reception quality
static void radioReceptionQuality(radioInstance_t *r, int8_t q) {
r->quality = utilFilter(&r->qualityFilter, (float)(q + 1)) * 0.5f * 100.0f;
}
