147 lines
3.9 KiB
C++
147 lines
3.9 KiB
C++
#include "pickup_cancel.h"
|
|
#include <numbers>
|
|
#include <algorithm>
|
|
#include <cstdio>
|
|
|
|
using namespace std::complex_literals;
|
|
|
|
static constexpr float sinc(float x) {
|
|
if(__builtin_expect(x * x <= 0x1p-24, 0)) {
|
|
return 1.0f;
|
|
} else {
|
|
return std::sin(x)/x;
|
|
}
|
|
}
|
|
|
|
static constexpr float cosc_help(float u) {
|
|
if(__builtin_expect((u - 0.5)*(u - 0.5) <= 0x1p-24, 0)) {
|
|
return 1.0f;
|
|
} else {
|
|
return std::cos(std::numbers::pi_v<float> * u)/(1.0f - 4*u*u);
|
|
}
|
|
}
|
|
|
|
/*
|
|
We have a cutoff at 480 * f0 and final 0 at (2048 - 480)*f0
|
|
beta = 480 / (2048 - 480)
|
|
Also, 1/T = "symbol rate" = (2048 - 480)*f0 and
|
|
1/dt = "sampling rate" = 2048*f0
|
|
|
|
Therefore t/T = (2048 - 480) / 2048 * n.
|
|
|
|
Use a lookup table?
|
|
*/
|
|
static constexpr float rc_filter(float spdist) {
|
|
|
|
float beta = 1.0 - 480.0f/(2048.0f - 480.0f);
|
|
float x = spdist * (2048.0 - 480.0) / 2048;
|
|
float u = beta*x;
|
|
|
|
return sinc(std::numbers::pi_v<float> * x) * cosc_help(u);
|
|
}
|
|
|
|
static constexpr float twopi = 2.0f*std::numbers::pi_v<float>;
|
|
static constexpr float pi = 2.0f*std::numbers::pi_v<float>;
|
|
|
|
pickup_cancel::pickup_cancel(float fst_f, float eps_u) :
|
|
act_f(fst_f), act_phi(0.0f),
|
|
max_f(fst_f+eps_u),min_f(fst_f-eps_u),
|
|
decay_cst(fst_f/200.0f), error_filter(0.0f),
|
|
lockin_filter(1024), filter_index(0), nom_f(fst_f), lock_phi(0.0f)
|
|
{
|
|
}
|
|
|
|
bool pickup_cancel::is_in_sync() {
|
|
return sin_power * sin_power >= 2.0f * cos_power * cos_power;
|
|
}
|
|
|
|
float pickup_cancel::do_sample(float s, bool maybevoice) {
|
|
lock_phi += nom_f;
|
|
if(lock_phi >= twopi) {
|
|
lock_phi -= twopi;
|
|
}
|
|
|
|
std::complex<float> ex = std::exp(std::complex<float>(1.0i) * lock_phi);
|
|
std::complex<float> c = s * ex;
|
|
|
|
filter_sum += c - lockin_filter[filter_index & 1023u];
|
|
lockin_filter[filter_index & 1023u] = c;
|
|
filter_index++;
|
|
float pll_sample = std::real(filter_sum * ex)/1024;
|
|
|
|
if(!maybevoice) {
|
|
mean_power += (pll_sample * pll_sample - mean_power) * 1.0f/48000;
|
|
}
|
|
|
|
// Sensitivity of Ks = 2.0/pi u/rad
|
|
float pd = std::clamp(
|
|
0.1f * pll_sample/std::sqrt(mean_power) * std::cos(act_phi),
|
|
-1.0f, 1.0f);
|
|
|
|
// See below
|
|
error_filter = (pd + (pd - pd_delayed) * 10.0f * pi/(max_f - min_f)
|
|
+ error_filter * 1.0/decay_cst) * decay_cst/(decay_cst + 1.0f);
|
|
|
|
// Kosc = (Delta f) * 0.5 (rad/sample) / u
|
|
act_f = std::clamp(nom_f + 0.5f*(max_f - min_f) * error_filter, min_f, max_f);
|
|
|
|
pd_delayed = pd;
|
|
// Integration gain is 1 rad / (rad / sample)
|
|
|
|
act_phi += act_f;
|
|
if(act_phi >= twopi) {
|
|
act_phi -= twopi;
|
|
}
|
|
/* Loop gain is 1/jw * Kosc * Ks * H_filter
|
|
we thus want that at w = Kosc*Ks = Df/pi,
|
|
H_filter has phase close to 0°.
|
|
|
|
H_filter = (1 + jw tau_1)/(1 + jwtau_2),
|
|
with tau_2 ~= 1000/nom_f.
|
|
we want tau_1 somewhat above pi/Df, say .5 * pi/Df
|
|
|
|
The difference equation is thus
|
|
y + (1000/nom_f) * (y - y_d)/te = pd + (pd - pd_l) * (.5 * pi/Df)/te
|
|
(here te = 1 samp).
|
|
*/
|
|
|
|
/* If error filter is at -1.0f, we expect to lock at pi phase difference,
|
|
if we are at 0 we expect +-pi/2, and if we are at 1.0f we expect to be in
|
|
phase.
|
|
|
|
Therefore, we can try to compute
|
|
E[s(t) cos(act_phi + pi/2 * (act_f - nom_f)/(max_f - min_f))]
|
|
and same for sin.
|
|
*/
|
|
sin_power +=
|
|
.1f/48000 * (pll_sample * std::sin(act_phi + pi * (act_f - nom_f)/(max_f - min_f)) - sin_power);
|
|
cos_power +=
|
|
.1f/48000 * (pll_sample * std::cos(act_phi + pi * (act_f - nom_f)/(max_f - min_f)) - cos_power);
|
|
|
|
float sampphase = act_phi/twopi * 2048; // in [0, 2048]
|
|
float corr = 0.0f;
|
|
|
|
// Do reconstruction using raised cosine filter
|
|
int head = (int)sampphase - 16;
|
|
float dist = head - sampphase;
|
|
float coeffs[32];
|
|
|
|
unsigned int iter = (unsigned)head;
|
|
for(int i = 0; i < 32; ++i) {
|
|
coeffs[i] = rc_filter(dist);
|
|
corr += pickup_timedomain[iter&2047u] * coeffs[i];
|
|
iter++;
|
|
|
|
dist += 1.0f;
|
|
}
|
|
|
|
if(!maybevoice) {
|
|
float err = s - corr;
|
|
iter = head;
|
|
for(int i = 0; i < 32; ++i) {
|
|
pickup_timedomain[iter&2047u] += mu * err * coeffs[i];
|
|
iter++;
|
|
}
|
|
}
|
|
if(!this->is_in_sync()) {return 0.0f;} else {return corr;}
|
|
}
|