Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +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;
}
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) {
int count;
int32_t averageSum = 0;
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;
/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */
float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]) {
int index;
float averageSum = 0.0f;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1];
averageState[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count];
for (index = count-1; index > 0; index--) filter[index] = filter[index-1];
filter[0] = input;
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/>.
*/
#define DELTA_MAX_SAMPLES 12
#define MAX_DENOISE_WINDOW_SIZE 40
typedef struct pt1Filter_s {
float state;
@ -32,6 +32,7 @@ typedef struct biquadFilter_s {
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_DENOISE
} filterType_e;
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 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 filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]);

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

@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
// Betaflight PID controller parameters
pidProfile->setpointRelaxRatio = 70;
pidProfile->setpointRelaxRatio = 85;
pidProfile->dtermSetpointWeight = 200;
pidProfile->yawRateAccelLimit = 220;
pidProfile->rateAccelLimit = 0;
@ -487,7 +487,7 @@ void createDefaultConfig(master_t *config)
config->pid_process_denom = 2;
#endif
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_cutoff_1 = 300;
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 biquadFilter_t dtermFilterLpf[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) {
int axis;
@ -120,9 +121,9 @@ void initFilters(const pidProfile_t *pidProfile) {
}
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);
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 (pidProfile->dterm_lpf_hz) {
if (dtermBiquadLpfInitialised) {
if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else {
else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
}
else
delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]);
}
DTerm = Kd[axis] * delta * tpaFactor;
@ -410,11 +412,13 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
// Filter delta
if (pidProfile->dterm_lpf_hz) {
float deltaf = delta; // single conversion
if (dtermBiquadLpfInitialised) {
if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else {
else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
}
else
delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]);
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 gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[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 uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_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
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)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
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)
@ -186,8 +192,10 @@ void gyroUpdate(void)
if (gyroSoftLpfType == FILTER_BIQUAD)
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
else
else if (gyroSoftLpfType == FILTER_BIQUAD)
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
else
gyroADCf[axis] = denoisingFilterUpdate((float) gyroADC[axis], 3, gyroFilterDenoise[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);