330 lines
9.0 KiB
C
330 lines
9.0 KiB
C
|
||
#include "xcorr.h"
|
||
#include "usart.h"
|
||
|
||
//float32_t x_rfft_out[1024];
|
||
//float32_t y_rfft_out[1024];
|
||
//
|
||
//float32_t x_temp[1024];
|
||
//
|
||
//float32_t cmplx_result[1024];
|
||
|
||
|
||
arm_rfft_fast_instance_f32 s;
|
||
|
||
|
||
//q15_t x_rfft_out[1024];
|
||
//q15_t y_rfft_out[1024];
|
||
//
|
||
//q15_t x_temp[1024];
|
||
//
|
||
//q15_t cmplx_result[1024];
|
||
|
||
float32_t corrx_out[DATA_LEN];
|
||
|
||
float32_t x_buf[DATA_LEN];
|
||
float32_t y_buf[DATA_LEN];
|
||
|
||
float32_t x_fft_O_f32[DATA_LEN] = {0};
|
||
float32_t y_fft_O_f32[DATA_LEN]= {0};
|
||
float32_t cmplx_mul_result_f32[DATA_LEN]= {0};
|
||
float32_t max_val_f32;
|
||
int32_t max_val_index_f32;
|
||
|
||
|
||
|
||
//void fft_acc_corr_f32_test()
|
||
//{
|
||
// arm_copy_f32((float32_t*)x,x_buf,DATA_LEN);
|
||
// arm_copy_f32((float32_t*)y,y_buf,DATA_LEN);
|
||
//
|
||
// __HAL_TIM_SET_COUNTER(&htim16,0);
|
||
// // 启定时器
|
||
// __HAL_TIM_ENABLE(&htim16);
|
||
//
|
||
//
|
||
// arm_offset_f32(x_buf,-2048,x_buf,DATA_LEN);
|
||
// arm_offset_f32(y_buf,-2048,y_buf,DATA_LEN);
|
||
//
|
||
// arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
//
|
||
// arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
|
||
// //arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
// arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
|
||
//
|
||
// arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,DATA_LEN);
|
||
//
|
||
// // __HAL_TIM_DISABLE(&htim7);
|
||
//
|
||
// //arm_cmplx_mult_cmplx_q15_user(x_fft_O,y_fft_O,cmplx_mul_result,DATA_LEN*2);
|
||
// arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,DATA_LEN>>1);
|
||
// //arm_rfft_init_q15(&s,DATA_LEN,1,1);
|
||
//
|
||
// // s.pTwiddleAReal
|
||
// arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
// arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
|
||
//
|
||
// arm_max_f32(corrx_out,DATA_LEN,&max_val_f32,&max_val_index_f32);
|
||
//
|
||
// // 关闭定时
|
||
// __HAL_TIM_DISABLE(&htim16);
|
||
//
|
||
//
|
||
//}
|
||
|
||
|
||
|
||
|
||
//void fft_acc_corr_q15_test()
|
||
//{
|
||
// arm_copy_f32((float32_t*)x,x_buf,DATA_LEN);
|
||
// arm_copy_f32((float32_t*)y,y_buf,DATA_LEN);
|
||
//
|
||
// __HAL_TIM_SET_COUNTER(&htim16,0);
|
||
// // 启定时器
|
||
// __HAL_TIM_ENABLE(&htim16);
|
||
//
|
||
//
|
||
// arm_offset_f32(x_buf,-2048,x_buf,DATA_LEN);
|
||
// arm_offset_f32(y_buf,-2048,y_buf,DATA_LEN);
|
||
//
|
||
// arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
//
|
||
// arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
|
||
// //arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
// arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
|
||
//
|
||
// arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,DATA_LEN);
|
||
//
|
||
// // __HAL_TIM_DISABLE(&htim7);
|
||
//
|
||
// //arm_cmplx_mult_cmplx_q15_user(x_fft_O,y_fft_O,cmplx_mul_result,DATA_LEN*2);
|
||
// arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,DATA_LEN>>1);
|
||
// //arm_rfft_init_q15(&s,DATA_LEN,1,1);
|
||
//
|
||
// // s.pTwiddleAReal
|
||
// arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
// arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
|
||
//
|
||
// arm_max_f32(corrx_out,DATA_LEN,&max_val_f32,&max_val_index_f32);
|
||
//
|
||
// // 关闭定时
|
||
// __HAL_TIM_DISABLE(&htim16);
|
||
//}
|
||
|
||
q15_t dc_offset;
|
||
float cal_tof(q15_t* x,uint32_t len)
|
||
{
|
||
q15_t max_val;
|
||
float32_t echo_p = 0,echo_dt = 0;
|
||
uint32_t max_val_p;
|
||
uint32_t i=0,stop_position = 0;
|
||
// 求取平均值
|
||
arm_mean_q15(x,100,&dc_offset);
|
||
|
||
// 查找数组中的最大值和最大值所在的索引
|
||
arm_max_q15(x,len,&max_val,&max_val_p);
|
||
|
||
max_val = max_val - dc_offset ;
|
||
//uint16_t echo_position = 0;
|
||
// 最大值的0.18倍
|
||
uint16_t max_val_zero_1R5 = (max_val*15/100)+dc_offset;
|
||
// 最大值的0.45倍
|
||
uint16_t max_val_zero_4R5 = (max_val*45/100)+dc_offset;
|
||
// 最大值的0.8倍
|
||
uint16_t max_val_zero_8R0 = (max_val*80/100)+dc_offset;
|
||
|
||
|
||
//如果最大值位置大于200 则从最大值前200个位置开始寻找起始波形。
|
||
// 优化的地方，从最大值位置开始找到达波,可以最大限度排除偶然噪声干扰，
|
||
// 因为噪声在波形到达出 噪声不是很大
|
||
//优化性能，如果不需要则从数组0位置开始寻找其实波形
|
||
if(max_val_p>=70)
|
||
{
|
||
i = max_val_p-70;
|
||
}else
|
||
{
|
||
i = 0;
|
||
}
|
||
// 在最大值前寻找起始波形
|
||
for( ; i < max_val_p ; i++)
|
||
{
|
||
// 建议判断顶点，但是容易遇到偶然数据异常 类似于 28 29 28 30 29 28这种情况
|
||
//if( x[i-1] < x[i] && x[i]> x[i+1] )
|
||
|
||
// 排除以上数据异常情况，但是有可能就无法检测到30 这个顶点。
|
||
// 故需要检测下一个周期的顶点，然后再减去一个周期的时间。
|
||
if( x[i-2]<x[i-1] && x[i-1] <= x[i] && x[i]>=x[i+1] && x[i+1]>x[i+2])
|
||
{
|
||
// 减去偏置电压
|
||
//temp_val_zero = arr[i]-2048;
|
||
// 判断顶点是否在 15%-%45之间。
|
||
if(x[i] >= max_val_zero_1R5 && x[i] <= max_val_zero_4R5 )
|
||
{
|
||
// 如果找到 函数退出
|
||
echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
|
||
echo_p = (float)i+echo_dt;
|
||
return echo_p;
|
||
}
|
||
// 如果15% ~45%之间的数据未找到，则找45-80%的顶点。
|
||
// 判断顶点是否在 45% -- 80% 之间
|
||
if(x[i] >= max_val_zero_4R5 && x[i] <= max_val_zero_8R0)
|
||
{
|
||
// 如果找到 函数推出
|
||
echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
|
||
// 换算成第二个顶点的位置。
|
||
echo_p = (float)i+echo_dt - 12.5;
|
||
return echo_p;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
// 2.5M/0.2k= 12.5 查找最大值前5个周期（63个点）并计算到达时间
|
||
if(max_val_p>=63)
|
||
{
|
||
stop_position = max_val_p-63;
|
||
}
|
||
else
|
||
{
|
||
stop_position = 0;
|
||
}
|
||
|
||
for(i = max_val_p-5 ; i >= stop_position ; i--)
|
||
{
|
||
if( x[i-2]<x[i-1] && x[i-1] <= x[i] && x[i]>=x[i+1] && x[i+1]>x[i+2])
|
||
{
|
||
// 减去偏置电压
|
||
//temp_val_zero = arr[i]-2048;
|
||
// 如果15% ~45%之间的数据未找到，则找45-80%的顶点。
|
||
// 判断顶点是否在 45% -- 80% 之间
|
||
if(x[i] >= max_val_zero_4R5 && x[i] <= max_val_zero_8R0)
|
||
{
|
||
// 如果找到 函数推出
|
||
echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
|
||
// 换算成第二个顶点的位置。
|
||
echo_p = (float)i+echo_dt - 12.5;
|
||
return echo_p;
|
||
}
|
||
// 判断顶点是否在 15%-%45之间。
|
||
if(x[i] >= max_val_zero_1R5 && x[i] <= max_val_zero_4R5 )
|
||
{
|
||
// 如果找到 函数退出
|
||
echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
|
||
echo_p = (float)i+echo_dt;
|
||
return echo_p;
|
||
}
|
||
}
|
||
} */
|
||
return 0;
|
||
}
|
||
|
||
|
||
float32_t cal_dtof(q15_t* x , q15_t* y , uint32_t len)
|
||
{
|
||
float a,b,c;
|
||
float w_val,x_val,y_val;
|
||
float tof;
|
||
arm_rfft_fast_init_f32(&s,len);
|
||
// 定点数转成float
|
||
arm_q15_to_float(x,x_buf,DATA_LEN);
|
||
// 定点数转成float
|
||
arm_q15_to_float(y,y_buf,DATA_LEN);
|
||
// 初始化 fft
|
||
arm_rfft_fast_init_f32(&s,DATA_LEN);
|
||
// fft 前一半和后一半是对称的,所以只求解了512组数据
|
||
arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
|
||
// fft
|
||
arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
|
||
|
||
// 复数取共轭
|
||
arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,DATA_LEN>>1);
|
||
// fft的数据
|
||
// 时域卷积==频域相乘 1024个数据 复数只有512个
|
||
arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,DATA_LEN>>1);
|
||
// ifft
|
||
arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
|
||
//
|
||
arm_max_f32(corrx_out,DATA_LEN,&max_val_f32,&max_val_index_f32);
|
||
|
||
|
||
b =corrx_out[max_val_index_f32];
|
||
if(max_val_index_f32 == 0)
|
||
{
|
||
a =corrx_out[len-1];
|
||
c =corrx_out[max_val_index_f32+1];
|
||
}
|
||
else
|
||
if(max_val_index_f32 == len-1)
|
||
{
|
||
a =corrx_out[max_val_index_f32-1];
|
||
c =corrx_out[0];
|
||
}else
|
||
{
|
||
a =corrx_out[max_val_index_f32-1];
|
||
c =corrx_out[max_val_index_f32+1];
|
||
}
|
||
|
||
// sin 插值 寻找最大值
|
||
w_val = acosf((a+c)/2.0/b);
|
||
|
||
float sin_val = sinf(w_val);
|
||
|
||
y_val = atanf((a-c)/2.0/b/sin_val);
|
||
|
||
x_val = (0-y_val)/w_val;
|
||
|
||
tof = max_val_index_f32+x_val;
|
||
|
||
if(tof>len/2)
|
||
tof= tof-len;
|
||
|
||
return tof;
|
||
}
|
||
|
||
|
||
|
||
|
||
float xcorr_max_position_uint16_t(q15_t* xcorr_out,q15_t* x , q15_t* y , uint32_t len)
|
||
{
|
||
uint32_t i = 0; // y参数移动的次数
|
||
q15_t max_val=0;
|
||
int32_t max_p=0;
|
||
q15_t max_val_point[3];
|
||
float w_val,x_val,y_val;
|
||
|
||
|
||
__HAL_TIM_SET_COUNTER(&htim16,0);
|
||
// 启定时器
|
||
__HAL_TIM_ENABLE(&htim16);
|
||
|
||
arm_correlate_fast_q15(x,len,y,len,xcorr_out);
|
||
// 关闭定时
|
||
__HAL_TIM_DISABLE(&htim16);
|
||
|
||
// xcorr(xcorr_out,x,y,len);
|
||
for(i=0;i<len*2-1;i++)
|
||
{
|
||
if(xcorr_out[i]>max_val)
|
||
{
|
||
max_val = xcorr_out[i];
|
||
max_p = i;
|
||
}
|
||
}
|
||
|
||
// 曲线拟合 找到最大点 当作中间数据
|
||
max_val_point[1] =xcorr_out[max_p];
|
||
max_val_point[0] =xcorr_out[max_p-1];
|
||
max_val_point[2] =xcorr_out[max_p+1];
|
||
|
||
// sin 插值 寻找最大值
|
||
w_val = acosf((max_val_point[0]+max_val_point[2]-4096)*1.0/(max_val_point[1]-2048)/(max_val_point[1]-2048));
|
||
y_val = atanf((max_val_point[0]-max_val_point[2])/2.0/(max_val_point[1]-2048)/sinf(w_val));
|
||
x_val = (0-y_val)/w_val;
|
||
|
||
//return ((float)max_p+0.1-0.1);
|
||
return (((float)max_p)-x_val);
|
||
|
||
// return -((float)max_p);
|
||
}
|