FM Demodulation

main.c

/*
 * Copyright 2020 Nico Sonack
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <fcntl.h>
#include <fftw3.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <unistd.h>

/*
 * from http://joshisanerd.com/projects/sdr_snippets/gnuradio_and_ipython//2%20Broadcast%20FM%20RDS%20Decode.html
 * things1 = data[1:] * np.conj(data[:-1])
 * things2 = 10 * np.arctan2(np.imag(things1), np.real(things1))
 *
 * # pogness: https://imgur.com/VVwrVhQ
 * psd(things2, Fs=sdr.sample_rate)
 * xlabel('Frequency (Hz)')
 * yylabel('Relative power (dB)')
 */

static
double clamp(double x, double min, double max) {
    if ( x < min ) {
        return min;
    } else if ( x > max ) {
        return max;
    } else {
        return x;
    }
}

int main(int argc, char** argv) {
    if (argc != 2) {
        fprintf(stderr, "Provide more shit please!\n");
        return EXIT_FAILURE;}


    int file = open(argv[1], O_RDONLY);
    if (file < 0) {
        fprintf(stderr, "Unable to open file\n");
        return EXIT_FAILURE;
    }

    struct stat buf;
    if (fstat(file, &buf)) {
        fprintf(stderr, "Unable to fstat the file\n");
        close(file);
        return EXIT_FAILURE;
    }

    size_t file_size = buf.st_size,
        n = file_size / 2;

    uint8_t *data = malloc(sizeof(uint8_t) * file_size);
    if ( !data ) {
        fprintf(stderr, "Unable to malloc. Please download more RAM for me!\n");
        return EXIT_FAILURE;
    }

    if (file_size != read(file, data, file_size)) {
        fprintf(stderr, "Unable to read(). You might be in serious trouble.\n");
        return EXIT_FAILURE;
    }

    double *things1 = malloc(sizeof(double) * 2 * (n - 1));
    if ( !things1 ) {
        fprintf(stderr, "Unable to malloc. Again\n");
        return EXIT_FAILURE;
    }

    for ( size_t i = 0; i < n - 1; ++i ) {
        /* z1 = a + bi, z0 = c + di */
        double a, b, c, d;
#define center(x) (((double)x - 128.0) / 128.0)
        a =  center(data[2*i + 0]);
        b =  center(data[2*i + 1]);
        c =  center(data[2*i + 2]);
        d =  center(data[2*i + 3]);
#undef center

        things1[2*i + 0] = ( a * c + b * d );
        things1[2*i + 1] = ( b * c - a * d );
    }

    double *dthetas = malloc(sizeof(double) * (n - 1));
    if ( !dthetas ) {
        fprintf(stderr, "Unable to malloc. Yet again.\n");
        return EXIT_FAILURE;
    }

    #define GAIN 1.0
    #define SAMPLE_RATE 256000.0
    #define BANDWIDTH 200000
    double magic = GAIN *  SAMPLE_RATE / (BANDWIDTH * 2 * M_PI);
    for ( size_t i = 0; i < n - 1; ++i ) {
        double x, y, r;
        x = things1[2*i];
        y = things1[2*i+1];
        r = sqrt(x*x+y*y);

        double rotation = atan2(y, x);
        rotation *= r / fabs(r);
        rotation = fmod(rotation + M_PI, 2 * M_PI) - M_PI;
        dthetas[i] = clamp(rotation * magic, -1.0, 1.0);
    }

    // dump raw data
    int dump_file = open("./dump.dat", O_WRONLY | O_CREAT);
    if (!dump_file) {
        fprintf(stderr, "Unable to open()\n");
        return EXIT_FAILURE;
    }

    write(dump_file, dthetas, sizeof(double) * (n - 1));
    close(dump_file);

    close(file);
    free(dthetas);
    free(things1);
    free(data);

    return EXIT_SUCCESS;
}

Makefile

CC=	/usr/bin/cc
PKGS=	fftw3
LDFLAGS=`pkg-config --libs $(PKGS)` -lm
CFLAGS=	-std=c99 -g -pedantic -Wall -Wextra `pkg-config --cflags $(PKGS)`
PROG=	main
MK_MAN=	no

.include <bsd.prog.mk>