Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 06:45:16 +03:00
Code Issues Wiki Activity

Default adjustments // Cleanups

Browse source
This commit is contained in:
borisbstyle 2016-10-07 02:24:22 +02:00
parent 9d4c240671
commit fc480fab5b
5 changed files with 36 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/main/common/filter.c
View file

@ -115,25 +115,15 @@ float biquadFilterApply(biquadFilter_t *filter, float input)
return result; return result;
} }
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) { /* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */
int count; float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]) {
int32_t averageSum = 0; int index;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1];
averageState[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count];
return averageSum / averageCount;
}
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) {
int count;
float averageSum = 0.0f; float averageSum = 0.0f;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; for (index = count-1; index > 0; index--) filter[index] = filter[index-1];
averageState[0] = input; filter[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count]; for (count = 0; count < count; index++) averageSum += filter[index];
return averageSum / averageCount; return averageSum / count;
} }

6
src/main/common/filter.h
View file

@ -15,7 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
#define DELTA_MAX_SAMPLES 12 #define MAX_DENOISE_WINDOW_SIZE 40
typedef struct pt1Filter_s { typedef struct pt1Filter_s {
float state; float state;
@ -32,6 +32,7 @@ typedef struct biquadFilter_s {
typedef enum { typedef enum {
FILTER_PT1 = 0, FILTER_PT1 = 0,
FILTER_BIQUAD, FILTER_BIQUAD,
FILTER_DENOISE
} filterType_e; } filterType_e;
typedef enum { typedef enum {
@ -48,6 +49,5 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]);
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);

4
src/main/config/config.c
View file

@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
// Betaflight PID controller parameters // Betaflight PID controller parameters
pidProfile->setpointRelaxRatio = 70; pidProfile->setpointRelaxRatio = 85;
pidProfile->dtermSetpointWeight = 200; pidProfile->dtermSetpointWeight = 200;
pidProfile->yawRateAccelLimit = 220; pidProfile->yawRateAccelLimit = 220;
pidProfile->rateAccelLimit = 0; pidProfile->rateAccelLimit = 0;
@ -487,7 +487,7 @@ void createDefaultConfig(master_t *config)
config->pid_process_denom = 2; config->pid_process_denom = 2;
#endif #endif
config->gyro_soft_type = FILTER_PT1; config->gyro_soft_type = FILTER_PT1;
config->gyro_soft_lpf_hz = 80; config->gyro_soft_lpf_hz = 90;
config->gyro_soft_notch_hz_1 = 400; config->gyro_soft_notch_hz_1 = 400;
config->gyro_soft_notch_cutoff_1 = 300; config->gyro_soft_notch_cutoff_1 = 300;
config->gyro_soft_notch_hz_2 = 0; config->gyro_soft_notch_hz_2 = 0;

22
src/main/flight/pid.c
View file

@ -108,7 +108,8 @@ static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter; static pt1Filter_t yawFilter;
static biquadFilter_t dtermFilterLpf[3]; static biquadFilter_t dtermFilterLpf[3];
static biquadFilter_t dtermFilterNotch[3]; static biquadFilter_t dtermFilterNotch[3];
static bool dtermNotchInitialised, dtermBiquadLpfInitialised; static float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
static bool dtermNotchInitialised, dtermLpfInitialised;
void initFilters(const pidProfile_t *pidProfile) { void initFilters(const pidProfile_t *pidProfile) {
int axis; int axis;
@ -120,9 +121,9 @@ void initFilters(const pidProfile_t *pidProfile) {
} }
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { if (pidProfile->dterm_lpf_hz && !dtermLpfInitialised) {
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);
dtermBiquadLpfInitialised = true; dtermLpfInitialised = true;
} }
} }
} }
@ -271,11 +272,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
if (pidProfile->dterm_lpf_hz) { if (pidProfile->dterm_lpf_hz) {
if (dtermBiquadLpfInitialised) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
delta = biquadFilterApply(&dtermFilterLpf[axis], delta); delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else { 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
delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]);
} }
DTerm = Kd[axis] * delta * tpaFactor; DTerm = Kd[axis] * delta * tpaFactor;
@ -410,11 +412,13 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
// Filter delta // Filter delta
if (pidProfile->dterm_lpf_hz) { if (pidProfile->dterm_lpf_hz) {
float deltaf = delta; // single conversion float deltaf = delta; // single conversion
if (dtermBiquadLpfInitialised) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
delta = biquadFilterApply(&dtermFilterLpf[axis], delta); delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else { 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
delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]);
delta = lrintf(deltaf); delta = lrintf(deltaf);
} }

14
src/main/sensors/gyro.c
View file

@ -48,6 +48,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 float gyroFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
static uint8_t gyroSoftLpfType; static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2; static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2;
static float gyroSoftNotchQ_1, gyroSoftNotchQ_2; static float gyroSoftNotchQ_1, gyroSoftNotchQ_2;
@ -76,14 +77,19 @@ void gyroInit(void)
{ {
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH);
if (gyroSoftLpfType == FILTER_BIQUAD) if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else else
gyroDt = (float) gyro.targetLooptime * 0.000001f; gyroDt = (float) gyro.targetLooptime * 0.000001f;
} }
} }
if ((gyroSoftNotchHz_1 || gyroSoftNotchHz_2) && gyro.targetLooptime) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH);
}
}
} }
bool isGyroCalibrationComplete(void) bool isGyroCalibrationComplete(void)
@ -186,8 +192,10 @@ void gyroUpdate(void)
if (gyroSoftLpfType == FILTER_BIQUAD) if (gyroSoftLpfType == FILTER_BIQUAD)
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
else else if (gyroSoftLpfType == FILTER_BIQUAD)
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
else
gyroADCf[axis] = denoisingFilterUpdate((float) gyroADC[axis], 3, gyroFilterDenoise[axis]);
if (debugMode == DEBUG_NOTCH) if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]); debug[axis] = lrintf(gyroADCf[axis]);