144 lines
3.1 KiB
C
144 lines
3.1 KiB
C
#include "collect_data.h"
|
||
#include <string.h>
|
||
#include "math.h"
|
||
#include "pdebug.h"
|
||
|
||
tagSmpValueBuffer g_smpValueBuffer;
|
||
tagAnlgeValue g_AnlgeValue;
|
||
|
||
#if 0
|
||
/**
|
||
* @brief fir滤波，更新数据
|
||
* @param
|
||
* @retval
|
||
*/
|
||
void FirFilter_RenewData(int16_t iXValue,int16_t iYValue,int16_t iZValue)
|
||
{
|
||
//X
|
||
writeIP = gs_FirFilterParam[0].WriteIP;
|
||
gs_FirFilterParam[0].pDataBuf[writeIP] = iXValue;
|
||
gs_FirFilterParam[0].WriteIP = (writeIP + 1) % gs_FirFilterParam[0].BufLen;
|
||
|
||
//Y
|
||
writeIP = gs_FirFilterParam[1].WriteIP;
|
||
gs_FirFilterParam[1].pDataBuf[writeIP] = iYValue;
|
||
gs_FirFilterParam[1].WriteIP = (writeIP + 1) % gs_FirFilterParam[1].BufLen;
|
||
|
||
}
|
||
#endif
|
||
|
||
/**
|
||
* @brief 读取采样值
|
||
* @param
|
||
* @retval
|
||
*/
|
||
void GetSmpValue()
|
||
{
|
||
int16_t X_Value = 0,Y_Value = 0,Z_Value;
|
||
for(int i=0; i<SMPVALUE_BUFFER_LEN; i++){
|
||
//采集数据
|
||
Lsm6ds3_ReadAccelerationRaw(&X_Value, &Y_Value, &Z_Value);
|
||
g_smpValueBuffer.XValueBuffer[i].iValue = X_Value;
|
||
g_smpValueBuffer.YValueBuffer[i].iValue = Y_Value;
|
||
}
|
||
|
||
//x轴采样值
|
||
g_smpValueBuffer.XValue.iValue = f_Middle((U_DataType*)g_smpValueBuffer.XValueBuffer);
|
||
//y轴采样值
|
||
g_smpValueBuffer.YValue.iValue= f_Middle((U_DataType*)g_smpValueBuffer.YValueBuffer);
|
||
}
|
||
|
||
/**
|
||
* @brief 计算角度值
|
||
* @param
|
||
* @retval
|
||
*/
|
||
#define LSB 16393.44
|
||
#define PI (180/3.1415926)
|
||
void AD_ComputeAngle(int SmpValue_X,int SmpValue_Y,float *pAngle_X,float *pAngle_Y)
|
||
{
|
||
float x_value = (float)SmpValue_X / LSB;
|
||
float y_value = (float)SmpValue_Y / LSB;
|
||
|
||
if(x_value >= 1)
|
||
x_value=1;
|
||
if(x_value <= -1)
|
||
x_value=-1;
|
||
|
||
if(y_value >= 1)
|
||
y_value=1;
|
||
|
||
if(y_value <= -1)
|
||
y_value=-1;
|
||
|
||
*pAngle_X = asinf(x_value)*PI;
|
||
*pAngle_Y = asinf(y_value)*PI;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取角度值
|
||
* @param
|
||
* @retval
|
||
*/
|
||
void ComputeAngle()
|
||
{
|
||
int16_t X_SmpValue_New,Y_SmpValue_New;
|
||
|
||
float angle_x_m = 0,angle_y_m = 0; //测量值
|
||
|
||
//当前采样值
|
||
X_SmpValue_New = g_smpValueBuffer.XValue.iValue;
|
||
Y_SmpValue_New = g_smpValueBuffer.YValue.iValue;
|
||
|
||
#ifdef FILTER_MIDDLE_ENABLE
|
||
u_int16_t f_Middle(int16_t *ArrDataBuffer);
|
||
#endif
|
||
|
||
|
||
#ifdef FILTER_FIR_ENABLE
|
||
FirFilter_RenewData(X_SmpValue_New,Y_SmpValue_New,Z_SmpValue_New);
|
||
|
||
X_SmpValue_New = filter_fir_plus(&gs_FirFilterParam[0],gs_FirFilterHParam);
|
||
Y_SmpValue_New = filter_fir_plus(&gs_FirFilterParam[1],gs_FirFilterHParam);
|
||
#endif
|
||
|
||
|
||
AD_ComputeAngle(X_SmpValue_New,Y_SmpValue_New,&angle_x_m,&angle_y_m);
|
||
|
||
if(angle_x_m > 60)
|
||
angle_x_m = 60;
|
||
|
||
if(angle_x_m < -60)
|
||
angle_x_m = -60;
|
||
|
||
if(angle_y_m > 60)
|
||
angle_y_m = 60;
|
||
|
||
if(angle_y_m < -60)
|
||
angle_y_m = -60;
|
||
|
||
g_AnlgeValue.Angle_X = angle_x_m;
|
||
g_AnlgeValue.Angle_Y = angle_y_m;
|
||
|
||
//term_printf("x_angele: %.2f, y_angle: %.2f\r\n",g_AnlgeValue.Angle_X,g_AnlgeValue.Angle_Y);
|
||
//g_AnlgeValue.isReady = SD_TRUE;
|
||
|
||
}
|
||
|
||
/**
|
||
* @brief 获取角度值
|
||
* @param
|
||
* @retval
|
||
*/
|
||
void GetAngleValue()
|
||
{
|
||
//if(SD_TRUE == g_ReadSmpIP)
|
||
//{
|
||
GetSmpValue();
|
||
ComputeAngle();
|
||
//SaveRealTimeData();
|
||
//g_ReadSmpIP = SD_FALSE;
|
||
//}
|
||
}
|
||
|