Skip to content
GitLab
Commit 20040ea0 authored by 杉下大河's avatar 杉下大河
Browse files

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

parent 576cd3b4
Hide whitespace changes
Inline Side-by-side
ANCcontroller.cpp
View file @ 20040ea0
......@@ -104,11 +104,14 @@ 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);
outputYBuf.assign(WaveBufLen, 0);
}
template <class T>
......@@ -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");
}
\ No newline at end of file
}
ANCcontroller.h
View file @ 20040ea0
......@@ -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};//ハウリング経路
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 float StepSizeW = 0.1;
const float StepSizeC = 0.95;
const float StepSizeH = 0.95;
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,7 +97,8 @@ 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
View file @ 20040ea0
#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 0 → 100644
View file @ 20040ea0
#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 0 → 100644
View file @ 20040ea0
#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
View file @ 20040ea0
......@@ -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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment