devbox
笔触狂放9
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

STM32-MEMS麦克风采集、高级音频处理和音频输出_stm32的音频采集和播放

作者：笔触狂放9 | 2024-07-19 23:42:42

赞

踩

stm32的音频采集和播放

 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功能包 - 意法半导体STMicroelectronicsicon-default.png?t=N7T8https://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.icon-default.png?t=N7T8https://github.com/STMicroelectronics/fp-aud-smartmic1

AcousticSL实时声源定位

本文针对该功能包中的声源定位算法进行探索研究，相关算法文件的可以从Github中下载

文件夹路径fp-aud-smartmic1-main\Middlewares\ST\STM32_AcousticSL_Library

重点文件内容

acoustic_sl.h
  1. /*
  2. 此文件包含声学声源定位库定义。
  3. */
  4. #ifndef __ACOUSTIC_SL_H
  5. #define __ACOUSTIC_SL_H
  6.  
  7. #include "stdint.h"
  8.  
  9. /*算法类型定义*/
  10. #define ACOUSTIC_SL_ALGORITHM_XCORR                	((uint32_t)0x00000001)
  11. #define ACOUSTIC_SL_ALGORITHM_GCCP                 	((uint32_t)0x00000002)
  12. #define ACOUSTIC_SL_ALGORITHM_BMPH                 	((uint32_t)0x00000004)
  13.  
  14. /*错误类型*/
  15. #define ACOUSTIC_SL_ALGORITHM_ERROR                	((uint32_t)0x00000001)
  16. #define ACOUSTIC_SL_PTR_CHANNELS_ERROR             	((uint32_t)0x00000002)
  17. #define ACOUSTIC_SL_CHANNEL_NUMBER_ERROR           	((uint32_t)0x00000004)
  18. #define ACOUSTIC_SL_SAMPLING_FREQ_ERROR            	((uint32_t)0x00000008)
  19. #define ACOUSTIC_SL_RESOLUTION_ERROR               	((uint32_t)0x00000010)
  20. #define ACOUSTIC_SL_THRESHOLD_ERROR                	((uint32_t)0x00000020)
  21. #define ACOUSTIC_SL_DISTANCE_ERROR                 	((uint32_t)0x00000040)
  22. #define ACOUSTIC_SL_NUM_OF_SAMPLES_ERROR           	((uint32_t)0x00000080)
  23. #define ACOUSTIC_SL_PROCESSING_ERROR               	((uint32_t)0x00000100)
  24.  
  25. #ifndef ACOUSTIC_LOCK_ERROR
  26. #define ACOUSTIC_LOCK_ERROR                      	((uint32_t)0x10000000)
  27. #endif 
  28.  
  29. /**/
  30. #define ACOUSTIC_SL_NO_AUDIO_DETECTED              -100  
  31.  
  32. /*输出参数结构体*/
  33. typedef struct
  34. {
  35.   /*算法类型选择，即上述定义的XCORR,GCCP和BMPH*/
  36.   uint32_t algorithm;        
  37.                    
  38.   /*设置采样频率*/
  39.   uint32_t sampling_frequency;                 
  40.   
  41.   /*设置通道数，2通道进行180°定位，4通道进行360°定位*/
  42.   uint32_t channel_number;                      
  43.  
  44.   /*麦克风通道选择*/
  45.   uint8_t ptr_M1_channels;                      
  46.   uint8_t ptr_M2_channels;                      
  47.   uint8_t ptr_M3_channels;                      
  48.   uint8_t ptr_M4_channels;                      
  49.   
  50.   /*麦克风1，2之间的距离（mm），默认150*/
  51.   uint16_t M12_distance; 
  52.   /*麦克风3,4之间的距离（mm），默认150*/                   
  53.   uint16_t M34_distance;
  54.   
  55.   /*内部存储空间大小设置，采用AcousticSL_getMemorySize()函数*/
  56.   uint32_t internal_memory_size;   
  57.             
  58.   /*指向用户分配的内存*/
  59.   uint32_t * pInternalMemory;     
  60.               
  61.   /*指向单次处理的样本数*/
  62.   int16_t samples_to_process; 
  63.   
  64. } AcousticSL_Handler_t;
  65.  
  66. typedef struct
  67. {
  68.   /*指定语音响度阈值，低于阈值算法不起作用，阈值范围从0到1000，默认值为24*/
  69.   uint16_t threshold;    
  70.    
  71.   /*算法的角度分辨率，默认值为4.如果使用XCORR，则忽略*/                  
  72.   uint32_t resolution;                         
  73.  
  74. } AcousticSL_Config_t;
  75.  
  76.  
  77. /*内存分配函数*/
  78. uint32_t AcousticSL_getMemorySize(AcousticSL_Handler_t * pHandler);
  79.  
  80. /*初始化函数*/
  81. uint32_t AcousticSL_Init(AcousticSL_Handler_t * pHandler);
  82.  
  83. /*数据输入函数*/
  84. uint32_t AcousticSL_Data_Input(void *pM1, void *pM2, void *pM3, void *pM4, AcousticSL_Handler_t * pHandler);
  85.  
  86. /*音频处理函数*/
  87. uint32_t AcousticSL_Process(int32_t * Estimated_Angle, AcousticSL_Handler_t * pHandler);
  88.  
  89. /*设置阈值与分辨率的值*/
  90. uint32_t AcousticSL_setConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig);
  91.  
  92. /*获取参数*/
  93. uint32_t AcousticSL_getConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig);
  94.  
  95. /*获取版本信息*/
  96. uint32_t AcousticSL_GetLibVersion(char *version);
  97.  
  98. #endif

该功能可以选择三种不同的声源定位算法，分别是XCORR（时域互相关），GCCP（频域变换广义互相关），BMPH（The block-matching pursuit with hangover）

XCORR算法应用在时域，需要的资源较少，分辨率低，对麦克风放置距离有要求

GCCP算法应用在频域，需要的资源脚多，分辨率高，对麦克风放置距离无要求

BMPH算法应用在频域，需要的资源适中，对麦克风放置距离无要求，需要执行一个遗传的步骤，增强稳定性

双通道麦克风放置示意图

四通道麦克风放置示意图

对于XCORR算法分辨率取决于采样频率与麦克风之间的距离，如下式所示（floor下取整）

以双通道放置为例，麦克风间距10cm，采样频率100kHz，声速343.1m/s，可以计算得到角度分辨率为3°。如需要更高精度，则需要提高麦克风间距与采样频率。

AcousticSL.c
  1. #include "libSoundSourceLoc.c"
  2.  
  3. /*初始化函数*/
  4. uint32_t AcousticSL_Init(AcousticSL_Handler_t * pHandler)
  5. {   
  6.   return libSoundSourceLoc_Init(pHandler);
  7. }
  8.  
  9. /*数据输入函数*/
  10. uint32_t AcousticSL_Data_Input(void *pM1, void *pM2, void *pM3, void *pM4, AcousticSL_Handler_t * pHandler)
  11. {
  12. return libSoundSourceLoc_Data_Input((int16_t *)pM1, (int16_t *)pM2, (int16_t *)pM3, (int16_t *)pM4, pHandler);
  13. }
  14.  
  15. /*音频分析函数*/
  16. uint32_t AcousticSL_Process(int32_t * Estimated_Angle, AcousticSL_Handler_t * pHandler)
  17. {
  18.   return libSoundSourceLoc_Process(Estimated_Angle, pHandler);
  19. }
  20.  
  21. /*参数设置函数*/
  22. uint32_t AcousticSL_setConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig)
  23. {
  24.   return libSoundSourceLoc_setConfig(pHandler, pConfig);
  25. }
  26.  
  27. /*获取参数函数*/
  28. uint32_t AcousticSL_getConfig(AcousticSL_Handler_t * pHandler, AcousticSL_Config_t * pConfig)
  29. {
  30.   return libSoundSourceLoc_getConfig(pHandler, pConfig);
  31. }
  32.  
  33. /*内存配置函数*/
  34. uint32_t AcousticSL_getMemorySize(AcousticSL_Handler_t * pHandler)
  35. {
  36.   return libSoundSourceLoc_getMemorySize(pHandler);
  37. }
  38.  
  39. /*获取版本信息*/
  40. uint32_t AcousticSL_GetLibVersion(char *version)
  41. {
  42.   return libSoundSourceLoc_GetLibVersion(version);
  43. }

可以看到在AcousticSL.c文件中的所有函数都是调用libSoundSourceLoc.c中的函数，之后我们仔细阅读该函数中的库

libSoundSourceLoc.c
  1. #ifndef __LIB_SOUNDSOURCELOC_H
  2. #define __LIB_SOUNDSOURCELOC_H
  3.  
  4. #include "acoustic_sl.h"
  5. #include "arm_math.h"
  6. #include "math.h"
  7.  
  8. struct _libSoundSourceLoc_Handler_Internal;
  9.  
  10. typedef struct {
  11.   float32_t  (*SourceLocFunction)(struct _libSoundSourceLoc_Handler_Internal *SLocInternal, int32_t * out_angles);
  12.   uint32_t  (*CheckEventFunction)(struct _libSoundSourceLoc_Handler_Internal *SLocInternal);
  13. }libSoundSourceLoc_Handler_Callbacks;
  14.  
  15. typedef struct _libSoundSourceLoc_Handler_Internal{
  16.   
  17.   /*采样频率*/
  18.   uint32_t sampling_frequency;
  19.   
  20.   /*算法类型*/
  21.   uint32_t Type;
  22.  
  23.   /**/
  24.   uint32_t Buffer_State;
  25.  
  26.   /*采样数目*/
  27.   uint32_t Sample_Number_To_Process;
  28.  
  29.   /*采样数目（每毫秒）*/
  30.   uint16_t Sample_Number_Each_ms;
  31.  
  32.   /*输入数据数目*/
  33.   uint16_t Input_Counter;
  34.  
  35.   /**/
  36.   uint16_t EVENT_THRESHOLD;
  37.  
  38.   /*麦克风配置*/
  39.   uint16_t Mic_Number;
  40.   uint8_t ptr_M1_channels;
  41.   uint8_t ptr_M2_channels;
  42.   uint8_t ptr_M3_channels;
  43.   uint8_t ptr_M4_channels;
  44.  
  45.   /*麦克风之间的距离和TAUD*/
  46.   float32_t M12_distance;
  47.   float32_t M34_distance;
  48.   int32_t M12_TAUD;
  49.   int32_t M34_TAUD;
  50.  
  51.   libSoundSourceLoc_Handler_Callbacks Callbacks;
  52.  
  53.   /*音频分析数据*/
  54.   int32_t Estimated_Angle_12;
  55.   int32_t Estimated_Angle_34;
  56.   uint32_t resolution;
  57.  
  58.   void * M1_Data;
  59.   void * M2_Data;
  60.   void * M3_Data;
  61.   void * M4_Data;
  62.  
  63.   arm_rfft_fast_instance_f32 * SFast;
  64.  
  65.   float32_t * FFT_Out;
  66.   float32_t * PowerSpectrum;
  67.   float32_t * Phase;
  68.   float32_t * sumArray;
  69.   int32_t * SourceLocFilterArray;
  70.   int32_t * SourceLocFilterArray1;
  71.   float32_t * window;
  72.   
  73.   //dz
  74.   uint32_t        Sample_Number_To_Store;
  75.   uint8_t *       remap_mic; // 4
  76.   uint16_t         num_of_freq;
  77.   uint16_t         num_of_angles;
  78.   uint16_t        freq_range_min;
  79.   uint16_t        freq_range_max;
  80.  
  81.   float32_t *     mics_distance; // AUDIO_CHANNELS
  82.   float32_t *     mics_angle; // AUDIO_CHANNELS
  83.   float32_t *     theta; // NUM_ANGLES
  84.   float32_t       phi;//
  85.   uint16_t        mics_write_offset; //starting point in every mic \in {0,16,32,...} \subseteq [0 MIC_LEN)
  86.   uint16_t        mics_read_offset; //starting point in every mic \in 0,MIC_LEN
  87.   int16_t *       score_theta;// RESOLUTION
  88.   uint8_t         reactivity;
  89.   uint8_t         stability;
  90.   uint8_t         array_type;// CIRCULAR_ARRAY
  91.   uint8_t         local_stabilizer;
  92.   float32_t *     mic_tmp;//[DFT_LEN]
  93.   float32_t *     ptr_freq_energies;// ptr
  94.   float32_t *     s;//[2*AUDIO_CHANNELS*NUMOF_FREQ]
  95.   int16_t *       frequencies_under_analysis; // NUM OF FREQ
  96.   int16_t *       sources; // 2* OUTPUT SOURCES
  97.   float32_t *     steering_tau;
  98.   
  99. } libSoundSourceLoc_Handler_Internal;
  100.  
  101.  
  102. #define FFT_POINTS 512
  103.  
  104. #define LIB_VERSION ((uint32_t)0x00030000)
  105.  
  106. #define FILTER_MIN 10
  107. #define FILTER_STEP_MAIN 10
  108. #define FILTER_MAX_MAIN 50
  109. #define FILTER_STEP_SEC 8
  110. #define FILTER_MAX_SEC 20
  111.  
  112. #define FACTOR_INDEX_2_HZ               (uint16_t)(SLocInternal->sampling_frequency/SLocInternal->Sample_Number_To_Process)
  113.  
  114. #define MIN_RESOLUTION                  4U
  115. #define MAX_NUM_OF_ANGLES               (360U/MIN_RESOLUTION)
  116. #define MAX_NUM_OF_FREQUENCIES          16U
  117. #define MAX_AUDIO_CHANNELS              8U
  118. #define MAX_THRESHOLD                   1000U
  119. #define NUM_OUTPUT_SOURCES              4U
  120. #define CIRCULAR_ARRAY                  1U
  121. #define LINEAR_ARRAY                    2U
  122.  
  123. #ifndef SOUND_SPEED
  124. #define SOUND_SPEED 	(float32_t)343.1f
  125. #endif
  126.  
  127. #ifndef UNUSED
  128. #define UNUSED(X) (void)X      /* To avoid gcc/g++ warnings */
  129. #endif
  130.  
  131. #include "doa_via_block_sparsity.c"
  132.  
  133. #define Abs(x) ( (((float32_t)x) < 0.0f) ? (-((float32_t)x)) : ((float32_t)x))
  134. #define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))
  135. #define SaturaL(N, L) (((N)<(L))?(L):(N))
  136. #define SaturaH(N, H) (((N)>(H))?(H):(N))
  137.  
  138. static uint32_t CheckEvent(libSoundSourceLoc_Handler_Internal * SLocInternal);
  139. static float32_t XCORR_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal,  int32_t * out_angles);
  140. static float32_t GCC_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal, int32_t * out_angles);
  141. static int32_t get_max_pos(libSoundSourceLoc_Handler_Internal * SLocInternal,int32_t length,int32_t step);
  142. 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);
  143.  
  144. /*版本信息*/
  145. static uint32_t libSoundSourceLoc_GetLibVersion(char *version)
  146. {
  147.   char str1[35] = "ST AcousticSL v3.0.0";
  148.   
  149.   (void)strcpy(version, str1);
  150.   return strlen(str1);
  151. }
  152.  
  153. /*数据输入*/
  154. static uint32_t libSoundSourceLoc_Data_Input(void *pM1, void *pM2, void *pM3, void *pM4, AcousticSL_Handler_t * pHandler)
  155. {  
  156.   libSoundSourceLoc_Handler_Internal * SLocInternal = (libSoundSourceLoc_Handler_Internal *)(pHandler->pInternalMemory);
  157.   
  158.   uint8_t ret = 0;
  159.   uint32_t i;
  160. /*如果XCORR和BMPH算法，输入数据格式为int16；如果GCCP算法，输入数据格式为float32*/
  161. /*兼容双通道麦克风与四通道麦克风*/
  162.   if((SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_XCORR) || (SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_BMPH) )
  163.   {
  164.     for (i = 0; i < SLocInternal->Sample_Number_Each_ms; i ++)
  165.     {
  166.       ((int16_t *)(SLocInternal->M1_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM1))[i*SLocInternal->ptr_M1_channels];
  167.       ((int16_t *)(SLocInternal->M2_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM2))[i*SLocInternal->ptr_M2_channels];
  168.       if (SLocInternal->Mic_Number == 4U)
  169.       {
  170.         ((int16_t *)(SLocInternal->M3_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM3))[i*SLocInternal->ptr_M3_channels];
  171.         ((int16_t *)(SLocInternal->M4_Data))[SLocInternal->Input_Counter] = ((int16_t *)(pM4))[i*SLocInternal->ptr_M4_channels];
  172.       }
  173.       SLocInternal->Input_Counter ++;
  174.     }
  175.   }
  176.   else if(SLocInternal->Type ==  ACOUSTIC_SL_ALGORITHM_GCCP )
  177.   {
  178.     for (i = 0; i < SLocInternal->Sample_Number_Each_ms; i ++)
  179.     {
  180.       ((float32_t *)(SLocInternal->M1_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM1))[i*SLocInternal->ptr_M1_channels];//todo interleaving
  181.       ((float32_t *)(SLocInternal->M2_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM2))[i*SLocInternal->ptr_M2_channels];
  182.       if (SLocInternal->Mic_Number == 4U)
  183.       {
  184.         ((float32_t *)(SLocInternal->M3_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM3))[i*SLocInternal->ptr_M3_channels];
  185.         ((float32_t *)(SLocInternal->M4_Data))[SLocInternal->Input_Counter] = (float32_t)((int16_t *)(pM4))[i*SLocInternal->ptr_M4_channels];
  186.       }
  187.       SLocInternal->Input_Counter ++;
  188.     }
  189.   }
  190.   else
  191.   {
  192.     /* no other use cases are handled */
  193.   }
  194.   
  195. /*如果算法是BMPH，需要设置mics_read_offset参数，对于双通道为0，四通道为Sample_Number_To_Store*/
  196.   if(SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_BMPH)
  197.   {
  198.     if(SLocInternal->Input_Counter == SLocInternal->Sample_Number_To_Store)
  199.     {
  200.       ret = 1;
  201.       SLocInternal->Buffer_State = 1;
  202.       SLocInternal->mics_read_offset= 0;
  203.     }
  204.     if(SLocInternal->Input_Counter == (SLocInternal->Sample_Number_To_Store * 2U))
  205.     {
  206.       ret = 1;
  207.       SLocInternal->Buffer_State = 2;
  208.       SLocInternal->Input_Counter = 0;
  209.       SLocInternal->mics_read_offset = (uint16_t)SLocInternal->Sample_Number_To_Store;
  210.     }
  211.   }
  212.   else
  213.   {
  214.     if(SLocInternal->Input_Counter == SLocInternal->Sample_Number_To_Store)
  215.     {
  216.       ret = 1;
  217.       SLocInternal->Buffer_State = 1;
  218.     }
  219.     if(SLocInternal->Input_Counter == (SLocInternal->Sample_Number_To_Store * 2U))
  220.     {
  221.       ret = 1;
  222.       SLocInternal->Buffer_State = 2;
  223.       SLocInternal->Input_Counter = 0;
  224.     }
  225.   }
  226.   
  227.   return ret;
  228. }
  229.  
  230. /*音频处理函数*/
  231. static uint32_t libSoundSourceLoc_Process(int32_t * Estimated_Angle, AcousticSL_Handler_t * pHandler)
  232. {
  233.   libSoundSourceLoc_Handler_Internal * SLocInternal = (libSoundSourceLoc_Handler_Internal *)(pHandler->pInternalMemory);
  234.   
  235. /*计算寄存变量*/
  236.   int32_t Estimated_temp_360[2];
  237.   
  238. /*Buffer_State不为0，即完成数据输入*/
  239.   if(SLocInternal->Buffer_State!=0U)
  240.   {
  241. /*如果BMPH算法，或者调用Check函数返回1，则采用定位函数。否则寄存变量1变为-1*/
  242.     if((SLocInternal->Type==ACOUSTIC_SL_ALGORITHM_BMPH) || (SLocInternal->Callbacks.CheckEventFunction(SLocInternal)==1))
  243.     {
  244.       SLocInternal->Callbacks.SourceLocFunction(SLocInternal,Estimated_temp_360);      
  245.     }
  246.     else
  247.     {
  248.       Estimated_temp_360[0]=-1;
  249.     }
  250.   }
  251.   
  252. /*如果BMPH算法，调用定位函数之后，如果寄存变量1为-500，则会重新赋值-100*/
  253.   if (SLocInternal->Type == ACOUSTIC_SL_ALGORITHM_BMPH)
  254.   {
  255.     if (Estimated_temp_360[0]==-500)
  256.     {
  257.       Estimated_temp_360[0]=-100;
  258.     }
  259.   }
  260. /*如果不是BMPH算法，但是四通道输入*/
  261.   else if(SLocInternal->Mic_Number==4U)
  262.   {
  263. STEP
  264.     /*角度计算前需要进行滤波处理，滤波器参数设计（360，STEPM，MAXM，STEPS，MAXS）*/
  265.     FilterAngle((int32_t *)&Estimated_temp_360[0], SLocInternal->SourceLocFilterArray, 360 , FILTER_STEP_MAIN , FILTER_MAX_MAIN, FILTER_STEP_SEC ,FILTER_MAX_SEC);
  266.    
  267.     /*如果寄存变量1为-1，寄存变量1重新赋值-100*/
  268.     if(Estimated_temp_360[0]==-1)
  269.     {
  270.       Estimated_temp_360[0]=-100;
  271.     }
  272.   }
  273. /*如果不是BMPH算法，且双通道输入*/
  274.   else
  275.   {
  276.     /*角度计算前需要进行滤波处理，滤波器固定参数（180，10，50，8，20）*/
  277.     FilterAngle((int32_t *)&Estimated_temp_360[0], SLocInternal->SourceLocFilterArray, 180 , 10 , 50, 8 ,20);
  278.  
  279.     /*如果寄存变量1为-1，寄存变量1重新赋值-100，否则寄存变量1值减90*/
  280.     if(Estimated_temp_360[0]==-1)
  281.     {
  282.       Estimated_temp_360[0]=-100;
  283.     }
  284.     else
  285.     {
  286.       Estimated_temp_360[0]-=90;
  287.     }
  288.   }
  289.   
  290. /*计算角度参数1为寄存变量1*/
  291.   Estimated_Angle[0] = Estimated_temp_360[0];
  292.   return 0;
  293. }
重点算法
XCORR_GetAngle()
  1. static float32_t XCORR_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal,  int32_t * out_angles)
  2. {
  3. /*tau时延*/
  4.   int32_t tau, k;
  5.   float32_t delta_t24 = 0.0f;
  6.   float32_t delta_t13 = 0.0f;
  7.   float32_t test = 0.0f;
  8.   uint32_t correlation, MAX13, MAX24;
  9.   if(SLocInternal->Mic_Number >= 2U)
  10.   {
  11.     MAX13=0x80000000U;  //-inf
  12. /*遍历所有离散时域信号延迟*/
  13.     for(tau=-SLocInternal->M12_TAUD;tau<=SLocInternal->M12_TAUD;tau++)
  14.     {
  15.       correlation=0;
  16. /*互相关计算*/
  17.       for(k=SLocInternal->M12_TAUD; k<((int32_t)SLocInternal->Sample_Number_To_Process-SLocInternal->M12_TAUD);k++)
  18.       {
  19.         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);
  20.       }
  21. /*寻找互相关计算的最大值与对应的时延值*/
  22.       if (correlation > MAX13)
  23.       {
  24.         MAX13 = correlation;
  25.         delta_t13=(float32_t)tau;
  26.       }
  27.     }
  28.   }
  29. /*针对四通道，格外计算M34之间的互相关*/
  30.   if(SLocInternal->Mic_Number == 4U)
  31.   {
  32.     MAX24=0x80000000U;  //-inf
  33.     for(tau=-SLocInternal->M34_TAUD;tau<=SLocInternal->M34_TAUD;tau++)
  34.     {
  35.       correlation=0;
  36.       for(k=SLocInternal->M34_TAUD; k<((int32_t)SLocInternal->Sample_Number_To_Process-SLocInternal->M34_TAUD); k++)
  37.       {
  38.         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);
  39.       }
  40.       if (correlation > MAX24)
  41.       {
  42.         MAX24 = correlation;
  43.         delta_t24=(float32_t)tau;
  44.       }
  45.     }
  46.   }
  47. /*通过计算得到的互相关最大时的时延和最大时延进行角度计算*/
  48.   SLocInternal->Buffer_State=0;
  49.   if(SLocInternal->Mic_Number == 2U)
  50.   {
  51.     test = (delta_t13+(float32_t)SLocInternal->M12_TAUD )* (180.0f/((float32_t)SLocInternal->M12_TAUD*2.0f));
  52.   }
  53.   else if(SLocInternal->Mic_Number == 4U)
  54.   {
  55.     test=(float32_t)atan2(delta_t13, delta_t24);
  56.     test= test * (360.0f / (3.14f*2.0f) );
  57.     if(test<0.0f)
  58.     {
  59.       test+=360.0f;    /* in [0 360] -> TODO: Optimize the whole angle computation from the maximum phase index*/
  60.     }
  61.   }
  62.   else
  63.   {
  64.     /* no other use cases are handled */
  65.   }
  66.   
  67.   out_angles[0]=(int32_t)test;
  68.   return test;
  69. }

使用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()
  1. static float32_t GCC_GetAngle(libSoundSourceLoc_Handler_Internal * SLocInternal, int32_t * out_angles)
  2. {
  3.   uint32_t j;
  4.   float32_t fi = 0.0f;;
  5.   uint32_t buffer_offset = (SLocInternal->Buffer_State-1U)*SLocInternal->Sample_Number_To_Process;
  6.   float32_t * M1_Data = (float32_t *)SLocInternal->M1_Data;
  7.   float32_t * M2_Data = (float32_t *)SLocInternal->M2_Data;
  8.   float32_t * M3_Data = (float32_t *)SLocInternal->M3_Data;
  9.   float32_t * M4_Data = (float32_t *)SLocInternal->M4_Data;
  10.   float32_t * Power_Spectrum = (float32_t *)SLocInternal->PowerSpectrum;
  11.   float32_t * FFT_Out = (float32_t *)SLocInternal->FFT_Out;
  12.   float32_t * hanning = (float32_t *)SLocInternal->window;
  13.   float32_t tempMag = 1e-7f;
  14.   /*WINDOWING*/
  15.   /*处理用的窗函数*/
  16.   for(j=0;j<SLocInternal->Sample_Number_To_Process;j++)
  17.   {
  18.     if(SLocInternal->Mic_Number >= 2U)
  19.     {
  20.       M1_Data[buffer_offset + j] *= hanning[j];
  21.       M2_Data[buffer_offset + j] *= hanning[j];
  22.     }
  23.     if(SLocInternal->Mic_Number == 4U)
  24.     {
  25.       M3_Data[buffer_offset + j] *= hanning[j];
  26.       M4_Data[buffer_offset + j] *= hanning[j];
  27.     }
  28.   }
  29.   /*FFTs*/
  30.   /*进行傅里叶变换，得到频域数据*/
  31.   if(SLocInternal->Mic_Number >= 2U)
  32.   {
  33.     /*M1 FFT*/
  34.     arm_rfft_fast_f32(SLocInternal->SFast,&M1_Data[buffer_offset],(float32_t *)FFT_Out,0);
  35.     for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
  36.     {
  37.       M1_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
  38.     }
  39.     /*M2 FFT*/
  40.     arm_rfft_fast_f32(SLocInternal->SFast,&M2_Data[buffer_offset],(float32_t *)FFT_Out,0);
  41.     for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
  42.     {
  43.       M2_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
  44.     }
  45.   }
  46.   if(SLocInternal->Mic_Number == 4U)
  47.   {
  48.     /*M3 FFT*/
  49.     arm_rfft_fast_f32(SLocInternal->SFast,&M3_Data[buffer_offset],(float32_t *)FFT_Out,0);
  50.     for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
  51.     {
  52.       M3_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
  53.     }
  54.     /*M4 FFT*/
  55.     arm_rfft_fast_f32(SLocInternal->SFast,&M4_Data[buffer_offset],(float32_t *)FFT_Out,0);
  56.     for (j=0;j<SLocInternal->Sample_Number_To_Process;j++)
  57.     {
  58.       M4_Data[buffer_offset + j]=((float32_t *)FFT_Out)[j];
  59.     }
  60.   }
  61.   
  62.   /*FILTERING*/
  63.   if(SLocInternal->Mic_Number >= 2U)
  64.   {
  65.     M1_Data[buffer_offset]=0.0f;
  66.     M2_Data[buffer_offset]=0.0f;
  67.     
  68.     M1_Data[buffer_offset+1U]=0.0f;
  69.     M2_Data[buffer_offset+1U]=0.0f;
  70.   }
  71.   if(SLocInternal->Mic_Number == 4U)
  72.   {
  73.     M3_Data[buffer_offset]=0.0f;
  74.     M4_Data[buffer_offset]=0.0f;
  75.     
  76.     M3_Data[buffer_offset+1U]=0.0f;
  77.     M4_Data[buffer_offset+1U]=0.0f;
  78.   }
  79.   
  80.   if(SLocInternal->Mic_Number >= 2U)
  81.   {
  82.     /*POWER SPECTRUM*/
  83.     for(j=0; j<(SLocInternal->Sample_Number_To_Process/8U); j++)
  84.     {
  85.       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]);
  86.       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)]);
  87.       
  88.       float32_t arg_sqrtf = (Power_Spectrum[(j*2U)]*Power_Spectrum[(j*2U)]) + (Power_Spectrum[(j*2U)+1U]*Power_Spectrum[(j*2U)+1U]);
  89.       if (arg_sqrtf >= 0.0f)
  90.       {
  91.         tempMag=sqrtf(arg_sqrtf);      
  92.         if(tempMag<(1e-7f))
  93.         {
  94.           tempMag=(1e-7f);
  95.         }
  96.       }
  97.       Power_Spectrum[(j*2U)]=(Power_Spectrum[(j*2U)])/tempMag;
  98.       Power_Spectrum[(j*2U)+1U]=(Power_Spectrum[(j*2U)+1U])/tempMag;
  99.     }
  100.     
  101.     int32_t angle;
  102.     float32_t anglesNum=(180.0f/(float32_t)SLocInternal->resolution);
  103.     
  104.     for(angle=0;angle<(int32_t)anglesNum;angle++)
  105.     {
  106.       float32_t idftSum=0.0f;
  107.       int32_t bin;
  108.       float32_t theta;
  109.       float32_t cosineTerm;
  110.       float32_t sineTerm;
  111.       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);
  112.       for(bin=0; bin<((int32_t)SLocInternal->Sample_Number_To_Process/8); bin++)
  113.       {
  114.         theta = theta_term*(float32_t)bin;
  115.         cosineTerm=arm_cos_f32(theta);
  116.         sineTerm=arm_sin_f32(theta);
  117.         idftSum+=(Power_Spectrum[2*bin]*cosineTerm)-(Power_Spectrum[(2*bin)+1]*sineTerm);
  118.       }
  119.       SLocInternal->Phase[angle]=idftSum;
  120.     }
  121.     
  122.     SLocInternal->Estimated_Angle_12=get_max_pos(SLocInternal,(int32_t)anglesNum,(((int32_t)anglesNum/40)+1));
  123.     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)));
  124.   }
  125.   if(SLocInternal->Mic_Number == 4U)
  126.   {
  127.     /*POWER SPECTRUM*/
  128.     for(j=0; j<(SLocInternal->Sample_Number_To_Process/8U); j++)
  129.     {
  130.       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]);
  131.       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)]);
  132.       
  133.       float32_t arg_sqrtf = (Power_Spectrum[(j*2U)]*Power_Spectrum[(j*2U)]) + (Power_Spectrum[(j*2U)+1U]*Power_Spectrum[(j*2U)+1U]);
  134.       if (arg_sqrtf >= 0.0f)
  135.       {
  136.         tempMag=sqrtf(arg_sqrtf);      
  137.         if(tempMag<(1e-7f))
  138.         {
  139.           tempMag=(1e-7f);
  140.         }
  141.       }
  142.       Power_Spectrum[(j*2U)]=(Power_Spectrum[(j*2U)])/tempMag;
  143.       Power_Spectrum[(j*2U)+1U]=(Power_Spectrum[(j*2U)+1U])/tempMag;
  144.     }
  145.     
  146.     int32_t angle;
  147.     float32_t anglesNum=(180.0f/(float32_t)SLocInternal->resolution);
  148.     
  149.     for(angle=0;angle<(int32_t)anglesNum;angle++)
  150.     {
  151.       float32_t idftSum=0.0f;
  152.       int32_t bin;
  153.       float32_t theta;
  154.       float32_t cosineTerm;
  155.       float32_t sineTerm;
  156.       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);
  157.       for(bin=0; bin<((int32_t)SLocInternal->Sample_Number_To_Process/8); bin++)
  158.       {
  159.         theta = theta_term*(float32_t)bin;
  160.         cosineTerm=arm_cos_f32(theta);
  161.         sineTerm=arm_sin_f32(theta);
  162.         idftSum+=(Power_Spectrum[2*bin]*cosineTerm)-(Power_Spectrum[(2*bin)+1]*sineTerm);
  163.       }
  164.       SLocInternal->Phase[angle]=idftSum;
  165.     }
  166.     
  167.     SLocInternal->Estimated_Angle_34=get_max_pos(SLocInternal,(int32_t)anglesNum,(((int32_t)anglesNum/40)+1));
  168.     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)));
  169.   }
  170.   float32_t angle_out_f = 0.0f;
  171.   
  172.   if(SLocInternal->Mic_Number == 2U)
  173.   {
  174.     angle_out_f=(float32_t)SLocInternal->Estimated_Angle_12;
  175.   }
  176.   else if (SLocInternal->Mic_Number == 4U)
  177.   {
  178.     if((SLocInternal->Estimated_Angle_12 + SLocInternal->Estimated_Angle_34) < (90 - ((int32_t)SLocInternal->resolution * 2)))
  179.     {
  180.       out_angles[0]=-1;
  181.       out_angles[1]=-1;
  182.       return -1.0f;
  183.     }
  184.     else
  185.     {
  186.       float32_t angle1_f = arm_cos_f32((((float32_t)SLocInternal->Estimated_Angle_12)/180.0f)*3.14f);
  187.       float32_t angle2_f = arm_cos_f32((((float32_t)SLocInternal->Estimated_Angle_34)/180.0f)*3.14f);
  188.       float32_t cos_fi = 0.0f;
  189.       float32_t arg_cos_fi = 1.0f - (angle1_f*angle1_f) - (angle2_f*angle2_f);
  190.       
  191.       if (arg_cos_fi >= 0.0f)
  192.       {
  193.         cos_fi = sqrtf(arg_cos_fi);
  194.       }
  195.       angle_out_f = atan2f((angle1_f),(angle2_f));
  196.       angle_out_f= angle_out_f * (360.0f / (3.14f*2.0f));
  197.       if(angle_out_f < 0.0f)
  198.       {
  199.         angle_out_f+=360.0f;    /* in [0 360] -> TODO: Optimize the whole angle computation from the maximum phase index*/
  200.       }
  201.       
  202.       if ((cos_fi <= 1.0f) && (cos_fi >= -1.0f))
  203.       {
  204.         fi = acosf(cos_fi)*180.0f/3.14f;
  205.       }
  206.     }
  207.   }
  208.   
  209.   SLocInternal->Buffer_State=0;
  210.   out_angles[0]=(int32_t)angle_out_f;
  211.   
  212.   if(isnan(fi)!= 0U)
  213.   {
  214.     out_angles[1]=(int32_t)fi;
  215.   }
  216.   else
  217.   {
  218.     out_angles[1]=-1;
  219.   }
  220.   
  221.   return angle_out_f;
  222. }

（更新中）

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/笔触狂放9/article/detail/854111
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号