first commit

.vscode/arduino.json

+{
+    "port": "COM5",
+    "board": "STM32:stm32:Nucleo_144",
+    "configuration": "pnum=NUCLEO_F767ZI,upload_method=MassStorage,xserial=generic,usb=none,xusb=FS,opt=osstd,rtlib=nano",
+    "sketch": "STM32F767_ANC_IMC.ino"
+}
\ No newline at end of file

.vscode/c_cpp_properties.json

+{
+    "configurations": [
+        {
+            "name": "Win32",
+            "includePath": [
+                "C:\\Users\\morson\\AppData\\Local\\Arduino15\\packages\\STM32\\tools\\**",
+                "C:\\Users\\morson\\AppData\\Local\\Arduino15\\packages\\STM32\\hardware\\stm32\\1.9.0\\**",
+                "C:\\Users\\morson\\Documents\\Arduino\\libraries\\**"
+            ],
+            "forcedInclude": [
+                "C:\\Users\\morson\\AppData\\Local\\Arduino15\\packages\\STM32\\hardware\\stm32\\1.9.0\\cores\\arduino\\Arduino.h"
+            ],
+            "intelliSenseMode": "msvc-x64",
+            "compilerPath": "C:/Program Files (x86)/Microsoft Visual Studio/2019/Professional/VC/Tools/MSVC/14.22.27905/bin/Hostx64/x64/cl.exe",
+            "cStandard": "c11",
+            "cppStandard": "c++17"
+        }
+    ],
+    "version": 4
+}
\ No newline at end of file

ANCcontroller.cpp

+#define _USE_MATH_DEFINES //for Visual Studio
+
+#include "ANCcontroller.h"
+WaveGenerator::WaveGenerator()
+{
+  CreateSinWave();
+  CreateChirpWave();
+}
+//sin波作る
+void WaveGenerator::CreateSinWave() {
+  auto rad_step = 2 * M_PI * FREQ / SAMPLE_RATE;
+  for (int i = 0; i < SAMPLE_RATE / FREQ; i++)
+  {
+    wave_sin.push_back(AS_16BIT_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) {
+    waveChirp.emplace_back(int(w * AS_16BIT_MAX_NUM * GAIN));
+  }
+}
+
+
+//16bit情報に変換したsinを吐き出す
+int16_t WaveGenerator::playWaveSin()
+{
+  auto out = wave_sin[sound_cnt % wave_sin.size()];
+  sound_cnt++;
+  return out;
+}
+//16bit情報に変換したチャープ信号を吐き出す
+int16_t WaveGenerator::playWaveCharp()
+{
+  auto out = waveChirp[sound_cnt % waveChirp.size()];
+  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 <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::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<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() {
+
+}
+
+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;
+}
\ No newline at end of file

ANCcontroller.h

+#pragma once
+
+//#include <Arduino.h>
+#include <deque>
+#include <numeric>
+#include <math.h>
+
+#include "array.h"
+
+class WaveGenerator
+{
+  private:
+    const int16_t AS_16BIT_MAX_NUM = 32767;
+    const float FREQ = 120;
+    const float SAMPLE_RATE = 48000;
+    const float GAIN = 0.8;
+
+    //charp信号用_Juceから移植したのでリファクタリング推奨
+    const float fMin = 50;
+    const float fMax = 700;
+    const float tLength = 0.15;
+
+    std::deque<int16_t> wave_sin;
+    std::deque<int16_t> waveChirp;
+
+    void CreateSinWave();
+    void CreateChirpWave();
+
+  public:
+    uint32_t sound_cnt = 0;
+
+    WaveGenerator();
+
+    int16_t playWaveSin();
+    int16_t playWaveCharp();
+
+    void dumpWaveDat() {
+
+#if 0 //チャープ信号の内容をCSVで出力できます
+      std::string outFileName = "Charpwave.csv";
+      std::ofstream wavedat(outFileName);
+      for (auto &w : waveChirpSignal )
+        wavedat << w << ",";
+#endif
+
+    }
+
+
+
+};
+
+//model
+class ANC_DataBase {
+  public:
+    ANC_DataBase();
+
+    const float samplerate = 48000;
+    static const int16_t WaveBufLen = 2000;
+
+    //WaveBufLen >= CoefLenであること
+    static const int16_t CoefWLen = (882 + 2400) ;
+    static const int16_t CoefCLen = 900 ;
+    static const int16_t CoefHLen = 3800;
+
+    inoArray<float, CoefCLen> coefC;//主経路
+    inoArray<float, CoefWLen> coefW;//二次経路
+    inoArray<float, CoefHLen> coefH;//ハウリング経路
+
+    const float StepSizeW = 0.1;
+    const float StepSizeC = 0.95;
+    const float StepSizeH = 0.95;
+
+    //LMS用バッファー
+    std::deque<float> squaredXBuf;
+    std::deque<float> inputXBuf;
+    std::deque<float> filteredXBuf;
+    std::deque<float> outputYBuf;
+
+    float y = 0;
+    float yc = 0;
+
+
+    template <class T>
+    void shiftBuffer(T inValue, std::deque<T>& ioBuffer);
+
+    template<class T, class U, size_t FilterSize >
+    float calcFIR(const std::deque<T>& inDataBuf, inoArray<U, FilterSize>& filter_ary);
+
+    template <class T>
+    void InitFilter(T &Filter);
+
+
+    //クラス内部で適応フィルタを計算する
+    //使い勝手を意識して引数多めにしているが、減らしてもいい
+    //アルゴリズムを変更する場合はオーバーロードが妥当
+    template<class T, size_t FilterSize >
+    void calcAptiveFilter(float inErrValue, inoArray<T, FilterSize> &ioW, float inStepSize, const std::deque<T>& inXBuf, const std::deque<T>& xAvaHelp );
+
+    template<class T, size_t FilterSize >
+    void IdentTransferFunction(float errGain, float TrainingWave, inoArray<T, FilterSize>& TF, float inStepSize);
+
+    //ANC用の波形を算出する(2マイク) CoefH CoefC 必須 TODO:未実装
+    void calcANCwave_FF(float errGain , float refGain);
+    
+    //ANC用の波形を算出する(2マイク) CoefH CoefC いらないTODO:未実装
+    void calcANCwave_Nopass(float errGain , float refGain);
+
+    //ANC用の波形を算出する(1マイク) CoefC 必須
+    void calcANCwave_IMC(float errGain);
+
+    //ANC用の波形を算出する(H∞フィルタ)CoefH CoefC 必須 TODO:未実装
+    void calcANCwave_HI(float errGain);
+
+
+};
+
+#define AS_IMC 1 //1マイク運用
+#define AS_FF 2  //2マイク運用
+#define AS_HI 3  //H∞フィルタ
+class ANC_Controller {
+
+
+
+  public:
+	  //ANC構成情報
+	  ANC_DataBase DB;
+
+    //システム状態管理用の変数
+    static const char IDENTIFICAT_COEFC = 0;
+    static const char ACTIVATION_ANC = 1;
+    //static const int16_t IDENTIFICAT_coefH
+
+    ANC_Controller();
+
+    //TrainingWaveと比較しながら経路の伝達関数を算出する。2ndとEchoで共通のプロパティを使っており、同時に実行することは想定していない。
+    //同時に動かすと地獄みたいに重くなるので1つずつ測定し、終了時にDB.IdentTransferFunction()すること
+    void calc2ndPassTF(float errGain, float TrainingWave) ;
+    void calcEchoPassTF(float errGain, float TrainingWave) ;
+
+    /* ANCの出力波形を計算して波形を出力
+      AS_IMC 1 //1マイク運用
+      AS_FF 2  //2マイク運用
+      AS_HI 3  //H∞フィルタ */
+    float calcANCwave(const char methodNum , float errGain , float refGain);
+
+    //算出後の波形データを吐き出す
+    float playANCwave() {return DB.y;}
+
+  private:
+
+};

STM32F767_ANC_IMC.ino

+HardwareTimer Timer1(TIM1);
+// the setup function runs once when you press reset or power the board
+uint8_t sw = LOW;
+int sensorPin = A6;    // select the input pin for the potentiometer
+int biasPin = A12;
+int audioOut = A13;
+int sensorValue = 0;  // variable to store the value coming from the sensor
+bool Sendflg = true;
+int sendTimingCnt=0;
+
+void sendDebugdat(){
+  sw ^=1;
+  digitalWrite(LED_GREEN, sw);
+  printf("%d\n",sensorValue);
+
+}
+
+void audioreader() {
+  sensorValue = analogRead(sensorPin);//多分12bitで読み取る
+  sendTimingCnt++;
+  analogWrite(audioOut,sensorValue*5);
+}
+
+void setup() {
+  // initialize digital pin LED_BUILTIN as an output.
+  pinMode(LED_GREEN, OUTPUT);
+  pinMode(audioOut, OUTPUT);
+  pinMode(biasPin, OUTPUT);
+
+  analogWriteResolution(12);//analogwriteを12bit(MAX4095)で利用できるように設定
+  analogReadResolution(12);
+  analogWrite(biasPin,2482);
+
+
+  Timer1.pause();                   // タイマー停止
+  //Timer1.setOverflow(4000,HERTZ_FORMAT);        // 4000Hzで呼び出し
+  Timer1.setOverflow(48000,HERTZ_FORMAT);        // 48000Hzで呼び出し
+  Timer1.attachInterrupt(           // 割り込みハンドラの登録
+      audioreader                  // 呼び出す関数
+    );
+
+  Timer1.setCount(0);               // カウンタを0に設定
+  Timer1.refresh();                 // タイマ更新
+  Timer1.resume();                  // タイマースタート
+  tone(PB4,200);
+  analogWrite(biasPin,2482);
+}
+
+// the loop function runs over and over again forever
+void loop() {
+  if(sendTimingCnt >= 2000){
+    sendDebugdat();
+    sendTimingCnt=0;
+  }
+}

array.h

+#pragma once
+
+//ArduinoSTL.hには衝突回避のためにarryが無く、SPRESENSE側はイテレーターが使えないので手動実装
+
+template <class T, size_t N>
+struct inoArray {
+    // Storage
+    T data[N];
+
+    static size_t length() { return N; }
+    using type = T;
+
+    // Item access
+    T &operator[](size_t index) { return data[index]; }
+    const T &operator[](size_t index) const { return data[index]; }
+
+    // Iterators
+    T *begin() { return &data[0]; }
+    const T *begin() const { return &data[0]; }
+    T *end() { return &data[N]; }
+    const T *end() const { return &data[N]; }
+
+    // Comparisons
+    bool operator==(const inoArray<T, N> &rhs) const {
+        if (this == &rhs)
+            return true;
+        for (size_t i = 0; i < N; i++)
+            if ((*this)[i] != rhs[i])
+                return false;
+        return true;
+    }
+    bool operator!=(const inoArray<T, N> &rhs) const {
+        return !(*this == rhs);
+    }
+
+    //Function
+    
+    void fill( const T& value )
+    {
+        for (size_t i=0;i<N ;i++) {
+            data[i] = value;
+        }
+    }
+
+    
+};

ws.code-workspace

+{
+	"folders": [
+		{
+			"path": "."
+		},
+		{
+			"path": "..\\..\\..\\AppData\\Local\\Arduino15\\packages\\STM32\\hardware\\stm32\\1.9.0\\variants\\NUCLEO_F767ZI"
+		},
+		{
+			"path": "..\\IMC_ANC_test"
+		}
+	],
+	"settings": {}
+}
\ No newline at end of file