/**
?********************************************************************?
?*??@filename	頻域一次/二次積分
?*??@author??	L-KAYA
?*??@date????	27-July-2018
?*??@brief???	該文件主要用于實現加速度在頻域的一次或二次積分
?*???????????	得到速度或位移，有帶通濾波功能，積分后漂移較小
?*??			建議采樣頻率1kHz以上，效果較為理想
?********************************************************************
?*/

#include?“double_integral.h“
#include?“arm_const_structs.h“

//結構體指針
dou_integ_t*?integ?=?NULL;

/**?
?*??@name	頻域一次/二次積分初始化函數
?*??@brief??初始化用于存儲函數的結構體，下方可更改部分根據實際情況更改
?*??@pram	len	用于二次積分的數組長度，建議大于16小于4096，2的冪次
?*??@retval	0	成功
?*??		else失敗
?*/
int?integ_init（uint16_t?len）
{
	integ?=?（dou_integ_t*）MALLOC（sizeof（dou_integ_t））;
	if（integ?==?NULL）
		return?-1;//內存申請失敗
	
	//?設置頻域二次積分的參數（可更改）
	integ->smple_frq??=?SMPLE_FRQ;								//采樣頻率
	integ->gravity????=?GRAVITY;								//單位變化系數（最后積分結果乘上該系數，根據重力加速度得到）
	integ->integ_time?=?INTEG_TIME;								//積分次數
	integ->frq_min????=?FRQ_MIN;								//高通濾波截止頻率
	integ->frq_max????=?FRQ_MAX;								//低通濾波截止頻率（應小于采樣頻率）
	if（integ->frq_max>=integ->smple_frq）
		return?-2;
	
	//?根據輸入數組長度，計算FFT數組長度
	uint8_t?i;
	for（i=4;?i<13;?i++）{										//數組長度范圍為2^4=16?~?2^12=4096
		if（POW（2i）>=len）{
			integ->fft_len?=?POW（2i）;							//大于并接近n的2的冪次方的FFT長度
			break;
		}
	}
	if（i?==?13）
		return?-3;//數據長度太大，范圍為16~4096
	
	//為fft_buf臨時數組分配內存
	integ->fft_buf?=?（float*）MALLOC（integ->fft_len*2*（sizeof（float）））;
	if（integ->fft_buf?==?NULL）
		return?-4;//內存申請失敗
	
	switch（integ->fft_len）
	{
		case?16:{
			integ->S?=?arm_cfft_sR_f32_len16;
			break;
		}
		case?32:{
			integ->S?=?arm_cfft_sR_f32_len32;
			break;
		}
		case?64:{
			integ->S?=?arm_cfft_sR_f32_len64;
			break;
		}
		case?128:{
			integ->S?=?arm_cfft_sR_f32_len128;
			break;
		}
		case?256:{
			integ->S?=?arm_cfft_sR_f32_len256;
			break;
		}
		case?512:{
			integ->S?=?arm_cfft_sR_f32_len512;
			break;
		}
		case?1024:{
			integ->S?=?arm_cfft_sR_f32_len1024;
			break;
		}
		case?2048:{
			integ->S?=?arm_cfft_sR_f32_len2048;
			break;
		}
		case?4096:{
			integ->S?=?arm_cfft_sR_f32_len4096;
			break;
		}
		default:{
			integ->S?=?arm_cfft_sR_f32_len1024;
		}
	}

	float?df?=?（float）integ->smple_frq?/?（float）integ->fft_len;			//計算頻率間隔（Hz/s）
	integ->high_pass?=?round（integ->frq_min/df）;						//高通頻率截止點
	integ->low_pass??=?round（integ->frq_max/df）;??						//低通頻率截止點
	
	float?dw?=?2*PI*df;													//圓頻率間隔（rad/s）
	integ->w_vec?=?（float*）MALLOC（（integ->fft_len-1）*（sizeof（float）））;
	for（int?i=0;?i<（integ->fft_len/2）;?i++）{
		integ->w_vec[i]?=?dw*i;											//正離散圓頻率向量
		integ->w_vec[i]?=?POW（integ->w_vec[i]integ->integ_time）;		//以積分次數為指數，建立圓頻率變量向量
	}
	for（int?i=integ->fft_len/2;?i<（integ->fft_len-1）;?i++）{
		integ->w_vec[i]?=?-dw*（integ->fft_len/2-1）?+?dw*（i?-?integ->fft_len/2）;	//負離散圓頻率向量
		integ->w_vec[i]?=?POW（integ->w_vec[i]integ->integ_time）;		//以積分次數為指數，建立圓頻率變量向量
	}
	
	return?0;
}

/**?
?*??@name	頻域一次/二次積分去初始化函數
?*??@brief	不需要用到頻域積分后可去初始化，釋放內

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????6116??2018-08-02?10:02??頻域二次積分_振動位移\double_integral.c
?????文件????????3409??2018-08-02?10:06??頻域二次積分_振動位移\double_integral.h
?????目錄???????????0??2018-08-02?09:59??頻域二次積分_振動位移\