2マイク運用ANCが不安定ながら動いた。音量が0に近づくと発散する性質があるので修正が必要。

ANCcontroller.cpp

@@ -104,6 +104,9 @@ int16_t WaveGenerator::playWaveCharp(char mode)
 
 
 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);
@@ -162,8 +165,19 @@ void ANC_DataBase::InitFilter(T &Filter)
   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) {
+void ANC_DataBase::calcANCwave_IMC(float errGain) {/*
   //消音波を出力する
   //Filtered N-LMS with IMC-FeedBack
   // coefC : 同定により決定。以下の処理においては定数的。
@@ -189,12 +203,29 @@ void ANC_DataBase::calcANCwave_IMC(float errGain) {
 
   //Filtered N-LMS by IMC
   calcAptiveFilter(errGain, coefW, StepSizeW, filteredXBuf, squaredXBuf);
-  if (abs(y) > 32767) InitFilter(coefW);//出力値が大きすぎる場合系が不安定になっているのでリセット
+  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) {
 }
 
@@ -202,7 +233,7 @@ 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の各要素の二乗を作っておく。
@@ -213,9 +244,12 @@ void ANC_DataBase::IdentTransferFunction(float errGain, float TrainingWave, inoA
   calcAptiveFilter(InternalErrGain, TF, inStepSize, inputXBuf, squaredXBuf);
 
   if (abs(yc) > 32767) InitFilter(coefC);//出力値が大きすぎる場合系が不安定になっているのでリセット
-
+//*/
 }
 
+/*======================================================================================================*/
+/*=================================ANC_Controller=======================================================*/
+/*======================================================================================================*/
 
 ANC_Controller::ANC_Controller() {
 
@@ -223,17 +257,18 @@ ANC_Controller::ANC_Controller() {
 
 void ANC_Controller::calc2ndPassTF(float errGain, float TrainingWave) {
 
-  DB.IdentTransferFunction(errGain, TrainingWave , DB.coefC, DB.StepSizeC);
+  //DB.IdentTransferFunction(errGain, TrainingWave , DB.coefC, DB.StepSizeC);
 
 }
 
 void ANC_Controller::calcEchoPassTF(float errGain, float TrainingWave) {
 
-  DB.IdentTransferFunction(errGain, TrainingWave , DB.coefH, DB.StepSizeH);
+  //DB.IdentTransferFunction(errGain, TrainingWave , DB.coefH, DB.StepSizeH);
 
 }
 
-float ANC_Controller::calcANCwave(const char methodNum , float errGain , float refGain = 0 ) {
+//出力値は全部float32
+float32_t ANC_Controller::calcANCwave(const char methodNum , float32_t errGain , float32_t refGain = 0 ) {
   switch (methodNum)
   {
     case AS_IMC:
@@ -244,27 +279,33 @@ float ANC_Controller::calcANCwave(const char methodNum , float errGain , float r
       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("%d,",e);
+    printf("%f,",e);
   }
   printf("\n");
 }
 
 void  ANC_Controller::printCoefH(){
     for(const auto& e : DB.coefH){
-    printf("%d,",e);
+    printf("%f,",e);
   }
   printf("\n");
 }
+

ANCcontroller.h

@@ -8,14 +8,16 @@
 #include <Arduino.h>
 
 #include "array.h"
+#include "ext_arm_lms_norm_f32.h"
 
+#define INT_WAVE 0
+#define UNSINGED_INT_WAVE 1
+#define FLOAT_WAVE 2
 class WaveGenerator
 {
   private:
 
 
-    const int16_t AS_16BIT_MAX_NUM = 32767;
-    const int16_t AS_12BIT_MAX_NUM = 4095;
     const float FREQ = 120;
     const float GAIN = 0.8;
 
@@ -34,9 +36,6 @@ class WaveGenerator
     void CreateChirpWave();
 
   public:
-    static const char INT_WAVE = 0;
-    static const char UNSINGED_INT_WAVE = 1;
-    static const char FLOAT_WAVE = 2;
 
     uint32_t sound_cnt = 0;
 
@@ -57,17 +56,26 @@ class ANC_DataBase {
     static const int16_t WaveBufLen = 2000;
 
     //WaveBufLen >= CoefLenであること
-    static const int16_t CoefWLen = (882 + 2400) ;
+    static const int16_t CoefWLen = 500;//(882 + 2400) ;
     static const int16_t CoefCLen = 300 ;//900
     static const int16_t CoefHLen = 3800;
 
-    inoArray<float, CoefCLen> coefC;//主経路
-    inoArray<float, CoefWLen> coefW;//二次経路
-    inoArray<float, CoefHLen> coefH;//ハウリング経路
+    float32_t coefW[CoefWLen]={0};//主経路
+    float32_t coefC[CoefCLen]={0};//二次経路
+    float32_t coefH[CoefHLen]={0};//ハウリング経路
     
-    const float StepSizeW = 0.1;
-    const float StepSizeC = 0.95;
-    const float StepSizeH = 0.95;
+    float32_t lmsStateW[CoefWLen]={0};
+    float32_t lmsStateC[CoefCLen]={0};
+    float32_t lmsStateH[CoefHLen]={0};
+
+    //ちなみにmuのこと
+    float32_t StepSizeW = 0.0004;
+    float32_t StepSizeC = 0.95;
+    float32_t StepSizeH = 0.95;
+
+    arm_lms_norm_instance_f32 lmsInstance;
+
+    const int SftBits = 0;
 
     //LMS用バッファー
     std::deque<float> squaredXBuf;
@@ -75,9 +83,10 @@ class ANC_DataBase {
     std::deque<float> filteredXBuf;
     std::deque<float> outputYBuf;
 
-    float y = 0;
-    float yc = 0;
+    float32_t y = 0;
+    float32_t yc = 0;
     
+    float32_t err = 0;
 
     template <class T>
     void shiftBuffer(T inValue, std::deque<T>& ioBuffer);
@@ -88,6 +97,7 @@ class ANC_DataBase {
     template <class T>
     void InitFilter(T &Filter);
 
+    void initCorfWFillter();
     
     //クラス内部で適応フィルタを計算する
     //使い勝手を意識して引数多めにしているが、減らしてもいい
@@ -102,7 +112,7 @@ class ANC_DataBase {
     void calcANCwave_FF(float errGain , float refGain);
     
     //ANC用の波形を算出する(2マイク) CoefH CoefC いらないTODO:未実装
-    void calcANCwave_Nopass(float errGain , float refGain);
+    void calcANCwave_Nopass(float32_t errGain , float32_t refGain);
 
     //ANC用の波形を算出する(1マイク) CoefC 必須
     void calcANCwave_IMC(float errGain);
@@ -115,7 +125,10 @@ class ANC_DataBase {
 
 #define AS_IMC 1 //1マイク運用
 #define AS_FF 2  //2マイク運用
-#define AS_HI 3  //H∞フィルタ
+#define AS_NP_FF 3  //2マイク運用前処理ナシ
+#define AS_HI 4  //H∞フィルタ
+
+
 class ANC_Controller {
 
 
@@ -142,8 +155,6 @@ class ANC_Controller {
       AS_HI 3  //H∞フィルタ */
     float calcANCwave(const char methodNum , float errGain , float refGain);
 
-    //算出後の波形データを吐き出す
-    float playANCwave() {return DB.y;}
 
     void printCoefH();
     void printCoefC();

STM32F767_ANC_IMC.ino

 #include "ANCcontroller.h"
 //#include <STM32ADC.h>
 //#include <libmaple/adc.h>
-//#include <CMSIS_DSP.h>
 
 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 audioOutPin = A13;
+const int audioInErrPin = A6;    // select the input pin for the potentiometer
+const int audioInRefPin = A7;    // select the input pin for the potentiometer
+
+const int biasPin = D27;
+const int audioOutPin = A13;
 
 const int Q_BIT_NUM = 12;
 const int AS_SYS_SAMPLERATE = 4000*2;
-const int AUDIO_BIAS = 2482;//3.3V中の2V(12bit)
+const int AS_MAX_BIT_NUM = 4095;
+//const int AUDIO_BIAS = 2048;//3.3V中の1.65V(12bit) 3.3Vの中心で交流を受け付け+-1.65Vのレンジを作る
 
 char anc_state;
 bool Sendflg = true;
 bool calocReadyFlg = false;
 int sendTimingCnt=0;
-int audioInValue = 0;  // variable to store the value coming from the sensor
+int inValueErr = 0;  // variable to store the value coming from the sensor
+int inValueRef = 0;
+
+float32_t inValueErrf32=0;
+float32_t inValueReff32=0;
+
+float32_t ancSig=0;
+
 int audioOutValue = 0;
 
 uint32_t t1,t2;
@@ -29,14 +38,18 @@ ANC_Controller ANC;
 void sendDebugdat(){
   sw ^=1;
   digitalWrite(LED_GREEN, sw);
-  printf("%d\n",audioInValue);
+  Serial.println(t2-t1);
+  //Serial.println(ancSig);
+  //Serial.println(audioOutValue);
 }
 
 //adcは最適値を設定すれば高速化できる余地あり現在30μs
 void audioreader() {
 
-  audioInValue = analogRead(sensorPin);//多分12bitで読み取る
-
+  inValueErr = analogRead(audioInErrPin);//12bitで読み取る　range:+-1
+  inValueErrf32 = (((float32_t)inValueErr - 2048) /2048);
+  inValueRef = analogRead(audioInRefPin);//12bitで読み取る　range:+-1
+  inValueReff32 = (((float32_t)inValueRef - 2048) /2048);
   analogWrite(audioOutPin,audioOutValue);
   sendTimingCnt++;
   calocReadyFlg = true;
@@ -46,23 +59,31 @@ void audioreader() {
 void audioreader_debug() {
   t1 = micros();
 
-  audioInValue = analogRead(sensorPin);//多分12bitで読み取る
+  inValueErr = analogRead(audioInErrPin);//多分12bitで読み取る
   sendTimingCnt++;
 
   t2 = micros();
-  printf(" t=%d \n",t2-t1);
+  printf(" t=%f \n",t2-t1);
 }
 
 void setup() {
+  Serial.begin(9600);
   // initialize digital pin LED_BUILTIN as an output.
   pinMode(LED_GREEN, OUTPUT);
+  pinMode(LED_RED, OUTPUT);
+  pinMode(biasPin,OUTPUT);
+  digitalWrite(LED_RED,LOW);
+  digitalWrite(biasPin,HIGH);
+
   pinMode(audioOutPin, OUTPUT);
-  pinMode(biasPin, OUTPUT);
-  pinMode(sensorPin,INPUT_ANALOG);
+  //pinMode(biasPin, OUTPUT);
+
+  pinMode(audioInErrPin,INPUT_ANALOG);
+  pinMode(audioInRefPin,INPUT_ANALOG);
 
   analogWriteResolution(Q_BIT_NUM);//analogwriteを12bit(MAX4095)で利用できるように設定
   analogReadResolution(Q_BIT_NUM);
-  analogWrite(biasPin,2482);//MAX3.3vの2.0V
+  //analogWrite(biasPin,AUDIO_BIAS);//MAX3.3vの2.0V
 
 
   Timer1.pause();                   // タイマー停止
@@ -75,8 +96,7 @@ void setup() {
   Timer1.setCount(0);               // カウンタを0に設定
   Timer1.refresh();                 // タイマ更新
   Timer1.resume();                  // タイマースタート
-  tone(PB4,200);
-  analogWrite(biasPin,AUDIO_BIAS);
+
 
   anc_state = ANC.IDENTIFICAT_COEFC;
 }
@@ -93,25 +113,36 @@ void loop() {
 
   //ANC状態の管理
   if (anc_state == ANC.IDENTIFICAT_COEFC) {
-    t1 = micros();
     //伝達関数の推定をしている。非同期処理なのでタイミングがずれるかも
     if(calocReadyFlg){
-      ANC.calc2ndPassTF(audioInValue-AUDIO_BIAS,audioOutValue);
-      audioOutValue = s_sound.playWaveCharp(s_sound.UNSINGED_INT_WAVE);
+      //ANC.calc2ndPassTF(inValueErr-AUDIO_BIAS,audioOutValue);
+      //audioOutValue = s_sound.playWaveCharp(s_sound.UNSINGED_INT_WAVE);
 
       calocReadyFlg=false;
     }
-    t2 = micros();
-    printf(" t=%d \n",t2-t1);
 
   //起動から15sでモード移行 正確に処理したい場合はタイマーを使おう！
-    if (millis() > 15 * 1000) {
+    if (millis() > 5 * 1000) {
       ANC.printCoefC();
       anc_state = ANC.ACTIVATION_ANC;
+      digitalWrite(LED_RED,HIGH);
     }
   }
+
   else if (anc_state == ANC.ACTIVATION_ANC) {
-    audioOutValue = 0;
+
+    if(calocReadyFlg){
+      t1 = micros();
+      ancSig = ANC.calcANCwave(AS_NP_FF,inValueErrf32,inValueReff32);
+      t2 = micros();
+      audioOutValue = (int)((ancSig+1)/2 * AS_MAX_BIT_NUM);
+      //audioOutValue = s_sound.playWaveSin(UNSINGED_INT_WAVE);
+
+      //audioOutValue = 0;
+
+      calocReadyFlg=false;
+    }
+
   }
 
 }

ext_arm_lms_norm_f32.cpp

+#include "ext_arm_lms_norm_f32.h"
+
+//エラーマイクの観測値をpErrMicに入力できるようにした。
+void arm_lms_norm_f32_2mic(
+        arm_lms_norm_instance_f32 * S,
+  const float32_t * pSrc,
+        float32_t * pErrMic,
+
+        float32_t * pOut,
+        float32_t * pErr,
+        uint32_t blockSize)
+{
+        float32_t *pState = S->pState;                 /* State pointer */
+        float32_t *pCoeffs = S->pCoeffs;               /* Coefficient pointer */
+        float32_t *pStateCurnt;                        /* Points to the current sample of the state */
+        float32_t *px, *pb;                            /* Temporary pointers for state and coefficient buffers */
+        float32_t mu = S->mu;                          /* Adaptive factor */
+        float32_t acc, e;                              /* Accumulator, error */
+        float32_t w;                                   /* Weight factor */
+        uint32_t numTaps = S->numTaps;                 /* Number of filter coefficients in the filter */
+        uint32_t tapCnt, blkCnt;                       /* Loop counters */
+        float32_t energy;                              /* Energy of the input */
+        float32_t x0, in;                              /* Temporary variable to hold input sample and state */
+
+  /* Initializations of error,  difference, Coefficient update */
+  e = 0.0f;
+  w = 0.0f;
+
+  energy = S->energy;
+  x0 = S->x0;
+
+  /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+  /* pStateCurnt points to the location where the new input data should be written */
+  pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+  /* initialise loop count */
+  blkCnt = blockSize;
+
+  while (blkCnt > 0U)
+  {
+    /* Copy the new input sample into the state buffer */
+    *pStateCurnt++ = *pSrc;
+
+    /* Initialize pState pointer */
+    px = pState;
+
+    /* Initialize coefficient pointer */
+    pb = pCoeffs;
+
+    /* Read the sample from input buffer */
+    in = *pSrc++;
+
+    /* Update the energy calculation */
+    energy -= x0 * x0;
+    energy += in * in;
+
+    /* Set the accumulator to zero */
+    acc = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+    /* Loop unrolling: Compute 4 taps at a time. */
+    tapCnt = numTaps >> 2U;
+
+    while (tapCnt > 0U)
+    {
+      /* Perform the multiply-accumulate */
+      acc += (*px++) * (*pb++);
+
+      acc += (*px++) * (*pb++);
+
+      acc += (*px++) * (*pb++);
+
+      acc += (*px++) * (*pb++);
+
+      /* Decrement loop counter */
+      tapCnt--;
+    }
+
+    /* Loop unrolling: Compute remaining taps */
+    tapCnt = numTaps % 0x4U;
+
+#else
+
+    /* Initialize tapCnt with number of samples */
+    tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+    while (tapCnt > 0U)
+    {
+      /* Perform the multiply-accumulate */
+      acc += (*px++) * (*pb++);
+
+      /* Decrement the loop counter */
+      tapCnt--;
+    }
+
+    /* Store the result from accumulator into the destination buffer. */
+    *pOut++ = acc;
+
+    /* Compute and store error */
+    e = *pErrMic;
+    *pErr++ = e;
+
+    /* Calculation of Weighting factor for updating filter coefficients */
+    /* epsilon value 0.000000119209289f */
+    w = (e * mu) / (energy + 0.000000119209289f);
+
+    /* Initialize pState pointer */
+    px = pState;
+
+    /* Initialize coefficient pointer */
+    pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+    /* Loop unrolling: Compute 4 taps at a time. */
+    tapCnt = numTaps >> 2U;
+
+    /* Update filter coefficients */
+    while (tapCnt > 0U)
+    {
+      /* Perform the multiply-accumulate */
+      *pb += w * (*px++);
+      pb++;
+
+      *pb += w * (*px++);
+      pb++;
+
+      *pb += w * (*px++);
+      pb++;
+
+      *pb += w * (*px++);
+      pb++;
+
+      /* Decrement loop counter */
+      tapCnt--;
+    }
+
+    /* Loop unrolling: Compute remaining taps */
+    tapCnt = numTaps % 0x4U;
+
+#else
+
+    /* Initialize tapCnt with number of samples */
+    tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+    while (tapCnt > 0U)
+    {
+      /* Perform the multiply-accumulate */
+      *pb += w * (*px++);
+      pb++;
+
+      /* Decrement loop counter */
+      tapCnt--;
+    }
+
+    x0 = *pState;
+
+    /* Advance state pointer by 1 for the next sample */
+    pState = pState + 1;
+
+    /* Decrement loop counter */
+    blkCnt--;
+  }
+
+  /* Save energy and x0 values for the next frame */
+  S->energy = energy;
+  S->x0 = x0;
+
+  /* Processing is complete.
+     Now copy the last numTaps - 1 samples to the start of the state buffer.
+     This prepares the state buffer for the next function call. */
+
+  /* Points to the start of the pState buffer */
+  pStateCurnt = S->pState;
+
+  /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+  /* Loop unrolling: Compute 4 taps at a time. */
+  tapCnt = (numTaps - 1U) >> 2U;
+
+  while (tapCnt > 0U)
+  {
+    *pStateCurnt++ = *pState++;
+    *pStateCurnt++ = *pState++;
+    *pStateCurnt++ = *pState++;
+    *pStateCurnt++ = *pState++;
+
+    /* Decrement loop counter */
+    tapCnt--;
+  }
+
+  /* Loop unrolling: Compute remaining taps */
+  tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+  /* Initialize tapCnt with number of samples */
+  tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+  while (tapCnt > 0U)
+  {
+    *pStateCurnt++ = *pState++;
+
+    /* Decrement loop counter */
+    tapCnt--;
+  }
+
+}

ext_arm_lms_norm_f32.h

+#pragma once
+#include <CMSIS_DSP.h>
+
+
+void arm_lms_norm_f32_2mic(
+        arm_lms_norm_instance_f32 * S,
+  const float32_t * pSrc,
+        float32_t * pErrMic,
+
+        float32_t * pOut,
+        float32_t * pErr,
+        uint32_t blockSize);
\ No newline at end of file

ws.code-workspace

@@ -5,16 +5,18 @@
 		},
 		{
 			"path": "..\\..\\..\\AppData\\Local\\Arduino15\\packages\\STM32\\hardware\\stm32\\1.9.0\\variants\\NUCLEO_F767ZI"
-		},
-		{
-			"path": "..\\IMC_ANC_test"
 		}
 	],
 	"settings": {
 		"files.associations": {
 			"xiosbase": "cpp",
 			"xlocale": "cpp",
-			"ios": "cpp"
+			"ios": "cpp",
+			"queue": "cpp",
+			"stack": "cpp",
+			"random": "cpp",
+			"associative_base": "cpp",
+			"deque": "cpp"
 		}
 	}
 }
\ No newline at end of file