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

some code reformatting

Browse source
This commit is contained in:
rav 2017-05-12 17:22:59 +02:00
parent c7f8e4d7a4
commit cd5307188e
3 changed files with 13 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -111,12 +111,11 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
}
// precompute the coefficients
a0 = 1.0f / a0;
filter->b0 = b0 * a0;
filter->b1 = b1 * a0;
filter->b2 = b2 * a0;
filter->a1 = a1 * a0;
filter->a2 = a2 * a0;
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
// zero initial samples
filter->x1 = filter->x2 = 0;

2
src/main/fc/settings.c
View file

@ -205,7 +205,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"ESC_SENSOR_RPM",
"ESC_SENSOR_TMP",
"ALTITUDE",
"FFT",
"FFT",
"FFT_TIME",
"FFT_FREQ"
};

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

@ -85,14 +85,14 @@ static biquadFilter_t fftFreqFilter[3];
static float hanningWindow[FFT_WINDOW_SIZE];
void initHanning() {
for (int i = 0; i < FFT_WINDOW_SIZE; ++i) {
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
}
}
void initGyroData() {
for (int axis = 0; axis < 3; ++axis) {
for (int i = 0; i < FFT_WINDOW_SIZE; ++i) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
gyroData[axis][i] = 0;
}
}
@ -117,7 +117,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
// recalculation of filters takes 4 calls per axis => each filter gets updated every 3 * 4 = 12 calls
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
float looptime = targetLooptimeUs * 4 * 3;
for (int axis = 0; axis < 3; ++axis) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftResult[axis].centerFreq = 200; // any init value
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
@ -142,7 +142,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
}
// if gyro sampling is > 1kHz, accumulate multiple samples
for (int axis = 0; axis < 3; ++axis) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftAcc[axis] += gyroDev->gyroADC[axis];
}
fftAccCount++;
@ -152,7 +152,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
fftAccCount = 0;
//calculate mean value of accumulated samples
for (int axis = 0; axis < 3; ++axis) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = fftAcc[axis] / fftSamplingScale;
sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample);
gyroData[axis][fftIdx] = sample;
@ -251,7 +251,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
fftResult[axis].maxVal = 0;
// iterate over fft data and calculate weighted indexes
float squaredData;
for (int i = 0; i < fftBinCount; ++i) {
for (int i = 0; i < fftBinCount; i++) {
squaredData = fftData[i] * fftData[i]; //more weight on higher peaks
fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData);
fftSum += squaredData;