diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 47b342df7..b7f05a615 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -51,33 +51,53 @@ void setBiQuadCoefficients(int type, biquad_t *state) { /* set coefficients */ switch(type) { case(GYRO_FILTER): - if (targetLooptime == 500) { - state->a0= 0.007820199; - state->a1= 0.015640399; - state->a2= 0.007820199; - state->a3= -1.73472382; - state->a4= 0.766004619; - } else { - state->a0= 0.027859711; - state->a1= 0.055719422; - state->a2= 0.027859711; - state->a3= -1.47547752; - state->a4= 0.586916365; + switch (targetLooptime) { + case(SAMPLE_RATE_2KHZ): + state->a0= 0.007820199; + state->a1= 0.015640399; + state->a2= 0.007820199; + state->a3= -1.73472382; + state->a4= 0.766004619; + break; + case(SAMPLE_RATE_2K6HZ): + state->a0= 0.004538377; + state->a1= 0.009076754; + state->a2= 0.004538377; + state->a3= -1.80059391; + state->a4= 0.818747415; + break; + default: + case(SAMPLE_RATE_1KHZ): + state->a0= 0.027859711; + state->a1= 0.055719422; + state->a2= 0.027859711; + state->a3= -1.47547752; + state->a4= 0.586916365; } break; case(DELTA_FILTER): - if (targetLooptime == 500) { - state->a0= 0.003621679; - state->a1= 0.007243357; - state->a2= 0.003621679; - state->a3= -1.82269350; - state->a4= 0.837180216; - } else { - state->a0= 0.013359181; - state->a1= 0.026718362; - state->a2= 0.013359181; - state->a3= -1.64745762; - state->a4= 0.700894342; + switch (targetLooptime) { + case(SAMPLE_RATE_2KHZ): + state->a0= 0.003621679; + state->a1= 0.007243357; + state->a2= 0.003621679; + state->a3= -1.82269350; + state->a4= 0.837180216; + break; + case(SAMPLE_RATE_2K6HZ): + state->a0= 0.002081573; + state->a1= 0.004163147; + state->a2= 0.002081573; + state->a3= -1.86685796; + state->a4= 0.875184256; + break; + default: + case(SAMPLE_RATE_1KHZ): + state->a0= 0.013359181; + state->a1= 0.026718362; + state->a2= 0.013359181; + state->a3= -1.64745762; + state->a4= 0.700894342; } } } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index ef5d5c97b..7178df0de 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -35,6 +35,13 @@ typedef enum { DELTA_FILTER } filterType_e; +typedef enum { + SAMPLE_RATE_1KHZ = 1000, + SAMPLE_RATE_2KHZ = 500, + SAMPLE_RATE_2K6HZ = 375, + SAMPLE_RATE_4KHZ = 250 +} sampleRates_e; + float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt); float applyBiQuadFilter(float sample, biquad_t * b); void setBiQuadCoefficients(int type, biquad_t *state); diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 4b1536d3f..98a94a4ed 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -47,7 +47,11 @@ void gyroUpdateSampleRate(uint8_t lpf) { if (!lpf) { gyroSamplePeriod = 125; #ifdef STM32F303xC +#ifdef COLIBRI_RACE + gyroSyncDenominator = 3; // Sample every 3d gyro measurement 2,6khz +#else gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz +#endif #else if (!sensors(SENSOR_ACC) && !sensors(SENSOR_BARO) && !sensors(SENSOR_MAG)) { gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz