dengcy
/
micro_climate
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
micro_climate/tools/xcorr.c

330 lines
9.0 KiB
C
Raw Normal View History

新建
2024-07-05 03:52:43 +00:00
#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);
}