Some fixes.

[bertos.git] / bertos / net / afsk.c

diff --git a/bertos/net/afsk.c b/bertos/net/afsk.c
index 4ebf54540c383c55b529b48e8bf17e31813d239a..d2291262858cefe7d7d68f3a395ac01b8a85792d 100644 (file)
--- a/bertos/net/afsk.c
+++ b/bertos/net/afsk.c
@@ -151,6 +151,7 @@ static uint8_t tx_buf[CONFIG_AFSK_TX_BUFLEN];
 
 static int16_t iir_x[2];
 static int16_t iir_y[2];
+
 static uint8_t sampled_bits;
 static uint8_t found_bits;
 static uint8_t demod_bits;
@@ -241,23 +242,44 @@ DEFINE_AFSK_ADC_ISR()
 
        /*
         * Frequency discrimination is achieved by simply multiplying
-        * the sample with a delayed sample of (bits per sample) / 2.
-        * Then the signal is lowpass filtered with a first order, 600 Hz
-        * Butterworth filter.
+        * the sample with a delayed sample of (samples per bit) / 2.
+        * Then the signal is lowpass filtered with a first order,
+        * 600 Hz filter. The filter implementation is selectable
+        * through the CONFIG_AFSK_FILTER config variable.
         */
 
        iir_x[0] = iir_x[1];
-       iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
+
+       #if (CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH)
+               iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
+               //iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) / 6.027339492;
+       #elif (CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV)
+               iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
+               //iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) / 3.558147322;
+       #else
+               #error Filter type not found!
+       #endif
+
        iir_y[0] = iir_y[1];
 
-       /*
-        * This strange sum + shift is an optimization for iir_y[0] * 0.668.
-        * iir * 0.668 ~= (iir * 21) / 32 =
-        * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
-        * = iir / 2 + iir / 8 + iir / 32 =
-        * = iir >> 1 + iir >> 3 + iir >> 5
-        */
-       iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0] >> 3) + (iir_y[0] >> 5);
+       #if CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH
+               /*
+                * This strange sum + shift is an optimization for iir_y[0] * 0.668.
+                * iir * 0.668 ~= (iir * 21) / 32 =
+                * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
+                * = iir / 2 + iir / 8 + iir / 32 =
+                * = iir >> 1 + iir >> 3 + iir >> 5
+                */
+               iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0] >> 3) + (iir_y[0] >> 5);
+               //iir_y[1] = iir_x[0] + iir_x[1] + iir_y[0] * 0.6681786379;
+       #elif CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV
+               /*
+                * This should be (iir_y[0] * 0.438) but
+                * (iir_y[0] >> 1) is a faster approximation :-)
+                */
+               iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1);
+               //iir_y[1] = iir_x[0] + iir_x[1] + iir_y[0] * 0.4379097269;
+       #endif
 
        /* Save this sampled bit in a delay line */
        sampled_bits <<= 1;
@@ -285,10 +307,17 @@ DEFINE_AFSK_ADC_ISR()
                found_bits <<= 1;
 
                /*
-                * TODO: maybe a better algorithm to find the sample bit
-                * other than reading the last one.
+                * Determine bit value by reading the last 3 sampled bits.
+                * If the number of ones is two or greater, the bit value is a 1,
+                * otherwise is a 0.
                 */
-               found_bits |= sampled_bits & 1;
+               uint8_t bits = sampled_bits & 0x07;
+               if (bits == 0x07 // 111, 3 bits set to 1
+                || bits == 0x06 // 110, 2 bits
+                || bits == 0x05 // 101, 2 bits
+                || bits == 0x03 // 011, 2 bits
+               )
+                       found_bits |= 1;
 
                /*
                 * NRZI coding: if 2 consecutive bits have the same value