156 lines
3.6 KiB
C++
156 lines
3.6 KiB
C++
#include "pickup_cancel.h"
|
|
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
|
|
#include <spa/pod/builder.h>
|
|
//#include "spa/param/audio/format-utils.h"
|
|
#include "spa/param/latency-utils.h"
|
|
#include <pipewire/pipewire.h>
|
|
#include <pipewire/filter.h>
|
|
|
|
struct dat;
|
|
struct port {
|
|
dat* data;
|
|
};
|
|
|
|
struct processing {
|
|
pickup_cancel pc;
|
|
pw_main_loop* lp;
|
|
pw_filter* filter;
|
|
|
|
port* mic_in_port;
|
|
port* mic_out_port;
|
|
};
|
|
|
|
static void on_process(void* pro, spa_io_position* position) {
|
|
auto* p = (processing*)pro;
|
|
auto n_samples = position->clock.duration;
|
|
|
|
float* in = (float*)pw_filter_get_dsp_buffer(p->mic_in_port, n_samples);
|
|
float* out = (float*)pw_filter_get_dsp_buffer(p->mic_out_port, n_samples);
|
|
|
|
if(in == NULL || out == NULL) return;
|
|
|
|
for(auto i = n_samples; i >= 1; --i) {
|
|
*out++ = *in - p->pc.do_sample(*in);
|
|
in++;
|
|
}
|
|
};
|
|
|
|
static void on_state_change(void*,
|
|
pw_filter_state old, pw_filter_state ne, const char*) {
|
|
std::printf("State changed : %s -> %s\n",
|
|
pw_filter_state_as_string(old),
|
|
pw_filter_state_as_string(ne));
|
|
}
|
|
|
|
static constexpr pw_filter_events filter_events = {
|
|
.version = PW_VERSION_FILTER_EVENTS,
|
|
.state_changed = on_state_change,
|
|
.process = on_process,
|
|
};
|
|
|
|
|
|
static void do_quit(void* pro, int) {
|
|
pw_main_loop_quit(((processing*)pro)->lp);
|
|
}
|
|
|
|
static void do_report(void* pro, int) {
|
|
auto* p = (processing*)pro;
|
|
|
|
std::printf("PLL status : %s\n", p->pc.is_in_sync() ? "locked" : "unlocked");
|
|
float s = p->pc.sin_power;
|
|
float c = p->pc.cos_power;
|
|
float ns = s*s/(s*s + c*c);
|
|
float nc = c*c/(s*s + c*c);
|
|
std::printf("Powers : %f %f\n", ns, nc);
|
|
std::printf("Instant. frequency : %f\n", p->pc.act_f * 48000 / (2*3.1415));
|
|
float energy = 0.0f;
|
|
|
|
//std::puts("TD=[");
|
|
for(auto i = 0u; i < p->pc.pickup_timedomain.size(); ++i) {
|
|
float x = p->pc.pickup_timedomain[i];
|
|
//std::printf("%e,", x);
|
|
energy += x*x;
|
|
}
|
|
//std::puts("]");
|
|
|
|
std::printf("Correction power : %f dB\n", 10*std::log10(energy/2048));
|
|
}
|
|
|
|
int main(int argc, char *argv[]) {
|
|
|
|
pw_init(&argc, &argv);
|
|
|
|
processing proc = {
|
|
pickup_cancel(
|
|
1./48000 * 2 * 3.1415 * 50, // 50 Hz
|
|
1./48000 * 2 * 3.1415 * 2 // +- 2 Hz
|
|
)};
|
|
|
|
proc.lp = pw_main_loop_new(NULL);
|
|
|
|
|
|
pw_loop_add_signal(pw_main_loop_get_loop(proc.lp), SIGTERM, do_quit, &proc);
|
|
pw_loop_add_signal(pw_main_loop_get_loop(proc.lp), SIGUSR1, do_report, &proc);
|
|
|
|
proc.filter = pw_filter_new_simple(
|
|
pw_main_loop_get_loop(proc.lp),
|
|
"audio-filter",
|
|
pw_properties_new(
|
|
PW_KEY_MEDIA_TYPE, "Audio",
|
|
PW_KEY_MEDIA_CATEGORY, "Filter",
|
|
PW_KEY_MEDIA_ROLE, "DSP",
|
|
NULL),
|
|
&filter_events,
|
|
&proc);
|
|
|
|
/*
|
|
uint8_t buffer[1024];
|
|
auto b = SPA_POD_BUILDER_INIT(buffer, sizeof(buffer));
|
|
spa_audio_info_raw fmt = {
|
|
.format = SPA_AUDIO_FORMAT_DSP_F32,
|
|
.rate = 48000,
|
|
.channels = 1,
|
|
.position = {SPA_AUDIO_CHANNEL_MONO}
|
|
};
|
|
|
|
const spa_pod* params[1];
|
|
params[0] = spa_format_audio_raw_build(&b, SPA_PARAM_EnumFormat, &fmt);
|
|
*/
|
|
|
|
proc.mic_in_port = (port*)pw_filter_add_port(proc.filter,
|
|
PW_DIRECTION_INPUT,
|
|
PW_FILTER_PORT_FLAG_MAP_BUFFERS,
|
|
sizeof(struct port),// prt_data_size
|
|
pw_properties_new(
|
|
PW_KEY_FORMAT_DSP, "32 bit float mono audio",
|
|
PW_KEY_PORT_NAME, "input",
|
|
NULL), // props
|
|
NULL, 0);
|
|
|
|
proc.mic_out_port = (port*)pw_filter_add_port(proc.filter,
|
|
PW_DIRECTION_OUTPUT,
|
|
PW_FILTER_PORT_FLAG_MAP_BUFFERS,
|
|
sizeof(struct port),
|
|
pw_properties_new(
|
|
PW_KEY_FORMAT_DSP, "32 bit float mono audio",
|
|
PW_KEY_PORT_NAME, "output",
|
|
NULL), // props
|
|
NULL, 0);
|
|
|
|
if(pw_filter_connect(proc.filter,
|
|
PW_FILTER_FLAG_RT_PROCESS,
|
|
NULL, 0) < 0) {
|
|
std::fprintf(stderr, "Cannot connect\n");
|
|
return -1;
|
|
}
|
|
|
|
pw_main_loop_run(proc.lp);
|
|
|
|
pw_filter_destroy(proc.filter);
|
|
pw_main_loop_destroy(proc.lp);
|
|
pw_deinit();
|
|
return 0;
|
|
}
|