aboutsummaryrefslogtreecommitdiffstats
path: root/src/libgoertzel/goertzel.c
diff options
context:
space:
mode:
authorAndreas Eversberg <jolly@eversberg.eu>2017-11-16 07:34:16 +0100
committerAndreas Eversberg <jolly@eversberg.eu>2017-11-25 19:23:54 +0100
commit16194c15f9827edf9cfde9a5c18ccb06353c34a3 (patch)
treeaacd3cfc675381911c80fd3055ee4d085d996bf2 /src/libgoertzel/goertzel.c
parentc84318b245793dd48aac30c0f30b5e8f356ba42e (diff)
Restructure: Move goertzel from common code to 'libgoertzel'
Diffstat (limited to 'src/libgoertzel/goertzel.c')
-rw-r--r--src/libgoertzel/goertzel.c104
1 files changed, 104 insertions, 0 deletions
diff --git a/src/libgoertzel/goertzel.c b/src/libgoertzel/goertzel.c
new file mode 100644
index 0000000..7c926fb
--- /dev/null
+++ b/src/libgoertzel/goertzel.c
@@ -0,0 +1,104 @@
+/* Goertzel functions
+ *
+ * (C) 2016 by Andreas Eversberg <jolly@eversberg.eu>
+ * All Rights Reserved
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <math.h>
+#include "../common/sample.h"
+#include "../common/debug.h"
+#include "goertzel.h"
+
+/*
+ * audio level calculation
+ */
+
+/* Return average value (rectified value)
+ * The input must not have any dc offset!
+ * For a perfect rectangualr wave, the result would equal the peak level.
+ * For a sine wave the result would be factor (2 / PI) below peak level.
+ */
+double audio_level(sample_t *samples, int length)
+{
+ double level, sk;
+ int n;
+
+ /* level calculation */
+ level = 0;
+ for (n = 0; n < length; n++) {
+ sk = samples[n];
+ if (sk < 0)
+ level -= (double)sk;
+ if (sk > 0)
+ level += (double)sk;
+ }
+ level = level / (double)length;
+
+ return level;
+}
+
+void audio_goertzel_init(goertzel_t *goertzel, double freq, int samplerate)
+{
+ memset(goertzel, 0, sizeof(*goertzel));
+ goertzel->coeff = 2.0 * cos(2.0 * M_PI * freq / (double)samplerate);
+}
+
+/*
+ * goertzel filter
+ */
+
+/* filter frequencies and return their levels
+ *
+ * samples: pointer to sample buffer
+ * length: length of buffer
+ * offset: for ring buffer, start here and wrap arround to 0 when length has been hit
+ * coeff: array of coefficients (coeff << 15)
+ * result: array of result levels (average value of the sine, that is 1 / (PI/2) of the sine's peak)
+ * k: number of frequencies to check
+ */
+void audio_goertzel(goertzel_t *goertzel, sample_t *samples, int length, int offset, double *result, int k)
+{
+ double sk, sk1, sk2;
+ double cos2pik;
+ int i, n;
+
+ /* we do goertzel */
+ for (i = 0; i < k; i++) {
+ sk = 0;
+ sk1 = 0;
+ sk2 = 0;
+ cos2pik = goertzel[i].coeff;
+ /* note: after 'length' cycles, offset is restored to its initial value */
+ for (n = 0; n < length; n++) {
+ sk = (cos2pik * sk1) - sk2 + samples[offset++];
+ sk2 = sk1;
+ sk1 = sk;
+ if (offset == length)
+ offset = 0;
+ }
+ /* compute level of signal */
+ result[i] = sqrt(
+ (sk * sk) -
+ (cos2pik * sk * sk2) +
+ (sk2 * sk2)
+ ) / (double)length * 2.0 * 0.63662; /* 1 / (PI/2) */
+ }
+}
+