示例#1
0
void updateAccelerationReadings(void)
{
    if (!acc.read(accADCRaw)) {
        return;
    }

    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];

    if (accLpfCutHz) {
        if (!accFilterInitialised) {
            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
                for (int axis = 0; axis < 3; axis++) {
                    biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
                }

                accFilterInitialised = true;
            }
        }

        if (accFilterInitialised) {
            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
            }
        }
    }

    if (!isAccelerationCalibrationComplete()) {
        performAcclerationCalibration();
    }

    applyAccelerationZero(accZero, accGain);

    alignSensors(accADC, accADC, accAlign);
}
示例#2
0
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
    acc.read(accADC);
    alignSensors(accADC, accADC, accAlign);

    if (!isAccelerationCalibrationComplete()) {
        performAcclerationCalibration(rollAndPitchTrims);
    }

    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
        performInflightAccelerationCalibration(rollAndPitchTrims);
    }

    applyAccelerationTrims(accelerationTrims);
}
示例#3
0
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
    int16_t accADCRaw[XYZ_AXIS_COUNT];

    if (!acc.read(accADCRaw)) {
        return;
    }

    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
        if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
        accSmooth[axis] = accADCRaw[axis];
    }

    if (accLpfCutHz) {
        if (!accFilterInitialised) {
            if (accTargetLooptime) {  /* Initialisation needs to happen once sample rate is known */
                for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                    biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
                }
                accFilterInitialised = true;
            }
        }

        if (accFilterInitialised) {
            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
            }
        }
    }

    alignSensors(accSmooth, accSmooth, accAlign);

    if (!isAccelerationCalibrationComplete()) {
        performAcclerationCalibration(rollAndPitchTrims);
    }

    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
        performInflightAccelerationCalibration(rollAndPitchTrims);
    }

    applyAccelerationTrims(accelerationTrims);
}