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

Changes from review 

Revise and simplify initialization logic and avoid unneeded code if USE_DUAL_GYRO is not defined.

Corrected double constant to float.

Split centerFreq filter apply and constrain into separate statements.
This commit is contained in:
Bruce Luckcuck 2018-07-31 08:53:55 -04:00
parent f252b09fdb
commit 9ba89f6da0
1 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/main/sensors/gyroanalyse.c
View File

@ -50,7 +50,8 @@
#define FFT_SAMPLING_RATE_HZ 1333 #define FFT_SAMPLING_RATE_HZ 1333
// following bin must be at least 2 times previous to indicate start of peak // following bin must be at least 2 times previous to indicate start of peak
#define FFT_MIN_BIN_RISE 2 #define FFT_MIN_BIN_RISE 2
// the desired approimate lower frequency when calculating bin offset
#define FFT_BIN_OFFSET_DESIRED_HZ 90
// lowpass frequency for smoothing notch centre point // lowpass frequency for smoothing notch centre point
#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 #define DYN_NOTCH_SMOOTH_FREQ_HZ 60
// notch centre point will not go below this, must be greater than cutoff, mid of bottom bin // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
@ -60,7 +61,6 @@
// we need 4 steps for each axis // we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
static bool FAST_RAM_ZERO_INIT gyroAnalyseInitialized;
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
// centre frequency of bandpass that constrains input to FFT // centre frequency of bandpass that constrains input to FFT
static uint16_t FAST_RAM_ZERO_INIT fftBpfHz; static uint16_t FAST_RAM_ZERO_INIT fftBpfHz;
@ -74,11 +74,17 @@ static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
static FAST_RAM_ZERO_INIT float dynamicNotchCutoff; static FAST_RAM_ZERO_INIT float dynamicNotchCutoff;
// Making this NOINLINE saves a few bytes to help F3's compile void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
// It's only called once at initialization so no performance concerns
NOINLINE void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{ {
const int gyroLoopRateHz = lrintf((1 / (float)targetLooptimeUs) * 1e6); #ifdef USE_DUAL_GYRO
static bool gyroAnalyseInitialized;
if (gyroAnalyseInitialized) {
return;
}
gyroAnalyseInitialized = true;
#endif
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
// If we get at least 3 samples then use the default FFT sample frequency // If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
@ -91,25 +97,19 @@ NOINLINE void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
// Calculate the FFT bin offset to try and get the lowest bin used // Calculate the FFT bin offset to try and get the lowest bin used
// in the center calc close to 90hz // in the center calc close to 90hz
// > 1333hz = 1, 889hz (2.67K) = 2, 666hz (2K) = 3 // > 1333hz = 1, 889hz (2.67K) = 2, 666hz (2K) = 3
fftBinOffset = MAX(1, lrintf(90 / fftResolution - 1.5f)); fftBinOffset = MAX(1, lrintf(FFT_BIN_OFFSET_DESIRED_HZ / fftResolution - 1.5f));
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
} }
dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100; dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100;
gyroAnalyseInitialized = true;
} }
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, 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
gyroDataAnalyseInit(targetLooptimeUs);
// Perform the main initialization on the first gyro sensor init. We need
// the target loop time and that's only available after the sensor is initialized.
if (!gyroAnalyseInitialized) {
gyroDataAnalyseInit(targetLooptimeUs);
}
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
state->maxSampleCount = samplingFrequency / fftSamplingRateHz; state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
@ -290,7 +290,8 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz); centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
} }
centerFreq = constrain(biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq), DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
state->centerFreq[state->updateAxis] = centerFreq; state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) { if (state->updateAxis == 0) {