53 lines
1.4 KiB
C++
53 lines
1.4 KiB
C++
|
#include "pickup_cancel.h"
|
||
|
|
#include <numbers>
|
||
|
|
#include <algorithm>
|
||
|
|
#include <cstdio>
|
||
|
|
|
||
|
|
static constexpr float twopi = 2.0f*std::numbers::pi_v<float>;
|
||
|
|
|
||
|
|
pickup_cancel::pickup_cancel(float fst_f, float eps_u, int nharm) :
|
||
|
|
act_f(fst_f), act_phi(0.0f),
|
||
|
|
max_f(fst_f+eps_u),min_f(fst_f-eps_u), decay_cst(fst_f/50.0f), error_filter(0.0f),
|
||
|
|
size(nharm), cosine_coeffs(nharm, 0.0f), sine_coeffs(nharm, 0.0f)
|
||
|
|
{}
|
||
|
|
|
||
|
|
bool pickup_cancel::is_in_sync() {
|
||
|
|
return cosine_coeffs[0]*cosine_coeffs[0]
|
||
|
|
<= 0.5f * sine_coeffs[0] * sine_coeffs[0];
|
||
|
|
}
|
||
|
|
|
||
|
|
float pickup_cancel::do_sample(float s) {
|
||
|
|
act_phi += act_f;
|
||
|
|
if(act_phi >= twopi) {
|
||
|
|
act_phi -= twopi;
|
||
|
|
}
|
||
|
|
|
||
|
|
float cosphi = std::cos(act_phi);
|
||
|
|
float sinphi = std::sin(act_phi);
|
||
|
|
|
||
|
|
// Always between -1 and 1
|
||
|
|
error_filter += decay_cst * (s * cosphi - error_filter);
|
||
|
|
|
||
|
|
// Sensitivity setting to tune?
|
||
|
|
float scaled_err = 10.0f * error_filter;
|
||
|
|
scaled_err = std::clamp(scaled_err, -1.0f, 1.0f);
|
||
|
|
|
||
|
|
act_f = (min_f + max_f)/2 + (max_f - min_f)/2 * scaled_err;
|
||
|
|
|
||
|
|
float corr = 0.0f;
|
||
|
|
|
||
|
|
float cos = 1.0f;
|
||
|
|
float sin = 0.0f;
|
||
|
|
for(int i = 0; i < size; ++i) {
|
||
|
|
sin = std::sin((i+1)*act_phi);
|
||
|
|
cos = std::cos((i+1)*act_phi);
|
||
|
|
|
||
|
|
corr += cos*cosine_coeffs[i] + sin*sine_coeffs[i];
|
||
|
|
|
||
|
|
cosine_coeffs[i] += (i+1) * decay_cst * (2 * s * cos - cosine_coeffs[i]);
|
||
|
|
sine_coeffs[i] += (i+1) * decay_cst * (2 * s * sin - sine_coeffs[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
if(!this->is_in_sync()) {return 0.0f;} else {return corr;}
|
||
|
|
}
|