rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Use full machine word size to prevent range checks

This commit is contained in:
KarateBrot 2021-06-11 20:40:32 +02:00
parent 1ae31fd3d5
commit 009ce31de1
4 changed files with 67 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/main/common/sdft.c
View File

@ -35,13 +35,13 @@ static FAST_DATA_ZERO_INIT complex_t twiddle[SDFT_BIN_COUNT];
static void applySqrt(const sdft_t *sdft, float *data); static void applySqrt(const sdft_t *sdft, float *data);
void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches) void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches)
{ {
if (!isInitialized) { if (!isInitialized) {
rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE); rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE);
const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE; const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
float phi = 0.0f; float phi = 0.0f;
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { for (int i = 0; i < SDFT_BIN_COUNT; i++) {
phi = c * i; phi = c * i;
twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi)); twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi));
} }
@ -54,47 +54,47 @@ void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const
sdft->numBatches = numBatches; sdft->numBatches = numBatches;
sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1; sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1;
for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) { for (int i = 0; i < SDFT_SAMPLE_SIZE; i++) {
sdft->samples[i] = 0.0f; sdft->samples[i] = 0.0f;
} }
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { for (int i = 0; i < SDFT_BIN_COUNT; i++) {
sdft->data[i] = 0.0f; sdft->data[i] = 0.0f;
} }
} }
// Add new sample to frequency spectrum // Add new sample to frequency spectrum
FAST_CODE void sdftPush(sdft_t *sdft, const float *sample) FAST_CODE void sdftPush(sdft_t *sdft, const float sample)
{ {
const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; const float delta = sample - rPowerN * sdft->samples[sdft->idx];
sdft->samples[sdft->idx] = *sample; sdft->samples[sdft->idx] = sample;
sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { for (int i = sdft->startBin; i <= sdft->endBin; i++) {
sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
} }
} }
// Add new sample to frequency spectrum in parts // Add new sample to frequency spectrum in parts
FAST_CODE void sdftPushBatch(sdft_t* sdft, const float *sample, const uint8_t *batchIdx) FAST_CODE void sdftPushBatch(sdft_t* sdft, const float sample, const int batchIdx)
{ {
const uint8_t batchStart = sdft->batchSize * *batchIdx; const int batchStart = sdft->batchSize * batchIdx;
uint8_t batchEnd = batchStart; int batchEnd = batchStart;
const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; const float delta = sample - rPowerN * sdft->samples[sdft->idx];
if (*batchIdx == sdft->numBatches - 1) { if (batchIdx == sdft->numBatches - 1) {
sdft->samples[sdft->idx] = *sample; sdft->samples[sdft->idx] = sample;
sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
batchEnd += sdft->endBin - batchStart + 1; batchEnd += sdft->endBin - batchStart + 1;
} else { } else {
batchEnd += sdft->batchSize; batchEnd += sdft->batchSize;
} }
for (uint8_t i = batchStart; i < batchEnd; i++) { for (int i = batchStart; i < batchEnd; i++) {
sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
} }
} }
@ -106,7 +106,7 @@ FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
float re; float re;
float im; float im;
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { for (int i = sdft->startBin; i <= sdft->endBin; i++) {
re = crealf(sdft->data[i]); re = crealf(sdft->data[i]);
im = cimagf(sdft->data[i]); im = cimagf(sdft->data[i]);
output[i] = re * re + im * im; output[i] = re * re + im * im;
@ -130,7 +130,7 @@ FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
float re; float re;
float im; float im;
for (uint8_t i = (sdft->startBin + 1); i < sdft->endBin; i++) { for (int i = (sdft->startBin + 1); i < sdft->endBin; i++) {
val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication
re = crealf(val); re = crealf(val);
im = cimagf(val); im = cimagf(val);
@ -150,7 +150,7 @@ FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
// Apply square root to the whole sdft range // Apply square root to the whole sdft range
static FAST_CODE void applySqrt(const sdft_t *sdft, float *data) static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
{ {
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { for (int i = sdft->startBin; i <= sdft->endBin; i++) {
data[i] = sqrtf(data[i]); data[i] = sqrtf(data[i]);
} }
} }

16
src/main/common/sdft.h
View File

@ -33,11 +33,11 @@ typedef float complex complex_t; // Better readability for type "float complex"
typedef struct sdft_s { typedef struct sdft_s {
uint8_t idx; // circular buffer index int idx; // circular buffer index
uint8_t startBin; int startBin;
uint8_t endBin; int endBin;
uint8_t batchSize; int batchSize;
uint8_t numBatches; int numBatches;
float samples[SDFT_SAMPLE_SIZE]; // circular buffer float samples[SDFT_SAMPLE_SIZE]; // circular buffer
complex_t data[SDFT_BIN_COUNT]; // complex frequency spectrum complex_t data[SDFT_BIN_COUNT]; // complex frequency spectrum
@ -45,9 +45,9 @@ typedef struct sdft_s {
STATIC_ASSERT(SDFT_SAMPLE_SIZE <= (uint8_t)-1, window_size_greater_than_underlying_type); STATIC_ASSERT(SDFT_SAMPLE_SIZE <= (uint8_t)-1, window_size_greater_than_underlying_type);
void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches); void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches);
void sdftPush(sdft_t *sdft, const float *sample); void sdftPush(sdft_t *sdft, const float sample);
void sdftPushBatch(sdft_t *sdft, const float *sample, const uint8_t *batchIdx); void sdftPushBatch(sdft_t *sdft, const float sample, const int batchIdx);
void sdftMagSq(const sdft_t *sdft, float *output); void sdftMagSq(const sdft_t *sdft, float *output);
void sdftMagnitude(const sdft_t *sdft, float *output); void sdftMagnitude(const sdft_t *sdft, float *output);
void sdftWinSq(const sdft_t *sdft, float *output); void sdftWinSq(const sdft_t *sdft, float *output);

69
src/main/flight/gyroanalyse.c
View File

@ -29,7 +29,6 @@
*/ */
#include <math.h> #include <math.h>
#include <stdint.h>
#include "platform.h" #include "platform.h"
@ -98,7 +97,7 @@ typedef enum {
typedef struct peak_s { typedef struct peak_s {
uint8_t bin; int bin;
float value; float value;
} peak_t; } peak_t;
@ -106,19 +105,19 @@ typedef struct peak_s {
static sdft_t FAST_DATA_ZERO_INIT sdft[XYZ_AXIS_COUNT]; static sdft_t FAST_DATA_ZERO_INIT sdft[XYZ_AXIS_COUNT];
static peak_t FAST_DATA_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX]; static peak_t FAST_DATA_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX];
static float FAST_DATA_ZERO_INIT sdftData[SDFT_BIN_COUNT]; static float FAST_DATA_ZERO_INIT sdftData[SDFT_BIN_COUNT];
static uint16_t FAST_DATA_ZERO_INIT sdftSampleRateHz; static float FAST_DATA_ZERO_INIT sdftSampleRateHz;
static float FAST_DATA_ZERO_INIT sdftResolutionHz; static float FAST_DATA_ZERO_INIT sdftResolutionHz;
static uint8_t FAST_DATA_ZERO_INIT sdftStartBin; static int FAST_DATA_ZERO_INIT sdftStartBin;
static uint8_t FAST_DATA_ZERO_INIT sdftEndBin; static int FAST_DATA_ZERO_INIT sdftEndBin;
static float FAST_DATA_ZERO_INIT sdftMeanSq; static float FAST_DATA_ZERO_INIT sdftMeanSq;
static uint16_t FAST_DATA_ZERO_INIT dynNotchQ; static float FAST_DATA_ZERO_INIT dynNotchQ;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMinHz; static float FAST_DATA_ZERO_INIT dynNotchMinHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxHz; static float FAST_DATA_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT; static int FAST_DATA_ZERO_INIT dynNotchMaxFFT;
static float FAST_DATA_ZERO_INIT gain; static float FAST_DATA_ZERO_INIT gain;
static uint8_t FAST_DATA_ZERO_INIT numSamples; static int FAST_DATA_ZERO_INIT numSamples;
void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs)
{ {
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f; dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
@ -126,7 +125,7 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz); dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
// gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs) // gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs)
const int32_t targetLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); const float targetLoopRateHz = 1.0f / targetLooptimeUs * 1e6f;
numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00 numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00
sdftSampleRateHz = targetLoopRateHz / numSamples; sdftSampleRateHz = targetLoopRateHz / numSamples;
@ -137,20 +136,20 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) sdftSampleRateHz = 1000hz, range 500Hz // eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) sdftSampleRateHz = 1000hz, range 500Hz
// the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2) // the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2)
sdftResolutionHz = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolutionHz + 0.5f)); // can't use bin 0 because it is DC. sdftStartBin = MAX(2, dynNotchMinHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC.
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotchMaxHz / sdftResolutionHz + 0.5f)); // can't use more than SDFT_BIN_COUNT bins. sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotchMaxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins.
gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / (float)targetLoopRateHz); // minimum PT1 k value gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / targetLoopRateHz); // minimum PT1 k value
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples); sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
} }
state->maxSampleCount = numSamples; state->maxSampleCount = numSamples;
state->maxSampleCountRcp = 1.0f / state->maxSampleCount; state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
// any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker // any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker
state->centerFreq[axis][p] = (p + 0.5f) * (dynNotchMaxHz - dynNotchMinHz) / (float)gyro.notchFilterDynCount + dynNotchMinHz; state->centerFreq[axis][p] = (p + 0.5f) * (dynNotchMaxHz - dynNotchMinHz) / (float)gyro.notchFilterDynCount + dynNotchMinHz;
} }
@ -158,7 +157,7 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
} }
// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function // Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample) void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
{ {
state->oversampledGyroAccumulator[axis] += sample; state->oversampledGyroAccumulator[axis] += sample;
} }
@ -174,7 +173,7 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
state->sampleCount = 0; state->sampleCount = 0;
// calculate mean value of accumulated samples // calculate mean value of accumulated samples
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
state->downsampledGyroData[axis] = sample; state->downsampledGyroData[axis] = sample;
if (axis == 0) { if (axis == 0) {
@ -192,8 +191,8 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
// 2us @ F722 // 2us @ F722
// SDFT processing in batches to synchronize with incoming downsampled data // SDFT processing in batches to synchronize with incoming downsampled data
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftPushBatch(&sdft[axis], &state->downsampledGyroData[axis], &state->sampleCount); sdftPushBatch(&sdft[axis], state->downsampledGyroData[axis], state->sampleCount);
} }
state->sampleCount++; state->sampleCount++;
@ -222,7 +221,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
// Calculate mean square over frequency range (= average power of vibrations) // Calculate mean square over frequency range (= average power of vibrations)
sdftMeanSq = 0.0f; sdftMeanSq = 0.0f;
for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq) sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq)
} }
sdftMeanSq /= sdftEndBin - sdftStartBin - 1; sdftMeanSq /= sdftEndBin - sdftStartBin - 1;
@ -234,20 +233,20 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
case STEP_DETECT_PEAKS: // 6us @ F722 case STEP_DETECT_PEAKS: // 6us @ F722
{ {
// Get memory ready for new peak data on current axis // Get memory ready for new peak data on current axis
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
peaks[p].bin = 0; peaks[p].bin = 0;
peaks[p].value = 0.0f; peaks[p].value = 0.0f;
} }
// Search for N biggest peaks in frequency spectrum // Search for N biggest peaks in frequency spectrum
for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) {
// Check if bin is peak // Check if bin is peak
if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) { if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) {
// Check if peak is big enough to be one of N biggest peaks. // Check if peak is big enough to be one of N biggest peaks.
// If so, insert peak and sort peaks in descending height order // If so, insert peak and sort peaks in descending height order
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
if (sdftData[bin] > peaks[p].value) { if (sdftData[bin] > peaks[p].value) {
for (uint8_t k = gyro.notchFilterDynCount - 1; k > p; k--) { for (int k = gyro.notchFilterDynCount - 1; k > p; k--) {
peaks[k] = peaks[k - 1]; peaks[k] = peaks[k - 1];
} }
peaks[p].bin = bin; peaks[p].bin = bin;
@ -260,8 +259,8 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
} }
// Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0) // Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0)
for (uint8_t p = gyro.notchFilterDynCount - 1; p > 0; p--) { for (int p = gyro.notchFilterDynCount - 1; p > 0; p--) {
for (uint8_t k = 0; k < p; k++) { for (int k = 0; k < p; k++) {
// Swap peaks but ignore swapping void peaks (bin = 0). This leaves // Swap peaks but ignore swapping void peaks (bin = 0). This leaves
// void peaks at the end of peaks array without moving them // void peaks at the end of peaks array without moving them
if (peaks[k].bin > peaks[k + 1].bin && peaks[k + 1].bin != 0) { if (peaks[k].bin > peaks[k + 1].bin && peaks[k + 1].bin != 0) {
@ -278,7 +277,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
} }
case STEP_CALC_FREQUENCIES: // 4us @ F722 case STEP_CALC_FREQUENCIES: // 4us @ F722
{ {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor // Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
@ -309,13 +308,13 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
} }
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) { if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]); dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
} }
} }
if (state->updateAxis == gyro.gyroDebugAxis) { if (state->updateAxis == gyro.gyroDebugAxis) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount && p < 3; p++) { for (int p = 0; p < gyro.notchFilterDynCount && p < 3; p++) {
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p])); DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
} }
DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0])); DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0]));
@ -327,7 +326,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
} }
case STEP_UPDATE_FILTERS: // 7us @ F722 case STEP_UPDATE_FILTERS: // 7us @ F722
{ {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step // Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
@ -344,7 +343,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
} }
uint16_t getMaxFFT(void) { int getMaxFFT(void) {
return dynNotchMaxFFT; return dynNotchMaxFFT;
} }

10
src/main/flight/gyroanalyse.h
View File

@ -20,6 +20,10 @@
#pragma once #pragma once
#include <stdint.h>
#include "common/axis.h"
#define DYN_NOTCH_COUNT_MAX 5 #define DYN_NOTCH_COUNT_MAX 5
typedef struct gyroAnalyseState_s { typedef struct gyroAnalyseState_s {
@ -42,8 +46,8 @@ typedef struct gyroAnalyseState_s {
} gyroAnalyseState_t; } gyroAnalyseState_t;
void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs); void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs);
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample); void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample);
void gyroDataAnalyse(gyroAnalyseState_t *state); void gyroDataAnalyse(gyroAnalyseState_t *state);
uint16_t getMaxFFT(void); int getMaxFFT(void);
void resetMaxFFT(void); void resetMaxFFT(void);