社区导航

 
查看: 2492|回复: 40

[数字麦克风] 声源定位代码

  [复制链接]

5209

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

发表于 2017-10-19 12:28:39 | 显示全部楼层 |阅读模式
本帖最后由 littleshrimp 于 2017-10-19 12:28 编辑

完成SensorTile声源定位代码
所有计算代码全部有STM32L476完成
计算出角度后通过USB CDC将数据发送到电脑
上位机根据角度控制图片转动

ezgif-4-0a4ae49ab8.gif

具体过程可以参考下边贴子
为SensorTile添加一颗外部麦克风实现双通道输入
http://bbs.eeworld.com.cn/thread-569074-1-1.html
利用SensorTile和EXCEL对双声道麦克风的数据进行简单分析
http://bbs.eeworld.com.cn/thread-569171-1-1.html
双麦克风声源定位分析
http://bbs.eeworld.com.cn/thread-570113-1-1.html
主要代码
  1. int main( void )
  2. {
  3.   uint16_t i;
  4.   float a,b,c;
  5.   /* STM32F4xx HAL library initialization:
  6.   - Configure the Flash prefetch, instruction and Data caches
  7.   - Configure the Systick to generate an interrupt each 1 msec
  8.   - Set NVIC Group Priority to 4
  9.   - Global MSP (MCU Support Package) initialization
  10.   */
  11.   HAL_Init();
  12.   
  13.   /* Configure the system clock */
  14.   SystemClock_Config();
  15.   
  16.   /* Configure Audio Output peripheral (SAI) and external DAC */
  17.   BSP_AUDIO_OUT_Init(PCM1774_0, &PCM1774_X_0_handle, 0, 20, AUDIO_SAMPLING_FREQUENCY);
  18.   BSP_AUDIO_OUT_SetVolume(PCM1774_X_0_handle, 10);
  19.   
  20.   /* Configure Audio Input peripheral - DFSDM */  
  21.   BSP_AUDIO_IN_Init(AUDIO_SAMPLING_FREQUENCY, 16, AUDIO_CHANNELS);  
  22.   
  23.     /* enable USB power on Pwrctrl CR2 register */
  24.   HAL_PWREx_EnableVddUSB();   
  25.   /*** USB CDC Configuration ***/
  26.   /* Init Device Library */
  27.   USBD_Init(&USBD_Device, &VCP_Desc, 0);
  28.   /* Add Supported Class */
  29.   USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
  30.   /* Add Interface callbacks for AUDIO and CDC Class */
  31.   USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
  32.   /* Start Device Process */
  33.   USBD_Start(&USBD_Device);
  34.   
  35.   
  36.   /* Start Microphone acquisition */
  37.   BSP_AUDIO_IN_Record(PCM_Buffer,0);
  38.   
  39.   
  40.   while (1)
  41.   {
  42.     //数据达到FFT_SIZE时做FFT
  43.     if(fft_flag)
  44.     {
  45.       //填充左声道数据到fft_data
  46.       for(i=0;i<FFT_SIZE;i++)
  47.       {
  48.           fft_data[i*2] = audio_out1[i];[/i]
  49. [i]          fft_data[i*2 + 1] = 0;[/i]
  50. [i]      }[/i]
  51. [i]      //FFT转换[/i]
  52. [i]      fft();[/i]
  53. [i]      //计算最大幅度[/i]
  54. [i]      amplitude[0] = get_max_index_amplitude();[/i]
  55. [i]      //判断幅度是否大于预设幅度[/i]
  56. [i]      if(amplitude[0] > AMPLITUDE_LEVEL)[/i]
  57. [i]      {[/i]
  58. [i]          //判断最大幅度对应的频率是否超过测试允许的最大频率[/i]
  59. [i]          while(get_max_index_freq() > MAX_FREQ)[/i]
  60. [i]          {[/i]
  61. [i]              //如果频率过高,去掉最大值,读取第二大幅度数据[/i]
  62. [i]              fft_mod[max_index] = 0;[/i]
  63. [i]              arm_max_f32(fft_mod, FFT_SIZE, &maxValue, &max_index);[/i]
  64. [i]          }[/i]
  65. [i]      }  [/i]
  66. [i]      //获得当前频率[/i]
  67. [i]      freq[0] = get_max_index_freq();[/i]
  68. [i]      //获得当前频率的相位[/i]
  69. [i]      phase[0] = get_max_index_phase();[/i]

  70. [i]      //填充右声道数据到fft_data[/i]
  71. [i]      for(i=0;i<FFT_SIZE;i++)[/i]
  72. [i]      {[/i]
  73. [i]          fft_data[i*2] = audio_out2;[/i]
  74. [i]          fft_data[i*2 + 1] = 0;[/i]
  75. [i]      }[/i]
  76. [i]      //FFT转换[/i]
  77. [i]      fft();[/i]
  78. [i]      //计算最大幅度[/i]
  79. [i]      amplitude[1] = get_max_index_amplitude();[/i]

  80. [i]      //判断幅度是否大于预设幅度[/i]
  81. [i]      if(amplitude[1] > AMPLITUDE_LEVEL)[/i]
  82. [i]      {[/i]
  83. [i]          //判断最大幅度对应的频率是否超过测试允许的最大频率[/i]
  84. [i]          while(get_max_index_freq() > MAX_FREQ)[/i]
  85. [i]          {[/i]
  86. [i]              //如果频率过高,去掉最大值,读取第二大幅度数据[/i]
  87. [i]              fft_mod[max_index] = 0;[/i]
  88. [i]              arm_max_f32(fft_mod, FFT_SIZE, &maxValue, &max_index);[/i]
  89. [i]          }[/i]
  90. [i]      }  [/i]
  91. [i]      //获得当前频率[/i]
  92. [i]      freq[1] = get_max_index_freq();[/i]
  93. [i]      //获得当前频率的相位[/i]
  94. [i]      phase[1] = get_max_index_phase();[/i]
  95. [i]      //如果左右声道符合频率要求的最大幅度对应的频率相等,且幅度大于预设值[/i]
  96. [i]      if( (amplitude[0] > AMPLITUDE_LEVEL ||  amplitude[1] > AMPLITUDE_LEVEL) && freq[0] == freq[1])[/i]
  97. [i]      {[/i]
  98. [i]          //计算相位差[/i]
  99. [i]          phase_diff = phase[0] - phase[1];[/i]
  100. [i]          //相位差大于180度时取反[/i]
  101. [i]          if(phase_diff < -180) phase_diff = 360 + phase_diff;[/i]
  102. [i]          else if(phase_diff > 180) phase_diff =  phase_diff - 360;[/i]
  103. [i]          //计算声源到2个麦克风的距离差[/i]
  104. [i]          length_diff = 346.00/freq[0]/360.00*phase_diff;[/i]
  105. [i]          //如果因为误差导致距离差不符合2边之合大于第3边时,限定length_diff的最大长度[/i]
  106. [i]          if(length_diff > 0.159999999)length_diff = 0.15999999;[/i]
  107. [i]          else if(length_diff < -0.15999999)length_diff = -0.15999999;[/i]
  108. [i]          //读取预设麦克风间距和声源到麦克风的距离[/i]
  109. [i]          c = DISTANCE_LEFT_MIC_RIGHT_MIC;[/i]
  110. [i]          a = DISTANCE_SOUND_SOURCE_LEFT_MIC;[/i]
  111. [i]          b = DISTANCE_SOUND_SOURCE_LEFT_MIC + length_diff;[/i]
  112. [i]          //计算弧度[/i]
  113. [i]          angle = acos((pow(a,2)+pow(c,2)-pow(b,2))/(2*a*c));[/i]
  114. [i]          //计算角度[/i]
  115. [i]          angle = angle / PI * 180;[/i]
  116. [i]          //通过虚拟串口发送到电脑[/i]
  117. [i]          sprintf( dataOut, "freq:%.2f\tphaseL:%.2f\tphaseR:%.2f\tphaseDiff:%.2f\tlength:%.2f\tangle:%.2f\r\n", freq[0], phase[0], phase[1], phase_diff,length_diff, angle);[/i]
  118. [i]          CDC_Fill_Buffer(( uint8_t * )dataOut, strlen( dataOut ));[/i]
  119. [i]          __NOP();[/i]
  120. [i]      }[/i]

  121. [i]      __NOP();[/i]
  122. [i]      fft_flag = 0;[/i]
  123. [i]    }[/i]
  124. [i]  }[/i]
  125. [i]}[/i]
  126. [i]//快速符里叶变换[/i]
  127. [i]void fft(void)[/i]
  128. [i]{        [/i]
  129. [i]  /* Process the data through the CFFT/CIFFT module */[/i]
  130. [i]  arm_cfft_f32(&arm_cfft_sR_f32_len4096, fft_data, ifftFlag, doBitReverse);[/i]

  131. [i]  arm_cmplx_mag_f32(fft_data, fft_mod, FFT_SIZE);[/i]
  132. [i]  arm_max_f32(fft_mod, FFT_SIZE, &maxValue, &max_index);        [/i]
  133. [i]}[/i]
  134. [i]//获得最大幅值[/i]
  135. [i]float get_max_index_amplitude(void)[/i]
  136. [i]{[/i]
  137. [i]  if(max_index > 0)[/i]
  138. [i]  {[/i]
  139. [i]    return fft_mod[max_index] / (FFT_SIZE/2);[/i]
  140. [i]  }[/i]
  141. [i]  return 0;[/i]
  142. [i]}[/i]
  143. [i]//获得相位[/i]
  144. [i]float get_max_index_phase(void)[/i]
  145. [i]{ [/i]
  146. [i]  float phase;[/i]
  147. [i]  phase = atan2(fft_data[max_index * 2 + 1],fft_data[max_index * 2]) / PI * 180;[/i]
  148. [i]  if(phase < 0)phase += 360;[/i]
  149. [i]  return phase;[/i]
  150. [i]}[/i]
  151. [i]//获得频率[/i]
  152. [i]float get_max_index_freq(void)[/i]
  153. [i]{[/i]
  154. [i]  return (max_index - 1.00f) * (float)AUDIO_SAMPLING_FREQUENCY / (float)FFT_SIZE;[/i]
  155. [i]}[/i]
  156. [i]/**[/i]
  157. [i]* [/i][i][url=home.php?mod=space&uid=159083]@brief[/url][/i][i]  User function that is called when 1 ms of PDM data is available.[/i]
  158. [i]*                   In this application only PDM to PCM conversion and USB streaming[/i]
  159. [i]*                  is performed.[/i]
  160. [i]*                   User can add his own code here to perform some DSP or audio analysis.[/i]
  161. [i]* @param  none[/i]
  162. [i]* @retval None[/i]
  163. [i]*/[/i]
  164. [i]void AudioProcess(void)[/i]
  165. [i]{[/i]
  166. [i]  /*for L4 PDM to PCM conversion is performed in hardware by DFSDM peripheral*/[/i]
  167. [i]  static uint32_t IndexOut = 0;[/i]
  168. [i]  static uint32_t AudioOutActive = 0;[/i]
  169. [i]  uint32_t indexIn;[/i]

  170. [i]//  Send_Audio_to_USB((int16_t *)PCM_Buffer, AUDIO_SAMPLING_FREQUENCY/1000*AUDIO_CHANNELS);  [/i]

  171. [i]  for(indexIn=0;indexIn<AUDIO_IN_BUF_LEN;indexIn+=2)[/i]
  172. [i]  {[/i]
  173. [i]    audio_out_buffer[IndexOut++] = PCM_Buffer[indexIn];[/i]
  174. [i]    audio_out_buffer[IndexOut++] = PCM_Buffer[indexIn+1];[/i]

  175. [i]    if(fft_flag == 0)[/i]
  176. [i]    {[/i]
  177. [i]      audio_out1[audio_out_index] = PCM_Buffer[indexIn];[/i]
  178. [i]      audio_out2[audio_out_index] = -PCM_Buffer[indexIn+1];[/i]
  179. [i]      audio_out_index++;[/i]
  180. [i]      if( audio_out_index >= FFT_SIZE)[/i]
  181. [i]      {[/i]
  182. [i]          audio_out_index = 0;[/i]
  183. [i]          fft_flag = 1;[/i]
  184. [i]      }[/i]
  185. [i]    }[/i]
  186. [i]  }[/i]

  187. [i]  if(!AudioOutActive && IndexOut==AUDIO_OUT_BUF_LEN/2)[/i]
  188. [i]  {[/i]
  189. [i]    BSP_AUDIO_OUT_Play(PCM1774_X_0_handle,(uint16_t*)audio_out_buffer, AUDIO_OUT_BUF_LEN);[/i]
  190. [i]    AudioOutActive=1;[/i]
  191. [i]  }[/i]
  192. [i]  if(IndexOut==AUDIO_OUT_BUF_LEN)[/i]
  193. [i]  {[/i]
  194. [i]    IndexOut=0;[/i]
  195. [i]  }[/i]

  196. [i]}[/i]

  197. [i]/**[/i]
  198. [i]* @brief  Transfer Complete user callback, called by BSP functions.[/i]
  199. [i]* @param  None[/i]
  200. [i]* @retval None[/i]
  201. [i]*/[/i]
  202. [i]void BSP_AUDIO_IN_TransferComplete_CallBack(void)[/i]
  203. [i]{[/i]
  204. [i]  AudioProcess();[/i]
  205. [i]}[/i]

  206. [i]/**[/i]
  207. [i]* @brief  Half Transfer Complete user callback, called by BSP functions.[/i]
  208. [i]* @param  None[/i]
  209. [i]* @retval None[/i]
  210. [i]*/[/i]
  211. [i]void BSP_AUDIO_IN_HalfTransfer_CallBack(void)[/i]
  212. [i]{[/i]
  213. [i]  AudioProcess();[/i]
  214. [i]}
复制代码



视频
http://training.eeworld.com.cn/course/4063/learn?preview=1#lesson/11047

例程基于STSW-STLKT01_V1.3.1修改
游客,如果您要查看本帖隐藏内容请回复


虾扯蛋


回复

使用道具 举报

540

TA的帖子

20

TA的资源

一粒金砂(高级)

Rank: 3Rank: 3

发表于 2017-10-19 13:12:10 | 显示全部楼层
收藏,谢谢了。


回复

使用道具 举报

271

TA的帖子

2

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-19 14:05:07 | 显示全部楼层
很厉害啊,学习了新知识


回复

使用道具 举报

3244

TA的帖子

0

TA的资源

纯净的硅(高级)

Rank: 6Rank: 6

发表于 2017-10-19 14:11:57 | 显示全部楼层
牛掰
距离定位准确吗?

点评

精度和采样率和声源频率有关 2颗麦克风只能用来估算声源的方向  详情 回复 发表于 2017-10-19 21:01
So what......


回复

使用道具 举报

336

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-19 14:28:07 | 显示全部楼层
什么东东


回复

使用道具 举报

1053

TA的帖子

656

TA的资源

五彩晶圆(初级)

Rank: 7Rank: 7Rank: 7

发表于 2017-10-19 15:27:38 | 显示全部楼层
谢谢!
天下难事,必做于易;天下大事,必做于细。
与其博览群书,不如精读一本。


回复

使用道具 举报

417

TA的帖子

1

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-19 16:17:03 | 显示全部楼层
学习学习


回复

使用道具 举报

5209

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 2017-10-19 21:01:01 | 显示全部楼层
ljj3166 发表于 2017-10-19 14:11
牛掰
距离定位准确吗?

精度和采样率和声源频率有关
2颗麦克风只能用来估算声源的方向
虾扯蛋


回复

使用道具 举报

149

TA的帖子

2

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-20 11:24:14 | 显示全部楼层
厉害学习学习


回复

使用道具 举报

3552

TA的帖子

6

TA的资源

裸片初长成(初级)

Rank: 10Rank: 10Rank: 10

荣誉会员勋章

发表于 2017-10-20 18:44:40 | 显示全部楼层
最近很多城市的鸣笛抓拍就是这个原理吧。楼主再继续搞一搞呗。


回复

使用道具 举报

24

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2017-10-21 09:00:30 | 显示全部楼层
good job


回复

使用道具 举报

186

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-21 17:21:45 | 显示全部楼层
看着很棒


回复

使用道具 举报

24

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2017-10-21 19:35:50 | 显示全部楼层
it very good topic!!!!!!!!!


回复

使用道具 举报

53

TA的帖子

1

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-25 16:48:33 | 显示全部楼层
厉害了
BlueCoin套件上有4个硅麦,可以玩一下
http://www.st.com/content/st_com ... eval-bcnkt01v1.html

点评

看到过这个套件,不知道哪卖,估计也不会便宜 里边有很多算法不错,相信厂家也不会提供源码  详情 回复 发表于 2017-10-25 17:24


回复

使用道具 举报

5209

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 2017-10-25 17:24:46 | 显示全部楼层
朵拉A萌 发表于 2017-10-25 16:48
厉害了
BlueCoin套件上有4个硅麦,可以玩一下
http://www.st.com/content/st_com/zh/products/e ...

看到过这个套件,不知道哪卖,估计也不会便宜
里边有很多算法不错,相信厂家也不会提供源码
虾扯蛋


回复

使用道具 举报

44

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-10-25 23:15:40 | 显示全部楼层
学习学习


回复

使用道具 举报

1171

TA的帖子

2

TA的资源

版主

Rank: 6Rank: 6

发表于 2017-11-23 13:33:13 | 显示全部楼层


回复

使用道具 举报

358

TA的帖子

2

TA的资源

一粒金砂(高级)

Rank: 3Rank: 3

发表于 2017-11-24 10:11:41 | 显示全部楼层
太棒了,刚刚好需要呢


回复

使用道具 举报

13

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2018-1-5 16:27:23 | 显示全部楼层
刚刚好需要


回复

使用道具 举报

2

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2018-2-12 16:54:27 | 显示全部楼层
真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒真棒


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

  • 论坛活动 E手掌握

    扫码关注
    EEWORLD 官方微信

  • EE福利  唾手可得

    扫码关注
    EE福利 唾手可得

小黑屋|手机版|Archiver|电子工程世界 ( 京ICP证 060456

GMT+8, 2018-9-22 10:52 , Processed in 0.592344 second(s), 19 queries , Gzip On, Redis On.

快速回复 返回顶部 返回列表