float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {
pt1FilterInit(filter, f_cut, dT);
}
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
{
filterApplyFnPtr *lowpassFilterApplyFn;
gyroLowpassFilter_t *lowpassFilter = NULL;
switch (slot) {
case FILTER_LOWPASS:
lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn;
lowpassFilter = gyroSensor->lowpassFilter;
break;
case FILTER_LOWPASS2:
lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
lowpassFilter = gyroSensor->lowpass2Filter;
break;
default:
return;
}
// Establish some common constants
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
const float gyroDt = gyro.targetLooptime * 1e-6f;
// Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
const float gain = pt1FilterGain(lpfHz, gyroDt);
// Dereference the pointer to null before checking valid cutoff and filter
// type. It will be overridden for positive cases.
*lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
switch (type) {
case FILTER_PT1:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
}
break;
case FILTER_BIQUAD:
#ifdef USE_DYN_LPF
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
#else
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
#endif
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
}
break;
}
}
}
void gyroInit(void)
{
if (gyroConfig()->gyro_soft_lpf_hz) { // Initialisation needs to happen once sampling rate is known
const uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
for (int axis = 0; axis < 3; axis++) {
biquadFilterInitNotch(&gyroFilterNotch[axis], gyroPeriodUs, gyroConfig()->gyro_soft_notch_hz, gyroConfig()->gyro_soft_notch_cutoff_hz);
if (gyroConfig()->gyro_soft_type == FILTER_BIQUAD) {
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig()->gyro_soft_lpf_hz, gyroPeriodUs);
} else {
const float gyroDt = (float)gyroPeriodUs * 0.000001f;
pt1FilterInit(&gyroFilterPt1[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt);
}
}
}
}