Mirror/RFnexus___modem73
1
0
Fork 0
mirror of https://github.com/RFnexus/modem73.git synced 2026-04-27 14:30:33 +00:00
Code Issues Projects Releases Packages Wiki Activity
RFnexus___modem73/modem.hh

1092 lines
39 KiB
C++
Raw Blame History

#pragma once
#include <vector>
#include <cstring>
#include <cmath>
#include <functional>
#include <atomic>
#include "phy/common.hh"
#include "schmidl_cox.hh"
#include "bip_buffer.hh"
#include "theil_sen.hh"
#include "blockdc.hh"
#include "hilbert.hh"
#include "phasor.hh"
#include "delay.hh"
#include "polar_encoder.hh"
#include "polar_list_decoder.hh"
#include "hadamard_decoder.hh"
template<typename T>
class BufferWritePCM {
public:
BufferWritePCM(int rate, int bits, int channels)
: rate_(rate), bits_(bits), channels_(channels) {}
void write(const T* buffer, int count, int ch = 1) {
for (int i = 0; i < count; ++i) {
// 2 channels, only take real part for mono output
if (ch == 2) {
samples_.push_back(buffer[i * 2]); // real
} else {
samples_.push_back(buffer[i]);
}
}
}
void silence(int count) {
for (int i = 0; i < count; ++i) {
samples_.push_back(T(0));
}
}
const std::vector<T>& samples() const { return samples_; }
std::vector<T>& samples() { return samples_; }
void clear() { samples_.clear(); }
int rate() const { return rate_; }
int bits() const { return bits_; }
int channels() const { return channels_; }
private:
std::vector<T> samples_;
int rate_, bits_, channels_;
};
// Modem configuration
struct ModemConfig {
int sample_rate = 48000;
int center_freq = 1500;
int64_t call_sign = 0;
int oper_mode = 0;
static int64_t encode_callsign(const char* str) {
int64_t acc = 0;
for (char c = *str++; c; c = *str++) {
acc *= 40;
if (c == '/')
acc += 3;
else if (c >= '0' && c <= '9')
acc += c - '0' + 4;
else if (c >= 'a' && c <= 'z')
acc += c - 'a' + 14;
else if (c >= 'A' && c <= 'Z')
acc += c - 'A' + 14;
else if (c != ' ')
return -1;
}
return acc;
}
static int encode_mode(const char* modulation, const char* code_rate, bool short_frame) {
int mode = 0;
if (!strcmp(modulation, "BPSK"))
mode |= 0 << 4;
else if (!strcmp(modulation, "QPSK"))
mode |= 1 << 4;
else if (!strcmp(modulation, "8PSK"))
mode |= 2 << 4;
else if (!strcmp(modulation, "QAM16"))
mode |= 3 << 4;
else if (!strcmp(modulation, "QAM64"))
mode |= 4 << 4;
else if (!strcmp(modulation, "QAM256"))
mode |= 5 << 4;
else if (!strcmp(modulation, "QAM1024"))
mode |= 6 << 4;
else if (!strcmp(modulation, "QAM4096"))
mode |= 7 << 4;
else
return -1;
if (!strcmp(code_rate, "1/2"))
mode |= 0 << 1;
else if (!strcmp(code_rate, "2/3"))
mode |= 1 << 1;
else if (!strcmp(code_rate, "3/4"))
mode |= 2 << 1;
else if (!strcmp(code_rate, "5/6"))
mode |= 3 << 1;
else if (!strcmp(code_rate, "1/4"))
mode |= 4 << 1;
else
return -1;
if (!short_frame)
mode |= 1;
return mode;
}
};
// Encoder
template<typename value, typename cmplx, int rate>
class ModemEncoder : public Common {
public:
typedef int8_t code_type;
static const int guard_len = rate / 300;
static const int symbol_len = guard_len * 40;
ModemEncoder() {}
// encode our data to audio samples
std::vector<value> encode(const uint8_t* input_data, size_t input_len,
int freq_off, int64_t call_sign, int oper_mode) {
BufferWritePCM<value> pcm(rate, 32, 1);
if (!setup(oper_mode)) {
std::cerr << "Encoder: invalid mode" << std::endl;
return {};
}
int offset = (freq_off * symbol_len) / rate;
tone_off = offset - tone_count / 2;
guard_interval_weights();
meta_data((call_sign << 8) | oper_mode);
// leading noise
CODE::MLS noise(mls2_poly);
for (int j = 0; j < 1; ++j) {
for (int i = 0; i < tone_count; ++i)
tone[i] = nrz(noise());
symbol(&pcm, -3);
}
// Copy input data (pad if necessary)
std::memset(data, 0, data_max);
std::memcpy(data, input_data, std::min(input_len, (size_t)data_bytes));
// Scramble
CODE::Xorshift32 scrambler;
for (int i = 0; i < data_bytes; ++i)
data[i] ^= scrambler();
// Schmidl-Cox preamble
CODE::MLS seq0(mls0_poly, mls0_seed);
for (int i = 0; i < tone_count; ++i)
tone[i] = nrz(seq0());
symbol(&pcm, -2);
symbol(&pcm, -1);
// Encode payload
for (int i = 0; i < data_bits; ++i)
mesg[i] = nrz(CODE::get_le_bit(data, i));
crc1.reset();
for (int i = 0; i < data_bytes; ++i)
crc1(data[i]);
for (int i = 0; i < 32; ++i)
mesg[i + data_bits] = nrz((crc1() >> i) & 1);
polar_encoder(code, mesg, frozen_bits, code_order);
shuffle(perm, code, code_order);
// Generate symbols
CODE::MLS seq1(mls1_poly);
for (int j = 0, k = 0, m = 0; j < symbol_count + 1; ++j) {
seed_off = (block_skew * j + first_seed) % block_length;
for (int i = 0; i < tone_count; ++i) {
if (i % block_length == seed_off) {
tone[i] = nrz(seq1());
} else if (j) {
int bits = mod_bits;
if (mod_bits == 3 && k % 32 == 30) bits = 2;
if (mod_bits == 6 && k % 64 == 60) bits = 4;
if (mod_bits == 10 && k % 128 == 120) bits = 8;
if (mod_bits == 12 && k % 128 == 120) bits = 8;
tone[i] = map_bits(perm + k, bits);
k += bits;
} else {
tone[i] = map_bits(meta + m++, 1);
}
}
symbol(&pcm, j);
}
for (int i = 0; i < guard_len; ++i)
guard[i] *= 1 - weight[i];
pcm.write(reinterpret_cast<value*>(guard), guard_len, 2);
return std::move(pcm.samples());
}
int get_payload_size(int oper_mode) {
if (!setup(oper_mode)) return 0;
return data_bytes;
}
private:
DSP::FastFourierTransform<symbol_len, cmplx, -1> fwd;
DSP::FastFourierTransform<symbol_len, cmplx, 1> bwd;
CODE::PolarEncoder<code_type> polar_encoder;
code_type code[bits_max], perm[bits_max], mesg[bits_max], meta[data_tones];
cmplx fdom[symbol_len];
cmplx tdom[symbol_len];
cmplx test[symbol_len];
cmplx kern[symbol_len];
cmplx guard[guard_len];
cmplx tone[tone_count];
cmplx temp[tone_count];
value weight[guard_len];
value papr[symbols_max];
static int bin(int carrier) {
return (carrier + symbol_len) % symbol_len;
}
static int nrz(bool bit) {
return 1 - 2 * bit;
}
cmplx map_bits(code_type* b, int bits) {
switch (bits) {
case 1: return PhaseShiftKeying<2, cmplx, code_type>::map(b);
case 2: return PhaseShiftKeying<4, cmplx, code_type>::map(b);
case 3: return PhaseShiftKeying<8, cmplx, code_type>::map(b);
case 4: return QuadratureAmplitudeModulation<16, cmplx, code_type>::map(b);
case 6: return QuadratureAmplitudeModulation<64, cmplx, code_type>::map(b);
case 8: return QuadratureAmplitudeModulation<256, cmplx, code_type>::map(b);
case 10: return QuadratureAmplitudeModulation<1024, cmplx, code_type>::map(b);
case 12: return QuadratureAmplitudeModulation<4096, cmplx, code_type>::map(b);
}
return 0;
}
void shuffle(code_type* dest, const code_type* src, int order) {
if (order == 8) {
CODE::XorShiftMask<int, 8, 1, 1, 2, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 256; ++i) dest[i] = src[seq()];
} else if (order == 11) {
CODE::XorShiftMask<int, 11, 1, 3, 4, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 2048; ++i) dest[i] = src[seq()];
} else if (order == 12) {
CODE::XorShiftMask<int, 12, 1, 1, 4, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 4096; ++i) dest[i] = src[seq()];
} else if (order == 13) {
CODE::XorShiftMask<int, 13, 1, 1, 9, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 8192; ++i) dest[i] = src[seq()];
} else if (order == 14) {
CODE::XorShiftMask<int, 14, 1, 5, 10, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 16384; ++i) dest[i] = src[seq()];
} else if (order == 15) {
CODE::XorShiftMask<int, 15, 1, 1, 3, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 32768; ++i) dest[i] = src[seq()];
} else if (order == 16) {
CODE::XorShiftMask<int, 16, 1, 1, 14, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 65536; ++i) dest[i] = src[seq()];
}
}
void guard_interval_weights() {
for (int i = 0; i < guard_len / 4; ++i)
weight[i] = 0;
for (int i = guard_len / 4; i < guard_len / 4 + guard_len / 2; ++i) {
value x = value(i - guard_len / 4) / value(guard_len / 2 - 1);
weight[i] = value(0.5) * (value(1) - std::cos(DSP::Const<value>::Pi() * x));
}
for (int i = guard_len / 4 + guard_len / 2; i < guard_len; ++i)
weight[i] = 1;
}
void clipping_and_filtering(value scale) {
for (int i = 0; i < symbol_len; ++i) {
value pwr = norm(tdom[i]);
if (pwr > value(1))
tdom[i] /= sqrt(pwr);
}
fwd(fdom, tdom);
for (int i = 0; i < symbol_len; ++i) {
int j = bin(i + tone_off);
if (i >= tone_count)
fdom[j] = 0;
else
fdom[j] *= 1 / (scale * symbol_len);
}
bwd(tdom, fdom);
for (int i = 0; i < symbol_len; ++i)
tdom[i] *= scale;
auto clamp = [](value v) { return v < value(-1) ? value(-1) : v > value(1) ? value(1) : v; };
for (int i = 0; i < symbol_len; ++i)
tdom[i] = cmplx(clamp(tdom[i].real()), clamp(tdom[i].imag()));
}
void symbol(BufferWritePCM<value>* pcm, int symbol_number) {
value scale = value(0.5) / std::sqrt(value(tone_count));
if (symbol_number < 0) {
for (int i = 0; i < symbol_len; ++i)
fdom[i] = 0;
for (int i = 0; i < tone_count; ++i)
fdom[bin(i + tone_off)] = tone[i];
bwd(tdom, fdom);
for (int i = 0; i < symbol_len; ++i)
tdom[i] *= scale;
} else {
value best_papr = 1000;
for (int seed_value = 0; seed_value < 128; ++seed_value) {
for (int i = 0; i < tone_count; ++i)
temp[i] = tone[i];
hadamard_encoder(seed, seed_value);
for (int i = 0; i < seed_tones; ++i)
temp[i * block_length + seed_off] *= seed[i];
if (seed_value) {
CODE::MLS seq(mls2_poly, seed_value);
for (int i = 0; i < tone_count; ++i)
if (i % block_length != seed_off)
temp[i] *= nrz(seq());
}
for (int i = 0; i < symbol_len; ++i)
fdom[i] = 0;
for (int i = 0; i < tone_count; ++i)
fdom[bin(i + tone_off)] = temp[i];
bwd(test, fdom);
for (int i = 0; i < symbol_len; ++i)
test[i] *= scale;
value peak = 0, mean = 0;
for (int i = 0; i < symbol_len; ++i) {
value power(norm(test[i]));
peak = std::max(peak, power);
mean += power;
}
mean /= symbol_len;
value test_papr(peak / mean);
if (test_papr < best_papr) {
best_papr = test_papr;
papr[symbol_number] = test_papr;
for (int i = 0; i < symbol_len; ++i)
tdom[i] = test[i];
if (test_papr < 5)
break;
}
}
}
clipping_and_filtering(scale);
if (symbol_number != -1) {
for (int i = 0; i < guard_len; ++i)
guard[i] = DSP::lerp(guard[i], tdom[i + symbol_len - guard_len], weight[i]);
pcm->write(reinterpret_cast<value*>(guard), guard_len, 2);
}
for (int i = 0; i < guard_len; ++i)
guard[i] = tdom[i];
pcm->write(reinterpret_cast<value*>(tdom), symbol_len, 2);
}
void meta_data(uint64_t md) {
for (int i = 0; i < 56; ++i)
mesg[i] = nrz((md >> i) & 1);
crc0.reset();
crc0(md << 8);
for (int i = 0; i < 16; ++i)
mesg[i + 56] = nrz((crc0() >> i) & 1);
polar_encoder(code, mesg, frozen_256_72, 8);
shuffle(meta, code, 8);
}
};
// Decoder
template<typename value, typename cmplx, int rate>
class ModemDecoder : public Common {
public:
typedef int16_t code_type;
typedef SIMD<code_type, 32> mesg_type;
typedef DSP::Const<value> Const;
static const int guard_len = rate / 300;
static const int symbol_len = guard_len * 40;
static const int filter_len = 129;
static const int extended_len = symbol_len + guard_len;
static const int buffer_len = 5 * extended_len;
static const int search_pos = extended_len;
static const int tone_off_const = -tone_count / 2;
using FrameCallback = std::function<void(const uint8_t*, size_t)>;
// Constellation callback - called after each symbol is demodulated
// Parameters: pointer to demodulated symbols, count, modulation bits
std::function<void(const cmplx*, int, int)> constellation_callback;
ModemDecoder() {
// init fdom_mls before correlator uses it
init_mls0_seq();
correlator_ptr = new SchmidlCox<value, cmplx, search_pos, symbol_len, guard_len>(fdom_mls);
blockdc.samples(filter_len);
}
~ModemDecoder() {
delete correlator_ptr;
delete seq1_ptr;
}
void process(const value* samples, size_t count, FrameCallback callback) {
for (size_t i = 0; i < count; ++i) {
process_sample(samples[i], callback);
}
}
// Reset decoder state
void reset() {
state_ = State::SEARCHING;
sample_count_ = 0;
symbol_index_ = 0;
samples_needed_ = 0;
k_ = 0;
}
// Get average SNR from last successful decode
value get_last_snr() const { return last_avg_snr_; }
// Get current modulation bits
int get_mod_bits() const { return mod_bits; }
// Get last per-frame BER
value get_last_ber() const { return last_ber_; }
// Get smoothed BER via EMA
value get_ber_ema() const { return ber_ema_; }
void reset_ber() {
last_ber_ = -1;
ber_ema_ = -1;
}
// decode statistics
int stats_sync_count = 0; // corelator
int stats_preamble_errors = 0; // preamble decoding failed
int stats_symbol_errors = 0; // seed damage
int stats_crc_errors = 0; // polar CRC failed
private:
enum class State {
SEARCHING, // looking for preamble
COLLECTING_SYMBOLS, // Collecting data symbols
};
// Arrays used by correlator
cmplx fdom_mls[symbol_len];
cmplx fdom[symbol_len], tdom[symbol_len];
DSP::FastFourierTransform<symbol_len, cmplx, -1> fwd;
DSP::BlockDC<value, value> blockdc;
DSP::Hilbert<cmplx, filter_len> hilbert;
DSP::BipBuffer<cmplx, buffer_len> input_hist;
DSP::TheilSenEstimator<value, tone_count> tse;
SchmidlCox<value, cmplx, search_pos, symbol_len, guard_len>* correlator_ptr = nullptr;
CODE::HadamardDecoder<7> hadamard_decoder;
CODE::PolarListDecoder<mesg_type, code_max> polar_decoder;
CODE::PolarEncoder<int8_t> ber_encoder;
int8_t ber_mesg[bits_max], ber_code[bits_max];
DSP::Phasor<cmplx> osc;
mesg_type mesg[bits_max];
code_type code[bits_max], perm[bits_max];
cmplx demod[tone_count], chan[tone_count], tone[tone_count];
cmplx saved_demod[symbols_max * tone_count];
int saved_seed_off[symbols_max];
int fwd_perm_table[bits_max];
value index[tone_count], phase[tone_count];
value snr[symbols_max];
value cfo_rad;
int symbol_pos;
value last_avg_snr_ = 0;
value last_ber_ = -1;
value ber_ema_ = -1;
State state_ = State::SEARCHING;
size_t sample_count_ = 0;
int symbol_index_ = 0;
int samples_needed_ = 0;
int k_ = 0;
const cmplx* buf_ = nullptr;
CODE::MLS* seq1_ptr = nullptr;
static int bin(int carrier) {
return (carrier + symbol_len) % symbol_len;
}
static value nrz(bool bit) {
return 1 - 2 * bit;
}
static cmplx demod_or_erase(cmplx curr, cmplx prev) {
if (norm(prev) > 0) {
cmplx d = curr / prev;
if (norm(d) < 4)
return d;
}
return 0;
}
void init_mls0_seq() {
CODE::MLS seq0(mls0_poly, mls0_seed);
value cur = 0, prv = 0;
for (int i = 0; i < tone_count; ++i, prv = cur)
fdom_mls[bin(i + tone_off_const)] = prv * (cur = nrz(seq0()));
}
cmplx map_bits(code_type* b, int bits) {
switch (bits) {
case 1: return PhaseShiftKeying<2, cmplx, code_type>::map(b);
case 2: return PhaseShiftKeying<4, cmplx, code_type>::map(b);
case 3: return PhaseShiftKeying<8, cmplx, code_type>::map(b);
case 4: return QuadratureAmplitudeModulation<16, cmplx, code_type>::map(b);
case 6: return QuadratureAmplitudeModulation<64, cmplx, code_type>::map(b);
case 8: return QuadratureAmplitudeModulation<256, cmplx, code_type>::map(b);
case 10: return QuadratureAmplitudeModulation<1024, cmplx, code_type>::map(b);
case 12: return QuadratureAmplitudeModulation<4096, cmplx, code_type>::map(b);
}
return 0;
}
void demap_soft(code_type* b, cmplx c, value precision, int bits) {
switch (bits) {
case 1: return PhaseShiftKeying<2, cmplx, code_type>::soft(b, c, precision);
case 2: return PhaseShiftKeying<4, cmplx, code_type>::soft(b, c, precision);
case 3: return PhaseShiftKeying<8, cmplx, code_type>::soft(b, c, precision);
case 4: return QuadratureAmplitudeModulation<16, cmplx, code_type>::soft(b, c, precision);
case 6: return QuadratureAmplitudeModulation<64, cmplx, code_type>::soft(b, c, precision);
case 8: return QuadratureAmplitudeModulation<256, cmplx, code_type>::soft(b, c, precision);
case 10: return QuadratureAmplitudeModulation<1024, cmplx, code_type>::soft(b, c, precision);
case 12: return QuadratureAmplitudeModulation<4096, cmplx, code_type>::soft(b, c, precision);
}
}
void demap_hard(code_type* b, cmplx c, int bits) {
switch (bits) {
case 1: return PhaseShiftKeying<2, cmplx, code_type>::hard(b, c);
case 2: return PhaseShiftKeying<4, cmplx, code_type>::hard(b, c);
case 3: return PhaseShiftKeying<8, cmplx, code_type>::hard(b, c);
case 4: return QuadratureAmplitudeModulation<16, cmplx, code_type>::hard(b, c);
case 6: return QuadratureAmplitudeModulation<64, cmplx, code_type>::hard(b, c);
case 8: return QuadratureAmplitudeModulation<256, cmplx, code_type>::hard(b, c);
case 10: return QuadratureAmplitudeModulation<1024, cmplx, code_type>::hard(b, c);
case 12: return QuadratureAmplitudeModulation<4096, cmplx, code_type>::hard(b, c);
}
}
void shuffle(code_type* dest, const code_type* src, int order) {
if (order == 8) {
CODE::XorShiftMask<int, 8, 1, 1, 2, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 256; ++i) dest[seq()] = src[i];
} else if (order == 11) {
CODE::XorShiftMask<int, 11, 1, 3, 4, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 2048; ++i) dest[seq()] = src[i];
} else if (order == 12) {
CODE::XorShiftMask<int, 12, 1, 1, 4, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 4096; ++i) dest[seq()] = src[i];
} else if (order == 13) {
CODE::XorShiftMask<int, 13, 1, 1, 9, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 8192; ++i) dest[seq()] = src[i];
} else if (order == 14) {
CODE::XorShiftMask<int, 14, 1, 5, 10, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 16384; ++i) dest[seq()] = src[i];
} else if (order == 15) {
CODE::XorShiftMask<int, 15, 1, 1, 3, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 32768; ++i) dest[seq()] = src[i];
} else if (order == 16) {
CODE::XorShiftMask<int, 16, 1, 1, 14, 1> seq;
dest[0] = src[0];
for (int i = 1; i < 65536; ++i) dest[seq()] = src[i];
}
}
static void base40_decoder(char* str, int64_t val, int len) {
for (int i = len - 1; i >= 0; --i, val /= 40)
str[i] = " /0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[val % 40];
}
int64_t meta_data() {
shuffle(code, perm, 8);
polar_decoder(nullptr, mesg, code, frozen_256_72, 8);
int best = -1;
for (int k = 0; k < mesg_type::SIZE; ++k) {
crc0.reset();
for (int i = 0; i < 72; ++i)
crc0(mesg[i].v[k] < 0);
if (crc0() == 0) {
best = k;
break;
}
}
if (best < 0)
return -1;
uint64_t md = 0;
for (int i = 0; i < 56; ++i)
md |= uint64_t(mesg[i].v[best] < 0) << i;
return md;
}
void process_sample(value sample, FrameCallback callback) {
// Convert to complex via Hilbert transform
cmplx tmp = hilbert(blockdc(sample));
buf_ = input_hist(tmp);
++sample_count_;
switch (state_) {
case State::SEARCHING:
if ((*correlator_ptr)(buf_)) {
// Sync found
++stats_sync_count;
symbol_pos = correlator_ptr->symbol_pos;
cfo_rad = correlator_ptr->cfo_rad;
std::cerr << "Decoder: Sync found at sample " << sample_count_ << std::endl;
std::cerr << "Decoder: CFO = " << cfo_rad * (rate / Const::TwoPi()) << " Hz" << std::endl;
// Initialize seq1 for the whole frame
delete seq1_ptr;
seq1_ptr = new CODE::MLS(mls1_poly);
// Process preamble and start collecting symbols
if (process_preamble()) {
state_ = State::COLLECTING_SYMBOLS;
symbol_index_ = 1; // Symbol 0 (meta) already processed
// Need to advance past preamble: symbol_pos + symbol_len + extended_len
// Plus extended_len for the first data symbol
samples_needed_ = symbol_pos + symbol_len + 2 * extended_len;
} else {
++stats_preamble_errors;
reset();
}
}
break;
case State::COLLECTING_SYMBOLS:
// Keep feeding correlator to maintain buffer
(*correlator_ptr)(buf_);
samples_needed_--;
if (samples_needed_ <= 0) {
// Process this symbol
if (!process_symbol(symbol_index_)) {
// Error, go back to searching
++stats_symbol_errors;
reset();
break;
}
symbol_index_++;
if (symbol_index_ > symbol_count) {
// All symbols collected
decode_frame(callback);
reset();
} else {
samples_needed_ = extended_len;
}
}
break;
}
}
bool process_preamble() {
// Process Schmidl-Cox preamble symbols
osc.omega(-cfo_rad);
// First preamble symbol
for (int i = 0; i < symbol_len; ++i)
tdom[i] = buf_[i + symbol_pos] * osc();
fwd(fdom, tdom);
for (int i = 0; i < tone_count; ++i)
tone[i] = fdom[bin(i + tone_off_const)];
// Second preamble symbol
for (int i = 0; i < symbol_len; ++i)
tdom[i] = buf_[i + symbol_pos + symbol_len] * osc();
for (int i = 0; i < guard_len; ++i)
osc();
fwd(fdom, tdom);
for (int i = 0; i < tone_count; ++i)
chan[i] = fdom[bin(i + tone_off_const)];
// Estimate SFO
for (int i = 0; i < tone_count; ++i) {
index[i] = tone_off_const + i;
phase[i] = arg(demod_or_erase(chan[i], tone[i]));
}
tse.compute(index, phase, tone_count);
std::cerr << "Decoder: SFO = " << -1000000 * tse.slope() / Const::TwoPi() << " ppm" << std::endl;
// Correct channel estimate
for (int i = 0; i < tone_count; ++i)
tone[i] *= DSP::polar<value>(1, tse(i + tone_off_const));
for (int i = 0; i < tone_count; ++i)
chan[i] = DSP::lerp(chan[i], tone[i], value(0.5));
// Remove preamble sequence
CODE::MLS seq0(mls0_poly, mls0_seed);
for (int i = 0; i < tone_count; ++i)
chan[i] *= nrz(seq0());
// Process meta symbol (symbol 0)
for (int i = 0; i < symbol_len; ++i)
tdom[i] = buf_[i + symbol_pos + symbol_len + extended_len] * osc();
for (int i = 0; i < guard_len; ++i)
osc();
fwd(fdom, tdom);
// Decode meta symbol
seed_off = first_seed;
auto clamp = [](int v) { return v < -127 ? -127 : v > 127 ? 127 : v; };
for (int i = 0; i < tone_count; ++i)
tone[i] = fdom[bin(i + tone_off_const)];
for (int i = seed_off; i < tone_count; i += block_length)
tone[i] *= nrz((*seq1_ptr)());
for (int i = 0; i < tone_count; ++i)
demod[i] = demod_or_erase(tone[i], chan[i]);
// Decode seed for meta symbol
for (int i = 0; i < seed_tones; ++i)
seed[i] = clamp(std::nearbyint(127 * demod[i * block_length + seed_off].real()));
int seed_value = hadamard_decoder(seed);
if (seed_value < 0) {
std::cerr << "Decoder: Seed value damaged in meta" << std::endl;
return false;
}
hadamard_encoder(seed, seed_value);
for (int i = 0; i < seed_tones; ++i) {
tone[block_length * i + seed_off] *= seed[i];
demod[block_length * i + seed_off] *= seed[i];
}
// Phase correction
for (int i = 0; i < seed_tones; ++i) {
index[i] = tone_off_const + block_length * i + seed_off;
phase[i] = arg(demod[block_length * i + seed_off]);
}
tse.compute(index, phase, seed_tones);
for (int i = 0; i < tone_count; ++i)
demod[i] *= DSP::polar<value>(1, -tse(i + tone_off_const));
for (int i = 0; i < tone_count; ++i)
chan[i] *= DSP::polar<value>(1, tse(i + tone_off_const));
if (seed_value) {
CODE::MLS seq(mls2_poly, seed_value);
for (int i = 0; i < tone_count; ++i)
if (i % block_length != seed_off)
demod[i] *= nrz(seq());
}
// SNR estimation and demapping for meta symbol (mod_bits = 1 for meta)
value sp = 0, np = 0;
for (int i = 0, l = 0; i < tone_count; ++i) {
cmplx hard(1, 0);
if (i % block_length != seed_off) {
demap_hard(perm + l, demod[i], 1);
hard = map_bits(perm + l, 1);
l += 1;
}
cmplx error = demod[i] - hard;
sp += norm(hard);
np += norm(error);
}
value precision = sp / np;
precision = std::min(precision, value(1023));
// std::cerr << "Decoder: Meta symbol SNR = " << 10 * std::log10(precision) << " dB" << std::endl;
// Soft demap meta symbol
int k = 0;
for (int i = 0; i < tone_count; ++i) {
if (i % block_length != seed_off) {
demap_soft(perm + k, demod[i], precision, 1);
k += 1;
}
}
// Update channel for meta symbol pilots
for (int i = seed_off; i < tone_count; i += block_length)
chan[i] = DSP::lerp(chan[i], tone[i], value(0.5));
// Decode meta data
int64_t meta_info = meta_data();
if (meta_info < 0) {
std::cerr << "Decoder: Preamble decoding error" << std::endl;
return false;
}
int64_t call = meta_info >> 8;
if (call == 0 || call >= 262144000000000L) {
std::cerr << "Decoder: Invalid call sign" << std::endl;
return false;
}
char call_sign[10];
base40_decoder(call_sign, call, 9);
call_sign[9] = 0;
std::cerr << "Decoder: Call sign: " << call_sign << std::endl;
int mode = meta_info & 255;
if (!setup(mode)) {
std::cerr << "Decoder: Invalid mode" << std::endl;
return false;
}
std::cerr << "Decoder: Mode " << oper_mode << ", " << symbol_count << " data symbols, mod_bits=" << mod_bits << ", code_order=" << code_order << ", data_bytes=" << data_bytes << std::endl;
// Reset for data collection
k_ = 0;
snr[0] = 100;
return true;
}
void build_fwd_perm(int* table, int order) {
int len = 1 << order;
table[0] = 0;
if (order == 11) {
CODE::XorShiftMask<int, 11, 1, 3, 4, 1> seq;
for (int i = 1; i < len; ++i) table[i] = seq();
} else if (order == 12) {
CODE::XorShiftMask<int, 12, 1, 1, 4, 1> seq;
for (int i = 1; i < len; ++i) table[i] = seq();
} else if (order == 13) {
CODE::XorShiftMask<int, 13, 1, 1, 9, 1> seq;
for (int i = 1; i < len; ++i) table[i] = seq();
} else if (order == 14) {
CODE::XorShiftMask<int, 14, 1, 5, 10, 1> seq;
for (int i = 1; i < len; ++i) table[i] = seq();
} else if (order == 15) {
CODE::XorShiftMask<int, 15, 1, 1, 3, 1> seq;
for (int i = 1; i < len; ++i) table[i] = seq();
} else if (order == 16) {
CODE::XorShiftMask<int, 16, 1, 1, 14, 1> seq;
for (int i = 1; i < len; ++i) table[i] = seq();
}
}
bool process_symbol(int j) {
seed_off = (block_skew * j + first_seed) % block_length;
auto clamp = [](int v) { return v < -127 ? -127 : v > 127 ? 127 : v; };
// FFT the current symbol
for (int i = 0; i < symbol_len; ++i)
tdom[i] = buf_[i] * osc();
for (int i = 0; i < guard_len; ++i)
osc();
fwd(fdom, tdom);
for (int i = 0; i < tone_count; ++i)
tone[i] = fdom[bin(i + tone_off_const)];
// Remove pilot sequence
for (int i = seed_off; i < tone_count; i += block_length)
tone[i] *= nrz((*seq1_ptr)());
for (int i = 0; i < tone_count; ++i)
demod[i] = demod_or_erase(tone[i], chan[i]);
// Decode seed
for (int i = 0; i < seed_tones; ++i)
seed[i] = clamp(std::nearbyint(127 * demod[i * block_length + seed_off].real()));
int seed_value = hadamard_decoder(seed);
if (seed_value < 0) {
std::cerr << "Decoder: Seed damaged at symbol " << j << std::endl;
return false;
}
hadamard_encoder(seed, seed_value);
for (int i = 0; i < seed_tones; ++i) {
tone[block_length * i + seed_off] *= seed[i];
demod[block_length * i + seed_off] *= seed[i];
}
// Phase correction
for (int i = 0; i < seed_tones; ++i) {
index[i] = tone_off_const + block_length * i + seed_off;
phase[i] = arg(demod[block_length * i + seed_off]);
}
tse.compute(index, phase, seed_tones);
for (int i = 0; i < tone_count; ++i)
demod[i] *= DSP::polar<value>(1, -tse(i + tone_off_const));
for (int i = 0; i < tone_count; ++i)
chan[i] *= DSP::polar<value>(1, tse(i + tone_off_const));
if (seed_value) {
CODE::MLS seq(mls2_poly, seed_value);
for (int i = 0; i < tone_count; ++i)
if (i % block_length != seed_off)
demod[i] *= nrz(seq());
}
// Save demod for post-decode corrected SNR
std::memcpy(&saved_demod[j * tone_count], demod, tone_count * sizeof(cmplx));
saved_seed_off[j] = seed_off;
// Notify constellation callback with fully-corrected demodulated symbols
if (constellation_callback) {
constellation_callback(demod, tone_count, mod_bits);
}
// SNR estimation from data tones only excluding seed/pilot tones
value sp = 0, np = 0;
for (int i = 0, l = k_; i < tone_count; ++i) {
if (i % block_length != seed_off) {
int bits = mod_bits;
if (mod_bits == 3 && l % 32 == 30) bits = 2;
if (mod_bits == 6 && l % 64 == 60) bits = 4;
if (mod_bits == 10 && l % 128 == 120) bits = 8;
if (mod_bits == 12 && l % 128 == 120) bits = 8;
demap_hard(perm + l, demod[i], bits);
cmplx hard = map_bits(perm + l, bits);
cmplx error = demod[i] - hard;
sp += norm(hard);
np += norm(error);
l += bits;
}
}
value precision = sp / np;
snr[j] = precision;
precision = std::min(precision, value(1023));
std::cerr << "Decoder: Symbol " << j << " SNR = " << 10 * std::log10(snr[j]) << " dB, k=" << k_ << std::endl;
for (int i = 0; i < tone_count; ++i) {
if (i % block_length != seed_off) {
int bits = mod_bits;
if (mod_bits == 3 && k_ % 32 == 30) bits = 2;
if (mod_bits == 6 && k_ % 64 == 60) bits = 4;
if (mod_bits == 10 && k_ % 128 == 120) bits = 8;
if (mod_bits == 12 && k_ % 128 == 120) bits = 8;
demap_soft(perm + k_, demod[i], precision, bits);
k_ += bits;
}
}
for (int i = seed_off; i < tone_count; i += block_length)
chan[i] = DSP::lerp(chan[i], tone[i], value(0.5));
return true;
}
void decode_frame(FrameCallback callback) {
std::cerr << "Decoder: Decoding frame, k_=" << k_ << " bits collected" << std::endl;
std::cerr << "Decoder: Expected code_order=" << code_order << " (code length=" << (1 << code_order) << ")" << std::endl;
int crc_bits = data_bits + 32;
shuffle(code, perm, code_order);
polar_decoder(nullptr, mesg, code, frozen_bits, code_order);
int best = -1;
for (int k = 0; k < mesg_type::SIZE; ++k) {
crc1.reset();
for (int i = 0; i < crc_bits; ++i)
crc1(mesg[i].v[k] < 0);
if (crc1() == 0) {
best = k;
break;
}
}
if (best < 0) {
std::cerr << "Decoder: CRC failed" << std::endl;
++stats_crc_errors;
return;
}
// Fallback: average per-symbol SNR
value total_snr = 0;
int snr_count = 0;
for (int i = 1; i < symbol_index_; ++i) {
if (snr[i] > 0) {
total_snr += snr[i];
snr_count++;
}
}
if (snr_count > 0) {
last_avg_snr_ = 10 * std::log10(total_snr / snr_count);
}
// Extract data
for (int i = 0; i < data_bits; ++i)
CODE::set_le_bit(data, i, mesg[i].v[best] < 0);
// Descramble
CODE::Xorshift32 scrambler;
for (int i = 0; i < data_bytes; ++i)
data[i] ^= scrambler();
// BER: re-encode decoded message and compare against received hard decisions
int code_len = 1 << code_order;
for (int i = 0; i < data_bits + 32; ++i)
ber_mesg[i] = (mesg[i].v[best] < 0) ? -1 : 1;
ber_encoder(ber_code, ber_mesg, frozen_bits, code_order);
int bit_errors = 0;
for (int i = 0; i < code_len; ++i) {
if ((code[i] < 0) != (ber_code[i] < 0))
bit_errors++;
}
last_ber_ = value(bit_errors) / value(code_len);
if (ber_ema_ < 0)
ber_ema_ = last_ber_;
else
ber_ema_ = value(0.3) * last_ber_ + value(0.7) * ber_ema_;
// use known-correct codeword as referenc
build_fwd_perm(fwd_perm_table, code_order);
value corr_sp = 0, corr_np = 0;
int bk = 0;
for (int sj = 1; sj <= symbol_count; ++sj) {
int soff = saved_seed_off[sj];
for (int i = 0; i < tone_count; ++i) {
if (i % block_length != soff) {
int bits = mod_bits;
if (mod_bits == 3 && bk % 32 == 30) bits = 2;
if (mod_bits == 6 && bk % 64 == 60) bits = 4;
if (mod_bits == 10 && bk % 128 == 120) bits = 8;
if (mod_bits == 12 && bk % 128 == 120) bits = 8;
code_type ideal_bits[mod_max];
for (int b = 0; b < bits; ++b)
ideal_bits[b] = ber_code[fwd_perm_table[bk + b]];
cmplx ideal = map_bits(ideal_bits, bits);
cmplx error = saved_demod[sj * tone_count + i] - ideal;
corr_sp += norm(ideal);
corr_np += norm(error);
bk += bits;
}
}
}
if (corr_np > 0)
last_avg_snr_ = 10 * std::log10(corr_sp / corr_np);
std::cerr << "Decoder: Frame decoded " << data_bytes << " bytes, SNR=" << last_avg_snr_ << " dB, BER=" << (last_ber_ * 100) << "%" << std::endl;
callback(data, data_bytes);
}
};
using Encoder48k = ModemEncoder<float, DSP::Complex<float>, 48000>;
using Decoder48k = ModemDecoder<float, DSP::Complex<float>, 48000>;
Reference in a new issue View git blame Copy permalink