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<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* @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 <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
* @param
|
|||
|
|
* @retval
|
|||
|
|
*/
|
|||
|
|
void GetSmpValue()
|
|||
|
|
{
|
|||
|
|
int16_t X_Value = 0,Y_Value = 0,Z_Value;
|
|||
|
|
for(int i=0; i<SMPVALUE_BUFFER_LEN; i++){
|
|||
|
|
//<2F>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Lsm6ds3_ReadAccelerationRaw(&X_Value, &Y_Value, &Z_Value);
|
|||
|
|
g_smpValueBuffer.XValueBuffer[i].iValue = X_Value;
|
|||
|
|
g_smpValueBuffer.YValueBuffer[i].iValue = Y_Value;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//x<><78><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
g_smpValueBuffer.XValue.iValue = f_Middle((U_DataType*)g_smpValueBuffer.XValueBuffer);
|
|||
|
|
//y<><79><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
g_smpValueBuffer.YValue.iValue= f_Middle((U_DataType*)g_smpValueBuffer.YValueBuffer);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
* @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 <EFBFBD><EFBFBD>ȡ<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
* @param
|
|||
|
|
* @retval
|
|||
|
|
*/
|
|||
|
|
void ComputeAngle()
|
|||
|
|
{
|
|||
|
|
int16_t X_SmpValue_New,Y_SmpValue_New;
|
|||
|
|
|
|||
|
|
float angle_x_m = 0,angle_y_m = 0; //<2F><><EFBFBD><EFBFBD>ֵ
|
|||
|
|
|
|||
|
|
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
|
|||
|
|
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 <EFBFBD><EFBFBD>ȡ<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
* @param
|
|||
|
|
* @retval
|
|||
|
|
*/
|
|||
|
|
void GetAngleValue()
|
|||
|
|
{
|
|||
|
|
//if(SD_TRUE == g_ReadSmpIP)
|
|||
|
|
//{
|
|||
|
|
GetSmpValue();
|
|||
|
|
ComputeAngle();
|
|||
|
|
//SaveRealTimeData();
|
|||
|
|
//g_ReadSmpIP = SD_FALSE;
|
|||
|
|
//}
|
|||
|
|
}
|
|||
|
|
|