Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +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 // precompute the coefficients
a0 = 1.0f / a0; filter->b0 = b0 / a0;
filter->b0 = b0 * a0; filter->b1 = b1 / a0;
filter->b1 = b1 * a0; filter->b2 = b2 / a0;
filter->b2 = b2 * a0; filter->a1 = a1 / a0;
filter->a1 = a1 * a0; filter->a2 = a2 / a0;
filter->a2 = a2 * a0;
// zero initial samples // zero initial samples
filter->x1 = filter->x2 = 0; filter->x1 = filter->x2 = 0;

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

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