Fix CFO problems and Hamming debugging

lib/decoder_impl.cc

@@ -61,13 +61,12 @@ namespace gr {
         d_bits_per_symbol = (uint32_t)(d_bits_per_second / d_symbols_per_second);
         d_samples_per_symbol = (uint32_t)(d_samples_per_second / d_symbols_per_second);
         d_delay_after_sync = d_samples_per_symbol / 4; //262;
-        d_delay_after_sync += (uint32_t)d_delay_after_sync * 2.4f / 100.0f;
+        //d_delay_after_sync += (uint32_t)d_delay_after_sync * 2.4f / 100.0f;
         d_corr_decim_factor = 8; // samples_per_symbol / corr_decim_factor = correlation window. Also serves as preamble decimation factor
         d_number_of_bins = (uint32_t)pow(2, d_sf);
         d_number_of_bins_hdr = d_number_of_bins / 4;
         d_payload_symbols = 0;
         d_cfo_estimation = 0.0f;
-        d_cfo_step = 0;
         d_dt = 1.0f / d_samples_per_second;
 
         // Some preparations
@@ -372,8 +371,8 @@ namespace gr {
     }
 
     bool decoder_impl::demodulate(gr_complex* samples, bool is_header) {
-        //unsigned int bin_idx = max_frequency_gradient_idx(samples);
+        unsigned int bin_idx = max_frequency_gradient_idx(samples);
-        unsigned int bin_idx = get_shift_fft(samples);
+        //unsigned int bin_idx = get_shift_fft(samples);
         //unsigned int bin_idx_test = get_shift_fft(samples);
         unsigned int bin_idx_test = 0;
 
@@ -515,13 +514,20 @@ namespace gr {
         unsigned int n = ceil(d_words_dewhitened.size() * 4.0f / (4.0f + d_cr));
         fec_scheme fs = LIQUID_FEC_HAMMING84;
 
-        if(d_cr == 4)
+        if(d_cr == 4) {
             fs = LIQUID_FEC_HAMMING84;
-        else if(d_cr == 3)
+            /*uint32_t j = 0;
+            for(uint32_t i = 1; i < d_words_dewhitened.size(); i+=2) { // TODO: Fix me when uneven number of symbols
+                out_data[j] = (hamming_decode_soft(d_words_dewhitened[i-1]) << 4) | hamming_decode_soft(d_words_dewhitened[i]);
+                j++;
+            }
+            d_words_dewhitened.clear();
+            return;*/
+        } else if(d_cr == 3) {
             fs = LIQUID_FEC_HAMMING74;
-        else if(d_cr == 2)
+        } else if(d_cr == 2) {
             fs = LIQUID_FEC_HAMMING128;
-        else if(d_cr == 1) { // TODO: Temporary, since Liquid doesn't support 4/5 Hamming so we need to implement it ourselves / extend Liquid.
+        } else if(d_cr == 1) { // TODO: Temporary, since Liquid doesn't support 4/5 Hamming so we need to implement it ourselves / extend Liquid.
             uint8_t indices[4] = {1, 2, 3, 5};
             fec_extract_data_only(&d_words_dewhitened[0], d_words_dewhitened.size(), indices, 4, out_data);
             d_words_dewhitened.clear();
@@ -572,8 +578,7 @@ namespace gr {
 
     void decoder_impl::correct_cfo(gr_complex* samples, int num_samples) {
         for(uint32_t i = 0; i < num_samples; i++) {
-            samples[i] = samples[i] * gr_expj(2.0f * M_PI * -d_cfo_estimation * (d_dt * d_cfo_step));
+            samples[i] = samples[i] * gr_expj(2.0f * M_PI * -d_cfo_estimation * (d_dt * i));
-            d_cfo_step += 1;
         }
     }
 

lib/utilities.h

@@ -60,6 +60,98 @@ namespace gr {
             }
         }
 
+
+        uint8_t pow2[8] = {
+            0x01,
+            0x02,
+            0x04,
+            0x08,
+            0x10,
+            0x20,
+            0x40,
+            0x80
+        };
+
+        uint8_t bit(uint8_t v, uint8_t i) {
+            return ((v >> i) & 0x01);
+        }
+
+        uint8_t pack_byte(uint8_t a, uint8_t b, uint8_t c, uint8_t d, uint8_t e, uint8_t f, uint8_t g, uint8_t h) {
+            return a | (b << 1) | (c << 2) | (d << 3) | (e << 4) | (f << 5) | (g << 6) | (h << 7);
+        }
+
+        uint8_t pack_nibble(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
+            return a | (b << 1) | (c << 2) | (d << 3);
+        }
+
+        uint8_t hamming_encode_soft(uint8_t v) {
+            uint8_t p1 = bit(v, 1) ^ bit(v, 2) ^ bit(v, 3);
+            uint8_t p2 = bit(v, 0) ^ bit(v, 1) ^ bit(v, 2);
+            uint8_t p3 = bit(v, 0) ^ bit(v, 1) ^ bit(v, 3);
+            uint8_t p4 = bit(v, 0) ^ bit(v, 2) ^ bit(v, 3);
+
+            return pack_byte(p1, bit(v, 0), bit(v, 1), bit(v, 2), p2, bit(v, 3), p3, p4);
+        }
+
+        uint8_t hamming_decode_soft(uint8_t v) {
+            // Precalculation
+            // Which bits are covered (including self)?
+            // p1 10110100
+            // p2 01111000
+            // p3 01100110
+            // p4 01010101
+
+            // Syndrome matrix = columns of "cover bits" above
+            uint8_t H[16];
+
+            for(uint8_t i = 0; i < 16; i++) {
+                H[i] = 0;
+            }
+
+            uint8_t i0 = pack_nibble(1, 0, 0, 0);
+            uint8_t i1 = pack_nibble(0, 1, 1, 1);
+            uint8_t i2 = pack_nibble(1, 1, 1, 0);
+            uint8_t i3 = pack_nibble(1, 1, 0, 1);
+            uint8_t i4 = pack_nibble(0, 1, 0, 0);
+            uint8_t i5 = pack_nibble(1, 0, 1, 1);
+            uint8_t i6 = pack_nibble(0, 0, 1, 0);
+            uint8_t i7 = pack_nibble(0, 0, 0, 1);
+
+            H[i0] = 0;
+            H[i1] = 1;
+            H[i2] = 2;
+            H[i3] = 3;
+            H[i4] = 4;
+            H[i5] = 5;
+            H[i6] = 6;
+            H[i7] = 7;
+
+            // Decode
+            // Bit positions for data bits in codeword
+            uint8_t p1 = bit(v, 0);
+            uint8_t p2 = bit(v, 4);
+            uint8_t p3 = bit(v, 6);
+            uint8_t p4 = bit(v, 7);
+            uint8_t p1c = bit(v, 2) ^ bit(v, 3) ^ bit(v, 5);
+            uint8_t p2c = bit(v, 1) ^ bit(v, 2) ^ bit(v, 3);
+            uint8_t p3c = bit(v, 1) ^ bit(v, 2) ^ bit(v, 5);
+            uint8_t p4c = bit(v, 1) ^ bit(v, 3) ^ bit(v, 5);
+
+            uint8_t syndrome = pack_nibble((uint8_t)(p1 != p1c), (uint8_t)(p2 != p2c), (uint8_t)(p3 != p3c), (uint8_t)(p4 != p4c));
+
+            if(syndrome != 0) {
+                uint8_t index = H[syndrome];
+                v = v ^ pow2[index];
+            }
+
+            uint8_t d1 = bit(v, 1);
+            uint8_t d2 = bit(v, 2);
+            uint8_t d3 = bit(v, 3);
+            uint8_t d4 = bit(v, 5);
+
+            return pack_nibble(d1, d2, d3, d4);
+        }
+
   }
 }