kopia lustrzana https://github.com/mobilinkd/tnc3-firmware
Major change to M17 demodulator. Improves demod performance, perhaps at the expense of tracking signals with extremely inaccurate clocks. It should be able to handle up to ~500ppm error between TX & RX. Improve LICH decoding.
Rob Riggs
rodzic
5d3a3b35de
commit
8d1d34142b
|
|
@ -1,78 +1,458 @@
|
|||
// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
|
||||
// Copyright 2020-2021 Rob Riggs <rob@mobilinkd.com>
|
||||
// All rights reserved.
|
||||
|
||||
#include "AudioLevel.hpp"
|
||||
#include "M17Demodulator.h"
|
||||
#include "Util.h"
|
||||
|
||||
#include "main.h"
|
||||
|
||||
#include "stm32l4xx_hal.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
|
||||
namespace mobilinkd { namespace tnc {
|
||||
|
||||
//m17::Indicator lsf_indicator{GPIOB, GPIO_PIN_0};
|
||||
//m17::Indicator dcd_indicator{GPIOA, GPIO_PIN_7};
|
||||
//m17::Indicator str_indicator{GPIOA, GPIO_PIN_2};
|
||||
|
||||
void M17Demodulator::start()
|
||||
{
|
||||
SysClock80();
|
||||
SysClock72();
|
||||
HAL_RCCEx_DisableLSCO();
|
||||
|
||||
demod_filter.init(m17::rrc_taps_11.data());
|
||||
demod_filter.init(m17::rrc_taps_f15.data());
|
||||
passall(kiss::settings().options & KISS_OPTION_PASSALL);
|
||||
polarity = kiss::settings().rx_rev_polarity() ? -1 : 1;
|
||||
audio::virtual_ground = (VREF + 1) / 2;
|
||||
|
||||
hadc1.Init.OversamplingMode = DISABLE;
|
||||
if (HAL_ADC_Init(&hadc1) != HAL_OK)
|
||||
{
|
||||
CxxErrorHandler();
|
||||
}
|
||||
// hadc1.Init.OversamplingMode = DISABLE;
|
||||
// if (HAL_ADC_Init(&hadc1) != HAL_OK)
|
||||
// {
|
||||
// CxxErrorHandler();
|
||||
// }
|
||||
|
||||
ADC_ChannelConfTypeDef sConfig;
|
||||
|
||||
sConfig.Channel = AUDIO_IN;
|
||||
sConfig.Rank = ADC_REGULAR_RANK_1;
|
||||
sConfig.SingleDiff = ADC_SINGLE_ENDED;
|
||||
sConfig.SamplingTime = ADC_SAMPLETIME_92CYCLES_5;
|
||||
sConfig.SamplingTime = ADC_SAMPLETIME_12CYCLES_5;
|
||||
sConfig.OffsetNumber = ADC_OFFSET_NONE;
|
||||
sConfig.Offset = 0;
|
||||
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
|
||||
CxxErrorHandler();
|
||||
|
||||
startADC(1665, ADC_BLOCK_SIZE);
|
||||
startADC(1499, ADC_BLOCK_SIZE);
|
||||
// getModulator().start_loopback();
|
||||
dcd_off();
|
||||
}
|
||||
|
||||
void M17Demodulator::update_values(uint8_t index)
|
||||
{
|
||||
correlator.apply([this,index](float t){dev.sample(t);}, index);
|
||||
dev.update();
|
||||
idev = dev.idev() * polarity;
|
||||
sync_sample_index = index;
|
||||
}
|
||||
|
||||
void M17Demodulator::dcd_on()
|
||||
{
|
||||
// Data carrier newly detected.
|
||||
INFO("dcd = %d", int(dcd.level() * 100));
|
||||
dcd_ = true;
|
||||
sync_count = 0;
|
||||
dev.reset();
|
||||
framer.reset();
|
||||
decoder.reset();
|
||||
missing_sync_count = 0;
|
||||
// dcd_indicator.off();
|
||||
}
|
||||
|
||||
void M17Demodulator::dcd_off()
|
||||
{
|
||||
// Just lost data carrier.
|
||||
INFO("dcd = %d", int(dcd.level() * 100));
|
||||
dcd_ = false;
|
||||
demodState = DemodState::UNLOCKED;
|
||||
// dcd_indicator.on();
|
||||
adc_timing_adjust = 0;
|
||||
prev_clock_estimate = 1.;
|
||||
}
|
||||
|
||||
void M17Demodulator::initialize(const q15_t* input)
|
||||
{
|
||||
auto filtered = demod_filter(input, 1.f / 2560.f);
|
||||
for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i)
|
||||
{
|
||||
auto filtered_sample = filtered[i];
|
||||
correlator.sample(filtered_sample);
|
||||
}
|
||||
}
|
||||
|
||||
void M17Demodulator::update_dcd(const q15_t* input)
|
||||
{
|
||||
static constexpr float inv = 1.0 / 2048.0;
|
||||
|
||||
if (!dcd_ && dcd.dcd())
|
||||
{
|
||||
dcd_on();
|
||||
need_clock_reset_ = true;
|
||||
}
|
||||
else if (dcd_ && !dcd.dcd())
|
||||
{
|
||||
dcd_off();
|
||||
}
|
||||
|
||||
for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i)
|
||||
{
|
||||
dcd(input[i] * inv);
|
||||
}
|
||||
}
|
||||
|
||||
[[gnu::noinline]]
|
||||
void M17Demodulator::do_unlocked()
|
||||
{
|
||||
// We expect to find the preamble immediately after DCD.
|
||||
if (missing_sync_count < 1920)
|
||||
{
|
||||
missing_sync_count += 1;
|
||||
auto sync_index = preamble_sync(correlator);
|
||||
auto sync_updated = preamble_sync.updated();
|
||||
if (sync_updated)
|
||||
{
|
||||
sync_count = 0;
|
||||
missing_sync_count = 0;
|
||||
need_clock_reset_ = true;
|
||||
dev.reset();
|
||||
update_values(sync_index);
|
||||
sample_index = sync_index;
|
||||
demodState = DemodState::LSF_SYNC;
|
||||
INFO("P sync %d", sync_index);
|
||||
}
|
||||
}
|
||||
else // Otherwise we start searching for a sync word.
|
||||
{
|
||||
auto sync_index = lsf_sync(correlator);
|
||||
auto sync_updated = lsf_sync.updated();
|
||||
if (sync_updated)
|
||||
{
|
||||
sync_count = 0;
|
||||
missing_sync_count = 0;
|
||||
need_clock_reset_ = true;
|
||||
dev.reset();
|
||||
update_values(sync_index);
|
||||
sample_index = sync_index;
|
||||
demodState = DemodState::FRAME;
|
||||
if (sync_updated < 0)
|
||||
{
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
INFO("S sync %d", int(sync_index));
|
||||
}
|
||||
else
|
||||
{
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::LSF;
|
||||
INFO("L sync %d", int(sync_index));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for LSF sync word. We only enter the DemodState::LSF_SYNC state
|
||||
* if a preamble sync has been detected, which also means that sample_index
|
||||
* has been initialized to a sane value for the baseband.
|
||||
*/
|
||||
[[gnu::noinline]]
|
||||
void M17Demodulator::do_lsf_sync()
|
||||
{
|
||||
float sync_triggered = 0.;
|
||||
|
||||
if (correlator.index() == sample_index)
|
||||
{
|
||||
sync_triggered = preamble_sync.triggered(correlator);
|
||||
// INFO("PSync = %d", int(sync_triggered));
|
||||
if (sync_triggered > 0.1)
|
||||
{
|
||||
ITM_SendChar('.');
|
||||
return;
|
||||
}
|
||||
sync_triggered = lsf_sync.triggered(correlator);
|
||||
if (std::abs(sync_triggered) > 0.1)
|
||||
{
|
||||
missing_sync_count = 0;
|
||||
need_clock_update_ = true;
|
||||
update_values(sample_index);
|
||||
INFO("LSync = %d", int(sync_triggered));
|
||||
if (sync_triggered > 0)
|
||||
{
|
||||
demodState = DemodState::FRAME;
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::LSF;
|
||||
INFO("+l sync %d", int(sample_index));
|
||||
}
|
||||
else
|
||||
{
|
||||
demodState = DemodState::FRAME;
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
INFO("+s sync %d", int(sample_index));
|
||||
}
|
||||
}
|
||||
else if (++missing_sync_count > 192)
|
||||
{
|
||||
demodState = DemodState::UNLOCKED;
|
||||
INFO("l unlock %d", int(missing_sync_count));
|
||||
missing_sync_count = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
update_values(sample_index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for a stream sync word (LSF sync word that is maximally negative).
|
||||
* We can enter DemodState::STREAM_SYNC from either a valid LSF decode for
|
||||
* an audio stream, or from a stream frame decode.
|
||||
*
|
||||
*/
|
||||
[[gnu::noinline]]
|
||||
void M17Demodulator::do_stream_sync()
|
||||
{
|
||||
uint8_t sync_index = lsf_sync(correlator);
|
||||
int8_t sync_updated = lsf_sync.updated();
|
||||
sync_count += 1;
|
||||
if (sync_updated < 0)
|
||||
{
|
||||
missing_sync_count = 0;
|
||||
if (sync_count <= 70)
|
||||
{
|
||||
update_values(sync_index);
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
demodState = DemodState::FRAME;
|
||||
INFO("s early");
|
||||
}
|
||||
else if (sync_count > 70)
|
||||
{
|
||||
update_values(sync_index);
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
demodState = DemodState::FRAME;
|
||||
INFO("s sync %d", int(sync_index));
|
||||
}
|
||||
return;
|
||||
}
|
||||
else if (sync_count > 87)
|
||||
{
|
||||
update_values(sync_index);
|
||||
missing_sync_count += 1;
|
||||
if (missing_sync_count < MAX_MISSING_SYNC)
|
||||
{
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
demodState = DemodState::FRAME;
|
||||
INFO("s unsync %d", int(missing_sync_count));
|
||||
}
|
||||
else
|
||||
{
|
||||
demodState = DemodState::LSF_SYNC;
|
||||
INFO("s unlock");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for a packet sync word. DemodState::PACKET_SYNC can only be
|
||||
* entered from a valid LSF frame decode with the data/packet type bit set.
|
||||
*/
|
||||
[[gnu::noinline]]
|
||||
void M17Demodulator::do_packet_sync()
|
||||
{
|
||||
auto sync_index = packet_sync(correlator);
|
||||
auto sync_updated = packet_sync.updated();
|
||||
sync_count += 1;
|
||||
if (sync_count > 70 && sync_updated)
|
||||
{
|
||||
missing_sync_count = 0;
|
||||
update_values(sync_index);
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::PACKET;
|
||||
demodState = DemodState::FRAME;
|
||||
INFO("k sync");
|
||||
return;
|
||||
}
|
||||
else if (sync_count > 87)
|
||||
{
|
||||
missing_sync_count += 1;
|
||||
if (missing_sync_count < MAX_MISSING_SYNC)
|
||||
{
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::PACKET;
|
||||
demodState = DemodState::FRAME;
|
||||
INFO("k unsync");
|
||||
}
|
||||
else
|
||||
{
|
||||
demodState = DemodState::UNLOCKED;
|
||||
INFO("k unlock");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[[gnu::noinline]]
|
||||
void M17Demodulator::do_frame(float filtered_sample, hdlc::IoFrame*& frame_result)
|
||||
{
|
||||
if (correlator.index() != sample_index) return;
|
||||
|
||||
float sample = filtered_sample - dev.offset();
|
||||
sample *= idev;
|
||||
|
||||
auto n = llr<float, 4>(sample);
|
||||
int8_t* tmp;
|
||||
auto len = framer(n, &tmp);
|
||||
if (len != 0)
|
||||
{
|
||||
need_clock_update_ = true;
|
||||
|
||||
std::copy(tmp, tmp + len, buffer.begin());
|
||||
auto valid = decoder(sync_word_type, buffer, frame_result, ber);
|
||||
INFO("demod: %d, dt: %7d, evma: %5d, dev: %5d, freq: %5d, index: %d, %d, %d ber: %d",
|
||||
int(decoder.state()), int(clock_recovery.clock_estimate() * 1000000),
|
||||
int(dev.error() * 1000), int(dev.deviation() * 1000),
|
||||
int(dev.offset() * 1000),
|
||||
int(sample_index), int(clock_recovery.sample_index()), int(sync_sample_index),
|
||||
ber);
|
||||
|
||||
switch (decoder.state())
|
||||
{
|
||||
case M17FrameDecoder::State::STREAM:
|
||||
demodState = DemodState::STREAM_SYNC;
|
||||
break;
|
||||
case M17FrameDecoder::State::LSF:
|
||||
// If state == LSF, we need to recover LSF from LICH.
|
||||
demodState = DemodState::STREAM_SYNC;
|
||||
break;
|
||||
default:
|
||||
demodState = DemodState::PACKET_SYNC;
|
||||
break;
|
||||
}
|
||||
|
||||
sync_count = 0;
|
||||
|
||||
switch (valid)
|
||||
{
|
||||
case M17FrameDecoder::DecodeResult::FAIL:
|
||||
WARN("decode invalid");
|
||||
if (frame_result && !passall_)
|
||||
{
|
||||
hdlc::release(frame_result);
|
||||
frame_result = nullptr;
|
||||
}
|
||||
break;
|
||||
case M17FrameDecoder::DecodeResult::EOS:
|
||||
demodState = DemodState::LSF_SYNC;
|
||||
INFO("EOS");
|
||||
break;
|
||||
case M17FrameDecoder::DecodeResult::OK:
|
||||
break;
|
||||
case M17FrameDecoder::DecodeResult::INCOMPLETE:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
hdlc::IoFrame* M17Demodulator::operator()(const q15_t* input)
|
||||
{
|
||||
hdlc::IoFrame* result = nullptr;
|
||||
static int16_t initializing = 10;
|
||||
|
||||
auto filtered = demod_filter.filter(const_cast<q15_t*>(input));
|
||||
hdlc::IoFrame* frame_result = nullptr;
|
||||
|
||||
// We need to pump a few frames through on startup to initialize
|
||||
// the demodulator.
|
||||
if (__builtin_expect((initializing), 0))
|
||||
{
|
||||
--initializing;
|
||||
initialize(input);
|
||||
return frame_result;
|
||||
}
|
||||
|
||||
// str_indicator.on();
|
||||
update_dcd(input);
|
||||
|
||||
if (!dcd_)
|
||||
{
|
||||
dcd.update();
|
||||
// str_indicator.off();
|
||||
return frame_result;
|
||||
}
|
||||
|
||||
// Do adc_micro_adjustment() here?
|
||||
// adc_micro_adjustment();
|
||||
|
||||
auto filtered = demod_filter(input, 1.f / 2560.f);
|
||||
// getModulator().loopback(filtered);
|
||||
|
||||
for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i)
|
||||
{
|
||||
auto filtered_sample = filtered[i];
|
||||
correlator.sample(filtered_sample);
|
||||
|
||||
if (sample_now)
|
||||
if (correlator.index() == 0)
|
||||
{
|
||||
samples[2] = filtered_sample;
|
||||
sample_now = false;
|
||||
frame(demod(), result);
|
||||
if (need_clock_reset_)
|
||||
{
|
||||
clock_recovery.reset();
|
||||
need_clock_reset_ = false;
|
||||
sample_index = sync_sample_index;
|
||||
adc_timing_adjust = 0;
|
||||
}
|
||||
else if (need_clock_update_) // must avoid update immediately after reset.
|
||||
{
|
||||
clock_recovery.update();
|
||||
uint8_t clock_index = clock_recovery.sample_index();
|
||||
uint8_t clock_diff = std::abs(sample_index - clock_index);
|
||||
uint8_t sync_diff = std::abs(sample_index - sync_sample_index);
|
||||
bool clock_diff_ok = clock_diff <= 1 || clock_diff == 9;
|
||||
bool sync_diff_ok = sync_diff <= 1 || sync_diff == 9;
|
||||
if (clock_diff_ok) sample_index = clock_index;
|
||||
else if (sync_diff_ok) sample_index = sync_sample_index;
|
||||
// else unchanged.
|
||||
need_clock_update_ = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
clock_recovery(filtered_sample);
|
||||
|
||||
if (demodState != DemodState::UNLOCKED && correlator.index() == sample_index)
|
||||
{
|
||||
dev.sample(filtered_sample);
|
||||
}
|
||||
|
||||
switch (demodState)
|
||||
{
|
||||
t += dt;
|
||||
if (t < 1.0)
|
||||
{
|
||||
samples[0] = filtered_sample;
|
||||
}
|
||||
else
|
||||
{
|
||||
t -= 1.0;
|
||||
samples[1] = filtered_sample;
|
||||
sample_now = true;
|
||||
}
|
||||
case DemodState::UNLOCKED:
|
||||
// In this state, the sample_index is unknown. We need to find
|
||||
// a sync word to find the proper sample_index. We only leave
|
||||
// this state if we believe that we have a valid sample_index.
|
||||
do_unlocked();
|
||||
break;
|
||||
case DemodState::LSF_SYNC:
|
||||
do_lsf_sync();
|
||||
break;
|
||||
case DemodState::STREAM_SYNC:
|
||||
do_stream_sync();
|
||||
break;
|
||||
case DemodState::PACKET_SYNC:
|
||||
do_packet_sync();
|
||||
break;
|
||||
case DemodState::FRAME:
|
||||
do_frame(filtered_sample,frame_result);
|
||||
break;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
dcd.update();
|
||||
// str_indicator.off();
|
||||
|
||||
return frame_result;
|
||||
}
|
||||
|
||||
}} // mobilinkd::tnc
|
||||
|
|
|
|||
|
|
@ -1,32 +1,29 @@
|
|||
// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
|
||||
// Copyright 2020-2021 Rob Riggs <rob@mobilinkd.com>
|
||||
// All rights reserved.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Demodulator.hpp"
|
||||
#include "AudioLevel.hpp"
|
||||
#include "AudioInput.hpp"
|
||||
#include "AudioLevel.hpp"
|
||||
#include "ClockRecovery.h"
|
||||
#include "Correlator.h"
|
||||
#include "DataCarrierDetect.h"
|
||||
#include "Demodulator.hpp"
|
||||
#include "FreqDevEstimator.h"
|
||||
#include "GPIO.hpp"
|
||||
#include "Log.h"
|
||||
#include "PhaseEstimator.h"
|
||||
#include "DeviationError.h"
|
||||
#include "FrequencyError.h"
|
||||
#include "SymbolEvm.h"
|
||||
#include "Util.h"
|
||||
#include "CarrierDetect.h"
|
||||
#include "M17Synchronizer.h"
|
||||
#include "M17Framer.h"
|
||||
#include "M17FrameDecoder.h"
|
||||
#include "KissHardware.hpp"
|
||||
#include "ModulatorTask.hpp"
|
||||
#include "Modulator.hpp"
|
||||
#include "Log.h"
|
||||
#include "M17.h"
|
||||
#include "M17FrameDecoder.h"
|
||||
#include "M17Framer.h"
|
||||
#include "Modulator.hpp"
|
||||
#include "ModulatorTask.hpp"
|
||||
#include "Util.h"
|
||||
|
||||
#include <arm_math.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <experimental/array>
|
||||
#include <optional>
|
||||
#include <tuple>
|
||||
|
||||
|
|
@ -37,65 +34,80 @@ struct M17Demodulator : IDemodulator
|
|||
static constexpr uint32_t ADC_BLOCK_SIZE = 192;
|
||||
static_assert(audio::ADC_BUFFER_SIZE >= ADC_BLOCK_SIZE);
|
||||
|
||||
static constexpr auto evm_b = std::experimental::make_array<float>(0.02008337, 0.04016673, 0.02008337);
|
||||
static constexpr auto evm_a = std::experimental::make_array<float>(1.0, -1.56101808, 0.64135154);
|
||||
|
||||
static constexpr uint32_t SAMPLE_RATE = 48000;
|
||||
static constexpr uint16_t VREF = 4095;
|
||||
static constexpr uint32_t SYMBOL_RATE = 4800;
|
||||
static constexpr uint32_t SAMPLES_PER_SYMBOL = SAMPLE_RATE / SYMBOL_RATE;
|
||||
static constexpr uint16_t VREF = 16383;
|
||||
static constexpr int16_t MAX_SAMPLE_ADJUST = 140;
|
||||
static constexpr int16_t MIN_SAMPLE_ADJUST = -140;
|
||||
|
||||
using audio_filter_t = Q15FirFilter<ADC_BLOCK_SIZE, m17::FILTER_TAP_NUM_11>;
|
||||
using demod_result_t = std::tuple<float, float, int, float>;
|
||||
static constexpr float sample_rate = SAMPLE_RATE;
|
||||
static constexpr float symbol_rate = SYMBOL_RATE;
|
||||
|
||||
enum class DemodState { UNLOCKED, LSF_SYNC, FRAME_SYNC, FRAME };
|
||||
static constexpr uint8_t MAX_MISSING_SYNC = 5;
|
||||
|
||||
using audio_filter_t = FirFilter<ADC_BLOCK_SIZE, m17::FILTER_TAP_NUM_15>;
|
||||
using sync_word_t = m17::SyncWord<m17::Correlator>;
|
||||
|
||||
enum class DemodState { UNLOCKED, LSF_SYNC, STREAM_SYNC, PACKET_SYNC, FRAME };
|
||||
|
||||
audio_filter_t demod_filter;
|
||||
m17::DataCarrierDetect<float, SAMPLE_RATE, 500> dcd{2500, 4000, 1.0, 10.0};
|
||||
m17::ClockRecovery<float, SAMPLE_RATE, SYMBOL_RATE> clock_recovery;
|
||||
|
||||
m17::Correlator correlator;
|
||||
sync_word_t preamble_sync{{+3,-3,+3,-3,+3,-3,+3,-3}, 29.f};
|
||||
sync_word_t lsf_sync{{+3,+3,+3,+3,-3,-3,+3,-3}, 31.f, -31.f};
|
||||
sync_word_t packet_sync{{3,-3,3,3,-3,-3,-3,-3}, 32.f};
|
||||
|
||||
m17::FreqDevEstimator<float> dev;
|
||||
|
||||
audio_filter_t demod_filter{m17::rrc_taps_11.data()};
|
||||
const float sample_rate = 48000.0;
|
||||
const float symbol_rate = 4800.0;
|
||||
float gain = 0.01;
|
||||
std::array<q15_t, 3> samples;
|
||||
std::array<float, 3> f_samples;
|
||||
std::array<int8_t, 368> buffer;
|
||||
float t = 0.0f;
|
||||
float dt = 0.1f; // symbol_rate / sample_rate.
|
||||
const float ideal_dt = dt;
|
||||
bool sample_now = false;
|
||||
float estimated_deviation = 1.0;
|
||||
float estimated_frequency_offset = 0.0;
|
||||
float evm_average = 0.0;
|
||||
bool decoding = false;
|
||||
PhaseEstimator<float> phase = PhaseEstimator<float>(sample_rate, symbol_rate);
|
||||
DeviationError<float> deviation;
|
||||
FrequencyError<float, 32> frequency{evm_b, evm_a};
|
||||
SymbolEvm<float, std::tuple_size<decltype(evm_b)>::value> symbol_evm{evm_b, evm_a};
|
||||
CarrierDetect<float> dcd{evm_b, evm_a, 0.01, 0.75};
|
||||
M17Synchronizer lsf_sync_1{m17::sync_word(m17::LSF_SYNC), 1};
|
||||
M17Synchronizer stream_sync_1{m17::sync_word(m17::STREAM_SYNC), 1};
|
||||
M17Synchronizer stream_sync_3{m17::sync_word(m17::STREAM_SYNC), 3};
|
||||
M17Synchronizer packet_sync_3{m17::sync_word(m17::PACKET_SYNC), 3};
|
||||
int8_t polarity = 1;
|
||||
M17Framer<368> framer;
|
||||
M17FrameDecoder decoder;
|
||||
DemodState demodState = DemodState::UNLOCKED;
|
||||
M17FrameDecoder::SyncWordType sync_word_type = M17FrameDecoder::SyncWordType::LSF;
|
||||
uint8_t sample_index = 0;
|
||||
float idev;
|
||||
|
||||
bool dcd_ = false;
|
||||
bool need_clock_reset_ = false;
|
||||
bool need_clock_update_ = false;
|
||||
|
||||
bool locked_ = false;
|
||||
bool passall_ = false;
|
||||
int ber = -1;
|
||||
int sync_count = 0;
|
||||
int16_t sync_count = 0;
|
||||
uint16_t missing_sync_count = 0;
|
||||
uint8_t sync_sample_index = 0;
|
||||
int16_t adc_timing_adjust = 0;
|
||||
float prev_clock_estimate = 1.;
|
||||
|
||||
|
||||
virtual ~M17Demodulator() {}
|
||||
|
||||
void start() override;
|
||||
|
||||
void dcd_on();
|
||||
void dcd_off();
|
||||
void initialize(const q15_t* input);
|
||||
void update_dcd(const q15_t* input);
|
||||
void do_unlocked();
|
||||
void do_lsf_sync();
|
||||
void do_packet_sync();
|
||||
void do_stream_sync();
|
||||
void do_frame(float filtered_sample, hdlc::IoFrame*& frame_result);
|
||||
|
||||
void stop() override
|
||||
{
|
||||
// getModulator().stop_loopback();
|
||||
stopADC();
|
||||
locked_ = false;
|
||||
dcd_off();
|
||||
}
|
||||
|
||||
bool locked() const override
|
||||
{
|
||||
return locked_;
|
||||
return dcd_;
|
||||
}
|
||||
|
||||
size_t size() const override
|
||||
|
|
@ -109,144 +121,7 @@ struct M17Demodulator : IDemodulator
|
|||
decoder.passall(enabled);
|
||||
}
|
||||
|
||||
[[gnu::noinline]]
|
||||
void frame(demod_result_t demod_result, hdlc::IoFrame*& result)
|
||||
{
|
||||
auto [sample, phase, symbol, evm] = demod_result;
|
||||
auto [locked, evma] = dcd(evm);
|
||||
static size_t count = 0;
|
||||
gain = locked ? 0.0025f : 0.01f;
|
||||
|
||||
switch (demodState)
|
||||
{
|
||||
case DemodState::UNLOCKED:
|
||||
if (!locked) {
|
||||
locked_ = false;
|
||||
break;
|
||||
}
|
||||
demodState = DemodState::LSF_SYNC;
|
||||
framer.reset();
|
||||
decoder.reset();
|
||||
sync_count = 0;
|
||||
[[fallthrough]];
|
||||
case DemodState::LSF_SYNC:
|
||||
if (!locked)
|
||||
{
|
||||
demodState = DemodState::UNLOCKED;
|
||||
}
|
||||
else if (lsf_sync_1(from_4fsk(symbol))) // LSF SYNC?
|
||||
{
|
||||
INFO("LSF_SYNC/LSF");
|
||||
demodState = DemodState::FRAME;
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::LSF;
|
||||
}
|
||||
else if (stream_sync_1(from_4fsk(symbol))) // STREAM SYNC for LICH?
|
||||
{
|
||||
INFO("LSF_SYNC/STREAM");
|
||||
demodState = DemodState::FRAME;
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
}
|
||||
break;
|
||||
case DemodState::FRAME_SYNC:
|
||||
if (!locked)
|
||||
{
|
||||
INFO("state: %d, dt: %5d, evm: %5d, evma: %5d, dev: %5d, freq: %5d, locked: %d, ber: %d",
|
||||
int(demodState), int(dt * 10000), int(evm * 1000),
|
||||
int(evma * 1000), int((1.0 / estimated_deviation) * 1000),
|
||||
int(estimated_frequency_offset * 1000),
|
||||
locked_, ber);
|
||||
demodState = DemodState::UNLOCKED;
|
||||
}
|
||||
else if (stream_sync_3(from_4fsk(symbol)) && sync_count == 7)
|
||||
{
|
||||
INFO("STREAM_SYNC");
|
||||
demodState = DemodState::FRAME;
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::STREAM;
|
||||
}
|
||||
else if (packet_sync_3(from_4fsk(symbol)) && sync_count == 7)
|
||||
{
|
||||
INFO("PACKET_SYNC");
|
||||
demodState = DemodState::FRAME;
|
||||
sync_word_type = M17FrameDecoder::SyncWordType::PACKET;
|
||||
}
|
||||
else if (++sync_count == 8)
|
||||
{
|
||||
demodState = DemodState::UNLOCKED;
|
||||
locked_ = false;
|
||||
}
|
||||
break;
|
||||
case DemodState::FRAME:
|
||||
{
|
||||
locked_ = true;
|
||||
auto n = llr<float, 4>(sample);
|
||||
int8_t* tmp;
|
||||
auto len = framer(n, &tmp);
|
||||
if (len != 0)
|
||||
{
|
||||
demodState = DemodState::FRAME_SYNC;
|
||||
sync_count = 0;
|
||||
std::copy(tmp, tmp + len, buffer.begin());
|
||||
auto valid = decoder(sync_word_type, buffer, result, ber);
|
||||
switch (valid)
|
||||
{
|
||||
case M17FrameDecoder::DecodeResult::FAIL:
|
||||
WARN("decode invalid");
|
||||
if (result && !passall_)
|
||||
{
|
||||
|
||||
if (result) hdlc::release(result);
|
||||
result = 0;
|
||||
}
|
||||
break;
|
||||
case M17FrameDecoder::DecodeResult::EOS:
|
||||
demodState = DemodState::LSF_SYNC;
|
||||
break;
|
||||
case M17FrameDecoder::DecodeResult::OK:
|
||||
INFO("valid frame for sync word type %d", int(sync_word_type));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
if ((count++ % 192) == 0)
|
||||
{
|
||||
INFO("state: %d, dt: %5d, evm: %5d, evma: %5d, dev: %5d, freq: %5d, locked: %d, ber: %d",
|
||||
int(demodState), int(dt * 10000), int(evm * 1000),
|
||||
int(evma * 1000), int((1.0 / estimated_deviation) * 1000),
|
||||
int(estimated_frequency_offset * 1000),
|
||||
locked, ber);
|
||||
}
|
||||
}
|
||||
|
||||
[[gnu::noinline]]
|
||||
demod_result_t demod()
|
||||
{
|
||||
int16_t polarity = kiss::settings().rx_rev_polarity() ? -1 : 1;
|
||||
|
||||
f_samples[0] = float(samples[0]) / 8192.0;
|
||||
f_samples[1] = float(samples[1]) / 8192.0;
|
||||
f_samples[2] = float(samples[2]) / 8192.0;
|
||||
|
||||
estimated_deviation = deviation(f_samples[1]);
|
||||
for (auto& sample : f_samples) sample *= estimated_deviation;
|
||||
|
||||
estimated_frequency_offset = frequency(f_samples[1]);
|
||||
for (auto& sample : f_samples) sample -= estimated_frequency_offset;
|
||||
|
||||
auto phase_estimate = phase(f_samples);
|
||||
if (f_samples[1] < 0.0) phase_estimate *= -1.0;
|
||||
|
||||
dt = ideal_dt - (phase_estimate * gain);
|
||||
// dt = std::min(std::max(0.095f, dt), 0.105f);
|
||||
t += dt;
|
||||
|
||||
auto [symbol, evm] = symbol_evm(f_samples[1]);
|
||||
evm_average = symbol_evm.evm();
|
||||
samples[0] = samples[2];
|
||||
|
||||
return std::make_tuple(f_samples[1], phase_estimate, symbol * polarity, evm);
|
||||
}
|
||||
void update_values(uint8_t index);
|
||||
|
||||
hdlc::IoFrame* operator()(const q15_t* input) override;
|
||||
|
||||
|
|
|
|||
|
|
@ -155,7 +155,7 @@ struct M17FrameDecoder
|
|||
|
||||
enum class State {LSF, STREAM, BASIC_PACKET, FULL_PACKET};
|
||||
enum class SyncWordType { LSF, STREAM, PACKET, RESERVED };
|
||||
enum class DecodeResult { FAIL, OK, EOS };
|
||||
enum class DecodeResult { FAIL, OK, EOS, INCOMPLETE };
|
||||
|
||||
State state_ = State::LSF;
|
||||
|
||||
|
|
@ -206,6 +206,8 @@ struct M17FrameDecoder
|
|||
~M17FrameDecoder()
|
||||
{}
|
||||
|
||||
State state() const { return state_; }
|
||||
|
||||
void reset() { state_ = State::LSF; }
|
||||
|
||||
void passall(bool enabled) { passall_ = enabled; }
|
||||
|
|
@ -275,6 +277,9 @@ struct M17FrameDecoder
|
|||
for (auto c : current_lsf) crc_(c);
|
||||
auto checksum = crc_.get();
|
||||
INFO("LSF crc = %04x", checksum);
|
||||
#ifdef KISS_LOGGING
|
||||
dump(current_lsf);
|
||||
#endif
|
||||
|
||||
if (checksum == 0)
|
||||
{
|
||||
|
|
@ -319,7 +324,7 @@ struct M17FrameDecoder
|
|||
return false;
|
||||
}
|
||||
decoded >>= 12; // Remove check bits and parity.
|
||||
DEBUG("Golay decode good for %08lx (%du)", decoded, i);
|
||||
TNC_DEBUG("Golay decode good for %08lx (%du)", decoded, i);
|
||||
// append codeword.
|
||||
if (i & 1)
|
||||
{
|
||||
|
|
@ -351,7 +356,7 @@ struct M17FrameDecoder
|
|||
|
||||
lich_segments |= (1 << fragment_number); // Indicate segment received.
|
||||
INFO("got segment %d, have %02x", int(fragment_number), int(lich_segments));
|
||||
if (lich_segments != 0x3F) return DecodeResult::FAIL; // More to go...
|
||||
if (lich_segments != 0x3F) return DecodeResult::INCOMPLETE; // More to go...
|
||||
|
||||
crc_.reset();
|
||||
for (auto c : output.lich) crc_(c);
|
||||
|
|
@ -359,6 +364,7 @@ struct M17FrameDecoder
|
|||
INFO("LICH crc = %04x", checksum);
|
||||
if (checksum == 0)
|
||||
{
|
||||
lich_segments = 0;
|
||||
state_ = State::STREAM;
|
||||
lsf = tnc::hdlc::acquire_wait();
|
||||
for (auto c : output.lich) lsf->push_back(c);
|
||||
|
|
@ -366,6 +372,7 @@ struct M17FrameDecoder
|
|||
lsf->push_back(0);
|
||||
lsf->source(0x20);
|
||||
ber = 0;
|
||||
dump(output.lich);
|
||||
return DecodeResult::OK;
|
||||
}
|
||||
#ifdef KISS_LOGGING
|
||||
|
|
@ -375,13 +382,14 @@ struct M17FrameDecoder
|
|||
lich_segments = 0;
|
||||
output.lich.fill(0);
|
||||
ber = 128;
|
||||
return DecodeResult::FAIL;
|
||||
return DecodeResult::INCOMPLETE;
|
||||
}
|
||||
|
||||
[[gnu::noinline]]
|
||||
DecodeResult decode_stream(buffer_t& buffer, tnc::hdlc::IoFrame*& stream, int& ber)
|
||||
{
|
||||
std::array<uint8_t, 20> stream_segment;
|
||||
DecodeResult result = DecodeResult::OK;
|
||||
|
||||
unpack_lich(buffer);
|
||||
|
||||
|
|
@ -403,11 +411,12 @@ struct M17FrameDecoder
|
|||
{
|
||||
INFO("EOS");
|
||||
state_ = State::LSF;
|
||||
result = DecodeResult::EOS;
|
||||
}
|
||||
stream->push_back(0);
|
||||
stream->push_back(0);
|
||||
stream->source(0x20);
|
||||
return state_ == State::LSF ? DecodeResult::EOS : DecodeResult::OK;
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue