#define _USE_MATH_DEFINES //for Visual Studio #include "ANCcontroller.h" WaveGenerator::WaveGenerator(int sp_rate,int quantifying_bit_number) { AS_nBIT_MAX_NUM = pow(2,quantifying_bit_number)-1; SAMPLE_RATE = sp_rate; CreateSinWave(); CreateChirpWave(); } //符号あり整数値sin波を生成する(-AS_nBIT_MAX_NUM ~ AS_nBIT_MAX_NUM) void WaveGenerator::CreateSinWave() { auto rad_step = 2 * M_PI * FREQ / SAMPLE_RATE; for (int i = 0; i < SAMPLE_RATE ; i++) { wave_sin.push_back(AS_nBIT_MAX_NUM * sin(rad_step * i) *GAIN); } } void WaveGenerator::CreateChirpWave() { std::deque CharpBase(int(SAMPLE_RATE * tLength / 2), 0); std::deque waveChirpSignal; //float f = fMax; float fz = fMin; float t = 0; float t_step = 1.0 / SAMPLE_RATE; float f_step = ((fMax - fMin) / (tLength * 0.5)); for (auto& w : CharpBase) { w = cos(2.0 * M_PI * ((fz * t) + (f_step * 0.5 * t * t))); //アップチャープ生成 t += t_step; } float w = 0;//波形を連続にする while (-0.99 < w && w < 0.99) { w = cos(2.0 * M_PI * ((fz * t) + (f_step * 0.5 * t * t))); //アップチャープ生成 t += t_step; CharpBase.emplace_back(w);//アップチャープ挿入 } waveChirpSignal = CharpBase; //ダウンチャープ生成 //こう…イテレーター使うより…早い気がしたねんな… for (auto i = CharpBase.size() - 1; i > 0; i--) { waveChirpSignal.emplace_back(CharpBase[i]);//挿入 } for (const auto & w : waveChirpSignal) { waveIntChirp.emplace_back(int(w * AS_nBIT_MAX_NUM * GAIN)); } for (const auto & w : waveChirpSignal) { waveUnsingedIntChirp.emplace_back(int((w+1) * AS_nBIT_MAX_NUM * GAIN * 0.5)); } } //1コールに付き1信号分sin波を吐き出す int16_t WaveGenerator::playWaveSin(char mode) { int out=0; switch(mode){ case INT_WAVE: out = wave_sin[sound_cnt % wave_sin.size()]; break; case UNSINGED_INT_WAVE: out = (wave_sin[sound_cnt % wave_sin.size()]+AS_nBIT_MAX_NUM)/2; break; case FLOAT_WAVE: //TODO もし実装するならCreateSinWaveを直すところから break; } sound_cnt++; return out; } //1コールに付き1信号分チャープ信号を吐き出す int16_t WaveGenerator::playWaveCharp(char mode) { int out=0; switch(mode){ case INT_WAVE: out = waveIntChirp[sound_cnt % waveIntChirp.size()]; break; case UNSINGED_INT_WAVE: out = waveUnsingedIntChirp[sound_cnt % waveUnsingedIntChirp.size()]; break; case FLOAT_WAVE: //TODO もし実装するならCreateSinWaveを直すところから break; } sound_cnt++; return out; } ANC_DataBase::ANC_DataBase() { squaredXBuf.assign(WaveBufLen, 0); inputXBuf.assign(WaveBufLen, 0); filteredXBuf.assign(WaveBufLen, 0); outputYBuf.assign(WaveBufLen, 0); } template void ANC_DataBase::shiftBuffer(T inValue, std::deque& ioBuffer) { if (isnan(inValue))inValue = 0;//数字のみ受け取り ioBuffer.emplace_front(inValue);//行頭に追加 ioBuffer.pop_back();//行末を消す } template float ANC_DataBase::calcFIR(const std::deque& inDataBuf, inoArray& filter_ary) { //assert(inDataBuf.size() >= filter_ary.size()); //内積 float outValue = std::inner_product( filter_ary.begin(), filter_ary.end(), // ひとつ目のベクトル inDataBuf.begin(), // ふたつ目のベクトル(v以上の次元数を持つこと) 0.0f);//ここの型にキャストされる return outValue; } template void ANC_DataBase::calcAptiveFilter(float inErrValue, inoArray &ioW, float inStepSize, const std::deque& inXBuf, const std::deque& xAvaHelp ) { auto xpower = 1.0e-9; xpower += std::accumulate(xAvaHelp.begin(), xAvaHelp.begin() + FilterSize , xpower); //対象フィルタサイズ分のパワーを計算 //conf = conf - inStepSize * inErrValue * inXBuf[idx] / xpower; float Alpha = inStepSize * inErrValue / xpower;//定数部分を先に計算し、キャッシュして高速化 int idx = 0; for (auto& conf : ioW) { //e(n)の導出を+,LMS更新式を-にするとノイズキャンセリング同定になる（正負を反転させるとシステム同定） conf = conf - Alpha * inXBuf[idx]; idx++; } } template void ANC_DataBase::InitFilter(T &Filter) { Filter.fill(0); inputXBuf.assign(WaveBufLen, 0); squaredXBuf.assign(WaveBufLen, 0); } void ANC_DataBase::calcANCwave_IMC(float errGain) { //消音波を出力する //Filtered N-LMS with IMC-FeedBack // coefC : 同定により決定。以下の処理においては定数的。 // coefW : 適応フィルタ係数。動的に変化する。 // coefH : エコーキャンセル(ハウリング減退)を行う //errGain: 誤差信号は観測値となる e = d(所望信号) + y(CtrSpk_out) shiftBuffer(errGain - yc, inputXBuf);//音声バッファにデータを溜め込み、フィルタの適応対象とする shiftBuffer(inputXBuf[0] * inputXBuf[0], squaredXBuf);//NLMS高速化のためxbufの各要素の二乗を作っておく。 //X(n)をロハ信号化 shiftBuffer(calcFIR(inputXBuf, coefC), filteredXBuf); // 適応フィルタをかけ、制御信号y(n)を発行する y = calcFIR(inputXBuf, coefW); shiftBuffer(y, outputYBuf); //二次経路の影響を受けるycを予想する yc = calcFIR(outputYBuf, coefC); //Filtered N-LMS by IMC calcAptiveFilter(errGain, coefW, StepSizeW, filteredXBuf, squaredXBuf); if (abs(y) > 32767) InitFilter(coefW);//出力値が大きすぎる場合系が不安定になっているのでリセット } void ANC_DataBase::calcANCwave_FF(float errGain,float refGain) { } void ANC_DataBase::calcANCwave_HI(float errGain) { } //入力波形を利用して任意の経路の伝達関数を推測(とりあえずチャープ信号がおすすめ) template void ANC_DataBase::IdentTransferFunction(float errGain, float TrainingWave, inoArray& TF, float inStepSize) { shiftBuffer(TrainingWave, inputXBuf);//出力した信号をトレーニングデータとする shiftBuffer(TrainingWave * TrainingWave, squaredXBuf);//NLMS高速化のためxbufの各要素の二乗を作っておく。 //CofH測定 auto InternalErrGain = errGain + yc;//システム内部でCofc利用のキャンセリングを行う。 yc = calcFIR(inputXBuf, TF); calcAptiveFilter(InternalErrGain, TF, inStepSize, inputXBuf, squaredXBuf); if (abs(yc) > 32767) InitFilter(coefC);//出力値が大きすぎる場合系が不安定になっているのでリセット } ANC_Controller::ANC_Controller() { } void ANC_Controller::calc2ndPassTF(float errGain, float TrainingWave) { DB.IdentTransferFunction(errGain, TrainingWave , DB.coefC, DB.StepSizeC); } void ANC_Controller::calcEchoPassTF(float errGain, float TrainingWave) { DB.IdentTransferFunction(errGain, TrainingWave , DB.coefH, DB.StepSizeH); } float ANC_Controller::calcANCwave(const char methodNum , float errGain , float refGain = 0 ) { switch (methodNum) { case AS_IMC: DB.calcANCwave_IMC(errGain); break; case AS_FF: DB.calcANCwave_FF(errGain, refGain); break; case AS_HI: DB.calcANCwave_HI(errGain); break; default: break; } return DB.y; } void ANC_Controller::printCoefC(){ for(const auto& e : DB.coefC){ printf("%d,",e); } printf("\n"); } void ANC_Controller::printCoefH(){ for(const auto& e : DB.coefH){ printf("%d,",e); } printf("\n"); }