static void updateSensors(void)
{
uint8_t i;
float temp[3];
if(sensorData.accelSamples) {
for(i = 0; i < 3; ++i)
temp[i] = stateData.accel[i];
stateData.accel[X] = ((float)sensorData.accelAccum[X] / sensorData.accelSamples) * sensorParams.accelScaleFactor;
stateData.accel[Y] = ((float)sensorData.accelAccum[Y] / sensorData.accelSamples) * sensorParams.accelScaleFactor;
stateData.accel[Z] = ((float)sensorData.accelAccum[Z] / sensorData.accelSamples) * sensorParams.accelScaleFactor;
if(cfg.accelSmoothFactor < 1.0f)
for(i = 0; i < 3; ++i)
stateData.accel[i] = filterSmooth(stateData.accel[i], temp[i], cfg.accelSmoothFactor);
if(cfg.accelLPF)
for(i = 0; i < 3; ++i)
stateData.accel[i] = fourthOrderFilter(stateData.accel[i], &accelFilter[i], accelLPF_A, accelLPF_B);
}
if(sensorData.gyroSamples) {
stateData.gyro[X] = ((float)sensorData.gyroAccum[X] / sensorData.gyroSamples - sensorParams.gyroTCBias[X]) * sensorParams.gyroScaleFactor;
stateData.gyro[Y] = ((float)sensorData.gyroAccum[Y] / sensorData.gyroSamples - sensorParams.gyroTCBias[Y]) * sensorParams.gyroScaleFactor;
stateData.gyro[Z] = (float)(sensorData.gyroAccum[Z] / sensorData.gyroSamples - sensorParams.gyroTCBias[Z]) * sensorParams.gyroScaleFactor;
}
if(sensorData.magSamples) {
stateData.mag[X] = ((float)sensorData.magAccum[X] / sensorData.magSamples) * sensorParams.magScaleFactor;
stateData.mag[Y] = ((float)sensorData.magAccum[Y] / sensorData.magSamples) * sensorParams.magScaleFactor;
stateData.mag[Z] = ((float)sensorData.magAccum[Z] / sensorData.magSamples) * sensorParams.magScaleFactor;
}
}
// Updates all raw measurements from the accelerometer
void Accel::updateAll(){
sendReadRequest(0x32);
requestBytes(6);
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
dataRaw[axis] = zero[axis] - readNextWordFlip();
dataSmoothed[axis] = filterSmooth(gConstant[axis] * dataRaw[axis] + gB[axis], dataSmoothed[axis], _smoothFactor);
}
}
