123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778 |
- /*
- * afsk_demodulator.c
- *
- * Created on: Sep 10, 2019
- * Author: curiousmuch
- */
- #include "stdio.h"
- #include "stdint.h"
- #include "math.h"
- #include "afsk_demodulator.h"
- int8_t window[WINDOW_SIZE];
- //int32_t lpf_window[WINDOW_SIZE];
- double goertzelFilter(int8_t samples[], double freq, unsigned int N)
- {
- double s_prev = 0.0;
- double s_prev2 = 0.0;
- double coeff,normalizedfreq,power,s;
- unsigned int i;
- normalizedfreq = freq / SAMPLEFREQUENCY;
- coeff = 2*cos(2*M_PI*normalizedfreq);
- 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;
- }
- float goertzelFilter2(int8_t samples[], float freq, unsigned int N)
- {
- float s_prev = 0.0;
- float s_prev2 = 0.0;
- float coeff,normalizedfreq,power,s;
- unsigned int i;
- normalizedfreq = freq / SAMPLEFREQUENCY;
- coeff = 2*cos(2*M_PI*normalizedfreq);
- 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;
- }
- void window_init(void)
- {
- // TODO: setup dynamic allocation
- for(unsigned int i=0;i<WINDOW_SIZE;i++)
- {
- window[i] = 0;
- }
- }
- void window_add(int8_t sample)
- {
- for(uint32_t i=0;i<(WINDOW_SIZE-1);i++)
- {
- window[(WINDOW_SIZE-1)-i] = window[(WINDOW_SIZE-2)-i];
- }
- window[0] = sample;
- }
- int8_t* window_get(void)
- {
- return window;
- }
- unsigned int window_get_size(void)
- {
- return sizeof(window);
- }
|