Example #1
0
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;
    }
}
Example #2
0
// 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);
  }
}