152 lines
3.2 KiB
C
152 lines
3.2 KiB
C
// SPDX-License-Identifier: Apache-2.0
|
|
/*
|
|
* Copyright (C) 2024 ArtInChip Technology Co., Ltd.
|
|
* Author: weijie.ding <weijie.ding@artinchip.com>
|
|
*/
|
|
#include "goertzel.h"
|
|
|
|
#define FFTSIZE 480
|
|
#define Q14 16384
|
|
#define Q12 4096
|
|
#define Q16 65535
|
|
|
|
extern int16_t audio_rx_data[480];
|
|
|
|
double logarithm(unsigned int i, double temp, double base, double num,
|
|
unsigned int acc)
|
|
{
|
|
if(acc == 1)
|
|
return 0;
|
|
if(num >= base) {
|
|
return 1 + logarithm(0, 1, base, num / base, acc - 1);
|
|
} else {
|
|
while(i++ < base)
|
|
temp *= num;
|
|
return (1/base) * logarithm(0, 1, base, temp, acc - 1);
|
|
}
|
|
}
|
|
|
|
double log10(double x)
|
|
{
|
|
return logarithm(0, 1.0, 10.0, x, 100);
|
|
}
|
|
|
|
double goertzel(short samples[], double freq, int N)
|
|
{
|
|
int i;
|
|
double s_prev = 0.0;
|
|
double s_prev2 = 0.0;
|
|
double coeff, power, s;
|
|
double cosval[6] = {1.9957178, 1.9828897, 1.9318517, 1.847759,
|
|
1.9832297, 1.9825463};
|
|
|
|
if(freq == 500)
|
|
coeff = cosval[0];
|
|
else if(freq == 1000)
|
|
coeff = cosval[1];
|
|
else if(freq == 2000)
|
|
coeff = cosval[2];
|
|
else if(freq == 3000)
|
|
coeff = cosval[3];
|
|
else if(freq == 990)
|
|
coeff = cosval[4];
|
|
else if(freq == 1010)
|
|
coeff = cosval[5];
|
|
else
|
|
coeff = 0;
|
|
|
|
for(i = 0; i < N; i++) {
|
|
s = samples[i] + coeff * s_prev - s_prev2;
|
|
s_prev2 = s_prev;
|
|
s_prev = s;
|
|
}
|
|
|
|
power = s_prev2 * s_prev2 + s_prev * s_prev - coeff * s_prev * s_prev2;
|
|
|
|
return power;
|
|
}
|
|
|
|
int goertzel_convert(short *fSinedata)
|
|
{
|
|
int i, ret=0;
|
|
double fVpp=0.0;
|
|
int FVPP=0;
|
|
|
|
float frequency_row[6] = {500, 1000, 2000, 3000, 990, 1010};
|
|
float magnitude_row[6];
|
|
float snr0 = 0.0, snr1 = 0.0, snr2 = 0.0, rate3 = 0.0, rate4 = 0.0;
|
|
int Snr0 = 0, Snr1 = 0, Snr2 = 0, Rate3 = 0, Rate4 = 0;
|
|
short fRmax, fRmin;
|
|
|
|
//Get the max and min sine data
|
|
fRmax = 0x800;
|
|
fRmin = 0x7FF;
|
|
for(i = 0; i < FFTSIZE; i++) {
|
|
if(fSinedata[i] > fRmax)
|
|
fRmax = fSinedata[i];
|
|
if(fSinedata[i] < fRmin)
|
|
fRmin = fSinedata[i];
|
|
}
|
|
|
|
printf("fRmax: %d, fRmin: %d\n", fRmax, fRmin);
|
|
|
|
fVpp = (double)fRmax / Q12 - (double)fRmin / Q12;
|
|
FVPP = fVpp * 1000;
|
|
printf("fVpp=%dmV!\n", FVPP);
|
|
|
|
if((fVpp < 0.8) || (fVpp > 2.0)) {
|
|
ret = -1;
|
|
goto __exit;
|
|
}
|
|
|
|
//Get the Freq and THD
|
|
for(i = 0; i < 6; i++)
|
|
magnitude_row[i] = \
|
|
goertzel(fSinedata, frequency_row[i], FFTSIZE);
|
|
|
|
snr0 = 20 * log10(magnitude_row[1] / magnitude_row[0]);
|
|
snr1 = 20 * log10(magnitude_row[1] / magnitude_row[2]);
|
|
snr2 = 20 * log10(magnitude_row[1] / magnitude_row[3]);
|
|
rate3 = magnitude_row[1] / magnitude_row[4];
|
|
rate4 = magnitude_row[1] / magnitude_row[5];
|
|
Snr0 = snr0 * 1000;
|
|
Snr1 = snr1 * 1000;
|
|
Snr2 = snr2 * 1000;
|
|
Rate3 = rate3 * 1000;
|
|
Rate4 = rate4 * 1000;
|
|
|
|
printf("snr0 * 1000 = %d!\n", Snr0);
|
|
printf("snr1 * 1000 = %d!\n", Snr1);
|
|
printf("snr2 * 1000 = %d!\n", Snr2);
|
|
printf("rate3 * 1000 = %d!\n", Rate3);
|
|
printf("rate4 * 1000 = %d!\n", Rate4);
|
|
|
|
if((Snr0 < 20000) || (Snr1 < 20000) || (Snr2 < 20000)) {
|
|
ret = -2;
|
|
goto __exit;
|
|
}
|
|
|
|
if(rate3 < 1 || rate4 < 1)
|
|
ret = -3;
|
|
|
|
__exit:
|
|
return ret;
|
|
}
|
|
|
|
int goertzel_test(void)
|
|
{
|
|
unsigned int i;
|
|
int ret=0;
|
|
static short fAdcdt[FFTSIZE];
|
|
|
|
int16_t *test_data = (int16_t *)audio_rx_data;
|
|
|
|
for(i = 0; i < FFTSIZE; i++)
|
|
fAdcdt[i] = test_data[i];
|
|
|
|
ret = goertzel_convert(fAdcdt);
|
|
|
|
return ret;
|
|
}
|
|
|