Skip to content

Commit

Permalink
Minor cleanup
Browse files Browse the repository at this point in the history 
- gyroanalyse.c: changed rounding type from "floor" to "nearest"
  • Loading branch information
@KarateBrot
KarateBrot committed Sep 24, 2020
1 parent 8d701e2 commit b5f03f3
Show file tree
Hide file tree
Showing 5 changed files with 150 additions and 150 deletions.
10 changes: 5 additions & 5 deletions src/main/common/maths.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,14 +116,14 @@ float sqrt_approx(float x)

if (x > 5.877471754e-39)
{
float sum;
float sum;
float powr;
long intPart;

union {
float f;
long i;
} bits;
union {
float f;
long i;
} bits;

bits.f = x;

Expand Down
2 changes: 1 addition & 1 deletion src/main/common/maths.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@

// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
#define FAST_MATH // order 9 approximation
#define VERY_FAST_MATH // order 7 approximation
#define VERY_FAST_MATH // order 7 approximation

// Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f
Expand Down
126 changes: 63 additions & 63 deletions src/main/common/sdft.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,103 +36,103 @@ static void applySqrt(const sdft_t *sdft, float *data);

void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin)
{
if (!constsInitialized) {
dampingFactor = nexttowardf(1.0f, 0.0f);
r_to_n = powf(dampingFactor, SDFT_SAMPLE_SIZE);
const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
float phi = 0.0f;
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
phi = c*i;
twiddle[i] = cos_approx(phi) + _Complex_I * sin_approx(phi);
}
constsInitialized = true;
}

sdft->idx = 0;
sdft->startBin = startBin;
sdft->endBin = endBin;

for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) {
sdft->samples[i] = 0.0f;
}

for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
sdft->data[i] = 0.0f;
}
if (!constsInitialized) {
dampingFactor = nexttowardf(1.0f, 0.0f);
r_to_n = powf(dampingFactor, SDFT_SAMPLE_SIZE);
const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
float phi = 0.0f;
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
phi = c*i;
twiddle[i] = cos_approx(phi) + _Complex_I * sin_approx(phi);
}
constsInitialized = true;
}

sdft->idx = 0;
sdft->startBin = startBin;
sdft->endBin = endBin;

for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) {
sdft->samples[i] = 0.0f;
}

for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
sdft->data[i] = 0.0f;
}
}

// Add new sample to frequency spectrum
FAST_CODE void sdftPush(sdft_t *sdft, const float sample)
{
const float delta = sample - r_to_n * sdft->samples[sdft->idx];
sdft->samples[sdft->idx] = sample;
const float delta = sample - r_to_n * sdft->samples[sdft->idx];
sdft->samples[sdft->idx] = sample;

for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
sdft->data[i] = twiddle[i] * (dampingFactor * sdft->data[i] + delta);
}
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
sdft->data[i] = twiddle[i] * (dampingFactor * sdft->data[i] + delta);
}

sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
}


// Get squared magnitude of frequency spectrum
FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
{
float re;
float im;
for (uint8_t i = sdft->startBin; i < sdft->endBin; i++) {
re = crealf(sdft->data[i]);
im = cimagf(sdft->data[i]);
output[i] = re * re + im * im;
}
float re;
float im;
for (uint8_t i = sdft->startBin; i < sdft->endBin; i++) {
re = crealf(sdft->data[i]);
im = cimagf(sdft->data[i]);
output[i] = re * re + im * im;
}
}


// Get magnitude of frequency spectrum (slower)
FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output)
{
sdftMagSq(sdft, output);
applySqrt(sdft, output);
sdftMagSq(sdft, output);
applySqrt(sdft, output);
}


// Get quared magnitude of frequency spectrum with Hann window applied
FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
{
complex_t val;
float re;
float im;

val = 0.5f * sdft->data[sdft->startBin] - 0.25f * sdft->data[sdft->endBin] + sdft->data[(sdft->startBin + 1)];
re = crealf(val);
im = cimagf(val);
output[sdft->startBin] = re * re + im * im;

for (uint8_t i = (sdft->startBin + 1); i < sdft->endBin; i++) {
val = 0.5f * sdft->data[i] - 0.25f * sdft->data[i - 1] + sdft->data[i + 1];
re = crealf(val);
im = cimagf(val);
output[i] = re * re + im * im;
}

val = 0.5f * sdft->data[sdft->endBin] - 0.25f * sdft->data[sdft->endBin - 1] + sdft->data[(sdft->startBin)];
re = crealf(val);
im = cimagf(val);
output[sdft->endBin] = re * re + im * im;
complex_t val;
float re;
float im;

val = 0.5f * sdft->data[sdft->startBin] - 0.25f * sdft->data[sdft->endBin] + sdft->data[(sdft->startBin + 1)];
re = crealf(val);
im = cimagf(val);
output[sdft->startBin] = re * re + im * im;

for (uint8_t i = (sdft->startBin + 1); i < sdft->endBin; i++) {
val = 0.5f * sdft->data[i] - 0.25f * sdft->data[i - 1] + sdft->data[i + 1];
re = crealf(val);
im = cimagf(val);
output[i] = re * re + im * im;
}

val = 0.5f * sdft->data[sdft->endBin] - 0.25f * sdft->data[sdft->endBin - 1] + sdft->data[(sdft->startBin)];
re = crealf(val);
im = cimagf(val);
output[sdft->endBin] = re * re + im * im;
}


// Get magnitude of frequency spectrum with Hann window applied (slower)
FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
{
sdftWinSq(sdft, output);
applySqrt(sdft, output);
sdftWinSq(sdft, output);
applySqrt(sdft, output);
}


static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
{
for (uint8_t i = sdft->startBin; i > sdft->endBin; i++) {
data[i] = sqrt_approx(data[i]);
}
for (uint8_t i = sdft->startBin; i > sdft->endBin; i++) {
data[i] = sqrt_approx(data[i]);
}
}
10 changes: 5 additions & 5 deletions src/main/common/sdft.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,11 @@ typedef float complex complex_t; // Better readability for type "float complex"

typedef struct sdft_s {

uint8_t idx; // circular buffer index
uint8_t startBin;
uint8_t endBin;
float samples[SDFT_SAMPLE_SIZE]; // circular buffer
complex_t data[SDFT_BIN_COUNT]; // complex frequency spectrum
uint8_t idx; // circular buffer index
uint8_t startBin;
uint8_t endBin;
float samples[SDFT_SAMPLE_SIZE]; // circular buffer
complex_t data[SDFT_BIN_COUNT]; // complex frequency spectrum

} sdft_t;

Expand Down
152 changes: 76 additions & 76 deletions src/main/flight/gyroanalyse.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,13 +130,13 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
// the upper limit of DN is always going to be Nyquist

sdftResolution = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
sdftStartBin = MAX(2, dynNotchMinHz / lrintf(sdftResolution)); // can't use bin 0 because it is DC.
sdftEndBin = lrintf(dynNotchMaxHz / sdftResolution);
smoothFactor = 2 * M_PIf * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); // minimum PT1 k value
sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolution + 0.5f)); // can't use bin 0 because it is DC.
sdftEndBin = lrintf(dynNotchMaxHz / sdftResolution + 0.5f);
smoothFactor = 2 * M_PIf * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); // minimum PT1 k value

for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
sdftInit(&sdft[i], sdftStartBin, sdftEndBin);
}
for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
sdftInit(&sdft[i], sdftStartBin, sdftEndBin);
}
}

void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
Expand Down Expand Up @@ -166,7 +166,7 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchF
// samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate and average multiple gyro samples
state->sampleCount++;
if (state->sampleCount == state->maxSampleCount) {
state->sampleCount = 0;

Expand Down Expand Up @@ -201,8 +201,8 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchF
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{
enum {
STEP_SDFT,
STEP_WINDOW,
STEP_SDFT,
STEP_WINDOW,
STEP_CALC_FREQUENCIES,
STEP_UPDATE_FILTERS,
STEP_COUNT
Expand All @@ -217,78 +217,78 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
switch (state->updateStep) {
case STEP_SDFT:
{
sdftPush(&sdft[state->updateAxis], state->downsampledGyroData[state->updateAxis]);
sdftPush(&sdft[state->updateAxis], state->downsampledGyroData[state->updateAxis]);

DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);

break;
}
case STEP_WINDOW:
{
sdftWinSq(&sdft[state->updateAxis], sdftData);

DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);

break;
break;
}
case STEP_WINDOW:
{
sdftWinSq(&sdft[state->updateAxis], sdftData);

DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);

break;
}
case STEP_CALC_FREQUENCIES:
{
// identify max bin and max/min heights
float dataMax = 0.0f;
float dataMin = 1.0f;
uint8_t binMax = 0;
float dataMinHi = 1.0f;
// Search for peaks
for (uint8_t bin = sdftStartBin + 1; bin < sdftEndBin; bin++) {
// Check if bin is a peak
if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) {
// Check if peak is biggest peak so far
if (sdftData[bin] > binMax) {
dataMax = sdftData[bin];
binMax = bin;
}
bin++; // If bin is peak, next bin can't be peak => jump it
}
}
if (binMax == 0) { // no bin increase, hold prev max bin, dataMin = 1 dataMax = 0, ie move slow
binMax = lrintf(state->centerFreq[state->updateAxis] / sdftResolution);
} else { // there was a max, find min
for (uint8_t bin = binMax - 1; bin > 1; bin--) { // look for min below max
if (sdftData[bin - 1] > sdftData[bin]) {
dataMin = sdftData[bin];
break;
}
}
for (uint8_t bin = binMax + 1; bin < SDFT_BIN_COUNT - 1; bin++) { // look for min above max
if (sdftData[bin + 1] > sdftData[bin]) {
dataMinHi = sdftData[bin];
break;
}
}
}
dataMin = fminf(dataMin, dataMinHi);
// accumulate sdftSum and sdftWeightedSum from peak bin, and shoulder bins either side of peak
float squaredData = sdftData[binMax]; // sdftData already squared (see sdftWinSq)
float sdftSum = squaredData;
float sdftWeightedSum = squaredData * binMax;

// accumulate upper shoulder unless it would be sdftEndBin
uint8_t shoulderBin = binMax + 1;
if (shoulderBin < sdftEndBin) {
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}

// accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
if (binMax > 1) {
shoulderBin = binMax - 1;
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}
// identify max bin and max/min heights
float dataMax = 0.0f;
float dataMin = 1.0f;
uint8_t binMax = 0;
float dataMinHi = 1.0f;
// Search for peaks
for (uint8_t bin = sdftStartBin + 1; bin < sdftEndBin; bin++) {
// Check if bin is a peak
if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) {
// Check if peak is biggest peak so far
if (sdftData[bin] > binMax) {
dataMax = sdftData[bin];
binMax = bin;
}
bin++; // If bin is peak, next bin can't be peak => jump it
}
}
if (binMax == 0) { // no bin increase, hold prev max bin, dataMin = 1 dataMax = 0, ie move slow
binMax = lrintf(state->centerFreq[state->updateAxis] / sdftResolution + 0.5f);
} else { // there was a max, find min
for (uint8_t bin = binMax - 1; bin > 1; bin--) { // look for min below max
if (sdftData[bin] < sdftData[bin - 1]) {
dataMin = sdftData[bin];
break;
}
}
for (uint8_t bin = binMax + 1; bin < SDFT_BIN_COUNT - 1; bin++) { // look for min above max
if (sdftData[bin] < sdftData[bin + 1]) {
dataMinHi = sdftData[bin];
break;
}
}
}
dataMin = fminf(dataMin, dataMinHi);
// accumulate sdftSum and sdftWeightedSum from peak bin, and shoulder bins either side of peak
float squaredData = sdftData[binMax]; // sdftData already squared (see sdftWinSq)
float sdftSum = squaredData;
float sdftWeightedSum = squaredData * binMax;

// accumulate upper shoulder unless it would be sdftEndBin
uint8_t shoulderBin = binMax + 1;
if (shoulderBin < sdftEndBin) {
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}

// accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
if (binMax > 1) {
shoulderBin = binMax - 1;
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}

// get centerFreq in Hz from weighted bins
float centerFreq = dynNotchMaxHz;
Expand Down

0 comments on commit b5f03f3

Please sign in to comment.