137 lines
3.0 KiB
C++
Executable File
137 lines
3.0 KiB
C++
Executable File
#include "goertzel.h"
|
|
|
|
/*
|
|
|
|
|
|
Goertzel formula audio detector
|
|
|
|
This code comes from http://www.skovholm.com/cwdecoder , http://www.skovholm.com/decoder11.ino
|
|
|
|
Hjalmar skovholm Hansen, Oz1jhm <hjh@skovholm.com>
|
|
|
|
|
|
|
|
|
|
*/
|
|
|
|
#if defined(ARDUINO) && ARDUINO >= 100
|
|
#include "Arduino.h"
|
|
#else
|
|
#include "WProgram.h"
|
|
#endif
|
|
|
|
|
|
|
|
float coeff = 0;
|
|
float Q1 = 0;
|
|
float Q2 = 0;
|
|
float sine = 0;
|
|
float cosine = 0;
|
|
//float sampling_freq = GOERTZ_SAMPLING_FREQ;
|
|
//float target_freq = GOERTZ_TARGET_FREQ;
|
|
//float n = GOERTZ_SAMPLES_FLOAT;
|
|
int testData[GOERTZ_SAMPLES];
|
|
int audioInPin = 0;
|
|
int realstate = LOW;
|
|
int realstatebefore = LOW;
|
|
int filteredstate = LOW;
|
|
unsigned long laststarttime = 0;
|
|
int magnitudelimit = 100;
|
|
float magnitude = 0;
|
|
|
|
//////////////////////////////
|
|
// Noise Blanker time which //
|
|
// shall be computed so //
|
|
// this is initial //
|
|
//////////////////////////////
|
|
|
|
int nbtime = GOERTZ_NOISE_BLANKER_INITIAL_MS;
|
|
|
|
|
|
Goertzdetector::Goertzdetector(){}
|
|
|
|
|
|
void Goertzdetector::init(int _audioInPin){
|
|
// The basic goertzel calculation
|
|
|
|
int k = 0;
|
|
float omega = 0;
|
|
//k = (int) (0.5 + ((n * target_freq) / sampling_freq));
|
|
k = (int) (0.5 + (((float)GOERTZ_SAMPLES * (float)GOERTZ_TARGET_FREQ) / (float)GOERTZ_SAMPLING_FREQ));
|
|
//omega = (2.0 * PI * k) / n;
|
|
omega = (2.0 * PI * k) / (float) GOERTZ_SAMPLES;
|
|
sine = sin(omega);
|
|
cosine = cos(omega);
|
|
coeff = 2.0 * cosine;
|
|
|
|
audioInPin = _audioInPin;
|
|
|
|
}
|
|
|
|
int Goertzdetector::detecttone(){
|
|
|
|
|
|
// The basic where we get the tone
|
|
|
|
|
|
for (char index = 0; index < GOERTZ_SAMPLES; index++){
|
|
testData[index] = analogRead(audioInPin);
|
|
}
|
|
|
|
for (char index = 0; index < GOERTZ_SAMPLES; index++){
|
|
float Q0;
|
|
Q0 = coeff * Q1 - Q2 + (float) testData[index];
|
|
Q2 = Q1;
|
|
Q1 = Q0;
|
|
}
|
|
|
|
float magnitudeSquared = (Q1*Q1)+(Q2*Q2)-Q1*Q2*coeff; // we do only need the real part //
|
|
|
|
magnitude = sqrt(magnitudeSquared);
|
|
|
|
Q2 = 0;
|
|
Q1 = 0;
|
|
|
|
|
|
///////////////////////////////////////////////////////////
|
|
// here we will try to set the magnitude limit automatic //
|
|
///////////////////////////////////////////////////////////
|
|
|
|
|
|
if (magnitude > GOERTZ_MAGNITUDE_LIMIT_LOW){
|
|
magnitudelimit = (magnitudelimit + ((magnitude - magnitudelimit) / GOERTZ_MOVING_AVERAGE_FILTER)); /// moving average filter
|
|
} else {
|
|
magnitudelimit = GOERTZ_MAGNITUDE_LIMIT_LOW;
|
|
}
|
|
|
|
////////////////////////////////////
|
|
// now we check for the magnitude //
|
|
////////////////////////////////////
|
|
|
|
if (magnitude > ((float) magnitudelimit * (float) GOERTZ_MAGNITUDE_THRESHOLD)){
|
|
realstate = HIGH;
|
|
} else {
|
|
realstate = LOW;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
// here we clean up the state with a noise blanker //
|
|
/////////////////////////////////////////////////////
|
|
|
|
if (realstate != realstatebefore){
|
|
laststarttime = millis();
|
|
}
|
|
|
|
if ((millis()-laststarttime) > nbtime){
|
|
if (realstate != filteredstate){
|
|
filteredstate = realstate;
|
|
}
|
|
}
|
|
|
|
|
|
realstatebefore = realstate;
|
|
|
|
return(filteredstate);
|
|
|
|
}
|