mirror of
https://github.com/portapack-mayhem/mayhem-firmware.git
synced 2026-09-10 08:39:29 +00:00
Added tetra rx ext app (#3257)
This commit is contained in:
Totoo
GitHub
@@ -0,0 +1,239 @@
|
||||
#include "proc_tetra.hpp"
|
||||
#include "portapack_shared_memory.hpp"
|
||||
#include "sine_table_int8.hpp"
|
||||
#include "event_m4.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
TetraProcessor::TetraProcessor() {
|
||||
decim_0.configure(taps_25k0_tetra_decim_0.taps);
|
||||
decim_1.configure(taps_25k0_tetra_decim_1.taps);
|
||||
|
||||
baseband_thread.start();
|
||||
configured = true;
|
||||
}
|
||||
|
||||
void TetraProcessor::execute(const buffer_c8_t& buffer) {
|
||||
if (!configured) return;
|
||||
|
||||
const auto decim_0_out = decim_0.execute(buffer, dst_buffer_0);
|
||||
const auto channel_out = decim_1.execute(decim_0_out, dst_buffer_1);
|
||||
|
||||
feed_channel_stats(channel_out);
|
||||
|
||||
for (size_t i = 0; i < channel_out.count; i++) {
|
||||
complex16_t sample = channel_out.p[i];
|
||||
|
||||
// Carrier PLL Rotation
|
||||
uint8_t phase_idx = (pll_phase >> 24) & 0xFF;
|
||||
uint8_t cos_idx = (phase_idx + 64) & 0xFF;
|
||||
int16_t rotated_real = (sample.real() * sine_table_i8[cos_idx] - sample.imag() * sine_table_i8[phase_idx]) >> 7;
|
||||
int16_t rotated_imag = (sample.real() * sine_table_i8[phase_idx] + sample.imag() * sine_table_i8[cos_idx]) >> 7;
|
||||
complex16_t rotated_sample = {rotated_real, rotated_imag};
|
||||
|
||||
uint32_t old_phase = symbol_phase;
|
||||
symbol_phase += symbol_phase_inc;
|
||||
|
||||
// Half-symbol timing
|
||||
if ((old_phase < 0x80000000) && (symbol_phase >= 0x80000000)) {
|
||||
mid_sample = rotated_sample;
|
||||
}
|
||||
|
||||
// Full-symbol timing
|
||||
if (symbol_phase < old_phase) {
|
||||
prev_prompt = prompt_sample;
|
||||
prompt_sample = rotated_sample;
|
||||
|
||||
// Gardner Timing Error Detector
|
||||
int32_t err_i = (mid_sample.real() * (prompt_sample.real() - prev_prompt.real()));
|
||||
int32_t err_q = (mid_sample.imag() * (prompt_sample.imag() - prev_prompt.imag()));
|
||||
int32_t timing_error = err_i + err_q;
|
||||
|
||||
const int64_t delta = (timing_error >> 16);
|
||||
int64_t inc = static_cast<int64_t>(symbol_phase_inc) + delta;
|
||||
// CLAMPING: Prevent the NCO from drifting into the noise!
|
||||
if (inc < 1600000000) inc = 1600000000;
|
||||
if (inc > 1620000000) inc = 1620000000;
|
||||
symbol_phase_inc = static_cast<uint32_t>(inc);
|
||||
|
||||
process_symbol(prompt_sample);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void TetraProcessor::process_symbol(const complex16_t& current_sample) {
|
||||
// Differential phase demodulation
|
||||
int32_t dot_i = (current_sample.real() * delay_sample.real()) + (current_sample.imag() * delay_sample.imag());
|
||||
int32_t dot_q = (current_sample.imag() * delay_sample.real()) - (current_sample.real() * delay_sample.imag());
|
||||
delay_sample = current_sample;
|
||||
|
||||
// Phase error for Carrier PLL
|
||||
int32_t phase_err = 0;
|
||||
if (dot_i > 0 && dot_q > 0)
|
||||
phase_err = dot_q - dot_i;
|
||||
else if (dot_i < 0 && dot_q > 0)
|
||||
phase_err = dot_q + dot_i;
|
||||
else if (dot_i < 0 && dot_q < 0)
|
||||
phase_err = -dot_q + dot_i;
|
||||
else
|
||||
phase_err = -dot_q - dot_i;
|
||||
|
||||
pll_freq += (phase_err * pll_beta) >> 8;
|
||||
|
||||
// CLAMPING: Prevent frequency tracking from flying away
|
||||
if (pll_freq > 400000000) pll_freq = 400000000;
|
||||
if (pll_freq < -400000000) pll_freq = -400000000;
|
||||
|
||||
pll_phase += ((phase_err * pll_alpha) >> 8) + pll_freq;
|
||||
|
||||
// ETSI EN 300 392-2 pi/4 DQPSK bit mapping
|
||||
uint8_t dibit = 0;
|
||||
if (dot_i > 0 && dot_q > 0)
|
||||
dibit = 0b00;
|
||||
else if (dot_i < 0 && dot_q > 0)
|
||||
dibit = 0b01;
|
||||
else if (dot_i < 0 && dot_q < 0)
|
||||
dibit = 0b11;
|
||||
else
|
||||
dibit = 0b10;
|
||||
|
||||
// Insert bits into history buffer and sync register
|
||||
for (int b = 1; b >= 0; b--) {
|
||||
uint8_t bit_val = (dibit >> b) & 0x01;
|
||||
|
||||
size_t byte_idx = (history_write_idx / 8) % 128;
|
||||
if ((history_write_idx % 8) == 0) bit_history_buffer[byte_idx] = 0;
|
||||
|
||||
bit_history_buffer[byte_idx] |= (bit_val << (7 - (history_write_idx % 8)));
|
||||
history_write_idx = (history_write_idx + 1) % 1024;
|
||||
bit_count++;
|
||||
|
||||
sync_register = ((sync_register << 1) | bit_val);
|
||||
|
||||
uint64_t sync =
|
||||
sync_register & 0x3FFFFFFFFFULL;
|
||||
|
||||
uint32_t err_pos =
|
||||
__builtin_popcountll(sync ^ Y_SYNC);
|
||||
|
||||
uint32_t err_neg =
|
||||
__builtin_popcountll(
|
||||
sync ^
|
||||
(~Y_SYNC & 0x3FFFFFFFFFULL));
|
||||
|
||||
if (!pending_dsb.valid && bit_count >= Y_SYNC_BITS && (err_pos <= 4 || err_neg <= 4)) {
|
||||
const uint64_t sync_start = bit_count - Y_SYNC_BITS;
|
||||
|
||||
if (sync_start >= SYNC_OFFSET) {
|
||||
pending_dsb.valid = true;
|
||||
pending_dsb.burst_start = sync_start - SYNC_OFFSET;
|
||||
pending_dsb.ready_at = pending_dsb.burst_start + BURST_BITS;
|
||||
pending_dsb.inverted = err_neg < err_pos;
|
||||
pending_dsb.errors = std::min(err_pos, err_neg);
|
||||
}
|
||||
}
|
||||
|
||||
if (ENABLE_DNB_MESSAGES) {
|
||||
const uint32_t train_mask = (1UL << DNB_TRAIN_BITS) - 1;
|
||||
const uint32_t train = sync_register & train_mask;
|
||||
const uint32_t n_err_pos = __builtin_popcount(train ^ N_SYNC);
|
||||
const uint32_t n_err_neg = __builtin_popcount(train ^ (~N_SYNC & train_mask));
|
||||
const uint32_t p_err_pos = __builtin_popcount(train ^ P_SYNC);
|
||||
const uint32_t p_err_neg = __builtin_popcount(train ^ (~P_SYNC & train_mask));
|
||||
|
||||
if (!pending_dnb.valid && bit_count >= DNB_TRAIN_BITS && (n_err_pos <= 1 || n_err_neg <= 1 || p_err_pos <= 1 || p_err_neg <= 1)) {
|
||||
const bool p_train = std::min(p_err_pos, p_err_neg) < std::min(n_err_pos, n_err_neg);
|
||||
const uint32_t pos_err = p_train ? p_err_pos : n_err_pos;
|
||||
const uint32_t neg_err = p_train ? p_err_neg : n_err_neg;
|
||||
|
||||
const uint64_t train_start = bit_count - DNB_TRAIN_BITS;
|
||||
const uint64_t block1_start = train_start - 230;
|
||||
const uint64_t block2_start = train_start + 38;
|
||||
|
||||
if (train_start >= 230) {
|
||||
pending_dnb.valid = true;
|
||||
pending_dnb.block1_start = block1_start;
|
||||
pending_dnb.block2_start = block2_start;
|
||||
pending_dnb.ready_at = block2_start + DNB_BLK_BITS;
|
||||
pending_dnb.inverted = neg_err < pos_err;
|
||||
pending_dnb.p_train = p_train;
|
||||
pending_dnb.errors = std::min(pos_err, neg_err);
|
||||
}
|
||||
}
|
||||
|
||||
if (pending_dnb.valid && bit_count >= pending_dnb.ready_at)
|
||||
push_dnb_to_ui();
|
||||
}
|
||||
|
||||
if (pending_dsb.valid && bit_count >= pending_dsb.ready_at)
|
||||
push_burst_to_ui();
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t TetraProcessor::history_bit(uint64_t absolute_bit) const {
|
||||
const size_t p = absolute_bit % 1024;
|
||||
|
||||
return (bit_history_buffer[p >> 3] >>
|
||||
(7 - (p & 7))) &
|
||||
1;
|
||||
}
|
||||
|
||||
void TetraProcessor::push_burst_to_ui() {
|
||||
std::array<uint8_t, 63> burst{};
|
||||
|
||||
for (size_t i = 0; i < BURST_BITS; i++) {
|
||||
uint8_t b = history_bit(pending_dsb.burst_start + i);
|
||||
|
||||
if (pending_dsb.inverted)
|
||||
b ^= 1;
|
||||
|
||||
burst[i >> 3] |=
|
||||
b << (7 - (i & 7));
|
||||
}
|
||||
|
||||
shared_memory.application_queue.push(
|
||||
TetraBurstMessage(
|
||||
burst.data(),
|
||||
pending_dsb.inverted,
|
||||
pending_dsb.errors));
|
||||
|
||||
pending_dsb.valid = false;
|
||||
}
|
||||
|
||||
void TetraProcessor::push_dnb_to_ui() {
|
||||
std::array<uint8_t, 54> burst{};
|
||||
|
||||
for (size_t i = 0; i < DNB_BLK_BITS; i++) {
|
||||
uint8_t b = history_bit(pending_dnb.block1_start + i);
|
||||
if (pending_dnb.inverted)
|
||||
b ^= 1;
|
||||
burst[i >> 3] |= b << (7 - (i & 7));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < DNB_BLK_BITS; i++) {
|
||||
uint8_t b = history_bit(pending_dnb.block2_start + i);
|
||||
if (pending_dnb.inverted)
|
||||
b ^= 1;
|
||||
const size_t out_bit = DNB_BLK_BITS + i;
|
||||
burst[out_bit >> 3] |= b << (7 - (out_bit & 7));
|
||||
}
|
||||
|
||||
shared_memory.application_queue.push(
|
||||
TetraDnbMessage(
|
||||
burst.data(),
|
||||
pending_dnb.inverted,
|
||||
pending_dnb.errors,
|
||||
pending_dnb.p_train));
|
||||
|
||||
pending_dnb.valid = false;
|
||||
}
|
||||
|
||||
void TetraProcessor::on_message(const Message* const msg) {
|
||||
(void)msg;
|
||||
}
|
||||
|
||||
int main() {
|
||||
EventDispatcher event_dispatcher{std::make_unique<TetraProcessor>()};
|
||||
event_dispatcher.run();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user