121 lines
3 KiB
C++
121 lines
3 KiB
C++
#include "aec.h"
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
|
|
void AEC::do_sample(float mic, float hp, bool maybevoice) {
|
|
if(status == LOST) {
|
|
lookback[(lb_index++)&8191u] = hp;
|
|
find_delay(mic, hp);
|
|
}
|
|
}
|
|
|
|
|
|
void AEC::do_buffer(float* res, float* mic, float* hp, bool maybevoice) {
|
|
if(status == LOST) {
|
|
std::memset(res, 0, sizeof(float)*1024);
|
|
return;
|
|
}
|
|
|
|
// FIXME hardcoded quantum
|
|
std::memcpy(fd.input_buffer, mic, sizeof(float) * 1024);
|
|
|
|
|
|
int head = lb_index - current_delay + 1024 + 64;
|
|
for(int i = 0; i < 1024; ++i) {
|
|
fd.aux_in[i] = lookback[(i + head) & 8191u];
|
|
}
|
|
|
|
// Copy input around
|
|
lb_index = (lb_index+1) & 8191u;
|
|
auto fst_xfer = 8192 - lb_index;
|
|
std::memcpy(&lookback[lb_index], hp, fst_xfer*sizeof(float));
|
|
if(fst_xfer < 1024) {
|
|
std::memcpy(&lookback[0], &hp[fst_xfer], (1024-fst_xfer)*sizeof(float));
|
|
}
|
|
lb_index += 1023;
|
|
|
|
std::memcpy(fd.input_buffer, mic, 1024*sizeof(float));
|
|
|
|
if(status == LEARNING) {
|
|
|
|
fd.do_step(true, 0.1f); // mu_max = 0.5f
|
|
if(fd.echoratio > .5f && fd.costheta < -.75f) {
|
|
status = ONLINE;
|
|
std::puts("Switched to online status");
|
|
}
|
|
|
|
} else {
|
|
fd.do_step(!maybevoice || fd.echoratio > .8f);
|
|
// Learn if there is no near-end activity OR we have mic activity
|
|
// but it is explained by the echo
|
|
|
|
// We could have more stringent parameters...
|
|
if(fd.costheta > -.5f) {
|
|
if(cnt++ == 5) {
|
|
cnt = 0; status = LEARNING;
|
|
std::puts("Fell back to LEARNING");
|
|
}
|
|
} else {cnt = 0;}
|
|
}
|
|
|
|
std::printf("echoratio = %f, adj. cos(theta) = %f\n",
|
|
10*std::log10(fd.echoratio), fd.costheta);
|
|
std::memcpy(res, fd.output_buffer, 1024*sizeof(float));
|
|
}
|
|
|
|
bool AEC::inject_noise() {
|
|
return status == LOST || status == LEARNING;
|
|
}
|
|
|
|
void AEC::find_delay(float mic, float hp) {
|
|
hp_acc += hp;
|
|
mic_acc += mic;
|
|
|
|
|
|
if(++lookback_phase % 8 == 0) {
|
|
lookback_power[lookback_phase/8 % lookback_power.size()] = hp_acc;
|
|
|
|
for(auto i = 0u; i < weighted_xcorr.size(); ++i) {
|
|
float y = lookback_power[(lookback_phase / 8 - i) % lookback_power.size()]
|
|
* mic_acc;
|
|
|
|
if(__builtin_expect(lookback_phase <= 8 * 512, 0)) {
|
|
weighted_xcorr[i] = y;
|
|
} else {
|
|
weighted_xcorr[i] += xcorr_decay * (y - weighted_xcorr[i]);
|
|
}
|
|
}
|
|
hp_acc = 0.0f;
|
|
mic_acc = 0.0f;
|
|
}
|
|
|
|
if(lookback_phase % (512*8) == 0) { // 4096 samples
|
|
float max_xcor = 0.0f;
|
|
int argmax_xcor = 0;
|
|
|
|
float low_max_xcor = 0.0f;
|
|
for(auto i = 0u; i < weighted_xcorr.size(); ++i) {
|
|
if(max_xcor < weighted_xcorr[i]*weighted_xcorr[i]) {
|
|
max_xcor = weighted_xcorr[i]*weighted_xcorr[i];
|
|
argmax_xcor = i;
|
|
}
|
|
if(i < 128 && weighted_xcorr[i]*weighted_xcorr[i] > low_max_xcor) {
|
|
low_max_xcor = weighted_xcorr[i]*weighted_xcorr[i];
|
|
}
|
|
|
|
}
|
|
if(max_xcor > 4.0f * low_max_xcor) {
|
|
std::printf("Found delay at %d chunks\n", argmax_xcor);
|
|
status = LEARNING;
|
|
current_delay = argmax_xcor * 8;
|
|
|
|
hp_acc = 0.0f;
|
|
mic_acc = 0.0f;
|
|
lookback_phase = 0;
|
|
for(auto i = lookback_power.size(); i >= 1; --i) {
|
|
lookback_power[lookback_power.size() - i] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|