src/baseband.c

   1 /**
   2  * Baseband
   3  *
   4  * Various functions for baseband sample processing
   5  *
   6  * Copyright (C) 2012 by Benjamin Larsson <benjamin@southpole.se>
   7  * Copyright (C) 2015 Tommy Vestermark
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation; either version 2 of the License, or
  12  * (at your option) any later version.
  13  */
  14
  15 #include "baseband.h"
  16 #include "util.h"
  17 #include <stdio.h>
  18 #include <stdlib.h>
  19 #include <string.h>
  20 #include <math.h>
  21
  22
  23 static uint16_t scaled_squares[256];
  24
  25 /* precalculate lookup table for envelope detection */
  26 static void calc_squares() {
  27     int i;
  28     for (i = 0; i < 256; i++)
  29         scaled_squares[i] = (127 - i) * (127 - i);
  30 }
  31
  32 /** This will give a noisy envelope of OOK/ASK signals
  33  *  Subtract the bias (-128) and get an envelope estimation
  34  *  The output will be written in the input buffer
  35  *  @returns   pointer to the input buffer
  36  */
  37 void envelope_detect(const uint8_t *iq_buf, uint16_t *y_buf, uint32_t len) {
  38     unsigned int i;
  39     for (i = 0; i < len; i++) {
  40         y_buf[i] = scaled_squares[iq_buf[2 * i ]] + scaled_squares[iq_buf[2 * i + 1]];
  41     }
  42 }
  43
  44
  45 /** Something that might look like a IIR lowpass filter
  46  *
  47  *  [b,a] = butter(1, Wc) # low pass filter with cutoff pi*Wc radians
  48  *  Q1.15*Q15.0 = Q16.15
  49  *  Q16.15>>1 = Q15.14
  50  *  Q15.14 + Q15.14 + Q15.14 could possibly overflow to 17.14
  51  *  but the b coeffs are small so it wont happen
  52  *  Q15.14>>14 = Q15.0 \o/
  53  */
  54 #define F_SCALE 15
  55 #define S_CONST (1<<F_SCALE)
  56 #define FIX(x) ((int)(x*S_CONST))
  57
  58 ///  [b,a] = butter(1, 0.01) -> 3x tau (95%) ~100 samples
  59 //static int a[FILTER_ORDER + 1] = {FIX(1.00000), FIX(0.96907)};
  60 //static int b[FILTER_ORDER + 1] = {FIX(0.015466), FIX(0.015466)};
  61 ///  [b,a] = butter(1, 0.05) -> 3x tau (95%) ~20 samples
  62 static int a[FILTER_ORDER + 1] = {FIX(1.00000), FIX(0.85408)};
  63 static int b[FILTER_ORDER + 1] = {FIX(0.07296), FIX(0.07296)};
  64
  65
  66 void baseband_low_pass_filter(const uint16_t *x_buf, int16_t *y_buf, uint32_t len, FilterState *state) {
  67     unsigned int i;
  68     // Fixme: Will Segmentation Fault if len < FILTERORDER
  69
  70     /* Calculate first sample */
  71     y_buf[0] = ((a[1] * state->y[0] >> 1) + (b[0] * x_buf[0] >> 1) + (b[1] * state->x[0] >> 1)) >> (F_SCALE - 1);
  72     for (i = 1; i < len; i++) {
  73         y_buf[i] = ((a[1] * y_buf[i - 1] >> 1) + (b[0] * x_buf[i] >> 1) + (b[1] * x_buf[i - 1] >> 1)) >> (F_SCALE - 1);
  74     }
  75
  76     /* Save last samples */
  77     memcpy(state->x, &x_buf[len - FILTER_ORDER], FILTER_ORDER * sizeof (int16_t));
  78     memcpy(state->y, &y_buf[len - FILTER_ORDER], FILTER_ORDER * sizeof (int16_t));
  79     //fprintf(stderr, "%d\n", y_buf[0]);
  80 }
  81
  82
  83 /// Integer implementation of atan2() with int16_t normalized output
  84 ///
  85 /// Returns arc tangent of y/x across all quadrants in radians
  86 /// Reference: http://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
  87 /// @param y: Numerator (imaginary value of complex vector)
  88 /// @param x: Denominator (real value of complex vector)
  89 /// @return angle in radians (Pi equals INT16_MAX)
  90 int16_t atan2_int16(int16_t y, int16_t x) {
  91         static const int32_t I_PI_4 = INT16_MAX/4;              // M_PI/4
  92         static const int32_t I_3_PI_4 = 3*INT16_MAX/4;  // 3*M_PI/4
  93         const int32_t abs_y = abs(y);
  94         int32_t r, angle;
  95
  96         if (x >= 0) {   // Quadrant I and IV
  97                 int32_t denom = (abs_y + x);
  98                 if (denom == 0) denom = 1;      // Prevent divide by zero
  99                 r = ((x - abs_y) << 16) / denom;
 100                 angle = I_PI_4;
 101         } else {                // Quadrant II and III
 102                 int32_t denom = (abs_y - x);
 103                 if (denom == 0) denom = 1;      // Prevent divide by zero
 104                 r = ((x + abs_y) << 16) / denom;
 105                 angle = I_3_PI_4;
 106         }
 107         angle -= (I_PI_4 * r) >> 16;    // Error max 0.07 radians
 108         if (y < 0) angle = -angle;      // Negate if in III or IV
 109         return angle;
 110 }
 111
 112
 113 ///  [b,a] = butter(1, 0.1) -> 3x tau (95%) ~10 samples
 114 //static int alp[2] = {FIX(1.00000), FIX(0.72654)};
 115 //static int blp[2] = {FIX(0.13673), FIX(0.13673)};
 116 ///  [b,a] = butter(1, 0.2) -> 3x tau (95%) ~5 samples
 117 static int alp[2] = {FIX(1.00000), FIX(0.50953)};
 118 static int blp[2] = {FIX(0.24524), FIX(0.24524)};
 119
 120 void baseband_demod_FM(const uint8_t *x_buf, int16_t *y_buf, unsigned num_samples, DemodFM_State *state) {
 121         int16_t ar, ai;         // New IQ sample: x[n]
 122         int16_t br, bi;         // Old IQ sample: x[n-1]
 123         int32_t pr, pi;         // Phase difference vector
 124         int16_t angle;          // Phase difference angle
 125         int16_t xlp, ylp, xlp_old, ylp_old;     // Low Pass filter variables
 126
 127         // Pre-feed old sample
 128         ar = state->br; ai = state->bi;
 129         xlp_old = state->xlp; ylp_old = state->ylp;
 130
 131         for (unsigned n = 0; n < num_samples; n++) {
 132                 // delay old sample
 133                 br = ar;
 134                 bi = ai;
 135                 // get new sample
 136                 ar = x_buf[2*n]-128;
 137                 ai = x_buf[2*n+1]-128;
 138                 // Calculate phase difference vector: x[n] * conj(x[n-1])
 139                 pr = ar*br+ai*bi;       // May exactly overflow an int16_t (-128*-128 + -128*-128)
 140                 pi = ai*br-ar*bi;
 141 //              xlp = (int16_t)((atan2f(pi, pr) / M_PI) * INT16_MAX);   // Floating point implementation
 142                 xlp = atan2_int16(pi, pr);      // Integer implementation
 143 //              xlp = pi;                                       // Cheat and use only imaginary part (works OK, but is amplitude sensitive)
 144                 // Low pass filter
 145                 ylp = ((alp[1] * ylp_old >> 1) + (blp[0] * xlp >> 1) + (blp[1] * xlp_old >> 1)) >> (F_SCALE - 1);
 146                 ylp_old = ylp; xlp_old = xlp;
 147                 y_buf[n] = ylp;
 148         }
 149
 150         // Store newest sample for next run
 151         state->br = ar; state->bi = ai;
 152         state->xlp = xlp_old; state->ylp = ylp_old;
 153 }
 154
 155
 156 void baseband_init(void) {
 157         calc_squares();
 158 }
 159
 160
 161 static FILE *dumpfile = NULL;
 162
 163 void baseband_dumpfile(const uint8_t *buf, uint32_t len) {
 164         if (dumpfile == NULL) {
 165                 dumpfile = fopen("dumpfile.dat", "wb");
 166         }
 167
 168         if (dumpfile == NULL) {
 169                 fprintf(stderr, "Error: could not open dumpfile.dat\n");
 170         } else {
 171                 fwrite(buf, 1, len, dumpfile);
 172                 fflush(dumpfile);               // Flush as file is not closed cleanly...
 173         }
 174 }