a goertzel demodulator

gortzelfsk.c

// Goertzel Demodulator
//
// This program demodulates two tone FSK using the goertzel algorithm.

#include <stdio.h>
#include <math.h>

struct gConstants {
  float f1norm;
  float f2norm;
  float f1coef;
  float f2coef;
} gConstants;

struct gMagnitudes {
  float f1mag;
  float f2mag;
} gMagnitudes;

struct gConstants initGoertzel(int fmark, int fspace, int sampleRate){

  int i = 0;

  struct gConstants comp;

  comp.f1norm = (float)fmark/((float)sampleRate);
  comp.f2norm = (float)fspace/((float)sampleRate);
  fprintf(stdout, "nmf:%f %f\n", comp.f1norm, comp.f2norm);

  comp.f1coef = 2*cosf(2*M_PI*comp.f1norm);
  comp.f2coef = 2*cosf(2*M_PI*comp.f2norm);
  fprintf(stdout, "coef:%f %f\n", comp.f1coef, comp.f2coef);

  return comp;

}

struct gMagnitudes goertzel(short* block, int blockSize, float fMarkCoef, float fSpaceCoef){

  struct gMagnitudes res;

  int blockLength = blockSize/sizeof(short);

  int i = 0;
 
  float f1[3] = {0};
  float f2[3] = {0};

  for(i = 0; i < blockLength; i++) {

    //do per sample computations for each frequency of interest
    f1[0] = block[i] + fMarkCoef*f1[1] - f1[2];
    f1[2] = f1[1];
    f1[1] = f1[0];

    f2[0] = block[i] + fSpaceCoef*f2[1] - f2[2];
    f2[2] = f2[1];
    f2[1] = f2[0];
  }

  //compute magnitudes of each frequency of interest
  res.f1mag = f1[2]*f1[2] + f1[1]*f1[1] - fMarkCoef*f1[1]*f1[2];
  res.f2mag = f2[2]*f2[2] + f2[1]*f2[1] - fSpaceCoef*f2[1]*f2[2];

  return res;

}

int main(int argc[], char* argv[]) {

  // open input file
  FILE* filein;
  if (strcmp(argv[1], "-") == 0){
    filein = stdin;
  } else if( (filein = fopen(argv[1], "rb")) == NULL){
    printf("Error opening input file...\n");
    return;
  }

  // open output file
  FILE* fileout;
  if (strcmp(argv[2], "-") == 0){
    fileout = stdout;
  } else if( (fileout = fopen(argv[2], "wb")) == NULL){
    printf("Error opening input file...\n");
    return;
  }

  struct gConstants con =  {0};
  con = initGoertzel(2200, 1200, 44100); 

  struct gMagnitudes mag = {0};

  int numblocks = 32;
  short readbuff[numblocks];

  while(!feof(filein)) {
    int sampsRead = 0;
    sampsRead = fread(&readbuff[0], sizeof(short), numblocks, filein);
    mag = goertzel(&readbuff[0], sampsRead, con.f1coef, con.f2coef);
    fprintf(fileout, "%f, %f,\n", mag.f1mag, mag.f2mag);
  }
}