ANCcontroller.cpp

#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<float> CharpBase(int(SAMPLE_RATE * tLength / 2), 0);
  std::deque<float>  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() {

  arm_lms_norm_init_f32(&lmsInstance, CoefWLen,coefW, lmsStateW, StepSizeW, 1);
  
  squaredXBuf.assign(WaveBufLen, 0);
  inputXBuf.assign(WaveBufLen, 0);
  filteredXBuf.assign(WaveBufLen, 0);
  outputYBuf.assign(WaveBufLen, 0);
  
}

template <class T>
void ANC_DataBase::shiftBuffer(T inValue, std::deque<T>& ioBuffer)
{
  if (isnan(inValue))inValue = 0;//数字のみ受け取り
  ioBuffer.emplace_front(inValue);//行頭に追加
  ioBuffer.pop_back();//行末を消す
}

template<class T, class U, size_t FilterSize >
float ANC_DataBase::calcFIR(const std::deque<T>& inDataBuf, inoArray<U, FilterSize>& 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<class T, size_t FilterSize >
void ANC_DataBase::calcAptiveFilter(float inErrValue, inoArray<T, FilterSize> &ioW, float inStepSize, const std::deque<T>& inXBuf, const std::deque<T>& 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 <class T>
void ANC_DataBase::InitFilter(T &Filter)
{
  Filter.fill(0);

  inputXBuf.assign(WaveBufLen, 0);
  squaredXBuf.assign(WaveBufLen, 0);
}

void ANC_DataBase::initCorfWFillter(){
  int i=0;
  for(i=0;i<CoefWLen;i++){
    coefW[i]=0;
    lmsStateW[i]=0;
  }

  arm_lms_norm_init_f32(&lmsInstance, CoefWLen,coefW, lmsStateW, StepSizeW, 1);

}


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_Nopass(float32_t errGain,float32_t refGain){
  //reference is d 

  arm_lms_norm_f32_2mic(&lmsInstance, &refGain, &errGain, &y, &err,1);
  if (y<-1){
    y=-1;
    initCorfWFillter();
  }
  else if(y>1){
    y=1;
    initCorfWFillter();
  }
  
}



void ANC_DataBase::calcANCwave_HI(float errGain) {
}


//入力波形を利用して任意の経路の伝達関数を推測(とりあえずチャープ信号がおすすめ)
template<class T, size_t FilterSize >
void ANC_DataBase::IdentTransferFunction(float errGain, float TrainingWave, inoArray<T, FilterSize>& 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::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);

}

//出力値は全部float32
float32_t ANC_Controller::calcANCwave(const char methodNum , float32_t errGain , float32_t refGain = 0 ) {
  switch (methodNum)
  {
    case AS_IMC:
      DB.calcANCwave_IMC(errGain);
      break;

    case AS_FF:
      DB.calcANCwave_FF(errGain, refGain);
      break;

    case AS_NP_FF:
      DB.calcANCwave_Nopass(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("%f,",e);
  }
  printf("\n");
}

void  ANC_Controller::printCoefH(){
    for(const auto& e : DB.coefH){
    printf("%f,",e);
  }
  printf("\n");
}