RecklessAndFeckless/MIL-STD-188-110C
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
RecklessAndFeckless
2024-10-14 16:32:56 -04:00
parent 60d6f6db7d
commit 010e479f89
10 changed files with 369 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/modulation/FSKDemodulator.h
-131
View File
@@ -1,131 +0,0 @@
#ifndef FSK_DEMODULATOR_H
#define FSK_DEMODULATOR_H
#include <cmath>
#include <cstdint>
#include <functional>
#include <memory>
#include <vector>
#include "BitStreamWriter.h"
class FSKDemodulatorConfig {
public:
int freq_lo;
int freq_hi;
int sample_rate;
int baud_rate;
std::shared_ptr<BitStreamWriter> bitstreamwriter;
};
namespace milstd {
class FSKDemodulator {
public:
FSKDemodulator(const FSKDemodulatorConfig& s) : freq_lo(s.freq_lo), freq_hi(s.freq_hi), sample_rate(s.sample_rate), baud_rate(s.baud_rate), bit_writer(s.bitstreamwriter) {
initialize();
}
void demodulate(const std::vector<int16_t>& samples) {
size_t nb = samples.size();
for (size_t i = 0; i < nb; i++) {
filter_buf[buf_ptr++] = samples[i];
if (buf_ptr == filter_buf.size()) {
std::copy(filter_buf.begin() + filter_buf.size() - filter_size, filter_buf.end(), filter_buf.begin());
buf_ptr = filter_size;
}
int corr;
int sum = 0;
corr = dotProduct(&filter_buf[buf_ptr - filter_size], filter_hi_i.data(), filter_size);
sum += corr * corr;
corr = dotProduct(&filter_buf[buf_ptr - filter_size], filter_hi_q.data(), filter_size);
sum += corr * corr;
corr = dotProduct(&filter_buf[buf_ptr - filter_size], filter_lo_i.data(), filter_size);
sum -= corr * corr;
corr = dotProduct(&filter_buf[buf_ptr - filter_size], filter_lo_q.data(), filter_size);
sum -= corr * corr;
int new_sample = (sum > 0) ? 1 : 0;
if (last_sample != new_sample) {
last_sample = new_sample;
if (baud_pll < 0.5)
baud_pll += baud_pll_adj;
else
baud_pll -= baud_pll_adj;
}
baud_pll += baud_incr;
if (baud_pll >= 1.0) {
baud_pll -= 1.0;
bit_writer->putBit(last_sample);
}
}
}
private:
int freq_lo;
int freq_hi;
int sample_rate;
int baud_rate;
std::shared_ptr<BitStreamWriter> bit_writer;
int filter_size;
std::vector<double> filter_lo_i;
std::vector<double> filter_lo_q;
std::vector<double> filter_hi_i;
std::vector<double> filter_hi_q;
std::vector<double> filter_buf;
size_t buf_ptr;
double baud_incr;
double baud_pll;
double baud_pll_adj;
int last_sample;
void initialize() {
baud_incr = static_cast<double>(baud_rate) / sample_rate;
baud_pll = 0.0;
baud_pll_adj = baud_incr / 4;
filter_size = sample_rate / baud_rate;
filter_buf.resize(filter_size * 2, 0.0);
buf_ptr = filter_size;
last_sample = 0;
filter_lo_i.resize(filter_size);
filter_lo_q.resize(filter_size);
filter_hi_i.resize(filter_size);
filter_hi_q.resize(filter_size);
for (int i = 0; i < filter_size; i++) {
double phase_lo = 2.0 * M_PI * freq_lo * i / sample_rate;
filter_lo_i[i] = std::cos(phase_lo);
filter_lo_q[i] = std::sin(phase_lo);
double phase_hi = 2.0 * M_PI * freq_hi * i / sample_rate;
filter_hi_i[i] = std::cos(phase_hi);
filter_hi_q[i] = std::sin(phase_hi);
}
}
double dotProduct(const double* x, const double* y, size_t size) {
double sum = 0.0;
for (size_t i = 0; i < size; i++) {
sum += x[i] * y[i];
}
return sum;
}
};
} // namespace milstd
#endif /* FSK_DEMODULATOR_H */
include/modulation/FSKModulator.h
-87
View File
@@ -1,87 +0,0 @@
#ifndef FSK_MODULATOR_H
#define FSK_MODULATOR_H
#include <cmath>
#include <cstdint>
#include <functional>
#include <memory>
#include <vector>
#include "BitStreamReader.h"
class FSKModulatorConfig {
public:
int freq_lo;
int freq_hi;
int sample_rate;
int baud_rate;
std::shared_ptr<BitStreamReader> bitstreamreader;
};
namespace milstd {
class FSKModulator {
public:
FSKModulator(const FSKModulatorConfig& s) : freq_lo(s.freq_lo), freq_hi(s.freq_hi), sample_rate(s.sample_rate), baud_rate(s.baud_rate), bit_reader(s.bitstreamreader) {
omega[0] = (2.0 * M_PI * freq_lo) / sample_rate;
omega[1] = (2.0 * M_PI * freq_hi) / sample_rate;
baud_incr = static_cast<double>(baud_rate) / sample_rate;
phase = 0.0;
baud_frac = 0.0;
current_bit = 0;
}
std::vector<int16_t> modulate(unsigned int num_samples) {
std::vector<int16_t> samples;
samples.reserve(num_samples);
int bit = current_bit;
for (unsigned int i = 0; i < num_samples; i++) {
baud_frac += baud_incr;
if (baud_frac >= 1.0) {
baud_frac -= 1.0;
try
{
bit = bit_reader->getNextBit();
}
catch(const std::out_of_range&)
{
bit = 0;
}
if (bit != 0 && bit != 1)
bit = 0;
}
double sample = std::cos(phase);
int16_t sample_int = static_cast<int16_t>(sample * 32767);
samples.push_back(sample_int);
phase += omega[bit];
if (phase >= 2.0 * M_PI) {
phase -= 2.0 * M_PI;
}
}
current_bit = bit;
return samples;
}
private:
// parameters
int freq_lo, freq_hi;
int sample_rate;
int baud_rate;
std::shared_ptr<BitStreamReader> bit_reader;
// state variables
double phase;
double baud_frac;
double baud_incr;
std::array<double, 2> omega;
int current_bit;
};
} // namespace milstd
#endif /* FSK_MODULATOR_H */
include/modulation/PSKDemodulator.h
View File
include/modulation/PSKModulator.h
+33 -81
View File
@@ -11,6 +11,9 @@
#include <vector>
#include <fftw3.h>
#include "costasloop.h"
#include "filters.h"
static constexpr double CARRIER_FREQ = 1800.0;
static constexpr size_t SYMBOL_RATE = 2400;
static constexpr double ROLLOFF_FACTOR = 0.35;
@@ -19,13 +22,14 @@ static constexpr double SCALE_FACTOR = 32767.0;
class PSKModulator {
public:
PSKModulator(const double _sample_rate, const bool _is_frequency_hopping, const size_t _num_taps)
: sample_rate(validateSampleRate(_sample_rate)), gain(1.0/sqrt(2.0)), is_frequency_hopping(_is_frequency_hopping), samples_per_symbol(static_cast<size_t>(sample_rate / SYMBOL_RATE)), num_taps(_num_taps) {
: sample_rate(validateSampleRate(_sample_rate)), gain(1.0/sqrt(2.0)), is_frequency_hopping(_is_frequency_hopping), samples_per_symbol(static_cast<size_t>(sample_rate / SYMBOL_RATE)), phase_detector(symbolMap), srrc_filter(48, _sample_rate, SYMBOL_RATE, ROLLOFF_FACTOR) {
initializeSymbolMap();
}
std::vector<int16_t> modulate(const std::vector<uint8_t>& symbols) const {
std::vector<int16_t> modulate(const std::vector<uint8_t>& symbols) {
std::vector<double> baseband_I(symbols.size() * samples_per_symbol);
std::vector<double> baseband_Q(symbols.size() * samples_per_symbol);
std::vector<std::complex<double>> baseband_components(symbols.size() * samples_per_symbol);
size_t symbol_index = 0;
for (const auto& symbol : symbols) {
@@ -35,23 +39,14 @@ public:
const std::complex<double> target_symbol = symbolMap[symbol];
for (size_t i = 0; i < samples_per_symbol; ++i) {
baseband_I[symbol_index * samples_per_symbol + i] = target_symbol.real();
baseband_Q[symbol_index * samples_per_symbol + i] = target_symbol.imag();
baseband_components[symbol_index * samples_per_symbol + i] = target_symbol;
}
symbol_index++;
}
// Filter the I/Q phase components
auto filter_taps = generateSRRCTaps(num_taps, sample_rate, SYMBOL_RATE, ROLLOFF_FACTOR);
auto filtered_I = applyFilter(baseband_I, filter_taps);
auto filtered_Q = applyFilter(baseband_Q, filter_taps);
std::vector<std::complex<double>> baseband_components(filtered_I.size());
for (size_t i = 0; i < filtered_I.size(); i++) {
std::complex<double> baseband_component = {filtered_I[i], filtered_Q[i]};
baseband_components[i] = baseband_component;
}
std::vector<std::complex<double>> filtered_components = srrc_filter.applyFilter(baseband_components);
// Combine the I and Q components
std::vector<double> passband_signal;
@@ -81,13 +76,37 @@ public:
return final_signal;
}
std::vector<uint8_t> demodulate(const std::vector<int16_t> passband_signal) {
// Carrier recovery. initialize the Costas loop.
CostasLoop costas_loop(sample_rate, symbolMap);
// Convert passband signal to doubles.
std::vector<double> normalized_passband(passband_signal.size());
for (size_t i = 0; i < passband_signal.size(); i++) {
normalized_passband[i] = passband_signal[i] / 32767.0;
}
// Downmix passband to baseband
std::vector<std::complex<double>> baseband_IQ = costas_loop.process(normalized_passband);
// Phase detection and symbol formation
std::vector<uint8_t> baseband_symbols;
for (size_t i = 0; i < baseband_IQ.size(); i++) {
baseband_symbols.emplace_back(phase_detector.getSymbol(baseband_IQ[i]));
}
return baseband_symbols;
}
private:
const double sample_rate; ///< The sample rate of the system.
const double gain; ///< The gain of the modulated signal.
size_t samples_per_symbol; ///< Number of samples per symbol, calculated to match symbol duration with cycle.
const size_t num_taps;
PhaseDetector phase_detector;
SRRCFilter srrc_filter;
std::vector<std::complex<double>> symbolMap; ///< The mapping of tribit symbols to I/Q components.
const bool is_frequency_hopping; ///< Whether to use frequency hopping methods. Not implemented (yet?)
static inline double validateSampleRate(const double rate) {
if (rate <= 2 * CARRIER_FREQ) {
@@ -108,73 +127,6 @@ private:
{gain * std::cos(2.0*M_PI*(7.0/8.0)), gain * std::sin(2.0*M_PI*(7.0/8.0))} // 7 (111) corresponds to I = cos(315), Q = sin(315)
};
}
std::vector<double> generateSRRCTaps(size_t num_taps,double sample_rate, double symbol_rate, 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> applyFilter(const std::vector<double>& signal, const std::vector<double>& filter_taps) const {
std::vector<double> filtered_signal(signal.size(), 0.0);
size_t filter_length = filter_taps.size();
size_t half_filter_length = filter_length / 2;
// Convolve the signal with the filter taps
for (size_t i = 0; i < signal.size(); ++i) {
double filtered_val = 0.0;
for (size_t j = 0; j < filter_length; ++j) {
size_t signal_index = i + j - half_filter_length;
if (signal_index < signal.size()) {
filtered_val += filter_taps[j] * signal[signal_index];
}
}
filtered_signal[i] = filtered_val;
}
return filtered_signal;
}
};
#endif