1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00

New Beebrain defaults // refactoring

This commit is contained in:
borisbstyle 2016-10-13 01:41:04 +02:00
parent 145481afb3
commit 0e827b331d
9 changed files with 33 additions and 30 deletions

View file

@ -140,7 +140,7 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo
firFilterInit2(filter, buf, bufLength, coeffs, bufLength); firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
} }
void firFilterUpdate(firFilter_t *filter, float input) void filterFirUpdate(firFilter_t *filter, float input)
{ {
filter->buf[filter->index++] = input; // index is at the first empty buffer positon filter->buf[filter->index++] = input; // index is at the first empty buffer positon
if (filter->index >= filter->bufLength) { if (filter->index >= filter->bufLength) {
@ -269,12 +269,12 @@ int16_t firFilterInt16Get(const firFilter_t *filter, int index)
return filter->buf[index]; return filter->buf[index];
} }
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); 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 */ /* 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->state[filter->index] = input;
filter->movingSum += filter->state[filter->index++]; filter->movingSum += filter->state[filter->index++];
if (filter->index == filter->targetCount) if (filter->index == filter->targetCount)
@ -287,3 +287,4 @@ float denoisingFilterUpdate(denoisingState_t *filter, float input) {
return filter->movingSum / ++filter->filledCount + 1; return filter->movingSum / ++filter->filledCount + 1;
} }

View file

@ -29,18 +29,18 @@ typedef struct biquadFilter_s {
float d1, d2; float d1, d2;
} biquadFilter_t; } biquadFilter_t;
typedef struct dennoisingState_s { typedef struct firFilterState_s {
int filledCount; int filledCount;
int targetCount; int targetCount;
int index; int index;
float movingSum; float movingSum;
float state[MAX_DENOISE_WINDOW_SIZE]; float state[MAX_DENOISE_WINDOW_SIZE];
} denoisingState_t; } firFilterState_t;
typedef enum { typedef enum {
FILTER_PT1 = 0, FILTER_PT1 = 0,
FILTER_BIQUAD, FILTER_BIQUAD,
FILTER_DENOISE FILTER_FIR
} filterType_e; } filterType_e;
typedef enum { typedef enum {
@ -77,7 +77,7 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
void firFilterUpdate(firFilter_t *filter, float input); void filterFirUpdate(firFilter_t *filter, float input);
void firFilterUpdateAverage(firFilter_t *filter, float input); void firFilterUpdateAverage(firFilter_t *filter, float input);
float firFilterApply(const firFilter_t *filter); float firFilterApply(const firFilter_t *filter);
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
@ -92,6 +92,6 @@ float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t c
float firFilterInt16CalcAverage(const firFilterInt16_t *filter); float firFilterInt16CalcAverage(const firFilterInt16_t *filter);
int16_t firFilterInt16LastInput(const firFilterInt16_t *filter); int16_t firFilterInt16LastInput(const firFilterInt16_t *filter);
int16_t firFilterInt16Get(const firFilter_t *filter, int index); int16_t firFilterInt16Get(const firFilter_t *filter, int index);
void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
float denoisingFilterUpdate(denoisingState_t *filter, float input); float firFilterUpdate(firFilterState_t *filter, float input);

View file

@ -103,7 +103,7 @@ biquadFilter_t dtermFilterLpf[3];
biquadFilter_t dtermFilterNotch[3]; biquadFilter_t dtermFilterNotch[3];
bool dtermNotchInitialised; bool dtermNotchInitialised;
bool dtermBiquadLpfInitialised; bool dtermBiquadLpfInitialised;
denoisingState_t dtermDenoisingState[3]; firFilterState_t dtermDenoisingState[3];
void initFilters(const pidProfile_t *pidProfile) { void initFilters(const pidProfile_t *pidProfile) {
int axis; int axis;
@ -120,8 +120,8 @@ void initFilters(const pidProfile_t *pidProfile) {
for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
} }
if (pidProfile->dterm_filter_type == FILTER_DENOISE) { if (pidProfile->dterm_filter_type == FILTER_FIR) {
for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); for (axis = 0; axis < 3; axis++) initFirFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
} }
lowpassFilterType = pidProfile->dterm_filter_type; lowpassFilterType = pidProfile->dterm_filter_type;
} }

View file

@ -60,7 +60,7 @@ extern pt1Filter_t yawFilter;
extern biquadFilter_t dtermFilterLpf[3]; extern biquadFilter_t dtermFilterLpf[3];
extern biquadFilter_t dtermFilterNotch[3]; extern biquadFilter_t dtermFilterNotch[3];
extern bool dtermNotchInitialised; extern bool dtermNotchInitialised;
extern denoisingState_t dtermDenoisingState[3]; extern firFilterState_t dtermDenoisingState[3];
void initFilters(const pidProfile_t *pidProfile); void initFilters(const pidProfile_t *pidProfile);
float getdT(void); float getdT(void);
@ -214,7 +214,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
else if (pidProfile->dterm_filter_type == FILTER_PT1) else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
else else
delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta); delta = firFilterUpdate(&dtermDenoisingState[axis], delta);
} }
DTerm = Kd[axis] * delta * tpaFactor; DTerm = Kd[axis] * delta * tpaFactor;

View file

@ -57,7 +57,7 @@ extern int32_t errorGyroI[3];
extern pt1Filter_t deltaFilter[3]; extern pt1Filter_t deltaFilter[3];
extern pt1Filter_t yawFilter; extern pt1Filter_t yawFilter;
extern biquadFilter_t dtermFilterLpf[3]; extern biquadFilter_t dtermFilterLpf[3];
extern denoisingState_t dtermDenoisingState[3]; extern firFilterState_t dtermDenoisingState[3];
void initFilters(const pidProfile_t *pidProfile); void initFilters(const pidProfile_t *pidProfile);
float getdT(void); float getdT(void);
@ -183,7 +183,7 @@ void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, c
else if (pidProfile->dterm_filter_type == FILTER_PT1) else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
else else
delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta); delta = firFilterUpdate(&dtermDenoisingState[axis], delta);
delta = lrintf(deltaf); delta = lrintf(deltaf);
} }

View file

@ -532,7 +532,7 @@ static const char * const lookupTableRcInterpolation[] = {
}; };
static const char * const lookupTableLowpassType[] = { static const char * const lookupTableLowpassType[] = {
"NORMAL", "HIGH", "DENOISE" "PT1", "BIQUAD", "FIR"
}; };
static const char * const lookupTableFailsafe[] = { static const char * const lookupTableFailsafe[] = {

View file

@ -49,7 +49,7 @@ static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[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 uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
static float gyroSoftNotchQ1, gyroSoftNotchQ2; static float gyroSoftNotchQ1, gyroSoftNotchQ2;
@ -83,7 +83,7 @@ void gyroInit(void)
else if (gyroSoftLpfType == FILTER_PT1) else if (gyroSoftLpfType == FILTER_PT1)
gyroDt = (float) gyro.targetLooptime * 0.000001f; gyroDt = (float) gyro.targetLooptime * 0.000001f;
else else
initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
} }
} }
@ -198,7 +198,7 @@ void gyroUpdate(void)
else if (gyroSoftLpfType == FILTER_PT1) else if (gyroSoftLpfType == FILTER_PT1)
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
else else
gyroADCf[axis] = denoisingFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH) if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]); debug[axis] = lrintf(gyroADCf[axis]);

View file

@ -53,11 +53,11 @@ void targetConfiguration(master_t *config)
for (int profileId = 0; profileId < 2; profileId++) { for (int profileId = 0; profileId < 2; profileId++) {
config->profile[profileId].pidProfile.P8[ROLL] = 55; config->profile[profileId].pidProfile.P8[ROLL] = 55;
config->profile[profileId].pidProfile.I8[ROLL] = 40; config->profile[profileId].pidProfile.I8[ROLL] = 50;
config->profile[profileId].pidProfile.D8[ROLL] = 20; config->profile[profileId].pidProfile.D8[ROLL] = 25;
config->profile[profileId].pidProfile.P8[PITCH] = 55; config->profile[profileId].pidProfile.P8[PITCH] = 65;
config->profile[profileId].pidProfile.I8[PITCH] = 40; config->profile[profileId].pidProfile.I8[PITCH] = 60;
config->profile[profileId].pidProfile.D8[PITCH] = 20; config->profile[profileId].pidProfile.D8[PITCH] = 28;
config->profile[profileId].pidProfile.P8[YAW] = 180; config->profile[profileId].pidProfile.P8[YAW] = 180;
config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.I8[YAW] = 45;
config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50;
@ -65,11 +65,11 @@ void targetConfiguration(master_t *config)
config->profile[profileId].pidProfile.levelSensitivity = 1.0f; config->profile[profileId].pidProfile.levelSensitivity = 1.0f;
for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) {
config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 120; config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110;
config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 120; config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110;
config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 99; config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80;
config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 99; config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80;
config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 99; config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80;
config->profile[profileId].pidProfile.setpointRelaxRatio = 100; config->profile[profileId].pidProfile.setpointRelaxRatio = 100;
} }

View file

@ -163,6 +163,8 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
// IO - assuming all IOs on 48pin package // IO - assuming all IOs on 48pin package
#define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTB 0xffff