Started demodulator class based on rtl_fm.c

CMakeLists.txt

@@ -80,6 +80,7 @@ SET (cubicsdr_sources
 	src/AppFrame.cpp
 	src/SDRThreadQueue.cpp
 	src/SDRThreadTask.cpp
+	src/Demodulate.cpp
 )
 
 SET (cubicsdr_headers
@@ -91,6 +92,7 @@ SET (cubicsdr_headers
 	src/CubicSDRDefs.h
 	src/SDRThreadQueue.h
 	src/SDRThreadTask.h
+	src/Demodulate.h
 )
 #configure_files(${PROJECT_SOURCE_DIR}/shaders ${PROJECT_BINARY_DIR}/shaders COPYONLY)
 #configure_files(${PROJECT_SOURCE_DIR}/png ${PROJECT_BINARY_DIR}/png COPYONLY)

src/Demodulate.cpp

@@ -0,0 +1,966 @@
+#include "Demodulate.h"
+#include "CubicSDRDefs.h"
+
+#include <math.h>
+#include <cstring>
+#include <stdlib.h>
+#include <iostream>
+
+#define DEFAULT_SAMPLE_RATE 24000
+#define DEFAULT_BUF_LENGTH  (1 * 16384)
+#define MAXIMUM_OVERSAMPLE  16
+#define MAXIMUM_BUF_LENGTH  (MAXIMUM_OVERSAMPLE * DEFAULT_BUF_LENGTH)
+#define AUTO_GAIN   -100
+#define BUFFER_DUMP 4096
+#define MAXIMUM_RATE    2400000
+#define PI_INT  (1<<14)
+#define ONE_INT (1<<14)
+static int *atan_lut = NULL;
+static int atan_lut_size = 131072; /* 512 KB */
+static int atan_lut_coef = 8;
+// rewrite as dynamic and thread-safe for multi demod/dongle
+#define SHARED_SIZE 6
+int16_t shared_samples[SHARED_SIZE][MAXIMUM_BUF_LENGTH];
+int ss_busy[SHARED_SIZE] = { 0 };
+
+/* more cond dumbness */
+#define safe_cond_signal(n, m) pthread_mutex_lock(m); pthread_cond_signal(n); pthread_mutex_unlock(m)
+#define safe_cond_wait(n, m) pthread_mutex_lock(m); pthread_cond_wait(n, m); pthread_mutex_unlock(m)
+/* {length, coef, coef, coef} and scaled by 2^15
+ for now, only length 9, optimal way to get +85% bandwidth */
+#define CIC_TABLE_MAX 10
+int cic_9_tables[][10] = { { 0, }, { 9, -156, -97, 2798, -15489, 61019, -15489, 2798, -97, -156 }, { 9, -128, -568, 5593, -24125, 74126, -24125, 5593,
+        -568, -128 }, { 9, -129, -639, 6187, -26281, 77511, -26281, 6187, -639, -129 },
+        { 9, -122, -612, 6082, -26353, 77818, -26353, 6082, -612, -122 }, { 9, -120, -602, 6015, -26269, 77757, -26269, 6015, -602, -120 }, { 9, -120,
+                -582, 5951, -26128, 77542, -26128, 5951, -582, -120 }, { 9, -119, -580, 5931, -26094, 77505, -26094, 5931, -580, -119 }, { 9, -119,
+                -578, 5921, -26077, 77484, -26077, 5921, -578, -119 }, { 9, -119, -577, 5917, -26067, 77473, -26067, 5917, -577, -119 }, { 9, -199,
+                -362, 5303, -25505, 77489, -25505, 5303, -362, -199 }, };
+
+#ifdef _MSC_VER
+double log2(double n)
+{
+    return log(n) / log(2.0);
+}
+#endif
+void rotate_90(unsigned char *buf, uint32_t len)
+/* 90 rotation is 1+0j, 0+1j, -1+0j, 0-1j
+ or [0, 1, -3, 2, -4, -5, 7, -6] */
+{
+    uint32_t i;
+    unsigned char tmp;
+    for (i = 0; i < len; i += 8) {
+        /* uint8_t negation = 255 - x */
+        tmp = 255 - buf[i + 3];
+        buf[i + 3] = buf[i + 2];
+        buf[i + 2] = tmp;
+        buf[i + 4] = 255 - buf[i + 4];
+        buf[i + 5] = 255 - buf[i + 5];
+        tmp = 255 - buf[i + 6];
+        buf[i + 6] = buf[i + 7];
+        buf[i + 7] = tmp;
+    }
+}
+
+int translate_init(struct translate_state *ts)
+/* two pass: first to find optimal length, second to alloc/fill */
+{
+    int max_length = 100000;
+    int i, s, c, best_i;
+    double a, a2, err, best_360;
+    if (fabs(ts->angle) < 2 * M_PI / max_length) {
+        std::cout << "angle too small or array too short\n" << std::endl;
+        return 1;
+    }
+    ts->i = 0;
+    ts->sincos = NULL;
+    if (ts->len) {
+        max_length = ts->len;
+        ts->sincos = (int16_t *) malloc(max_length * sizeof(int16_t));
+    }
+    a = 0.0;
+    err = 0.0;
+    best_i = 0;
+    best_360 = 4.0;
+    for (i = 0; i < max_length; i += 2) {
+        s = (int) round(sin(a + err) * (1 << 14));
+        c = (int) round(cos(a + err) * (1 << 14));
+        a2 = atan2(s, c);
+        err = fmod(a, 2 * M_PI) - a2;
+        a += ts->angle;
+        while (a > M_PI) {
+            a -= 2 * M_PI;
+        }
+        while (a < -M_PI) {
+            a += 2 * M_PI;
+        }
+        if (i != 0 && fabs(a) < best_360) {
+            best_i = i;
+            best_360 = fabs(a);
+        }
+        if (i != 0 && fabs(a) < 0.0000001) {
+            break;
+        }
+        if (ts->sincos) {
+            ts->sincos[i] = s;
+            ts->sincos[i + 1] = c;
+//fprintf(stderr, "%i %i %i\n", i, s, c);
+        }
+    }
+    if (ts->sincos) {
+        return 0;
+    }
+    ts->len = best_i + 2;
+    return translate_init(ts);
+}
+
+void translate(Demodulate *d) {
+    int i, len, sc_i, sc_len;
+    int32_t tmp, ar, aj, br, bj;
+    int16_t *buf = d->lowpassed;
+    int16_t *sincos = d->rotate.sincos;
+    len = d->lp_len;
+    sc_i = d->rotate.i;
+    sc_len = d->rotate.len;
+    for (i = 0; i < len; i += 2, sc_i += 2) {
+        sc_i = sc_i % sc_len;
+        ar = (int32_t) buf[i];
+        aj = (int32_t) buf[i + 1];
+        br = (int32_t) sincos[sc_i];
+        bj = (int32_t) sincos[sc_i + 1];
+        tmp = ar * br - aj * bj;
+        buf[i] = (int16_t) (tmp >> 14);
+        tmp = aj * br + ar * bj;
+        buf[i + 1] = (int16_t) (tmp >> 14);
+    }
+    d->rotate.i = sc_i;
+}
+
+void low_pass(Demodulate *d)
+/* simple square window FIR */
+{
+    int i = 0, i2 = 0;
+    while (i < d->lp_len) {
+        d->now_r += d->lowpassed[i];
+        d->now_j += d->lowpassed[i + 1];
+        i += 2;
+        d->prev_index++;
+        if (d->prev_index < d->downsample) {
+            continue;
+        }
+        d->lowpassed[i2] = d->now_r; // * d->output_scale;
+        d->lowpassed[i2 + 1] = d->now_j; // * d->output_scale;
+        d->prev_index = 0;
+        d->now_r = 0;
+        d->now_j = 0;
+        i2 += 2;
+    }
+    d->lp_len = i2;
+}
+
+int low_pass_simple(int16_t *signal2, int len, int step)
+// no wrap around, length must be multiple of step
+        {
+    int i, i2, sum;
+    for (i = 0; i < len; i += step) {
+        sum = 0;
+        for (i2 = 0; i2 < step; i2++) {
+            sum += (int) signal2[i + i2];
+        }
+//signal2[i/step] = (int16_t)(sum / step);
+        signal2[i / step] = (int16_t) (sum);
+    }
+    signal2[i / step + 1] = signal2[i / step];
+    return len / step;
+}
+
+void low_pass_real(Demodulate *s)
+/* simple square window FIR */
+// add support for upsampling?
+        {
+    int16_t *lp = s->lowpassed;
+    int i = 0, i2 = 0;
+    int fast = (int) s->rate_out;
+    int slow = s->rate_out2;
+    while (i < s->lp_len) {
+        s->now_lpr += lp[i];
+        i++;
+        s->prev_lpr_index += slow;
+        if (s->prev_lpr_index < fast) {
+            continue;
+        }
+        lp[i2] = (int16_t) (s->now_lpr / (fast / slow));
+        s->prev_lpr_index -= fast;
+        s->now_lpr = 0;
+        i2 += 1;
+    }
+    s->lp_len = i2;
+}
+
+void fifth_order(int16_t *data, int length, int16_t *hist)
+/* for half of interleaved data */
+{
+    int i;
+    int16_t a, b, c, d, e, f;
+    a = hist[1];
+    b = hist[2];
+    c = hist[3];
+    d = hist[4];
+    e = hist[5];
+    f = data[0];
+    /* a downsample should improve resolution, so don't fully shift */
+    data[0] = (a + (b + e) * 5 + (c + d) * 10 + f) >> 4;
+    for (i = 4; i < length; i += 4) {
+        a = c;
+        b = d;
+        c = e;
+        d = f;
+        e = data[i - 2];
+        f = data[i];
+        data[i / 2] = (a + (b + e) * 5 + (c + d) * 10 + f) >> 4;
+    }
+    /* archive */
+    hist[0] = a;
+    hist[1] = b;
+    hist[2] = c;
+    hist[3] = d;
+    hist[4] = e;
+    hist[5] = f;
+}
+
+void generic_fir(int16_t *data, int length, int *fir, int16_t *hist)
+/* Okay, not at all generic. Assumes length 9, fix that eventually. */
+{
+    int d, temp, sum;
+    for (d = 0; d < length; d += 2) {
+        temp = data[d];
+        sum = 0;
+        sum += (hist[0] + hist[8]) * fir[1];
+        sum += (hist[1] + hist[7]) * fir[2];
+        sum += (hist[2] + hist[6]) * fir[3];
+        sum += (hist[3] + hist[5]) * fir[4];
+        sum += hist[4] * fir[5];
+        data[d] = sum >> 15;
+        hist[0] = hist[1];
+        hist[1] = hist[2];
+        hist[2] = hist[3];
+        hist[3] = hist[4];
+        hist[4] = hist[5];
+        hist[5] = hist[6];
+        hist[6] = hist[7];
+        hist[7] = hist[8];
+        hist[8] = temp;
+    }
+}
+
+/* define our own complex math ops
+ because ARMv5 has no hardware float */
+void multiply(int ar, int aj, int br, int bj, int *cr, int *cj) {
+    *cr = ar * br - aj * bj;
+    *cj = aj * br + ar * bj;
+}
+int polar_discriminant(int ar, int aj, int br, int bj) {
+    int cr, cj;
+    double angle;
+    multiply(ar, aj, br, -bj, &cr, &cj);
+    angle = atan2((double) cj, (double) cr);
+    return (int) (angle / M_PI * (1 << 14));
+}
+int fast_atan2(int y, int x)
+/* pre scaled for int16 */
+{
+    int yabs, angle;
+    int pi4 = (1 << 12), pi34 = 3 * (1 << 12); // note pi = 1<<14
+    if (x == 0 && y == 0) {
+        return 0;
+    }
+    yabs = y;
+    if (yabs < 0) {
+        yabs = -yabs;
+    }
+    if (x >= 0) {
+        angle = pi4 - pi4 * (x - yabs) / (x + yabs);
+    } else {
+        angle = pi34 - pi4 * (x + yabs) / (yabs - x);
+    }
+    if (y < 0) {
+        return -angle;
+    }
+    return angle;
+}
+
+int polar_disc_fast(int ar, int aj, int br, int bj) {
+    int cr, cj;
+    multiply(ar, aj, br, -bj, &cr, &cj);
+    return fast_atan2(cj, cr);
+}
+
+int atan_lut_init(void) {
+    int i = 0;
+    atan_lut = (int *) malloc(atan_lut_size * sizeof(int));
+    for (i = 0; i < atan_lut_size; i++) {
+        atan_lut[i] = (int) (atan((double) i / (1 << atan_lut_coef)) / M_PI * (1 << 14));
+    }
+    return 0;
+}
+
+int polar_disc_lut(int ar, int aj, int br, int bj) {
+    int cr, cj, x, x_abs;
+    multiply(ar, aj, br, -bj, &cr, &cj);
+    /* special cases */
+    if (cr == 0 || cj == 0) {
+        if (cr == 0 && cj == 0) {
+            return 0;
+        }
+        if (cr == 0 && cj > 0) {
+            return 1 << 13;
+        }
+        if (cr == 0 && cj < 0) {
+            return -(1 << 13);
+        }
+        if (cj == 0 && cr > 0) {
+            return 0;
+        }
+        if (cj == 0 && cr < 0) {
+            return 1 << 14;
+        }
+    }
+    /* real range -32768 - 32768 use 64x range -> absolute maximum: 2097152 */
+    x = (cj << atan_lut_coef) / cr;
+    x_abs = abs(x);
+    if (x_abs >= atan_lut_size) {
+        /* we can use linear range, but it is not necessary */
+        return (cj > 0) ? 1 << 13 : -1 << 13;
+    }
+    if (x > 0) {
+        return (cj > 0) ? atan_lut[x] : atan_lut[x] - (1 << 14);
+    } else {
+        return (cj > 0) ? (1 << 14) - atan_lut[-x] : -atan_lut[-x];
+    }
+    return 0;
+}
+
+int esbensen(int ar, int aj, int br, int bj)
+/*
+ input signal: s(t) = a*exp(-i*w*t+p)
+ a = amplitude, w = angular freq, p = phase difference
+ solve w
+ s' = -i(w)*a*exp(-i*w*t+p)
+ s'*conj(s) = -i*w*a*a
+ s'*conj(s) / |s|^2 = -i*w
+ */
+{
+    int cj, dr, dj;
+    int scaled_pi = 2608; /* 1<<14 / (2*pi) */
+    dr = (br - ar) * 2;
+    dj = (bj - aj) * 2;
+    cj = bj * dr - br * dj; /* imag(ds*conj(s)) */
+    return (scaled_pi * cj / (ar * ar + aj * aj + 1));
+}
+
+void fm_demod(Demodulate *fm) {
+    int i, pcm = 0;
+    int16_t *lp = fm->lowpassed;
+    int16_t pr = fm->pre_r;
+    int16_t pj = fm->pre_j;
+    for (i = 0; i < (fm->lp_len - 1); i += 2) {
+        switch (fm->custom_atan) {
+        case 0:
+            pcm = polar_discriminant(lp[i], lp[i + 1], pr, pj);
+            break;
+        case 1:
+            pcm = polar_disc_fast(lp[i], lp[i + 1], pr, pj);
+            break;
+        case 2:
+            pcm = polar_disc_lut(lp[i], lp[i + 1], pr, pj);
+            break;
+        case 3:
+            pcm = esbensen(lp[i], lp[i + 1], pr, pj);
+            break;
+        }
+        pr = lp[i];
+        pj = lp[i + 1];
+        fm->lowpassed[i / 2] = (int16_t) pcm;
+    }
+    fm->pre_r = pr;
+    fm->pre_j = pj;
+    fm->lp_len = fm->lp_len / 2;
+}
+
+void am_demod(Demodulate *fm)
+// todo, fix this extreme laziness
+        {
+    int32_t i, pcm;
+    int16_t *lp = fm->lowpassed;
+    for (i = 0; i < fm->lp_len; i += 2) {
+// hypot uses floats but won't overflow
+//r[i/2] = (int16_t)hypot(lp[i], lp[i+1]);
+        pcm = lp[i] * lp[i];
+        pcm += lp[i + 1] * lp[i + 1];
+        lp[i / 2] = (int16_t) sqrt(pcm) * fm->output_scale;
+    }
+    fm->lp_len = fm->lp_len / 2;
+// lowpass? (3khz)
+}
+
+void usb_demod(Demodulate *fm) {
+    int i, pcm;
+    int16_t *lp = fm->lowpassed;
+    for (i = 0; i < fm->lp_len; i += 2) {
+        pcm = lp[i] + lp[i + 1];
+        lp[i / 2] = (int16_t) pcm * fm->output_scale;
+    }
+    fm->lp_len = fm->lp_len / 2;
+}
+
+void lsb_demod(Demodulate *fm) {
+    int i, pcm;
+    int16_t *lp = fm->lowpassed;
+    for (i = 0; i < fm->lp_len; i += 2) {
+        pcm = lp[i] - lp[i + 1];
+        lp[i / 2] = (int16_t) pcm * fm->output_scale;
+    }
+    fm->lp_len = fm->lp_len / 2;
+}
+
+void raw_demod(Demodulate *fm) {
+    return;
+}
+
+void deemph_filter(Demodulate *fm) {
+    static int avg; // cheating, not threadsafe
+    int i, d;
+    int16_t *lp = fm->lowpassed;
+// de-emph IIR
+// avg = avg * (1 - alpha) + sample * alpha;
+    for (i = 0; i < fm->lp_len; i++) {
+        d = lp[i] - avg;
+        if (d > 0) {
+            avg += (d + fm->deemph_a / 2) / fm->deemph_a;
+        } else {
+            avg += (d - fm->deemph_a / 2) / fm->deemph_a;
+        }
+        lp[i] = (int16_t) avg;
+    }
+}
+
+void dc_block_filter(Demodulate *fm) {
+    int i, avg;
+    int64_t sum = 0;
+    int16_t *lp = fm->lowpassed;
+    for (i = 0; i < fm->lp_len; i++) {
+        sum += lp[i];
+    }
+    avg = sum / fm->lp_len;
+    avg = (avg + fm->dc_avg * 9) / 10;
+    for (i = 0; i < fm->lp_len; i++) {
+        lp[i] -= avg;
+    }
+    fm->dc_avg = avg;
+}
+
+int mad(int16_t *samples, int len, int step)
+/* mean average deviation */
+{
+    int i = 0, sum = 0, ave = 0;
+    if (len == 0) {
+        return 0;
+    }
+    for (i = 0; i < len; i += step) {
+        sum += samples[i];
+    }
+    ave = sum / (len * step);
+    sum = 0;
+    for (i = 0; i < len; i += step) {
+        sum += abs(samples[i] - ave);
+    }
+    return sum / (len / step);
+}
+
+int rms(int16_t *samples, int len, int step)
+/* largely lifted from rtl_power */
+{
+    int i;
+    long p, t, s;
+    double dc, err;
+    p = t = 0L;
+    for (i = 0; i < len; i += step) {
+        s = (long) samples[i];
+        t += s;
+        p += s * s;
+    }
+    /* correct for dc offset in squares */
+    dc = (double) (t * step) / (double) len;
+    err = t * 2 * dc - dc * dc * len;
+    return (int) sqrt((p - err) / len);
+}
+
+int squelch_to_rms(int db, struct dongle_state *dongle, Demodulate *demod)
+/* 0 dB = 1 rms at 50dB gain and 1024 downsample */
+{
+    double linear, gain, downsample;
+    if (db == 0) {
+        return 0;
+    }
+    linear = pow(10.0, (double) db / 20.0);
+    gain = 50.0;
+//    if (dongle->gain != AUTO_GAIN) {
+//        gain = (double) (dongle->gain) / 10.0;
+//    }
+    gain = 50.0 - gain;
+    gain = pow(10.0, gain / 20.0);
+    downsample = 1024.0 / (double) demod->downsample;
+    linear = linear / gain;
+    linear = linear / downsample;
+    return (int) linear + 1;
+}
+
+Demodulate::Demodulate() {
+    rate_in = DEFAULT_SAMPLE_RATE;
+    rate_out = DEFAULT_SAMPLE_RATE;
+    squelch_level = 0;
+    conseq_squelch = 10;
+    terminate_on_squelch = 0;
+    squelch_hits = 11;
+    downsample_passes = 0;
+    comp_fir_size = 0;
+    prev_index = 0;
+    post_downsample = 1; // once this works, default = 4
+    custom_atan = 0;
+    deemph = 0;
+    rotate_enable = 0;
+    rotate.len = 0;
+    rotate.sincos = NULL;
+    rate_out2 = -1; // flag for disabled
+    mode_demod = &fm_demod;
+//            pre_j = s->pre_r = s->now_r = s->now_j = 0;
+    prev_lpr_index = 0;
+    deemph_a = 0;
+    now_lpr = 0;
+    dc_block = 1;
+    dc_avg = 0;
+    output_target = &output.results[0];
+    lowpassed = NULL;
+    /*
+     int capture_freq, capture_rate;
+     struct dongle_state *d = &dongle;
+     struct demod_state *dm = &demod;
+     struct controller_state *cs = &controller;
+     dm->downsample = (MINIMUM_RATE / dm->rate_in) + 1;
+     if (dm->downsample_passes) {
+     dm->downsample_passes = (int)log2(dm->downsample) + 1;
+     dm->downsample = 1 << dm->downsample_passes;
+     }
+     capture_freq = freq;
+     capture_rate = dm->downsample * dm->rate_in;
+     if (d->pre_rotate) {
+     capture_freq = freq + capture_rate/4;}
+     capture_freq += cs->edge * dm->rate_in / 2;
+     dm->output_scale = (1<<15) / (128 * dm->downsample);
+     if (dm->output_scale < 1) {
+     dm->output_scale = 1;}
+     if (dm->mode_demod == &fm_demod) {
+     dm->output_scale = 1;}
+     d->freq = (uint32_t)capture_freq;
+     d->rate = (uint32_t)capture_rate;
+
+     */
+
+    int r, opt;
+    int dev_given = 0;
+    int custom_ppm = 0;
+
+    output.results[0].trycond = 1;
+    output.results[0].buf = NULL;
+    output.results[1].trycond = 1;
+    output.results[1].buf = NULL;
+
+//    controller.freqs[0] = 100000000;
+//    controller.freq_len = 0;
+//    controller.edge = 0;
+//    controller.wb_mode = 0;
+
+//                if (strcmp("fm", optarg) == 0) {
+//                    demod.mode_demod = &fm_demod;
+//                }
+//                if (strcmp("raw", optarg) == 0) {
+//                    demod.mode_demod = &raw_demod;
+//                }
+//                if (strcmp("am", optarg) == 0) {
+//                    demod.mode_demod = &am_demod;
+//                }
+//                if (strcmp("usb", optarg) == 0) {
+//                    demod.mode_demod = &usb_demod;
+//                }
+//                if (strcmp("lsb", optarg) == 0) {
+//                    demod.mode_demod = &lsb_demod;
+//                }
+//                if (strcmp("wbfm", optarg) == 0) {
+//    controller.wb_mode = 1;
+    mode_demod = &fm_demod;
+    rate_in = SRATE; //170000
+    rate_out = SRATE; //170000
+    rate_out2 = 32000;
+    output.rate = 32000;
+    custom_atan = 1;
+    //demod.post_downsample = 4;
+    deemph = 1;
+    squelch_level = 0;
+//                }
+//                break;
+
+    rate_in *= post_downsample;
+    if (!output.rate) {
+        output.rate = rate_out;
+    }
+
+//    if (controller.freq_len > 1) {
+//        terminate_on_squelch = 0;
+//    }
+
+    output.padded = 0;
+}
+
+void Demodulate::demod(std::vector<int16_t> &buffer) {
+    int i, ds_p;
+    int do_squelch = 0;
+    int sr = 0;
+    if (rotate_enable) {
+        translate(this);
+    }
+    ds_p = downsample_passes;
+
+    lowpassed = &buffer[0];
+    lp_len = buffer.size();
+
+    if (ds_p) {
+        for (i = 0; i < ds_p; i++) {
+            fifth_order(lowpassed, (lp_len >> i), lp_i_hist[i]);
+            fifth_order(lowpassed + 1, (lp_len >> i) - 1, lp_q_hist[i]);
+        }
+        lp_len = lp_len >> ds_p;
+        /* droop compensation */
+        if (comp_fir_size == 9 && ds_p <= CIC_TABLE_MAX) {
+            generic_fir(lowpassed, lp_len, cic_9_tables[ds_p], droop_i_hist);
+            generic_fir(lowpassed + 1, lp_len - 1, cic_9_tables[ds_p], droop_q_hist);
+        }
+    } else {
+        low_pass(this);
+    }
+    /* power squelch */
+    if (squelch_level) {
+        sr = rms(lowpassed, lp_len, 1);
+        if (sr < squelch_level) {
+            do_squelch = 1;
+        }
+    }
+    if (do_squelch) {
+        squelch_hits++;
+        for (i = 0; i < lp_len; i++) {
+            lowpassed[i] = 0;
+        }
+    } else {
+        squelch_hits = 0;
+    }
+
+    mode_demod(this); /* lowpassed -> lowpassed */
+    if (mode_demod == &raw_demod) {
+        return;
+    }
+
+    if (dc_block) {
+        dc_block_filter(this);
+    }
+
+    /* todo, fm noise squelch */
+    // use nicer filter here too?
+    if (post_downsample > 1) {
+        lp_len = low_pass_simple(lowpassed, lp_len, post_downsample);
+    }
+    if (deemph) {
+        deemph_filter(this);
+    }
+    if (rate_out2 > 0) {
+        low_pass_real(this);
+    }
+}
+
+/*
+ static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx) {
+ int i;
+ struct dongle_state *s = ctx;
+ struct demod_state *d = s->targets[0];
+ struct demod_state *d2 = s->targets[1];
+ if (do_exit) {
+ return;
+ }
+ if (!ctx) {
+ return;
+ }
+ if (s->mute) {
+ for (i = 0; i < s->mute; i++) {
+ buf[i] = 127;
+ }
+ s->mute = 0;
+ }
+ if (s->pre_rotate) {
+ rotate_90(buf, len);
+ }
+ for (i = 0; i < (int) len; i++) {
+ s->buf16[i] = (int16_t) buf[i] - 127;
+ }
+ if (d2 != NULL) {
+ pthread_rwlock_wrlock(&d2->rw);
+ d2->lowpassed = mark_shared_buffer();
+ memcpy(d2->lowpassed, s->buf16, 2 * len);
+ d2->lp_len = len;
+ pthread_rwlock_unlock(&d2->rw);
+ safe_cond_signal(&d2->ready, &d2->ready_m);
+ }
+ pthread_rwlock_wrlock(&d->rw);
+ d->lowpassed = s->buf16;
+ d->lp_len = len;
+ pthread_rwlock_unlock(&d->rw);
+ safe_cond_signal(&d->ready, &d->ready_m);
+ s->buf16 = mark_shared_buffer();
+ }
+
+
+ static void *demod_thread_fn(void *arg) {
+ struct demod_state *d = arg;
+ struct buffer_bucket *o = d->output_target;
+ while (!do_exit) {
+ safe_cond_wait(&d->ready, &d->ready_m);
+ pthread_rwlock_wrlock(&d->rw);
+ full_demod(d);
+ pthread_rwlock_unlock(&d->rw);
+ if (d->exit_flag) {
+ do_exit = 1;
+ }
+ pthread_rwlock_wrlock(&o->rw);
+ o->buf = d->lowpassed;
+ o->len = d->lp_len;
+ pthread_rwlock_unlock(&o->rw);
+ if (controller.freq_len > 1 && d->squelch_level && d->squelch_hits > d->conseq_squelch) {
+ unmark_shared_buffer(d->lowpassed);
+ d->squelch_hits = d->conseq_squelch + 1;
+ safe_cond_signal(&controller.hop, &controller.hop_m);
+ continue;
+ }
+ safe_cond_signal(&o->ready, &o->ready_m);
+ pthread_mutex_lock(&o->trycond_m);
+ o->trycond = 0;
+ pthread_mutex_unlock(&o->trycond_m);
+ }
+ return 0;
+ }
+
+
+ #ifndef _WIN32
+ static int get_nanotime(struct timespec *ts)
+ {
+ int rv = ENOSYS;
+ #ifdef __unix__
+ rv = clock_gettime(CLOCK_MONOTONIC, ts);
+ #elif __APPLE__
+ struct timeval tv;
+ rv = gettimeofday(&tv, NULL);
+ ts->tv_sec = tv.tv_sec;
+ ts->tv_nsec = tv.tv_usec * 1000L;
+ #endif
+ return rv;
+ }
+ #endif
+ static void *output_thread_fn(void *arg) {
+ int j, r = 0;
+ struct output_state *s = arg;
+ struct buffer_bucket *b0 = &s->results[0];
+ struct buffer_bucket *b1 = &s->results[1];
+ struct timespec start_time;
+ struct timespec now_time;
+ int64_t i, duration, samples, samples_now;
+ samples = 0L;
+ #ifndef _WIN32
+ get_nanotime(&start_time);
+ #endif
+ while (!do_exit) {
+ if (s->lrmix) {
+ safe_cond_wait(&b0->ready, &b0->ready_m);
+ pthread_rwlock_rdlock(&b0->rw);
+ safe_cond_wait(&b1->ready, &b1->ready_m);
+ pthread_rwlock_rdlock(&b1->rw);
+ for (j = 0; j < b0->len; j++) {
+ fwrite(b0->buf + j, 2, 1, s->file);
+ fwrite(b1->buf + j, 2, 1, s->file);
+ }
+ unmark_shared_buffer(b1->buf);
+ pthread_rwlock_unlock(&b1->rw);
+ unmark_shared_buffer(b0->buf);
+ pthread_rwlock_unlock(&b0->rw);
+ continue;
+ }
+ if (!s->padded) {
+ safe_cond_wait(&b0->ready, &b0->ready_m);
+ pthread_rwlock_rdlock(&b0->rw);
+ fwrite(b0->buf, 2, b0->len, s->file);
+ unmark_shared_buffer(b0->buf);
+ pthread_rwlock_unlock(&b0->rw);
+ continue;
+ }
+ #ifndef _WIN32
+ // padding requires output at constant rate
+ // pthread_cond_timedwait is terrible, roll our own trycond
+ // figure out how to do this with windows HPET
+ usleep(2000);
+ pthread_mutex_lock(&b0->trycond_m);
+ r = b0->trycond;
+ b0->trycond = 1;
+ pthread_mutex_unlock(&b0->trycond_m);
+ if (r == 0) {
+ pthread_rwlock_rdlock(&b0->rw);
+ fwrite(b0->buf, 2, b0->len, s->file);
+ unmark_shared_buffer(b0->buf);
+ samples += (int64_t)b0->len;
+ pthread_rwlock_unlock(&b0->rw);
+ continue;
+ }
+ get_nanotime(&now_time);
+ duration = now_time.tv_sec - start_time.tv_sec;
+ duration *= 1000000000L;
+ duration += (now_time.tv_nsec - start_time.tv_nsec);
+ samples_now = (duration * (int64_t)s->rate) / 1000000000L;
+ if (samples_now < samples) {
+ continue;}
+ for (i=samples; i<samples_now; i++) {
+ fputc(0, s->file);
+ fputc(0, s->file);
+ }
+ samples = samples_now;
+ #endif
+ }
+ return 0;
+ }
+
+ static void optimal_settings(int freq, int rate) {
+ // giant ball of hacks
+ // seems unable to do a single pass, 2:1
+ int capture_freq, capture_rate;
+ struct dongle_state *d = &dongle;
+ struct demod_state *dm = &demod;
+ struct controller_state *cs = &controller;
+ dm->downsample = (MINIMUM_RATE / dm->rate_in) + 1;
+ if (dm->downsample_passes) {
+ dm->downsample_passes = (int) log2(dm->downsample) + 1;
+ dm->downsample = 1 << dm->downsample_passes;
+ }
+ capture_freq = freq;
+ capture_rate = dm->downsample * dm->rate_in;
+ if (d->pre_rotate) {
+ capture_freq = freq + capture_rate / 4;
+ }
+ capture_freq += cs->edge * dm->rate_in / 2;
+ dm->output_scale = (1 << 15) / (128 * dm->downsample);
+ if (dm->output_scale < 1) {
+ dm->output_scale = 1;
+ }
+ if (dm->mode_demod == &fm_demod) {
+ dm->output_scale = 1;
+ }
+ d->freq = (uint32_t) capture_freq;
+ d->rate = (uint32_t) capture_rate;
+ //d->pre_rotate = 0;
+ //demod.rotate_enable = 1;
+ //demod.rotate.angle = -0.25 * 2 * M_PI;
+ //translate_init(&demod.rotate);
+ }
+
+ void optimal_lrmix(void) {
+ double angle1, angle2;
+ uint32_t freq, freq1, freq2, bw, dongle_bw, mr;
+ if (controller.freq_len != 2) {
+ fprintf(stderr, "error: lrmix requires two frequencies\n");
+ do_exit = 1;
+ exit(1);
+ }
+ if (output.padded) {
+ fprintf(stderr, "warning: lrmix does not support padding\n");
+ }
+ freq1 = controller.freqs[0];
+ freq2 = controller.freqs[1];
+ bw = demod.rate_out;
+ freq = freq1 / 2 + freq2 / 2 + bw;
+ mr = (uint32_t) abs((int64_t) freq1 - (int64_t) freq2) + bw;
+ if (mr > MINIMUM_RATE) {
+ MINIMUM_RATE = mr;
+ }
+ dongle.pre_rotate = 0;
+ optimal_settings(freq, bw);
+ output.padded = 0;
+ clone_demod(&demod2, &demod);
+ //demod2 = demod;
+ demod2.output_target = &output.results[1];
+ dongle.targets[1] = &demod2;
+ dongle_bw = dongle.rate;
+ if (dongle_bw > MAXIMUM_RATE) {
+ fprintf(stderr, "error: unable to find optimal settings\n");
+ do_exit = 1;
+ exit(1);
+ }
+ angle1 = ((double) freq1 - (double) freq) / (double) dongle_bw;
+ demod.rotate.angle = angle1 * 2 * M_PI;
+ angle2 = ((double) freq2 - (double) freq) / (double) dongle_bw;
+ demod2.rotate.angle = angle2 * 2 * M_PI;
+ translate_init(&demod.rotate);
+ translate_init(&demod2.rotate);
+ //fprintf(stderr, "a1 %f, a2 %f\n", angle1, angle2);
+ }
+ static void *controller_thread_fn(void *arg) {
+ // thoughts for multiple dongles
+ // might be no good using a controller thread if retune/rate blocks
+ int i;
+ struct controller_state *s = arg;
+ if (s->wb_mode) {
+ for (i = 0; i < s->freq_len; i++) {
+ s->freqs[i] += 16000;
+ }
+ }
+ // set up primary channel
+ optimal_settings(s->freqs[0], demod.rate_in);
+ demod.squelch_level = squelch_to_rms(demod.squelch_level, &dongle, &demod);
+ if (dongle.direct_sampling) {
+ verbose_direct_sampling(dongle.dev, dongle.direct_sampling);
+ }
+ if (dongle.offset_tuning) {
+ verbose_offset_tuning(dongle.dev);
+ }
+ // set up lrmix
+ if (output.lrmix) {
+ optimal_lrmix();
+ }
+ // Set the frequency
+ verbose_set_frequency(dongle.dev, dongle.freq);
+ fprintf(stderr, "Oversampling input by: %ix.\n", demod.downsample);
+ fprintf(stderr, "Oversampling output by: %ix.\n", demod.post_downsample);
+ fprintf(stderr, "Buffer size: %0.2fms\n", 1000 * 0.5 * (float) ACTUAL_BUF_LENGTH / (float) dongle.rate);
+ // Set the sample rate
+ verbose_set_sample_rate(dongle.dev, dongle.rate);
+ fprintf(stderr, "Output at %u Hz.\n", demod.rate_in / demod.post_downsample);
+ while (!do_exit) {
+ safe_cond_wait(&s->hop, &s->hop_m);
+ if (s->freq_len <= 1) {
+ continue;
+ }
+ if (output.lrmix) {
+ continue;
+ }
+ // hacky hopping
+ s->freq_now = (s->freq_now + 1) % s->freq_len;
+ optimal_settings(s->freqs[s->freq_now], demod.rate_in);
+ rtlsdr_set_center_freq(dongle.dev, dongle.freq);
+ dongle.mute = BUFFER_DUMP;
+ }
+ return 0;
+ }
+
+ int main(int argc, char **argv) {
+
+ }*/

src/Demodulate.h

@@ -0,0 +1,85 @@
+#pragma once
+
+/*
+ * based on rtl_fm.c
+ * https://github.com/keenerd/rtl-sdr/blob/master/src/rtl_fm.c
+ */
+
+#include <vector>
+#include <stdint.h>
+#include <stddef.h>
+
+#define FREQUENCIES_LIMIT   1000
+
+struct translate_state {
+    double angle; /* radians */
+    int16_t *sincos; /* pairs */
+    int len;
+    int i;
+};
+
+struct buffer_bucket {
+    int16_t *buf;
+    int len;
+    int trycond;
+};
+
+struct output_state {
+    int exit_flag;
+    struct buffer_bucket results[2];
+    int rate;
+    int wav_format;
+    int padded;
+    int lrmix;
+};
+
+struct controller_state {
+    int exit_flag;
+    uint32_t freqs[FREQUENCIES_LIMIT];
+    int freq_len;
+    int freq_now;
+    int edge;
+    int wb_mode;
+};
+
+class Demodulate {
+
+public:
+    Demodulate();
+
+    ~Demodulate() {
+
+    }
+
+    void demod(std::vector<int16_t> &buffer);
+
+public:
+    int16_t *lowpassed;
+    int lp_len;
+    int16_t lp_i_hist[10][6];
+    int16_t lp_q_hist[10][6];
+    int16_t droop_i_hist[9];
+    int16_t droop_q_hist[9];
+    int rate_in;
+    int rate_out;
+    int rate_out2;
+    int now_r, now_j;
+    int pre_r, pre_j;
+    int prev_index;
+    int downsample; /* min 1, max 256 */
+    int post_downsample;
+    int output_scale;
+    int squelch_level, conseq_squelch, squelch_hits, terminate_on_squelch;
+    int downsample_passes;
+    int comp_fir_size;
+    int custom_atan;
+    int deemph, deemph_a;
+    int now_lpr;
+    int prev_lpr_index;
+    int dc_block, dc_avg;
+    int rotate_enable;
+    struct translate_state rotate;
+    void (*mode_demod)(Demodulate *);
+    struct buffer_bucket *output_target;
+    struct output_state output;
+};

src/PrimaryGLContext.cpp

@@ -14,6 +14,7 @@
 #include "CubicSDRDefs.h"
 #include "AppFrame.h"
 #include <algorithm>
+#include "Demodulate.h"
 
 wxString glGetwxString(GLenum name) {
     const GLubyte *v = glGetString(name);

src/SDRThreadTask.h

@@ -10,10 +10,10 @@ public:
     };
 
     SDRThreadTask() :
-            m_cmd(SDR_THREAD_NULL) {
+            m_cmd(SDR_THREAD_NULL), arg_int(0) {
     }
     SDRThreadTask(SDR_COMMAND cmd) :
-            m_cmd(cmd) {
+            arg_int(0), m_cmd(cmd) {
     }
 
     void setUInt(unsigned int i);