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

Cleanup // Fix some filter inits // More space Naze target

Browse source
This commit is contained in:
borisbstyle 2016-10-09 00:45:02 +02:00
parent 81623d4ac7
commit 70cca63875
6 changed files with 50 additions and 47 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -243,13 +243,15 @@ void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16
/* 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 denoisingFilterUpdate(denoisingState_t *filter, float input) {
int index; filter->state[filter->index] = input;
float averageSum = 0.0f; filter->movingSum += filter->state[filter->index++];
if (filter->index == filter->targetCount)
filter->index = 0;
filter->movingSum -= filter->state[filter->index];
for (index = filter->targetCount-1; index > 0; index--) filter->state[index] = filter->state[index-1]; if (filter->targetCount >= filter->filledCount)
filter->state[0] = input; return filter->movingSum / filter->targetCount;
for (int count = 0; count < filter->targetCount; index++) averageSum += filter->state[index]; else
return filter->movingSum / ++filter->filledCount + 1;
return averageSum / filter->targetCount;
} }

5
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 MAX_DENOISE_WINDOW_SIZE 6 #define MAX_DENOISE_WINDOW_SIZE 120
typedef struct pt1Filter_s { typedef struct pt1Filter_s {
float state; float state;
@ -30,7 +30,10 @@ typedef struct biquadFilter_s {
} biquadFilter_t; } biquadFilter_t;
typedef struct dennoisingState_s { typedef struct dennoisingState_s {
int filledCount;
int targetCount; int targetCount;
int index;
float movingSum;
float state[MAX_DENOISE_WINDOW_SIZE]; float state[MAX_DENOISE_WINDOW_SIZE];
} denoisingState_t; } denoisingState_t;

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

@ -97,17 +97,16 @@ float getdT(void)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
pt1Filter_t deltaFilter[3]; static pt1Filter_t deltaFilter[3];
pt1Filter_t yawFilter; static pt1Filter_t yawFilter;
biquadFilter_t dtermFilterLpf[3]; static biquadFilter_t dtermFilterLpf[3];
biquadFilter_t dtermFilterNotch[3]; static biquadFilter_t dtermFilterNotch[3];
bool dtermNotchInitialised; static denoisingState_t dtermDenoisingState[3];
bool dtermBiquadLpfInitialised; static bool dtermNotchInitialised;
denoisingState_t dtermDenoisingState[3];
bool dtermNotchInitialised, dtermLpfInitialised;
void initFilters(const pidProfile_t *pidProfile) { void initFilters(const pidProfile_t *pidProfile) {
int axis; int axis;
static uint8_t lowpassFilterType;
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
@ -115,18 +114,15 @@ void initFilters(const pidProfile_t *pidProfile) {
dtermNotchInitialised = true; dtermNotchInitialised = true;
} }
if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) {
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
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);
dtermLpfInitialised = true;
}
} }
if (pidProfile->dterm_filter_type == FILTER_DENOISE) { if (pidProfile->dterm_filter_type == FILTER_DENOISE) {
if (pidProfile->dterm_lpf_hz && !dtermLpfInitialised) {
for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
dtermLpfInitialised = true;
} }
lowpassFilterType = pidProfile->dterm_filter_type;
} }
} }

10
src/main/io/serial_cli.c
View file

@ -793,12 +793,12 @@ const clivalue_t valueTable[] = {
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_level", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch_hz_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
{ "gyro_notch_cutoff_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
{ "gyro_notch_hz_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
{ "gyro_notch_cutoff_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },

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

@ -51,8 +51,8 @@ static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_A
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT]; static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType; static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2; static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
static float gyroSoftNotchQ_1, gyroSoftNotchQ_2; static float gyroSoftNotchQ1, gyroSoftNotchQ2;
static uint8_t gyroSoftLpfHz; static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0; static uint16_t calibratingG = 0;
static float gyroDt; static float gyroDt;
@ -67,11 +67,11 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
{ {
gyroConfig = gyroConfigToUse; gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz_1 = gyro_soft_notch_hz_1; gyroSoftNotchHz1 = gyro_soft_notch_hz_1;
gyroSoftNotchHz_2 = gyro_soft_notch_hz_2; gyroSoftNotchHz2 = gyro_soft_notch_hz_2;
gyroSoftLpfType = gyro_soft_lpf_type; gyroSoftLpfType = gyro_soft_lpf_type;
gyroSoftNotchQ_1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
gyroSoftNotchQ_2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
} }
void gyroInit(void) void gyroInit(void)
@ -80,15 +80,17 @@ void gyroInit(void)
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
if (gyroSoftLpfType == FILTER_BIQUAD) if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else else if (gyroSoftLpfType == FILTER_PT1)
gyroDt = (float) gyro.targetLooptime * 0.000001f; gyroDt = (float) gyro.targetLooptime * 0.000001f;
else
initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
} }
} }
if ((gyroSoftNotchHz_1 || gyroSoftNotchHz_2) && gyro.targetLooptime) { if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
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_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH); biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
} }
} }
} }
@ -201,10 +203,10 @@ void gyroUpdate(void)
if (debugMode == DEBUG_NOTCH) if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]); debug[axis] = lrintf(gyroADCf[axis]);
if (gyroSoftNotchHz_1) if (gyroSoftNotchHz1)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
if (gyroSoftNotchHz_2) if (gyroSoftNotchHz2)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]);

14
src/main/target/NAZE/target.h
View file

@ -94,14 +94,14 @@
#define ACC_BMA280_ALIGN CW0_DEG #define ACC_BMA280_ALIGN CW0_DEG
#define ACC_MPU6500_ALIGN CW0_DEG #define ACC_MPU6500_ALIGN CW0_DEG
#define BARO //#define BARO
#define USE_BARO_MS5611 //#define USE_BARO_MS5611
#define USE_BARO_BMP085 //#define USE_BARO_BMP085
#define USE_BARO_BMP280 //#define USE_BARO_BMP280
#define MAG //#define MAG
#define USE_MAG_HMC5883 //#define USE_MAG_HMC5883
#define MAG_HMC5883_ALIGN CW180_DEG //#define MAG_HMC5883_ALIGN CW180_DEG
//#define SONAR //#define SONAR
//#define SONAR_TRIGGER_PIN PB0 //#define SONAR_TRIGGER_PIN PB0