代码收藏家 STM32-MEMS麦克风音频处理与输出详解
FP-AUD-SMARTMIC1简介
FP-AUD-SMARTMIC1是一个STM32Cube功能包。该软件包实现了一个完整的应用程序，目标是MEMS麦克风阵列的高级处理，包括数字MEMS麦克风采集、波束成形、源定位和回声消除。处理后的音频被发送到USB主机和连接到相关扩展板的扬声器。该功能包基于STM32Cube软件技术，可轻松在不同的STM32微控制器之间进行移植。
此示例实现支持两种系统：STM32 NUCLEO-F446RE开发板，配备X-NUCLEO-CA01M1或X-NUCLEO-CA02M2扩展板和STEVAL-MIC001V1、STEVAL-MIC002V1、STEVAL-MIC003V1或STEVAL-MIC 005V1数字麦克风评估板或BlueCoin
该软件也可以在GitHub上获得，用户可以通过“问题”和“拉取请求”选项卡发出错误信号并提出新想法。
所有功能
STM32Cube的软件扩展：
AcousticBF实时波束形成
AcousticEC实时声学回声消除
AcousticSL实时声源定位
完整的应用程序，包括单个示例应用程序中的所有声学功能软件图形用户界面，可从主机PC轻松控制参数和算法当连接到X-NUCLEO-CA01M1和X-NUCLEO-CA02M2扩展板时，NUCLEO-F446RE开发板上可用的示例实现
官网网站：FP-AUD-SMARTMIC1 – 专为MEMS麦克风采集、高级音频处理和音频输出打造的STM32Cube功能包 – 意法半导体STMicroelectronicshttps://www.st.com/zh/embedded-software/fp-aud-smartmic1.htmlGithub：
GitHub – STMicroelectronics/fp-aud-smartmic1: FP-AUD-SMARTMIC1 provides a firmware running on STM32 which acquires audio signals of four digital MEMS microphones, elaborates them by means of embedded DSP libraries and streams the processed audio to both an USB host and a loudspeaker connected to the relevant expansion board.https://github.com/STMicroelectronics/fp-aud-smartmic1
AcousticSL实时声源定位
本文针对该功能包中的声源定位算法进行探索研究，相关算法文件的可以从Github中下载
文件夹路径fp-aud-smartmic1-main\Middlewares\ST\STM32_AcousticSL_Library
重点文件内容
acoustic_sl.h
/*
此文件包含声学声源定位库定义。
*/
#ifndef __ACOUSTIC_SL_H
#define __ACOUSTIC_SL_H
#include "stdint.h"
/*算法类型定义*/
#define ACOUSTIC_SL_ALGORITHM_XCORR ((uint32_t)0x00000001)
#define ACOUSTIC_SL_ALGORITHM_GCCP ((uint32_t)0x00000002)
#define ACOUSTIC_SL_ALGORITHM_BMPH ((uint32_t)0x00000004)
/*错误类型*/
#define ACOUSTIC_SL_ALGORITHM_ERROR ((uint32_t)0x00000001)
#define ACOUSTIC_SL_PTR_CHANNELS_ERROR ((uint32_t)0x00000002)
#define ACOUSTIC_SL_CHANNEL_NUMBER_ERROR ((uint32_t)0x00000004)
#define ACOUSTIC_SL_SAMPLING_FREQ_ERROR ((uint32_t)0x00000008)
#define ACOUSTIC_SL_RESOLUTION_ERROR ((uint32_t)0x00000010)
#define ACOUSTIC_SL_THRESHOLD_ERROR ((uint32_t)0x00000020)
#define ACOUSTIC_SL_DISTANCE_ERROR ((uint32_t)0x00000040)
#define ACOUSTIC_SL_NUM_OF_SAMPLES_ERROR ((uint32_t)0x00000080)
#define ACOUSTIC_SL_PROCESSING_ERROR ((uint32_t)0x00000100)
#ifndef ACOUSTIC_LOCK_ERROR
#define ACOUSTIC_LOCK_ERROR ((uint32_t)0x10000000)
#endif
/**/
#define ACOUSTIC_SL_NO_AUDIO_DETECTED -100
/*输出参数结构体*/
typedef struct
{
/*算法类型选择，即上述定义的XCORR,GCCP和BMPH*/
uint32_t algorithm;
/*设置采样频率*/
uint32_t sampling_frequency;
/*设置通道数，2通道进行180°定位，4通道进行360°定位*/
uint32_t channel_number;
/*麦克风通道选择*/
uint8_t ptr_M1_channels;
uint8_t ptr_M2_channels;
uint8_t ptr_M3_channels;
uint8_t ptr_M4_channels;
/*麦克风1，2之间的距离（mm），默认150*/
uint16_t M12_distance;
/*麦克风3,4之间的距离（mm），默认150*/
uint16_t M34_distance;
/*内部存储空间大小设置，采用AcousticSL_getMemorySize()函数*/
uint32_t internal_memory_size;
/*指向用户分配的内存*/
uint32_t * pInternalMemory;
/*指向单次处理的样本数*/
int16_t samples_to_process;
} AcousticSL_Handler_t;
typedef struct
{
/*指定语音响度阈值，低于阈值算法不起作用，阈值范围从0到1000，默认值为24*/
uint16_t threshold;
/*算法的角度分辨率，默认值为4.如果使用XCORR，则忽略*/
uint32_t resolution;
} AcousticSL_Config_t;
/*内存分配函数*/
uint32_t AcousticSL_getMemorySize(AcousticSL_Handler_t * pHandler);
/*初始化函数*/
uint32_t AcousticSL_Init(AcousticSL_Handler_t * pHandler);
/*数据输入函数*/
uint32_t AcousticSL_Data_Input(void *pM1, void *pM2, void *pM3, void *pM4, AcousticSL_Handler_t * pHandler);
/*音频处理函数*/
uint32_t AcousticSL_Process(int32_t * Estimated_Angle, AcousticSL_Handler_t * pHandler);
/*设置阈值与分辨率的值*/
uint32_t AcousticSL_setConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig);
/*获取参数*/
uint32_t AcousticSL_getConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig);
/*获取版本信息*/
uint32_t AcousticSL_GetLibVersion(char *version);
#endif
该功能可以选择三种不同的声源定位算法，分别是XCORR（时域互相关），GCCP（频域变换广义互相关），BMPH（The block-matching pursuit with hangover）
XCORR算法应用在时域，需要的资源较少，分辨率低，对麦克风放置距离有要求
GCCP算法应用在频域，需要的资源脚多，分辨率高，对麦克风放置距离无要求
BMPH算法应用在频域，需要的资源适中，对麦克风放置距离无要求，需要执行一个遗传的步骤，增强稳定性
双通道麦克风放置示意图
四通道麦克风放置示意图
对于XCORR算法分辨率取决于采样频率与麦克风之间的距离，如下式所示（floor下取整）
以双通道放置为例，麦克风间距10cm，采样频率100kHz，声速343.1m/s，可以计算得到角度分辨率为3°。如需要更高精度，则需要提高麦克风间距与采样频率。
AcousticSL.c
#include "libSoundSourceLoc.c"
/*初始化函数*/
uint32_t AcousticSL_Init(AcousticSL_Handler_t * pHandler)
{
return libSoundSourceLoc_Init(pHandler);
}
/*数据输入函数*/
uint32_t AcousticSL_Data_Input(void *pM1, void *pM2, void *pM3, void *pM4, AcousticSL_Handler_t * pHandler)
{
return libSoundSourceLoc_Data_Input((int16_t *)pM1, (int16_t *)pM2, (int16_t *)pM3, (int16_t *)pM4, pHandler);
}
/*音频分析函数*/
uint32_t AcousticSL_Process(int32_t * Estimated_Angle, AcousticSL_Handler_t * pHandler)
{
return libSoundSourceLoc_Process(Estimated_Angle, pHandler);
}
/*参数设置函数*/
uint32_t AcousticSL_setConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig)
{
return libSoundSourceLoc_setConfig(pHandler, pConfig);
}
/*获取参数函数*/
uint32_t AcousticSL_getConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig)
{
return libSoundSourceLoc_getConfig(pHandler, pConfig);
}
/*内存配置函数*/
uint32_t AcousticSL_getMemorySize(AcousticSL_Handler_t * pHandler)
{
return libSoundSourceLoc_getMemorySize(pHandler);
}
/*获取版本信息*/
uint32_t AcousticSL_GetLibVersion(char *version)
{
return libSoundSourceLoc_GetLibVersion(version);
}可以看到在AcousticSL.c文件中的所有函数都是调用libSoundSourceLoc.c中的函数，之后我们仔细阅读该函数中的库
libSoundSourceLoc.c
#ifndef __LIB_SOUNDSOURCELOC_H
#define __LIB_SOUNDSOURCELOC_H
#include "acoustic_sl.h"
#include "arm_math.h"
#include "math.h"
struct _libSoundSourceLoc_Handler_Internal;
typedef struct {
float32_t (*SourceLocFunction)(struct _libSoundSourceLoc_Handler_Internal *SLocInternal, int32_t * out_angles);
uint32_t (*CheckEventFunction)(struct _libSoundSourceLoc_Handler_Internal *SLocInternal);
}libSoundSourceLoc_Handler_Callbacks;
typedef struct _libSoundSourceLoc_Handler_Internal{
/*采样频率*/
uint32_t sampling_frequency;
/*算法类型*/
uint32_t Type;
/**/
uint32_t Buffer_State;
/*采样数目*/
uint32_t Sample_Number_To_Process;
/*采样数目（每毫秒）*/
uint16_t Sample_Number_Each_ms;
/*输入数据数目*/
uint16_t Input_Counter;
/**/
uint16_t EVENT_THRESHOLD;
/*麦克风配置*/
uint16_t Mic_Number;
uint8_t ptr_M1_channels;
uint8_t ptr_M2_channels;
uint8_t ptr_M3_channels;
uint8_t ptr_M4_channels;
/*麦克风之间的距离和TAUD*/
float32_t M12_distance;
float32_t M34_distance;
int32_t M12_TAUD;
int32_t M34_TAUD;
libSoundSourceLoc_Handler_Callbacks Callbacks;
/*音频分析数据*/
int32_t Estimated_Angle_12;
int32_t Estimated_Angle_34;
uint32_t resolution;
void * M1_Data;
void * M2_Data;
void * M3_Data;
void * M4_Data;
arm_rfft_fast_instance_f32 * SFast;
float32_t * FFT_Out;
float32_t * PowerSpectrum;
float32_t * Phase;
float32_t * sumArray;
int32_t * SourceLocFilterArray;
int32_t * SourceLocFilterArray1;
float32_t * window;
//dz
uint32_t Sample_Number_To_Store;
uint8_t * remap_mic; // 4
uint16_t num_of_freq;
uint16_t num_of_angles;
uint16_t freq_range_min;
uint16_t freq_range_max;
float32_t * mics_distance; // AUDIO_CHANNELS
float32_t * mics_angle; // AUDIO_CHANNELS
float32_t * theta; // NUM_ANGLES
float32_t phi;//
uint16_t mics_write_offset; //starting point in every mic \in {0,16,32,...} \subseteq [0 MIC_LEN)
uint16_t mics_read_offset; //starting point in every mic \in 0,MIC_LEN
int16_t * score_theta;// RESOLUTION
uint8_t reactivity;
uint8_t stability;
uint8_t array_type;// CIRCULAR_ARRAY
uint8_t local_stabilizer;
float32_t * mic_tmp;//[DFT_LEN]
float32_t * ptr_freq_energies;// ptr
float32_t * s;//[2*AUDIO_CHANNELS*NUMOF_FREQ]
int16_t * frequencies_under_analysis; // NUM OF FREQ
int16_t * sources; // 2* OUTPUT SOURCES
float32_t * steering_tau;
} libSoundSourceLoc_Handler_Internal;
#define FFT_POINTS 512
#define LIB_VERSION ((uint32_t)0x00030000)
#define FILTER_MIN 10
#define FILTER_STEP_MAIN 10
#define FILTER_MAX_MAIN 50
#define FILTER_STEP_SEC 8
#define FILTER_MAX_SEC 20
#define FACTOR_INDEX_2_HZ (uint16_t)(SLocInternal->sampling_frequency/SLocInternal->Sample_Number_To_Process)
#define MIN_RESOLUTION 4U
#define MAX_NUM_OF_ANGLES (360U/MIN_RESOLUTION)
#define MAX_NUM_OF_FREQUENCIES 16U
#define MAX_AUDIO_CHANNELS 8U
#define MAX_THRESHOLD 1000U
#define NUM_OUTPUT_SOURCES 4U
#define CIRCULAR_ARRAY 1U
#define LINEAR_ARRAY 2U
#ifndef SOUND_SPEED
#define SOUND_SPEED (float32_t)343.1f
#endif
#ifndef UNUSED
#define UNUSED(X) (void)X /* To avoid gcc/g++ warnings */
#endif
#include "doa_via_block_sparsity.c"
#define Abs(x) ( (((float32_t)x) < 0.0f) ? (-((float32_t)x)) : ((float32_t)x))
#define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))
#define SaturaL(N, L) (((N)<(L))?(L):(N))
#define SaturaH(N, H) (((N)>(H))?(H):(N))
static uint32_t CheckEvent(libSoundSourceLoc_Handler_Internal * SLocInternal);
static float32_t XCORR_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal, int32_t * out_angles);
static float32_t GCC_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal, int32_t * out_angles);
static int32_t get_max_pos(libSoundSourceLoc_Handler_Internal * SLocInternal,int32_t length,int32_t step);
static void FilterAngle(int32_t *SourceAngle, int32_t* LedStatus, uint16_t max_value, uint16_t A, uint16_t SatA, uint16_t B, uint16_t SatB);
/*版本信息*/
static uint32_t libSoundSourceLoc_GetLibVersion(char *version)
{
char str1[35] = "ST AcousticSL v3.0.0";
(void)strcpy(version, str1);
return strlen(str1);
}
/*数据输入*/
static uint32_t libSoundSourceLoc_Data_Input(void *pM1, void *pM2, void *pM3, void *pM4, AcousticSL_Handler_t * pHandler)
{
libSoundSourceLoc_Handler_Internal * SLocInternal = (libSoundSourceLoc_Handler_Internal *)(pHandler->pInternalMemory);
uint8_t ret = 0;
uint32_t i;
/*如果XCORR和BMPH算法，输入数据格式为int16；如果GCCP算法，输入数据格式为float32*/
/*兼容双通道麦克风与四通道麦克风*/
if((SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_XCORR) || (SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_BMPH) )
{
for (i = 0; i < SLocInternal->Sample_Number_Each_ms; i ++)
{
((int16_t *)(SLocInternal->M1_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM1))[i*SLocInternal->ptr_M1_channels];
((int16_t *)(SLocInternal->M2_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM2))[i*SLocInternal->ptr_M2_channels];
if (SLocInternal->Mic_Number == 4U)
{
((int16_t *)(SLocInternal->M3_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM3))[i*SLocInternal->ptr_M3_channels];
((int16_t *)(SLocInternal->M4_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM4))[i*SLocInternal->ptr_M4_channels];
}
SLocInternal->Input_Counter ++;
}
}
else if(SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_GCCP )
{
for (i = 0; i < SLocInternal->Sample_Number_Each_ms; i ++)
{
((float32_t *)(SLocInternal->M1_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM1))[i*SLocInternal->ptr_M1_channels];//todo interleaving
((float32_t *)(SLocInternal->M2_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM2))[i*SLocInternal->ptr_M2_channels];
if (SLocInternal->Mic_Number == 4U)
{
((float32_t *)(SLocInternal->M3_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM3))[i*SLocInternal->ptr_M3_channels];
((float32_t *)(SLocInternal->M4_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM4))[i*SLocInternal->ptr_M4_channels];
}
SLocInternal->Input_Counter ++;
}
}
else
{
/* no other use cases are handled */
}
/*如果算法是BMPH，需要设置mics_read_offset参数，对于双通道为0，四通道为Sample_Number_To_Store*/
if(SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_BMPH)
{
if(SLocInternal->Input_Counter == SLocInternal->Sample_Number_To_Store)
{
ret = 1;
SLocInternal->Buffer_State = 1;
SLocInternal->mics_read_offset= 0;
}
if(SLocInternal->Input_Counter == (SLocInternal->Sample_Number_To_Store * 2U))
{
ret = 1;
SLocInternal->Buffer_State = 2;
SLocInternal->Input_Counter = 0;
SLocInternal->mics_read_offset = (uint16_t)SLocInternal->Sample_Number_To_Store;
}
}
else
{
if(SLocInternal->Input_Counter == SLocInternal->Sample_Number_To_Store)
{
ret = 1;
SLocInternal->Buffer_State = 1;
}
if(SLocInternal->Input_Counter == (SLocInternal->Sample_Number_To_Store * 2U))
{
ret = 1;
SLocInternal->Buffer_State = 2;
SLocInternal->Input_Counter = 0;
}
}
return ret;
}
/*音频处理函数*/
static uint32_t libSoundSourceLoc_Process(int32_t * Estimated_Angle, AcousticSL_Handler_t * pHandler)
{
libSoundSourceLoc_Handler_Internal * SLocInternal = (libSoundSourceLoc_Handler_Internal *)(pHandler->pInternalMemory);
/*计算寄存变量*/
int32_t Estimated_temp_360[2];
/*Buffer_State不为0，即完成数据输入*/
if(SLocInternal->Buffer_State!=0U)
{
/*如果BMPH算法，或者调用Check函数返回1，则采用定位函数。否则寄存变量1变为-1*/
if((SLocInternal->Type==ACOUSTIC_SL_ALGORITHM_BMPH) || (SLocInternal->Callbacks.CheckEventFunction(SLocInternal)==1))
{
SLocInternal->Callbacks.SourceLocFunction(SLocInternal,Estimated_temp_360);
}
else
{
Estimated_temp_360[0]=-1;
}
}
/*如果BMPH算法，调用定位函数之后，如果寄存变量1为-500，则会重新赋值-100*/
if (SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_BMPH)
{
if (Estimated_temp_360[0]==-500)
{
Estimated_temp_360[0]=-100;
}
}
/*如果不是BMPH算法，但是四通道输入*/
else if(SLocInternal->Mic_Number==4U)
{
STEP
/*角度计算前需要进行滤波处理，滤波器参数设计（360，STEPM，MAXM，STEPS，MAXS）*/
FilterAngle((int32_t *)&Estimated_temp_360[0], SLocInternal->SourceLocFilterArray, 360 , FILTER_STEP_MAIN , FILTER_MAX_MAIN, FILTER_STEP_SEC ,FILTER_MAX_SEC);
/*如果寄存变量1为-1，寄存变量1重新赋值-100*/
if(Estimated_temp_360[0]==-1)
{
Estimated_temp_360[0]=-100;
}
}
/*如果不是BMPH算法，且双通道输入*/
else
{
/*角度计算前需要进行滤波处理，滤波器固定参数（180，10，50，8，20）*/
FilterAngle((int32_t *)&Estimated_temp_360[0], SLocInternal->SourceLocFilterArray, 180 , 10 , 50, 8 ,20);
/*如果寄存变量1为-1，寄存变量1重新赋值-100，否则寄存变量1值减90*/
if(Estimated_temp_360[0]==-1)
{
Estimated_temp_360[0]=-100;
}
else
{
Estimated_temp_360[0]-=90;
}
}
/*计算角度参数1为寄存变量1*/
Estimated_Angle[0] = Estimated_temp_360[0];
return 0;
}重点算法
XCORR_GetAngle()
static float32_t XCORR_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal, int32_t * out_angles)
{
/*tau时延*/
int32_t tau, k;
float32_t delta_t24 = 0.0f;
float32_t delta_t13 = 0.0f;
float32_t test = 0.0f;
uint32_t correlation, MAX13, MAX24;
if(SLocInternal->Mic_Number >= 2U)
{
MAX13=0x80000000U; //-inf
/*遍历所有离散时域信号延迟*/
for(tau=-SLocInternal->M12_TAUD;tau<=SLocInternal->M12_TAUD;tau++)
{
correlation=0;
/*互相关计算*/
for(k=SLocInternal->M12_TAUD; k<((int32_t)SLocInternal->Sample_Number_To_Process-SLocInternal->M12_TAUD);k++)
{
correlation = correlation + (((uint32_t)((int16_t *)(SLocInternal->M2_Data))[((SLocInternal->Buffer_State-1U)*SLocInternal->Sample_Number_To_Process)+(uint32_t)k] * (uint32_t)((int16_t *)(SLocInternal->M1_Data))[((SLocInternal->Buffer_State-1U)*SLocInternal->Sample_Number_To_Process)+(uint32_t)k+(uint32_t)tau])/256U);
}
/*寻找互相关计算的最大值与对应的时延值*/
if (correlation > MAX13)
{
MAX13 = correlation;
delta_t13=(float32_t)tau;
}
}
}
/*针对四通道，格外计算M34之间的互相关*/
if(SLocInternal->Mic_Number == 4U)
{
MAX24=0x80000000U; //-inf
for(tau=-SLocInternal->M34_TAUD;tau<=SLocInternal->M34_TAUD;tau++)
{
correlation=0;
for(k=SLocInternal->M34_TAUD; k<((int32_t)SLocInternal->Sample_Number_To_Process-SLocInternal->M34_TAUD); k++)
{
correlation = correlation + (((uint32_t)((int16_t *)(SLocInternal->M4_Data))[((SLocInternal->Buffer_State-1U)*SLocInternal->Sample_Number_To_Process)+(uint32_t)k] * (uint32_t)((int16_t *)(SLocInternal->M3_Data))[((SLocInternal->Buffer_State-1U)*SLocInternal->Sample_Number_To_Process)+(uint32_t)k+(uint32_t)tau])/256U);
}
if (correlation > MAX24)
{
MAX24 = correlation;
delta_t24=(float32_t)tau;
}
}
}
/*通过计算得到的互相关最大时的时延和最大时延进行角度计算*/
SLocInternal->Buffer_State=0;
if(SLocInternal->Mic_Number == 2U)
{
test = (delta_t13+(float32_t)SLocInternal->M12_TAUD )* (180.0f/((float32_t)SLocInternal->M12_TAUD*2.0f));
}
else if(SLocInternal->Mic_Number == 4U)
{
test=(float32_t)atan2(delta_t13, delta_t24);
test= test * (360.0f / (3.14f*2.0f) );
if(test<0.0f)
{
test+=360.0f; /* in [0 360] -> TODO: Optimize the whole angle computation from the maximum phase index*/
}
}
else
{
/* no other use cases are handled */
}
out_angles[0]=(int32_t)test;
return test;
}使用XCORR算法，以双通道输入为例，基于采集到的麦克风数据，通过增加不同的时延，调整两个序列时间差进行互相关的计算，当补充时延到两个麦克风采集到的信号相同时互相关计算值最大。即可通过补充的时延值判断声源的角度。
SLocInternal->M12_TAUD = (int32_t)SLocInternal->M12_distance * ((int32_t)SLocInternal->sampling_frequency/(int32_t)SOUND_SPEED);
SLocInternal->M34_TAUD =(int16_t)floor((float64_t)SLocInternal->M34_distance * ((float64_t)SLocInternal->sampling_frequency/(float64_t)SOUND_SPEED));
GCC_GetAngle()
static float32_t GCC_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal, int32_t * out_angles)
{
uint32_t j;
float32_t fi = 0.0f;;
uint32_t buffer_offset = (SLocInternal->Buffer_State-1U)*SLocInternal->Sample_Number_To_Process;
float32_t * M1_Data = (float32_t *)SLocInternal->M1_Data;
float32_t * M2_Data = (float32_t *)SLocInternal->M2_Data;
float32_t * M3_Data = (float32_t *)SLocInternal->M3_Data;
float32_t * M4_Data = (float32_t *)SLocInternal->M4_Data;
float32_t * Power_Spectrum = (float32_t *)SLocInternal->PowerSpectrum;
float32_t * FFT_Out = (float32_t *)SLocInternal->FFT_Out;
float32_t * hanning = (float32_t *)SLocInternal->window;
float32_t tempMag = 1e-7f;
/*WINDOWING*/
/*处理用的窗函数*/
for(j=0;j<SLocInternal->Sample_Number_To_Process;j++)
{
if(SLocInternal->Mic_Number >= 2U)
{
M1_Data[buffer_offset + j] *= hanning[j];
M2_Data[buffer_offset + j] *= hanning[j];
}
if(SLocInternal->Mic_Number == 4U)
{
M3_Data[buffer_offset + j] *= hanning[j];
M4_Data[buffer_offset + j] *= hanning[j];
}
}
/*FFTs*/
/*进行傅里叶变换，得到频域数据*/
if(SLocInternal->Mic_Number >= 2U)
{
/*M1 FFT*/
arm_rfft_fast_f32(SLocInternal->SFast,&M1_Data[buffer_offset],(float32_t *)FFT_Out,0);
for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
{
M1_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
}
/*M2 FFT*/
arm_rfft_fast_f32(SLocInternal->SFast,&M2_Data[buffer_offset],(float32_t *)FFT_Out,0);
for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
{
M2_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
}
}
if(SLocInternal->Mic_Number == 4U)
{
/*M3 FFT*/
arm_rfft_fast_f32(SLocInternal->SFast,&M3_Data[buffer_offset],(float32_t *)FFT_Out,0);
for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
{
M3_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
}
/*M4 FFT*/
arm_rfft_fast_f32(SLocInternal->SFast,&M4_Data[buffer_offset],(float32_t *)FFT_Out,0);
for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
{
M4_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
}
}
/*FILTERING*/
if(SLocInternal->Mic_Number >= 2U)
{
M1_Data[buffer_offset]=0.0f;
M2_Data[buffer_offset]=0.0f;
M1_Data[buffer_offset+1U]=0.0f;
M2_Data[buffer_offset+1U]=0.0f;
}
if(SLocInternal->Mic_Number == 4U)
{
M3_Data[buffer_offset]=0.0f;
M4_Data[buffer_offset]=0.0f;
M3_Data[buffer_offset+1U]=0.0f;
M4_Data[buffer_offset+1U]=0.0f;
}
if(SLocInternal->Mic_Number >= 2U)
{
/*POWER SPECTRUM*/
for(j=0; j<(SLocInternal->Sample_Number_To_Process/8U); j++)
{
Power_Spectrum[(j*2U)] = (M2_Data[buffer_offset+(j*2U)] * M1_Data[buffer_offset+(j*2U)]) + (M2_Data[buffer_offset+(j*2U)+1U] * M1_Data[buffer_offset+(j*2U)+1U]);
Power_Spectrum[(j*2U)+1U] = (-((M2_Data[buffer_offset+(j*2U)])*(M1_Data[buffer_offset+(j*2U)+1U]))) + (M2_Data[buffer_offset+(j*2U)+1U] * M1_Data[buffer_offset+(j*2U)]);
float32_t arg_sqrtf = (Power_Spectrum[(j*2U)]*Power_Spectrum[(j*2U)]) + (Power_Spectrum[(j*2U)+1U]*Power_Spectrum[(j*2U)+1U]);
if (arg_sqrtf >= 0.0f)
{
tempMag=sqrtf(arg_sqrtf);
if(tempMag<(1e-7f))
{
tempMag=(1e-7f);
}
}
Power_Spectrum[(j*2U)]=(Power_Spectrum[(j*2U)])/tempMag;
Power_Spectrum[(j*2U)+1U]=(Power_Spectrum[(j*2U)+1U])/tempMag;
}
int32_t angle;
float32_t anglesNum=(180.0f/(float32_t)SLocInternal->resolution);
for(angle=0;angle<(int32_t)anglesNum;angle++)
{
float32_t idftSum=0.0f;
int32_t bin;
float32_t theta;
float32_t cosineTerm;
float32_t sineTerm;
float32_t theta_term= -2.0f*PI*(float32_t)SLocInternal->sampling_frequency*SLocInternal->M12_distance*(float32_t)arm_cos_f32(PI - ((PI*(float32_t)angle)/(anglesNum-1.0f)))/((float32_t)SLocInternal->Sample_Number_To_Process*SOUND_SPEED);
for(bin=0; bin<((int32_t)SLocInternal->Sample_Number_To_Process/8); bin++)
{
theta = theta_term*(float32_t)bin;
cosineTerm=arm_cos_f32(theta);
sineTerm=arm_sin_f32(theta);
idftSum+=(Power_Spectrum[2*bin]*cosineTerm)-(Power_Spectrum[(2*bin)+1]*sineTerm);
}
SLocInternal->Phase[angle]=idftSum;
}
SLocInternal->Estimated_Angle_12=get_max_pos(SLocInternal,(int32_t)anglesNum,(((int32_t)anglesNum/40)+1));
SLocInternal->Estimated_Angle_12= 180-(int32_t)floor((180.0/((float64_t)anglesNum*2.0))+((float64_t)SLocInternal->Estimated_Angle_12*(float64_t)(SLocInternal->resolution)));
}
if(SLocInternal->Mic_Number == 4U)
{
/*POWER SPECTRUM*/
for(j=0; j<(SLocInternal->Sample_Number_To_Process/8U); j++)
{
Power_Spectrum[(j*2U)] = (M4_Data[buffer_offset+(j*2U)] * M3_Data[buffer_offset+(j*2U)]) + (M4_Data[buffer_offset+(j*2U)+1U] * M3_Data[buffer_offset+(j*2U)+1U]);
Power_Spectrum[(j*2U)+1U] = (-((M4_Data[buffer_offset+(j*2U)])*(M3_Data[buffer_offset+(j*2U)+1U]))) + (M4_Data[buffer_offset+(j*2U)+1U] * M3_Data[buffer_offset+(j*2U)]);
float32_t arg_sqrtf = (Power_Spectrum[(j*2U)]*Power_Spectrum[(j*2U)]) + (Power_Spectrum[(j*2U)+1U]*Power_Spectrum[(j*2U)+1U]);
if (arg_sqrtf >= 0.0f)
{
tempMag=sqrtf(arg_sqrtf);
if(tempMag<(1e-7f))
{
tempMag=(1e-7f);
}
}
Power_Spectrum[(j*2U)]=(Power_Spectrum[(j*2U)])/tempMag;
Power_Spectrum[(j*2U)+1U]=(Power_Spectrum[(j*2U)+1U])/tempMag;
}
int32_t angle;
float32_t anglesNum=(180.0f/(float32_t)SLocInternal->resolution);
for(angle=0;angle<(int32_t)anglesNum;angle++)
{
float32_t idftSum=0.0f;
int32_t bin;
float32_t theta;
float32_t cosineTerm;
float32_t sineTerm;
float32_t theta_term= -2.0f*PI*(float32_t)SLocInternal->sampling_frequency*SLocInternal->M34_distance*(float32_t)arm_cos_f32(PI - ((PI*(float32_t)angle)/(anglesNum-1.0f)))/((float32_t)SLocInternal->Sample_Number_To_Process*SOUND_SPEED);
for(bin=0; bin<((int32_t)SLocInternal->Sample_Number_To_Process/8); bin++)
{
theta = theta_term*(float32_t)bin;
cosineTerm=arm_cos_f32(theta);
sineTerm=arm_sin_f32(theta);
idftSum+=(Power_Spectrum[2*bin]*cosineTerm)-(Power_Spectrum[(2*bin)+1]*sineTerm);
}
SLocInternal->Phase[angle]=idftSum;
}
SLocInternal->Estimated_Angle_34=get_max_pos(SLocInternal,(int32_t)anglesNum,(((int32_t)anglesNum/40)+1));
SLocInternal->Estimated_Angle_34= 180-(int32_t)floor((180.0/((float64_t)anglesNum*2.0))+((float64_t)SLocInternal->Estimated_Angle_34*(float64_t)(SLocInternal->resolution)));
}
float32_t angle_out_f = 0.0f;
if(SLocInternal->Mic_Number == 2U)
{
angle_out_f=(float32_t)SLocInternal->Estimated_Angle_12;
}
else if (SLocInternal->Mic_Number == 4U)
{
if((SLocInternal->Estimated_Angle_12 + SLocInternal->Estimated_Angle_34) < (90 - ((int32_t)SLocInternal->resolution * 2)))
{
out_angles[0]=-1;
out_angles[1]=-1;
return -1.0f;
}
else
{
float32_t angle1_f = arm_cos_f32((((float32_t)SLocInternal->Estimated_Angle_12)/180.0f)*3.14f);
float32_t angle2_f = arm_cos_f32((((float32_t)SLocInternal->Estimated_Angle_34)/180.0f)*3.14f);
float32_t cos_fi = 0.0f;
float32_t arg_cos_fi = 1.0f - (angle1_f*angle1_f) - (angle2_f*angle2_f);
if (arg_cos_fi >= 0.0f)
{
cos_fi = sqrtf(arg_cos_fi);
}
angle_out_f = atan2f((angle1_f),(angle2_f));
angle_out_f= angle_out_f * (360.0f / (3.14f*2.0f));
if(angle_out_f < 0.0f)
{
angle_out_f+=360.0f; /* in [0 360] -> TODO: Optimize the whole angle computation from the maximum phase index*/
}
if ((cos_fi <= 1.0f) && (cos_fi >= -1.0f))
{
fi = acosf(cos_fi)*180.0f/3.14f;
}
}
}
SLocInternal->Buffer_State=0;
out_angles[0]=(int32_t)angle_out_f;
if(isnan(fi)!= 0U)
{
out_angles[1]=(int32_t)fi;
}
else
{
out_angles[1]=-1;
}
return angle_out_f;
}（更新中）
