mirror
/
NucleoTNC
kopia lustrzana https://github.com/mobilinkd/NucleoTNC
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
NucleoTNC/TNC/M17Demodulator.h

341 wiersze
11 KiB
C++
Czysty Wina Historia

// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
// All rights reserved.
#pragma once
#include "Demodulator.hpp"
#include "AudioLevel.hpp"
#include "AudioInput.hpp"
#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 "M17.h"
#include <arm_math.h>
#include <algorithm>
#include <array>
#include <experimental/array>
#include <optional>
#include <tuple>
namespace mobilinkd { namespace tnc {
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;
using audio_filter_t = Q15FirFilter<ADC_BLOCK_SIZE, m17::FILTER_TAP_NUM_9>;
using demod_result_t = std::tuple<float, float, int, float>;
enum class DemodState { UNLOCKED, LSF_SYNC, FRAME_SYNC, FRAME };
audio_filter_t demod_filter{m17::rrc_taps_9.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};
M17Framer<368> framer;
M17FrameDecoder decoder;
DemodState demodState = DemodState::UNLOCKED;
M17FrameDecoder::SyncWordType sync_word_type = M17FrameDecoder::SyncWordType::LSF;
bool locked_ = false;
bool passall_ = false;
int ber = -1;
int sync_count = 0;
virtual ~M17Demodulator() {}
void start() override;
void stop() override
{
// getModulator().stop_loopback();
stopADC();
locked_ = false;
}
bool locked() const override
{
return locked_;
}
size_t size() const override
{
return ADC_BLOCK_SIZE;
}
void passall(bool enabled) override
{
passall_ = enabled;
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);
}
hdlc::IoFrame* operator()(const q15_t* input) override;
/*
* Return twist as a the difference in dB between mark and space. The
* expected values are about 0dB for discriminator output and about 5.5dB
* for de-emphasized audio.
*/
float readTwist() override
{
return 0;
}
uint32_t readBatteryLevel() override
{
#ifndef NUCLEOTNC
DEBUG("enter M17Demodulator::readBatteryLevel");
ADC_ChannelConfTypeDef sConfig;
sConfig.Channel = ADC_CHANNEL_VREFINT;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
CxxErrorHandler();
htim6.Init.Period = 48000;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
if (HAL_TIM_Base_Start(&htim6) != HAL_OK)
CxxErrorHandler();
if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
if (HAL_ADC_PollForConversion(&hadc1, 3) != HAL_OK) CxxErrorHandler();
auto vrefint = HAL_ADC_GetValue(&hadc1);
if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
// Disable battery charging while measuring battery voltage.
auto usb_ce = gpio::USB_CE::get();
gpio::USB_CE::on();
gpio::BAT_DIVIDER::off();
HAL_Delay(1);
sConfig.Channel = ADC_CHANNEL_15;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
CxxErrorHandler();
uint32_t vbat = 0;
if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
for (size_t i = 0; i != 8; ++i)
{
if (HAL_ADC_PollForConversion(&hadc1, 1) != HAL_OK) CxxErrorHandler();
vbat += HAL_ADC_GetValue(&hadc1);
}
vbat /= 8;
if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
if (HAL_TIM_Base_Stop(&htim6) != HAL_OK)
CxxErrorHandler();
gpio::BAT_DIVIDER::on();
// Restore battery charging state.
if (!usb_ce) gpio::USB_CE::off();
INFO("Vref = %lu", vrefint);
INFO("Vbat = %lu (raw)", vbat);
// Order of operations is important to avoid underflow.
vbat *= 6600;
vbat /= (VREF + 1);
uint32_t vref = ((vrefint * 3300) + (VREF / 2)) / VREF;
INFO("Vref = %lumV", vref)
INFO("Vbat = %lumV", vbat);
DEBUG("exit M17Demodulator::readBatteryLevel");
return vbat;
#else
return 0;
#endif
}
};
}} // mobilinkd::tnc
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik