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);
}
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
{
gyroSensor->notchFilterDynApplyFn = nullFilterApply;
gyroSensor->notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
if(gyroConfig()->dyn_notch_width_percent != 0) {
gyroSensor->notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
}
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else if (gyroSoftLpfType == FILTER_PT1)
gyroDt = (float) gyro.targetLooptime * 0.000001f;
else
firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}
static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
{
gyroSensor->notchFilter2ApplyFn = nullFilterApply;
notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
if (notchHz != 0 && notchCutoffHz != 0) {
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
}
void filterServos(void)
{
int servoIdx;
if (mixerConfig->servo_lowpass_enable) {
// Initialize servo lowpass filter (servos are calculated at looptime rate)
if (!servoFilterIsSet) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, 0);
}
servoFilterIsSet = true;
}
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
// Apply servo lowpass filter and do sanity cheching
servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]);
}
}
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}
}
/* sets up a biquad Filter */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t sampleDeltaUs)
{
biquadFilterInit(filter, filterFreq, sampleDeltaUs, BIQUAD_Q, FILTER_LPF);
}
void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t sampleDeltaUs, uint16_t filterHz, uint16_t cutoffHz)
{
float Q = biquadFilterCalculateNotchQ(filterHz, cutoffHz);
biquadFilterInit(filter, filterHz, sampleDeltaUs, Q, FILTER_NOTCH);
}
