diff --git a/README.md b/README.md index 37861ba..986a6c2 100644 --- a/README.md +++ b/README.md @@ -61,6 +61,7 @@ Calcuates the Fast Fourier Transform. Removes the DC component from the sample data. * **MajorPeak**(double *vD, uint16_t samples, double samplingFrequency); * **MajorPeak**(); +* **MajorPeakParabola**(); Looks for and returns the frequency of the biggest spike in the analyzed signal. * **Revision**(void); Returns the library revision. diff --git a/keywords.txt b/keywords.txt index 1daabde..0643678 100644 --- a/keywords.txt +++ b/keywords.txt @@ -3,7 +3,7 @@ ####################################### ####################################### -# Datatypes (KEYWORD1) +# Datatypes (KEYWORD1) ####################################### arduinoFFT KEYWORD1 @@ -19,6 +19,7 @@ Windowing KEYWORD2 Exponent KEYWORD2 Revision KEYWORD2 MajorPeak KEYWORD2 +MajorPeakParabola KEYWORD2 ####################################### # Constants (LITERAL1) diff --git a/src/arduinoFFT.cpp b/src/arduinoFFT.cpp index 790da4f..fb602a9 100644 --- a/src/arduinoFFT.cpp +++ b/src/arduinoFFT.cpp @@ -443,6 +443,53 @@ void arduinoFFT::MajorPeak(double *vD, uint16_t samples, double samplingFrequenc #endif } +double arduinoFFT::MajorPeakParabola() +{ + double maxY = 0; + uint16_t IndexOfMaxY = 0; + //If sampling_frequency = 2 * max_frequency in signal, + //value would be stored at position samples/2 + for (uint16_t i = 1; i < ((this->_samples >> 1) + 1); i++) + { + if ((this->_vReal[i-1] < this->_vReal[i]) && (this->_vReal[i] > this->_vReal[i+1])) + { + if (this->_vReal[i] > maxY) + { + maxY = this->_vReal[i]; + IndexOfMaxY = i; + } + } + } + + double freq = 0; + if( IndexOfMaxY>0 ) + { + // Assume the three points to be on a parabola + double a,b,c; + Parabola(IndexOfMaxY-1, this->_vReal[IndexOfMaxY-1], IndexOfMaxY, this->_vReal[IndexOfMaxY], IndexOfMaxY+1, this->_vReal[IndexOfMaxY+1], &a, &b, &c); + + // Peak is at the middle of the parabola + double x = -b/(2*a); + + // And magnitude is at the extrema of the parabola if you want It... + // double y = a*x*x+b*x+c; + + // Convert to frequency + freq = (x * this->_samplingFrequency) / (this->_samples); + } + + return freq; +} + +void arduinoFFT::Parabola(double x1, double y1, double x2, double y2, double x3, double y3, double *a, double *b, double *c) +{ + double reversed_denom = 1/((x1 - x2) * (x1 - x3) * (x2 - x3)); + + *a = (x3 * (y2 - y1) + x2 * (y1 - y3) + x1 * (y3 - y2)) * reversed_denom; + *b = (x3*x3 * (y1 - y2) + x2*x2 * (y3 - y1) + x1*x1 * (y2 - y3)) * reversed_denom; + *c = (x2 * x3 * (x2 - x3) * y1 + x3 * x1 * (x3 - x1) * y2 + x1 * x2 * (x1 - x2) * y3) *reversed_denom; +} + uint8_t arduinoFFT::Exponent(uint16_t value) { #warning("This method may not be accessible on future revisions.") diff --git a/src/arduinoFFT.h b/src/arduinoFFT.h index 73927dc..c3c1a0b 100644 --- a/src/arduinoFFT.h +++ b/src/arduinoFFT.h @@ -98,6 +98,8 @@ public: void MajorPeak(double *f, double *v); void Windowing(uint8_t windowType, uint8_t dir); + double MajorPeakParabola(); + private: /* Variables */ uint16_t _samples; @@ -107,6 +109,7 @@ private: uint8_t _power; /* Functions */ void Swap(double *x, double *y); + void Parabola(double x1, double y1, double x2, double y2, double x3, double y3, double *a, double *b, double *c); }; #endif