rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add 2.6KHZ Target options (For now only COLIBRI_RACE)

This commit is contained in:
borisbstyle 2016-01-12 17:41:44 +01:00
parent 77d9d1d2b9
commit 29a936e773
3 changed files with 55 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/main/common/filter.c
View File

@ -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;
}
}
}

7
src/main/common/filter.h
View File

@ -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);

4
src/main/drivers/gyro_sync.c
View File

@ -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