96 lines
2.5 KiB
C++
96 lines
2.5 KiB
C++
#pragma once
|
|
|
|
#include <cstdint>
|
|
#include <complex>
|
|
#include <chrono>
|
|
#include <thread>
|
|
|
|
typedef uint32_t phase_t;
|
|
#define PHASE_MAX UINT32_MAX
|
|
|
|
static phase_t frequency_to_ftw(double f) {
|
|
return f*(double)PHASE_MAX;
|
|
}
|
|
|
|
class DDS {
|
|
private:
|
|
phase_t phase = 0;
|
|
public:
|
|
phase_t ftw = 0;
|
|
double get() {
|
|
phase += ftw; // wraps on overflow
|
|
return sin(phase*2.0*M_PI/(double)PHASE_MAX);
|
|
}
|
|
phase_t get_phase() {
|
|
return phase;
|
|
}
|
|
};
|
|
|
|
template<typename T, unsigned int order>
|
|
class Lowpass {
|
|
private:
|
|
double k = 0.0;
|
|
T s[order] = {};
|
|
public:
|
|
void set_bandwidth(double f) {
|
|
k = 2.0*M_PI*f;
|
|
}
|
|
T update(T x) {
|
|
T a = x;
|
|
for(int i=0;i<order;i++) {
|
|
s[i] += (a - s[i])*k;
|
|
a = s[i];
|
|
}
|
|
return a;
|
|
}
|
|
};
|
|
|
|
template<unsigned int order>
|
|
class Lockin {
|
|
private:
|
|
double scale = 1.0;
|
|
phase_t multiplier = 1;
|
|
phase_t phase = 0;
|
|
Lowpass<std::complex<double>, order> lpf;
|
|
public:
|
|
phase_t ftw = 0;
|
|
void set_scale(double s) {
|
|
scale = s;
|
|
}
|
|
void set_multiplier(phase_t m) {
|
|
multiplier = m;
|
|
}
|
|
void set_bandwidth(double f) {
|
|
lpf.set_bandwidth(f);
|
|
}
|
|
std::complex<double> update(double x) {
|
|
std::complex<double> rotated;
|
|
rotated = x*std::polar(scale, multiplier*phase*2.0*M_PI/(double)PHASE_MAX);
|
|
phase -= ftw; // wraps on underflow
|
|
return lpf.update(rotated);
|
|
}
|
|
phase_t get_phase() {
|
|
return phase;
|
|
}
|
|
};
|
|
|
|
class Clocker {
|
|
private:
|
|
std::chrono::milliseconds period;
|
|
std::chrono::time_point<std::chrono::steady_clock> next_tick = std::chrono::time_point<std::chrono::steady_clock>::min();
|
|
public:
|
|
Clocker(std::chrono::milliseconds period): period(period) {};
|
|
void tick() {
|
|
if(next_tick == std::chrono::time_point<std::chrono::steady_clock>::min()) {
|
|
next_tick = std::chrono::steady_clock::now() + period;
|
|
} else {
|
|
auto duration = next_tick - std::chrono::steady_clock::now();
|
|
if(duration >= duration.zero())
|
|
std::this_thread::sleep_for(duration);
|
|
else
|
|
std::cerr << "missed tick" << std::endl;
|
|
next_tick += period;
|
|
}
|
|
}
|
|
};
|