First draft demodulator, refactored out the filters, started adding tests

2024-10-14 16:32:56 -04:00
parent 60d6f6db7d
commit 010e479f89
10 changed files with 369 additions and 322 deletions
@@ -0,0 +1,108 @@
+#include <complex>
+#include <cmath>
+#include <vector>
+#include <iostream>
+
+#include "filters.h"
+
+class PhaseDetector {
+public:
+    PhaseDetector(const std::vector<std::complex<double>>& _symbolMap) : symbolMap(_symbolMap) {}
+
+    uint8_t getSymbol(const std::complex<double>& input) {
+        double phase = std::atan2(input.imag(), input.real());
+        return symbolFromPhase(phase);
+    }
+
+private:
+    std::vector<std::complex<double>> symbolMap;
+
+    uint8_t symbolFromPhase(const double phase) {
+        // Calculate the closest symbol based on phase difference
+        double min_distance = 2 * M_PI; // Maximum possible phase difference
+        uint8_t closest_symbol = 0;
+
+        for (uint8_t i = 0; i < symbolMap.size(); ++i) {
+            double symbol_phase = std::atan2(symbolMap[i].imag(), symbolMap[i].real());
+            double distance = std::abs(symbol_phase - phase);
+
+            if (distance < min_distance) {
+                min_distance = distance;
+                closest_symbol = i;
+            }
+        }
+
+        return closest_symbol;
+    }
+};
+
+class CostasLoop {
+public:
+    CostasLoop(const double _sample_rate, const std::vector<std::complex<double>>& _symbolMap) 
+        : sample_rate(_sample_rate), k_factor(-5 / _sample_rate),
+          prev_in_iir(0), prev_out_iir(0), prev_in_vco(0), feedback(1.0, 0.0),
+          error_total(0), out_iir_total(0), in_vco_total(0),
+          srrc_filter(SRRCFilter(48, _sample_rate, 2400, 0.35)) {}
+
+    std::vector<std::complex<double>> process(const std::vector<double>& input_signal) {
+        std::vector<std::complex<double>> output_signal(input_signal.size());
+        double current_phase = 0.0;
+        
+        error_total = 0;
+        out_iir_total = 0;
+        in_vco_total = 0;
+
+        for (size_t i = 0; i < input_signal.size(); ++i) {
+            // Multiply input by feedback signal
+            std::complex<double> multiplied = input_signal[i] * feedback;
+
+            // Filter signal
+            std::complex<double> filtered = srrc_filter.filterSample(multiplied);
+
+            // Output best-guess corrected I/Q components
+            output_signal[i] = filtered;
+
+            // Generate limited components
+            std::complex<double> limited = limiter(filtered);
+            
+            // IIR Filter
+            double in_iir = std::asin(std::clamp(multiplied.imag() * limited.real() - multiplied.real() * limited.imag(), -1.0, 1.0));
+            error_total += in_iir;
+
+            double out_iir = 1.0001 * in_iir - prev_in_iir + prev_out_iir;
+            prev_in_iir = in_iir;
+            prev_out_iir = out_iir;
+            out_iir_total += out_iir;
+
+            // VCO Block
+            double in_vco = out_iir + prev_in_vco;
+            in_vco_total += in_vco;
+            prev_in_vco = in_vco;
+
+            // Generate feedback signal for next iteration
+            double feedback_real = std::cos(k_factor * in_vco);
+            double feedback_imag = -std::sin(k_factor * in_vco);
+            feedback = std::complex<double>(feedback_real, feedback_imag);
+        }
+
+        return output_signal;
+    }
+
+private:
+    double sample_rate;
+    double k_factor;
+    double prev_in_iir;
+    double prev_out_iir;
+    double prev_in_vco;
+    std::complex<double> feedback;
+    double error_total;
+    double out_iir_total;
+    double in_vco_total;
+    SRRCFilter srrc_filter;
+
+    std::complex<double> limiter(const std::complex<double>& sample) const {
+        double limited_I = std::clamp(sample.real(), -1.0, 1.0);
+        double limited_Q = std::clamp(sample.imag(), -1.0, 1.0);
+        return std::complex<double>(limited_I, limited_Q);
+    }
+};
@@ -0,0 +1,167 @@
+#ifndef FILTERS_H
+#define FILTERS_H
+
+#include <cmath>
+#include <cstdint>
+#include <fftw3.h>
+#include <numeric>
+#include <vector>
+
+class TapGenerators {
+    public:
+    std::vector<double> generateSRRCTaps(const size_t num_taps, const double sample_rate, const double symbol_rate, const double rolloff) const {
+        std::vector<double> freq_response(num_taps, 0.0);
+        std::vector<double> taps(num_taps);
+
+        double fn = symbol_rate / 2.0;
+        double f_step = sample_rate / num_taps;
+
+        for (size_t i = 0; i < num_taps / 2; i++) {
+            double f = i * f_step;
+
+            if (f <= fn * (1 - rolloff)) {
+                freq_response[i] = 1.0;
+            } else if (f <= fn * (1 + rolloff)) {
+                freq_response[i] = 0.5 * (1 - std::sin(M_PI * (f - fn * (1 - rolloff)) / (2 * rolloff * fn)));
+            } else {
+                freq_response[i] = 0.0;
+            }
+        }
+
+        for (size_t i = num_taps / 2; i < num_taps; i++) {
+            freq_response[i] = freq_response[num_taps - i - 1];
+        }
+
+        fftw_complex* freq_domain = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * num_taps);
+        for (size_t i = 0; i < num_taps; i++) {
+            freq_domain[i][0] = freq_response[i];
+            freq_domain[i][1] = 0.0;
+        }
+
+        std::vector<double> time_domain_taps(num_taps, 0.0);
+
+        fftw_plan plan = fftw_plan_dft_c2r_1d(num_taps, freq_domain, time_domain_taps.data(), FFTW_ESTIMATE);
+        fftw_execute(plan);
+        fftw_destroy_plan(plan);
+        fftw_free(freq_domain);
+
+        double norm_factor = std::sqrt(std::accumulate(time_domain_taps.begin(), time_domain_taps.end(), 0.0, [](double sum, double val) { return sum + val * val; }));
+        for (auto& tap : time_domain_taps) {
+            tap /= norm_factor;
+        }
+
+        return time_domain_taps;
+    }
+
+    std::vector<double> generateLowpassTaps(const size_t num_taps, const double cutoff_freq, const double sample_rate) const {
+        std::vector<double> freq_response(num_taps, 0.0);
+        std::vector<double> taps(num_taps);
+
+        double fn = cutoff_freq / 2.0;
+        double f_step = sample_rate / num_taps;
+
+        // Define frequency response
+        for (size_t i = 0; i < num_taps / 2; i++) {
+            double f = i * f_step;
+            if (f <= fn) {
+                freq_response[i] = 1.0;  // Passband
+            } else {
+                freq_response[i] = 0.0;  // Stopband
+            }
+        }
+
+        // Mirror the second half of the response for symmetry
+        for (size_t i = num_taps / 2; i < num_taps; i++) {
+            freq_response[i] = freq_response[num_taps - i - 1];
+        }
+
+        // Perform inverse FFT to get time-domain taps
+        fftw_complex* freq_domain = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * num_taps);
+        for (size_t i = 0; i < num_taps; i++) {
+            freq_domain[i][0] = freq_response[i];
+            freq_domain[i][1] = 0.0;
+        }
+
+        std::vector<double> time_domain_taps(num_taps, 0.0);
+        fftw_plan plan = fftw_plan_dft_c2r_1d(num_taps, freq_domain, time_domain_taps.data(), FFTW_ESTIMATE);
+        fftw_execute(plan);
+        fftw_destroy_plan(plan);
+        fftw_free(freq_domain);
+
+        // Normalize filter taps
+        double norm_factor = std::sqrt(std::accumulate(time_domain_taps.begin(), time_domain_taps.end(), 0.0, [](double sum, double val) { return sum + val * val; }));
+        for (auto& tap : time_domain_taps) {
+            tap /= norm_factor;
+        }
+
+        return time_domain_taps;
+    }
+};
+
+class Filter {
+    public:
+    Filter(const std::vector<double>& _filter_taps) : filter_taps(_filter_taps), buffer(_filter_taps.size(), 0.0), buffer_index(0) {}
+
+    double filterSample(const double sample) {
+        buffer[buffer_index] = std::complex<double>(sample,0.0);
+        double filtered_val = 0.0;
+        for (size_t j = 0; j < filter_taps.size(); j++) {
+            size_t signal_index = (buffer_index + j) % filter_taps.size();
+            filtered_val += filter_taps[j] * buffer[signal_index].real();
+        }
+
+        buffer_index = (buffer_index + 1) % filter_taps.size();
+        return filtered_val;
+    }
+
+    std::complex<double> filterSample(const std::complex<double>& sample) {
+        buffer[buffer_index] = sample;
+        std::complex<double> filtered_val = std::complex<double>(0.0, 0.0);
+        for (size_t j = 0; j < filter_taps.size(); j++) {
+            size_t signal_index = (buffer_index + j) % filter_taps.size();
+            filtered_val += filter_taps[j] * buffer[signal_index];
+        }
+
+        buffer_index = (buffer_index + 1) % filter_taps.size();
+        return filtered_val;
+    }
+
+    std::vector<double> applyFilter(const std::vector<double>& signal) {
+        std::vector<double> filtered_signal(signal.size(), 0.0);
+
+        // Convolve the signal with the filter taps
+        for (size_t i = 0; i < signal.size(); ++i) {
+            filtered_signal[i] = filterSample(signal[i]);
+        }
+
+        return filtered_signal;
+    }
+
+    std::vector<std::complex<double>> applyFilter(const std::vector<std::complex<double>>& signal) {
+        std::vector<std::complex<double>> filtered_signal(signal.size(), std::complex<double>(0.0, 0.0));
+
+        // Convolve the signal with the filter taps
+        for (size_t i = 0; i < signal.size(); ++i) {
+            filtered_signal[i] = filterSample(signal[i]);
+        }
+
+        return filtered_signal;
+    }
+
+    private:
+    std::vector<double> filter_taps;
+    std::vector<std::complex<double>> buffer;
+    size_t buffer_index;
+};
+
+class SRRCFilter : public Filter {
+    public:
+    SRRCFilter(const size_t num_taps, const double sample_rate, const double symbol_rate, const double rolloff) : Filter(TapGenerators().generateSRRCTaps(num_taps, sample_rate, symbol_rate, rolloff)) {}
+};
+
+class LowPassFilter : public Filter {
+    public:
+    LowPassFilter(const size_t num_taps, const double cutoff_freq, const double sample_rate) : Filter(TapGenerators().generateLowpassTaps(num_taps, cutoff_freq, sample_rate)) {}
+};
+
+#endif /* FILTERS_H */