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

littleshrimp 2017-10-19 12:28 楼主

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

具体过程可以参考下边贴子为SensorTile添加一颗外部麦克风实现双通道输入 https://bbs.eeworld.com.cn/thread-569074-1-1.html 利用SensorTile和EXCEL对双声道麦克风的数据进行简单分析 https://bbs.eeworld.com.cn/thread-569171-1-1.html 双麦克风声源定位分析 https://bbs.eeworld.com.cn/thread-570113-1-1.html 主要代码

int main( void )
{
uint16_t i;
float a,b,c;
/* STM32F4xx HAL library initialization:
- Configure the Flash prefetch, instruction and Data caches
- Configure the Systick to generate an interrupt each 1 msec
- Set NVIC Group Priority to 4
- Global MSP (MCU Support Package) initialization
*/
HAL_Init();
/* Configure the system clock */
SystemClock_Config();
/* Configure Audio Output peripheral (SAI) and external DAC */
BSP_AUDIO_OUT_Init(PCM1774_0, &PCM1774_X_0_handle, 0, 20, AUDIO_SAMPLING_FREQUENCY);
BSP_AUDIO_OUT_SetVolume(PCM1774_X_0_handle, 10);
/* Configure Audio Input peripheral - DFSDM */
BSP_AUDIO_IN_Init(AUDIO_SAMPLING_FREQUENCY, 16, AUDIO_CHANNELS);
/* enable USB power on Pwrctrl CR2 register */
HAL_PWREx_EnableVddUSB();
/*** USB CDC Configuration ***/
/* Init Device Library */
USBD_Init(&USBD_Device, &VCP_Desc, 0);
/* Add Supported Class */
USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
/* Add Interface callbacks for AUDIO and CDC Class */
USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
/* Start Device Process */
USBD_Start(&USBD_Device);
/* Start Microphone acquisition */
BSP_AUDIO_IN_Record(PCM_Buffer,0);
while (1)
{
//数据达到FFT_SIZE时做FFT
if(fft_flag)
{
//填充左声道数据到fft_data
for(i=0;i<FFT_SIZE;i++)
{
fft_data[i*2] = audio_out1[i];[/i]
[i] fft_data[i*2 + 1] = 0;[/i]
[i] }[/i]
[i] //FFT转换[/i]
[i] fft();[/i]
[i] //计算最大幅度[/i]
[i] amplitude[0] = get_max_index_amplitude();[/i]
[i] //判断幅度是否大于预设幅度[/i]
[i] if(amplitude[0] > AMPLITUDE_LEVEL)[/i]
[i] {[/i]
[i] //判断最大幅度对应的频率是否超过测试允许的最大频率[/i]
[i] while(get_max_index_freq() > MAX_FREQ)[/i]
[i] {[/i]
[i] //如果频率过高，去掉最大值，读取第二大幅度数据[/i]
[i] fft_mod[max_index] = 0;[/i]
[i] arm_max_f32(fft_mod, FFT_SIZE, &maxValue, &max_index);[/i]
[i] }[/i]
[i] } [/i]
[i] //获得当前频率[/i]
[i] freq[0] = get_max_index_freq();[/i]
[i] //获得当前频率的相位[/i]
[i] phase[0] = get_max_index_phase();[/i]
[i] //填充右声道数据到fft_data[/i]
[i] for(i=0;i<FFT_SIZE;i++)[/i]
[i] {[/i]
[i] fft_data[i*2] = audio_out2;[/i]
[i] fft_data[i*2 + 1] = 0;[/i]
[i] }[/i]
[i] //FFT转换[/i]
[i] fft();[/i]
[i] //计算最大幅度[/i]
[i] amplitude[1] = get_max_index_amplitude();[/i]
[i] //判断幅度是否大于预设幅度[/i]
[i] if(amplitude[1] > AMPLITUDE_LEVEL)[/i]
[i] {[/i]
[i] //判断最大幅度对应的频率是否超过测试允许的最大频率[/i]
[i] while(get_max_index_freq() > MAX_FREQ)[/i]
[i] {[/i]
[i] //如果频率过高，去掉最大值，读取第二大幅度数据[/i]
[i] fft_mod[max_index] = 0;[/i]
[i] arm_max_f32(fft_mod, FFT_SIZE, &maxValue, &max_index);[/i]
[i] }[/i]
[i] } [/i]
[i] //获得当前频率[/i]
[i] freq[1] = get_max_index_freq();[/i]
[i] //获得当前频率的相位[/i]
[i] phase[1] = get_max_index_phase();[/i]
[i] //如果左右声道符合频率要求的最大幅度对应的频率相等，且幅度大于预设值[/i]
[i] if( (amplitude[0] > AMPLITUDE_LEVEL || amplitude[1] > AMPLITUDE_LEVEL) && freq[0] == freq[1])[/i]
[i] {[/i]
[i] //计算相位差[/i]
[i] phase_diff = phase[0] - phase[1];[/i]
[i] //相位差大于180度时取反[/i]
[i] if(phase_diff < -180) phase_diff = 360 + phase_diff;[/i]
[i] else if(phase_diff > 180) phase_diff = phase_diff - 360;[/i]
[i] //计算声源到2个麦克风的距离差[/i]
[i] length_diff = 346.00/freq[0]/360.00*phase_diff;[/i]
[i] //如果因为误差导致距离差不符合2边之合大于第3边时，限定length_diff的最大长度[/i]
[i] if(length_diff > 0.159999999)length_diff = 0.15999999;[/i]
[i] else if(length_diff < -0.15999999)length_diff = -0.15999999;[/i]
[i] //读取预设麦克风间距和声源到麦克风的距离[/i]
[i] c = DISTANCE_LEFT_MIC_RIGHT_MIC;[/i]
[i] a = DISTANCE_SOUND_SOURCE_LEFT_MIC;[/i]
[i] b = DISTANCE_SOUND_SOURCE_LEFT_MIC + length_diff;[/i]
[i] //计算弧度[/i]
[i] angle = acos((pow(a,2)+pow(c,2)-pow(b,2))/(2*a*c));[/i]
[i] //计算角度[/i]
[i] angle = angle / PI * 180;[/i]
[i] //通过虚拟串口发送到电脑[/i]
[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]
[i] CDC_Fill_Buffer(( uint8_t * )dataOut, strlen( dataOut ));[/i]
[i] __NOP();[/i]
[i] }[/i]
[i] __NOP();[/i]
[i] fft_flag = 0;[/i]
[i] }[/i]
[i] }[/i]
[i]}[/i]
[i]//快速符里叶变换[/i]
[i]void fft(void)[/i]
[i]{ [/i]
[i] /* Process the data through the CFFT/CIFFT module */[/i]
[i] arm_cfft_f32(&arm_cfft_sR_f32_len4096, fft_data, ifftFlag, doBitReverse);[/i]
[i] arm_cmplx_mag_f32(fft_data, fft_mod, FFT_SIZE);[/i]
[i] arm_max_f32(fft_mod, FFT_SIZE, &maxValue, &max_index); [/i]
[i]}[/i]
[i]//获得最大幅值[/i]
[i]float get_max_index_amplitude(void)[/i]
[i]{[/i]
[i] if(max_index > 0)[/i]
[i] {[/i]
[i] return fft_mod[max_index] / (FFT_SIZE/2);[/i]
[i] }[/i]
[i] return 0;[/i]
[i]}[/i]
[i]//获得相位[/i]
[i]float get_max_index_phase(void)[/i]
[i]{ [/i]
[i] float phase;[/i]
[i] phase = atan2(fft_data[max_index * 2 + 1],fft_data[max_index * 2]) / PI * 180;[/i]
[i] if(phase < 0)phase += 360;[/i]
[i] return phase;[/i]
[i]}[/i]
[i]//获得频率[/i]
[i]float get_max_index_freq(void)[/i]
[i]{[/i]
[i] return (max_index - 1.00f) * (float)AUDIO_SAMPLING_FREQUENCY / (float)FFT_SIZE;[/i]
[i]}[/i]
[i]/**[/i]
[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]
[i]* In this application only PDM to PCM conversion and USB streaming[/i]
[i]* is performed.[/i]
[i]* User can add his own code here to perform some DSP or audio analysis.[/i]
[i]* @param none[/i]
[i]* @retval None[/i]
[i]*/[/i]
[i]void AudioProcess(void)[/i]
[i]{[/i]
[i] /*for L4 PDM to PCM conversion is performed in hardware by DFSDM peripheral*/[/i]
[i] static uint32_t IndexOut = 0;[/i]
[i] static uint32_t AudioOutActive = 0;[/i]
[i] uint32_t indexIn;[/i]
[i]// Send_Audio_to_USB((int16_t *)PCM_Buffer, AUDIO_SAMPLING_FREQUENCY/1000*AUDIO_CHANNELS); [/i]
[i] for(indexIn=0;indexIn<AUDIO_IN_BUF_LEN;indexIn+=2)[/i]
[i] {[/i]
[i] audio_out_buffer[IndexOut++] = PCM_Buffer[indexIn];[/i]
[i] audio_out_buffer[IndexOut++] = PCM_Buffer[indexIn+1];[/i]
[i] if(fft_flag == 0)[/i]
[i] {[/i]
[i] audio_out1[audio_out_index] = PCM_Buffer[indexIn];[/i]
[i] audio_out2[audio_out_index] = -PCM_Buffer[indexIn+1];[/i]
[i] audio_out_index++;[/i]
[i] if( audio_out_index >= FFT_SIZE)[/i]
[i] {[/i]
[i] audio_out_index = 0;[/i]
[i] fft_flag = 1;[/i]
[i] }[/i]
[i] }[/i]
[i] }[/i]
[i] if(!AudioOutActive && IndexOut==AUDIO_OUT_BUF_LEN/2)[/i]
[i] {[/i]
[i] BSP_AUDIO_OUT_Play(PCM1774_X_0_handle,(uint16_t*)audio_out_buffer, AUDIO_OUT_BUF_LEN);[/i]
[i] AudioOutActive=1;[/i]
[i] }[/i]
[i] if(IndexOut==AUDIO_OUT_BUF_LEN)[/i]
[i] {[/i]
[i] IndexOut=0;[/i]
[i] }[/i]
[i]}[/i]
[i]/**[/i]
[i]* @brief Transfer Complete user callback, called by BSP functions.[/i]
[i]* @param None[/i]
[i]* @retval None[/i]
[i]*/[/i]
[i]void BSP_AUDIO_IN_TransferComplete_CallBack(void)[/i]
[i]{[/i]
[i] AudioProcess();[/i]
[i]}[/i]
[i]/**[/i]
[i]* @brief Half Transfer Complete user callback, called by BSP functions.[/i]
[i]* @param None[/i]
[i]* @retval None[/i]
[i]*/[/i]
[i]void BSP_AUDIO_IN_HalfTransfer_CallBack(void)[/i]
[i]{[/i]
[i] AudioProcess();[/i]
[i]}

视频 https://training.eeworld.com.cn/course/4063/learn?preview=1#lesson/11047 例程基于STSW-STLKT01_V1.3.1修改 游客，如果您要查看本帖隐藏内容请回复
本帖最后由 littleshrimp 于 2017-10-19 12:28 编辑

虾扯蛋，蛋扯虾，虾扯蛋扯虾