1
0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-12-24 19:00:36 -05:00
sdrangel/modemm17/Fsk4Demod.h
2022-07-20 09:07:00 +02:00

157 lines
5.5 KiB
C++

// Copyright 2020 Mobilinkd LLC.
#pragma once
#include "FirFilter.h"
#include "PhaseEstimator.h"
#include "DeviationError.h"
#include "FrequencyError.h"
#include "SymbolEvm.h"
#include <array>
#include <tuple>
namespace modemm17
{
namespace detail
{
static const auto rrc_taps = std::array<float, 79>{
-0.009265784007800534, -0.006136551625729697, -0.001125978562075172, 0.004891777252042491,
0.01071805138282269, 0.01505751553351295, 0.01679337935001369, 0.015256245142156299,
0.01042830577908502, 0.003031522725559901, -0.0055333532968188165, -0.013403099825723372,
-0.018598682349642525, -0.01944761739590459, -0.015005271935951746, -0.0053887880354343935,
0.008056525910253532, 0.022816244158307273, 0.035513467692208076, 0.04244131815783876,
0.04025481153629372, 0.02671818654865632, 0.0013810216516704976, -0.03394615682795165,
-0.07502635967975885, -0.11540977897637611, -0.14703962203941534, -0.16119995609538576,
-0.14969512896336504, -0.10610329539459686, -0.026921412469634916, 0.08757875030779196,
0.23293327870303457, 0.4006012210123992, 0.5786324696325503, 0.7528286479934068,
0.908262741447522, 1.0309661131633199, 1.1095611856548013, 1.1366197723675815,
1.1095611856548013, 1.0309661131633199, 0.908262741447522, 0.7528286479934068,
0.5786324696325503, 0.4006012210123992, 0.23293327870303457, 0.08757875030779196,
-0.026921412469634916, -0.10610329539459686, -0.14969512896336504, -0.16119995609538576,
-0.14703962203941534, -0.11540977897637611, -0.07502635967975885, -0.03394615682795165,
0.0013810216516704976, 0.02671818654865632, 0.04025481153629372, 0.04244131815783876,
0.035513467692208076, 0.022816244158307273, 0.008056525910253532, -0.0053887880354343935,
-0.015005271935951746, -0.01944761739590459, -0.018598682349642525, -0.013403099825723372,
-0.0055333532968188165, 0.003031522725559901, 0.01042830577908502, 0.015256245142156299,
0.01679337935001369, 0.01505751553351295, 0.01071805138282269, 0.004891777252042491,
-0.001125978562075172, -0.006136551625729697, -0.009265784007800534
};
static const auto evm_b = std::array<float, 3>{0.02008337, 0.04016673, 0.02008337};
static const auto evm_a = std::array<float, 3>{1.0, -1.56101808, 0.64135154};
} // detail
struct Fsk4Demod
{
using demod_result_t = std::tuple<double, double, int, double>;
using result_t = std::tuple<double, double, int, double, double, double, double>;
BaseFirFilter<std::tuple_size<decltype(detail::rrc_taps)>::value> rrc = makeFirFilter(detail::rrc_taps);
PhaseEstimator phase = PhaseEstimator(48000, 4800);
DeviationError<10> deviation;
FrequencyError<32> frequency;
SymbolEvm<std::tuple_size<decltype(detail::evm_b)>::value> symbol_evm = makeSymbolEvm(makeIirFilter(detail::evm_b, detail::evm_a));
double sample_rate = 48000;
double symbol_rate = 4800;
double unlock_gain = 0.02;
double lock_gain = 0.001;
std::array<float, 3> samples{0};
double t = 0;
double dt = symbol_rate / sample_rate;
double ideal_dt = dt;
bool sample_now = false;
double estimated_deviation = 1.0;
double estimated_frequency_offset = 0.0;
double evm_average = 0.0;
Fsk4Demod(
double sample_rate,
double symbol_rate,
double unlock_gain = 0.02,
double lock_gain = 0.001
) :
sample_rate(sample_rate),
symbol_rate(symbol_rate),
unlock_gain(unlock_gain * symbol_rate / sample_rate),
lock_gain(lock_gain * symbol_rate / sample_rate),
dt(symbol_rate / sample_rate),
ideal_dt(dt)
{
samples.fill(0.0);
}
demod_result_t demod(bool lock)
{
estimated_deviation = deviation(samples[1]);
for (auto& sample : samples) sample *= estimated_deviation;
estimated_frequency_offset = frequency(samples[1]);
for (auto& sample : samples) sample -= estimated_frequency_offset;
auto phase_estimate = phase(samples);
if (samples[1] < 0) phase_estimate *= -1;
dt = ideal_dt - (phase_estimate * (lock ? lock_gain : unlock_gain));
t += dt;
symbol_evm(samples[1]);
int symbol;
float evm;
std::tie(symbol, evm) = symbol_evm(samples[1]);
evm_average = symbol_evm.evm();
samples[0] = samples[2];
return std::make_tuple(samples[1], phase_estimate, symbol, evm);
}
/**
* Process the sample. If a symbol is ready, return a tuple
* containing the sample used, the estimated phase, the decoded
* symbol, the EVM, the deviation error and the frequency error
* (sample, phase, symbol, evm, ed, ef), otherwise None.
*/
result_t operator()(double sample, bool lock)
{
auto filtered_sample = rrc(sample);
if (sample_now)
{
samples[2] = filtered_sample;
sample_now = false;
double prev_sample;
double phase_estimate;
int symbol;
double evm;
std::tie(prev_sample, phase_estimate, symbol, evm) = demod(lock);
return std::make_tuple(
prev_sample,
phase_estimate,
symbol,
evm,
estimated_deviation,
estimated_frequency_offset,
evm_average
);
}
t += dt;
if (t < 1.0)
{
samples[0] = filtered_sample;
}
else
{
t -= 1.0;
samples[1] = filtered_sample;
sample_now = true;
}
return result_t{0, 0, 0, 0, 0, 0, 0};
}
};
} // modemm17