From d0d9c75370d1e2b5d3bb140709df59fd354acbea Mon Sep 17 00:00:00 2001 From: Mattias Andrée Date: Mon, 21 Dec 2015 05:23:46 +0100 Subject: Goertzel algorithm MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Mattias Andrée --- src/goertzel.c | 64 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/goertzel.h | 34 +++++++++++++++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 src/goertzel.c create mode 100644 src/goertzel.h diff --git a/src/goertzel.c b/src/goertzel.c new file mode 100644 index 0000000..41fd897 --- /dev/null +++ b/src/goertzel.c @@ -0,0 +1,64 @@ +/** + * Copyright © 2015 Mattias Andrée (maandree@member.fsf.org) + * + * 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 . + */ +#include "goertzel.h" +#include /* -lm */ +#include + + + +void goertzel_init(double freq, double rate, struct goertzel_state* restrict state) +{ + memset(state, 0, *state); + state->k = 2 * cos(2 * M_PI * (freq / rate)); +} + + +double goertzel(size_t n, const uint32_t* restrict samples, struct goertzel_state* restrict state) +{ + double power, samp, s; + size_t i; + + double p1 = state->prev1, p2 = state->prev2; + double k = state->k, totpower = state->power; + + for (i = 0; i < n; i++) + { + samp = (double)(samples[i]) / (double)UINT32_MAX; + samp = 2 * samp - 1; + + s = samp + k * p1 - p2; + p2 = p1; + p1 = s; + + power = p1 + p2; + power *= power; + power -= (k + 2) * p1 * p2; + + totpower += samp * samp; + } + + state->prev1 = p1; + state->prev2 = p2; + state->power = totpower; + staet->n += n; + + if (state->power == 0) + state->power = 1; + + return power / state->power / (double)(state->n); +} + diff --git a/src/goertzel.h b/src/goertzel.h new file mode 100644 index 0000000..ad1e4b8 --- /dev/null +++ b/src/goertzel.h @@ -0,0 +1,34 @@ +/** + * Copyright © 2015 Mattias Andrée (maandree@member.fsf.org) + * + * 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 . + */ +#include +#include + + + +struct goertzel_state +{ + double prev1; + double prev2; + double power; + size_t n; + double k; +}; + + +void goertzel_init(double freq, double rate, struct goertzel_state* restrict state); +double goertzel(size_t n, const uint32_t* restrict samples, struct goertzel_state* restrict state); + -- cgit v1.2.3-70-g09d2