mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
New Beebrain defaults // refactoring
This commit is contained in:
parent
37c10e8a10
commit
6547741cfe
7 changed files with 28 additions and 26 deletions
|
@ -115,12 +115,12 @@ float biquadFilterApply(biquadFilter_t *filter, float input)
|
|||
return result;
|
||||
}
|
||||
|
||||
void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) {
|
||||
void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) {
|
||||
filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_DENOISE_WINDOW_SIZE);
|
||||
}
|
||||
|
||||
/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */
|
||||
float denoisingFilterUpdate(denoisingState_t *filter, float input) {
|
||||
float firFilterUpdate(firFilterState_t *filter, float input) {
|
||||
filter->state[filter->index] = input;
|
||||
filter->movingSum += filter->state[filter->index++];
|
||||
if (filter->index == filter->targetCount)
|
||||
|
|
|
@ -29,18 +29,18 @@ typedef struct biquadFilter_s {
|
|||
float d1, d2;
|
||||
} biquadFilter_t;
|
||||
|
||||
typedef struct dennoisingState_s {
|
||||
typedef struct firFilterState_s {
|
||||
int filledCount;
|
||||
int targetCount;
|
||||
int index;
|
||||
float movingSum;
|
||||
float state[MAX_DENOISE_WINDOW_SIZE];
|
||||
} denoisingState_t;
|
||||
} firFilterState_t;
|
||||
|
||||
typedef enum {
|
||||
FILTER_PT1 = 0,
|
||||
FILTER_BIQUAD,
|
||||
FILTER_DENOISE
|
||||
FILTER_FIR
|
||||
} filterType_e;
|
||||
|
||||
typedef enum {
|
||||
|
@ -56,6 +56,6 @@ float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
|
|||
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
|
||||
float pt1FilterApply(pt1Filter_t *filter, float input);
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
|
||||
void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
|
||||
float denoisingFilterUpdate(denoisingState_t *filter, float input);
|
||||
void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
|
||||
float firFilterUpdate(firFilterState_t *filter, float input);
|
||||
|
||||
|
|
|
@ -108,7 +108,7 @@ static pt1Filter_t deltaFilter[3];
|
|||
static pt1Filter_t yawFilter;
|
||||
static biquadFilter_t dtermFilterLpf[3];
|
||||
static biquadFilter_t dtermFilterNotch[3];
|
||||
static denoisingState_t dtermDenoisingState[3];
|
||||
static firFilterState_t dtermDenoisingState[3];
|
||||
static bool dtermNotchInitialised;
|
||||
|
||||
void initFilters(const pidProfile_t *pidProfile) {
|
||||
|
@ -126,8 +126,8 @@ void initFilters(const pidProfile_t *pidProfile) {
|
|||
for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
}
|
||||
|
||||
if (pidProfile->dterm_filter_type == FILTER_DENOISE) {
|
||||
for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
if (pidProfile->dterm_filter_type == FILTER_FIR) {
|
||||
for (axis = 0; axis < 3; axis++) initFirFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
}
|
||||
lowpassFilterType = pidProfile->dterm_filter_type;
|
||||
}
|
||||
|
@ -282,7 +282,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
|
|||
else if (pidProfile->dterm_filter_type == FILTER_PT1)
|
||||
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
|
||||
else
|
||||
delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta);
|
||||
delta = firFilterUpdate(&dtermDenoisingState[axis], delta);
|
||||
}
|
||||
|
||||
DTerm = Kd[axis] * delta * tpaFactor;
|
||||
|
@ -422,7 +422,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
|
|||
else if (pidProfile->dterm_filter_type == FILTER_PT1)
|
||||
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
|
||||
else
|
||||
delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta);
|
||||
delta = firFilterUpdate(&dtermDenoisingState[axis], delta);
|
||||
|
||||
delta = lrintf(deltaf);
|
||||
}
|
||||
|
|
|
@ -515,7 +515,7 @@ static const char * const lookupTableRcInterpolation[] = {
|
|||
};
|
||||
|
||||
static const char * const lookupTableLowpassType[] = {
|
||||
"NORMAL", "HIGH", "DENOISE"
|
||||
"PT1", "BIQUAD", "FIR"
|
||||
};
|
||||
|
||||
static const char * const lookupTableFailsafe[] = {
|
||||
|
|
|
@ -48,7 +48,7 @@ static const gyroConfig_t *gyroConfig;
|
|||
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
|
||||
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
|
||||
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
|
||||
static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT];
|
||||
static firFilterState_t gyroDenoiseState[XYZ_AXIS_COUNT];
|
||||
static uint8_t gyroSoftLpfType;
|
||||
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
|
||||
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
|
||||
|
@ -82,7 +82,7 @@ void gyroInit(void)
|
|||
else if (gyroSoftLpfType == FILTER_PT1)
|
||||
gyroDt = (float) gyro.targetLooptime * 0.000001f;
|
||||
else
|
||||
initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
|
||||
initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -197,7 +197,7 @@ void gyroUpdate(void)
|
|||
else if (gyroSoftLpfType == FILTER_PT1)
|
||||
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
|
||||
else
|
||||
gyroADCf[axis] = denoisingFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
|
||||
gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
|
||||
|
||||
if (debugMode == DEBUG_NOTCH)
|
||||
debug[axis] = lrintf(gyroADCf[axis]);
|
||||
|
|
|
@ -90,11 +90,11 @@ void targetConfiguration(master_t *config)
|
|||
|
||||
for (int profileId = 0; profileId < 2; profileId++) {
|
||||
config->profile[profileId].pidProfile.P8[ROLL] = 55;
|
||||
config->profile[profileId].pidProfile.I8[ROLL] = 40;
|
||||
config->profile[profileId].pidProfile.D8[ROLL] = 20;
|
||||
config->profile[profileId].pidProfile.P8[PITCH] = 55;
|
||||
config->profile[profileId].pidProfile.I8[PITCH] = 40;
|
||||
config->profile[profileId].pidProfile.D8[PITCH] = 20;
|
||||
config->profile[profileId].pidProfile.I8[ROLL] = 50;
|
||||
config->profile[profileId].pidProfile.D8[ROLL] = 25;
|
||||
config->profile[profileId].pidProfile.P8[PITCH] = 65;
|
||||
config->profile[profileId].pidProfile.I8[PITCH] = 60;
|
||||
config->profile[profileId].pidProfile.D8[PITCH] = 28;
|
||||
config->profile[profileId].pidProfile.P8[YAW] = 180;
|
||||
config->profile[profileId].pidProfile.I8[YAW] = 45;
|
||||
config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50;
|
||||
|
@ -102,11 +102,11 @@ void targetConfiguration(master_t *config)
|
|||
config->profile[profileId].pidProfile.levelSensitivity = 1.0f;
|
||||
|
||||
for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) {
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 120;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 120;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 99;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 99;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 99;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80;
|
||||
config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80;
|
||||
|
||||
config->profile[profileId].pidProfile.setpointRelaxRatio = 100;
|
||||
}
|
||||
|
|
|
@ -164,6 +164,8 @@
|
|||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||
|
||||
// IO - assuming all IOs on 48pin package
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue