You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
914 lines
29 KiB
914 lines
29 KiB
/*
|
|
* bsp_IMU.c
|
|
*
|
|
* Created on: Oct 26, 2023
|
|
* Author: shiya
|
|
*/
|
|
#include "bsp_IMU.h"
|
|
#include "bsp_include.h"
|
|
|
|
#define NNN 4
|
|
#define NNN_1 4
|
|
float Factor_G= 1;
|
|
//#define MPU_ACCEL_OFFS_REG 0X06 //accel_offs寄存器,可读取版本号,寄存器手册未提到
|
|
//#define MPU_PROD_ID_REG 0X0C //prod id寄存器,在寄存器手册未提到
|
|
#define MPU_SELF_TESTX_REG 0X0D //自检寄存器X
|
|
#define MPU_SELF_TESTY_REG 0X0E //自检寄存器Y
|
|
#define MPU_SELF_TESTZ_REG 0X0F //自检寄存器Z
|
|
#define MPU_SELF_TESTA_REG 0X10 //自检寄存器A
|
|
#define MPU_SAMPLE_RATE_REG 0X19 //采样频率分频器
|
|
#define MPU_CFG_REG 0X1A //配置寄存器
|
|
#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器
|
|
#define MPU_ACCEL_CFG_REG 0X1C //加速度计配置寄存器
|
|
#define MPU_MOTION_DET_REG 0X1F //运动检测阀值设置寄存器
|
|
#define MPU_FIFO_EN_REG 0X23 //FIFO使能寄存器
|
|
#define MPU_I2CMST_CTRL_REG 0X24 //IIC主机控制寄存器
|
|
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC从机0器件地址寄存器
|
|
#define MPU_I2CSLV0_REG 0X26 //IIC从机0数据地址寄存器
|
|
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC从机0控制寄存器
|
|
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC从机1器件地址寄存器
|
|
#define MPU_I2CSLV1_REG 0X29 //IIC从机1数据地址寄存器
|
|
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC从机1控制寄存器
|
|
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC从机2器件地址寄存器
|
|
#define MPU_I2CSLV2_REG 0X2C //IIC从机2数据地址寄存器
|
|
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC从机2控制寄存器
|
|
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC从机3器件地址寄存器
|
|
#define MPU_I2CSLV3_REG 0X2F //IIC从机3数据地址寄存器
|
|
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC从机3控制寄存器
|
|
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC从机4器件地址寄存器
|
|
#define MPU_I2CSLV4_REG 0X32 //IIC从机4数据地址寄存器
|
|
#define MPU_I2CSLV4_DO_REG 0X33 //IIC从机4写数据寄存器
|
|
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC从机4控制寄存器
|
|
#define MPU_I2CSLV4_DI_REG 0X35 //IIC从机4读数据寄存器
|
|
|
|
#define MPU_I2CMST_STA_REG 0X36 //IIC主机状态寄存器
|
|
#define MPU_INTBP_CFG_REG 0X37 //中断/旁路设置寄存器
|
|
#define MPU_INT_EN_REG 0X38 //中断使能寄存器
|
|
#define MPU_INT_STA_REG 0X3A //中断状态寄存器
|
|
|
|
#define MPU_ACCEL_XOUTH_REG 0X3B //加速度值,X轴高8位寄存器
|
|
#define MPU_ACCEL_XOUTL_REG 0X3C //加速度值,X轴低8位寄存器
|
|
#define MPU_ACCEL_YOUTH_REG 0X3D //加速度值,Y轴高8位寄存器
|
|
#define MPU_ACCEL_YOUTL_REG 0X3E //加速度值,Y轴低8位寄存器
|
|
#define MPU_ACCEL_ZOUTH_REG 0X3F //加速度值,Z轴高8位寄存器
|
|
#define MPU_ACCEL_ZOUTL_REG 0X40 //加速度值,Z轴低8位寄存器
|
|
|
|
#define MPU_TEMP_OUTH_REG 0X41 //温度值高八位寄存器
|
|
#define MPU_TEMP_OUTL_REG 0X42 //温度值低8位寄存器
|
|
|
|
#define MPU_GYRO_XOUTH_REG 0X43 //陀螺仪值,X轴高8位寄存器
|
|
#define MPU_GYRO_XOUTL_REG 0X44 //陀螺仪值,X轴低8位寄存器
|
|
#define MPU_GYRO_YOUTH_REG 0X45 //陀螺仪值,Y轴高8位寄存器
|
|
#define MPU_GYRO_YOUTL_REG 0X46 //陀螺仪值,Y轴低8位寄存器
|
|
#define MPU_GYRO_ZOUTH_REG 0X47 //陀螺仪值,Z轴高8位寄存器
|
|
#define MPU_GYRO_ZOUTL_REG 0X48 //陀螺仪值,Z轴低8位寄存器
|
|
|
|
#define MPU_I2CSLV0_DO_REG 0X63 //IIC从机0数据寄存器
|
|
#define MPU_I2CSLV1_DO_REG 0X64 //IIC从机1数据寄存器
|
|
#define MPU_I2CSLV2_DO_REG 0X65 //IIC从机2数据寄存器
|
|
#define MPU_I2CSLV3_DO_REG 0X66 //IIC从机3数据寄存器
|
|
|
|
#define MPU_I2CMST_DELAY_REG 0X67 //IIC主机延时管理寄存器
|
|
#define MPU_SIGPATH_RST_REG 0X68 //信号通道复位寄存器
|
|
#define MPU_MDETECT_CTRL_REG 0X69 //运动检测控制寄存器
|
|
#define MPU_USER_CTRL_REG 0X6A //用户控制寄存器
|
|
#define MPU_PWR_MGMT1_REG 0X6B //电源管理寄存器1
|
|
#define MPU_PWR_MGMT2_REG 0X6C //电源管理寄存器2
|
|
#define MPU_FIFO_CNTH_REG 0X72 //FIFO计数寄存器高八位
|
|
#define MPU_FIFO_CNTL_REG 0X73 //FIFO计数寄存器低八位
|
|
#define MPU_FIFO_RW_REG 0X74 //FIFO读写寄存器
|
|
#define MPU_DEVICE_ID_REG 0X75 //器件ID寄存器
|
|
|
|
//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
|
|
//如果接V3.3,则IIC地址为0X69(不包含最低位).
|
|
#define MPU_ADDR 0X69
|
|
#define MPU_WHO_AM_I_ADDR 0X12
|
|
|
|
////因为模块AD0默认接GND,所以转为读写地址后,为0XD1和0XD0(如果接VCC,则为0XD3和0XD2)
|
|
//#define MPU_READ 0XD1
|
|
//#define MPU_WRITE 0XD0
|
|
|
|
SP_BSP_MPU6050 *MPU_Angle;
|
|
|
|
|
|
|
|
typedef uint8_t u8;
|
|
typedef uint16_t u16;
|
|
|
|
float Kalman_filter_pitch(float newAngle, float newRate, float dt);
|
|
|
|
float Kalman_BM_pitch(float newAngle_pitch, float newRate_pitch, float dt, float Tol_Acc);
|
|
|
|
float Kalman_BM_roll(float newAngle_roll, float newRate_roll, float dt, float Tol_Acc);
|
|
|
|
float FirstOrder_pitch(float newAngle, float newRate, float dt);
|
|
float SecondOrder_pitch(float newAngle, float newRate, float dt);
|
|
|
|
float Kalman_filter_roll(float newAngle, float newRate, float dt);
|
|
|
|
float FirstOrder_roll(float newAngle, float newRate, float dt);
|
|
float SecondOrder_roll(float newAngle, float newRate, float dt);
|
|
|
|
|
|
/* Kalman filter variables */
|
|
float Q_angle_pitch = 0.001; // Process noise variance for the accelerometer
|
|
float Q_bias_pitch = 0.003; // Process noise variance for the gyro bias
|
|
float R_measure_pitch = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise
|
|
|
|
float angle_pitch = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
|
|
float angle_roll = 0.0;
|
|
float bias_pitch = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
|
|
float rate_pitch; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
|
|
|
|
float P_pitch[2][2]={{0.0,0.0},{0.0,0.0}}; // Error covariance matrix - This is a 2x2 matrix
|
|
///////////////////////////////////////////////////////////////////////////////////
|
|
float K1_pitch =0.1; // 对加速度计取值的权重
|
|
float FirstOrder_angle_pitch;
|
|
|
|
float K2_pitch =0.2; // 对加速度计取值的权重
|
|
float kx1_pitch,kx2_pitch,ky1_pitch;
|
|
float SecondOrder_angle_pitch;
|
|
|
|
|
|
float FirstOrder_angle_roll;
|
|
float SecondOrder_angle_roll;
|
|
//初始化MPU6050
|
|
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
|
|
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读
|
|
u8 MPU_Write_Byte(u8 devaddr,u8 reg,u8 data);
|
|
u8 MPU_Read_Byte(u8 devaddr,u8 reg);
|
|
|
|
u8 MPU_Set_Gyro_Fsr(u8 fsr);
|
|
u8 MPU_Set_Accel_Fsr(u8 fsr);
|
|
u8 MPU_Set_LPF(u16 lpf);
|
|
u8 MPU_Set_Rate(u16 rate);
|
|
u8 MPU_Set_Fifo(u8 sens);
|
|
|
|
double MPU_Get_Temperature(void);
|
|
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
|
|
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
|
|
void Mean_filtering(double G_XX, double G_YY, double G_ZZ,double A_XX, double A_YY, double A_ZZ,double *XYZ_Val);
|
|
void Mean_filtering_1(double G_XX_1, double G_YY_1, double G_ZZ_1,double A_XX_1, double A_YY_1, double A_ZZ_1,double *XYZ_Val_1);
|
|
void Mean_filter_Angle(double G_XX_1, int Conversion_Flag,double *XY_Angle) ;//double G_YY_1,
|
|
void GetAngle();
|
|
void GetPitch();
|
|
//void Judging_Symbols(double x,double y,double z,int *Symbol_Val);
|
|
|
|
|
|
|
|
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
|
|
float Kalman_filter_pitch(float newAngle_pitch, float newRate_pitch, float dt) {
|
|
//Pitch_Kalman = Kalman_filter(Pitch, GyroX, dt);
|
|
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
|
|
// Modified by Kristian Lauszus
|
|
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
|
|
|
|
// Discrete Kalman filter time update equations - Time Update ("Predict")
|
|
// Update xhat - Project the state ahead
|
|
/* Step 1 */
|
|
rate_pitch = newRate_pitch - bias_pitch;
|
|
angle_pitch += dt * rate_pitch;
|
|
|
|
// Update estimation error covariance - Project the error covariance ahead
|
|
/* Step 2 */
|
|
P_pitch[0][0] += dt * (dt*P_pitch[1][1] - P_pitch[0][1] - P_pitch[1][0] + Q_angle_pitch);
|
|
P_pitch[0][1] -= dt * P_pitch[1][1];
|
|
P_pitch[1][0] -= dt * P_pitch[1][1];
|
|
P_pitch[1][1] += Q_bias_pitch * dt;
|
|
|
|
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
|
|
// Calculate Kalman gain - Compute the Kalman gain
|
|
/* Step 4 */
|
|
float S_pitch = P_pitch[0][0] + R_measure_pitch; // Estimate error
|
|
/* Step 5 */
|
|
float K_pitch[2]; // Kalman gain - This is a 2x1 vector
|
|
K_pitch[0] = P_pitch[0][0] / S_pitch;
|
|
K_pitch[1] = P_pitch[1][0] / S_pitch;
|
|
|
|
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
|
|
/* Step 3 */
|
|
float y_pitch = newAngle_pitch - angle_pitch; // Angle difference
|
|
/* Step 6 */
|
|
angle_pitch += K_pitch[0] * y_pitch;
|
|
bias_pitch += K_pitch[1] * y_pitch;
|
|
|
|
// Calculate estimation error covariance - Update the error covariance
|
|
/* Step 7 */
|
|
float P00_temp_pitch = P_pitch[0][0];
|
|
float P01_temp_pitch = P_pitch[0][1];
|
|
|
|
P_pitch[0][0] -= K_pitch[0] * P00_temp_pitch;
|
|
P_pitch[0][1] -= K_pitch[0] * P01_temp_pitch;
|
|
P_pitch[1][0] -= K_pitch[1] * P00_temp_pitch;
|
|
P_pitch[1][1] -= K_pitch[1] * P01_temp_pitch;
|
|
return angle_pitch;
|
|
};
|
|
float angle_pitch_BM = 0.0;
|
|
float bias_pitch_BM =0.0;
|
|
float Q_angle_pitch_BM = 0.001; // Process noise variance for the accelerometer
|
|
float PP_pitch_BM[2][2]={{1.0,0.0},{0.0,1.0}}; // Error covariance matrix - This is a 2x2 matrix
|
|
float R_angle_Pit_mea_BM= 0.1;
|
|
float Pitch_kal_BM = 0.0;
|
|
|
|
//【算法】基于STM32的MPU6050卡尔曼滤波算法（入门级）
|
|
|
|
//在该算法中对角度噪声进行处理，根据加速度的值，重新调整角度计算权值
|
|
float Kalman_BM_pitch(float newAngle_pitch, float newRate_pitch, float dt, float Tol_Acc) {
|
|
|
|
Pitch_kal_BM += dt * (newRate_pitch-bias_pitch_BM); //过程计算值
|
|
//设置权重
|
|
// if((Tol_Acc>1.001)&&(Tol_Acc<0.999))
|
|
// {
|
|
// Factor_G=1;
|
|
// }
|
|
// else
|
|
// {
|
|
// //Factor_G= 1/(fabs((Tol_Acc-1)+0.02)) +0.1 ;
|
|
// Factor_G= 20 ;
|
|
// }
|
|
// Factor_G=1;
|
|
//预测协方差矩阵
|
|
PP_pitch_BM[0][0] = Factor_G*(PP_pitch_BM[0][0] - (PP_pitch_BM[0][1] + PP_pitch_BM[1][0]-PP_pitch_BM[1][1]*dt)*dt)+ Q_angle_pitch_BM;
|
|
PP_pitch_BM[0][1] = Factor_G*(PP_pitch_BM[0][1] - PP_pitch_BM[1][1]*dt);
|
|
PP_pitch_BM[1][0] = Factor_G*(PP_pitch_BM[1][0] - PP_pitch_BM[1][1]*dt);
|
|
PP_pitch_BM[1][1] = Factor_G*(PP_pitch_BM[1][1] )+Q_bias_pitch;
|
|
|
|
float K_deno_Pitch=PP_pitch_BM[0][0]+R_angle_Pit_mea_BM;
|
|
|
|
//卡尔曼增益计算0
|
|
float K_pitch_BM[2]; // Kalman gain - This is a 2x1 vector
|
|
K_pitch_BM[0] = PP_pitch_BM[0][0] / K_deno_Pitch;
|
|
K_pitch_BM[1] = PP_pitch_BM[1][0] / K_deno_Pitch;
|
|
|
|
bias_pitch_BM= bias_pitch_BM+ K_pitch_BM[1] * (newAngle_pitch - Pitch_kal_BM);
|
|
Pitch_kal_BM = Pitch_kal_BM + K_pitch_BM[0] * (newAngle_pitch - Pitch_kal_BM);
|
|
// float PP_pitch_BM
|
|
float PP_pitch_BM_L[2][2];
|
|
PP_pitch_BM_L[0][0]=PP_pitch_BM[0][0];PP_pitch_BM_L[0][1]=PP_pitch_BM[0][1];PP_pitch_BM_L[1][0]=PP_pitch_BM[1][0];PP_pitch_BM_L[1][1]=PP_pitch_BM[1][1];
|
|
|
|
PP_pitch_BM[0][0] = PP_pitch_BM_L[0][0] -K_pitch_BM[0] * PP_pitch_BM_L[0][0];
|
|
PP_pitch_BM[0][1] = PP_pitch_BM_L[0][1] -K_pitch_BM[0] * PP_pitch_BM_L[0][1];
|
|
PP_pitch_BM[1][0] = PP_pitch_BM_L[1][0] -K_pitch_BM[1] * PP_pitch_BM_L[0][0];
|
|
PP_pitch_BM[1][1] = PP_pitch_BM_L[1][1] -K_pitch_BM[1] * PP_pitch_BM_L[0][1];
|
|
return Pitch_kal_BM;
|
|
}
|
|
|
|
float PP_roll_BM[2][2]={{1.0,0.0},{0.0,1.0}};
|
|
float bias_roll_BM =0.0;
|
|
float Q_bias_roll = 0.003; // Process noise variance for the gyro bias
|
|
float Q_angle_roll_BM = 0.001; // Process noise variance for the accelerometer
|
|
float Roll_kal_BM = 0.0;
|
|
float R_angle_Roll_mea_BM= 0.1;
|
|
float Factor_G;
|
|
|
|
float Kalman_BM_roll(float newAngle_roll, float newRate_roll, float dt, float Tol_Acc) {
|
|
|
|
Roll_kal_BM += dt * (newRate_roll-bias_roll_BM); //过程计算值
|
|
//设置权重
|
|
// float Factor_G= 1/fabs((Tol_Acc-1+0.01)) +0.1 ;
|
|
|
|
if((Tol_Acc>1.001)&&(Tol_Acc<0.999))
|
|
{
|
|
Factor_G=1;
|
|
}
|
|
else
|
|
{
|
|
//Factor_G= 1/(fabs((Tol_Acc-1)+0.02)) +0.1 ;
|
|
Factor_G= 20 ;
|
|
}
|
|
// Factor_G=1;
|
|
|
|
//预测协方差矩阵
|
|
PP_roll_BM[0][0] = Factor_G*(PP_roll_BM[0][0] - (PP_roll_BM[0][1] + PP_roll_BM[1][0]-PP_roll_BM[1][1]*dt)*dt)+ Q_angle_roll_BM;
|
|
PP_roll_BM[0][1] = Factor_G*(PP_roll_BM[0][1] - PP_roll_BM[1][1]*dt);
|
|
PP_roll_BM[1][0] = Factor_G*(PP_roll_BM[1][0] - PP_roll_BM[1][1]*dt);
|
|
PP_roll_BM[1][1] = Factor_G*(PP_roll_BM[1][1] )+Q_bias_roll;
|
|
|
|
float K_deno_Roll=PP_roll_BM[0][0]+R_angle_Roll_mea_BM;
|
|
|
|
//卡尔曼增益计算0
|
|
float K_roll_BM[2]; // Kalman gain - This is a 2x1 vector
|
|
K_roll_BM[0] = PP_roll_BM[0][0] / K_deno_Roll;
|
|
K_roll_BM[1] = PP_roll_BM[1][0] / K_deno_Roll;
|
|
|
|
bias_roll_BM= bias_roll_BM+ K_roll_BM[1] * (newAngle_roll - Roll_kal_BM);
|
|
Roll_kal_BM = Roll_kal_BM + K_roll_BM[0] * (newAngle_roll - Roll_kal_BM);
|
|
// float PP_roll_BM
|
|
float PP_roll_BM_L[2][2];
|
|
PP_roll_BM_L[0][0]=PP_roll_BM[0][0];PP_roll_BM_L[0][1]=PP_roll_BM[0][1];PP_roll_BM_L[1][0]=PP_roll_BM[1][0];PP_roll_BM_L[1][1]=PP_roll_BM[1][1];
|
|
|
|
PP_roll_BM[0][0] = PP_roll_BM_L[0][0] -K_roll_BM[0] * PP_roll_BM_L[0][0];
|
|
PP_roll_BM[0][1] = PP_roll_BM_L[0][1] -K_roll_BM[0] * PP_roll_BM_L[0][1];
|
|
PP_roll_BM[1][0] = PP_roll_BM_L[1][0] -K_roll_BM[1] * PP_roll_BM_L[0][0];
|
|
PP_roll_BM[1][1] = PP_roll_BM_L[1][1] -K_roll_BM[1] * PP_roll_BM_L[0][1];
|
|
return Roll_kal_BM;
|
|
}
|
|
|
|
uint8_t GF_MSP_IMU_6050_Init(SP_BSP_MPU6050 *_IMU6050_Angle)
|
|
{
|
|
MPU_Angle = _IMU6050_Angle;
|
|
GF_BSP_Interrupt_Add_CallBack(
|
|
DF_BSP_InterCall_TIM1_100ms_PeriodElapsedCallback,
|
|
GF_BSP_GetAngle);
|
|
return 1;
|
|
}
|
|
|
|
//void GF_BSP_GetAngle();//如，数据有问题，请分开调用
|
|
|
|
|
|
float FirstOrder_pitch(float newAngle_pitch, float newRate_pitch, float dt)//采集后计算的角度和角加速度
|
|
{
|
|
FirstOrder_angle_pitch = K1_pitch * newAngle_pitch + (1-K1_pitch) * (angle_pitch + newRate_pitch * dt);
|
|
return FirstOrder_angle_pitch;
|
|
}
|
|
|
|
float SecondOrder_pitch(float newAngle_pitch, float newRate_pitch, float dt)//采集后计算的角度和角加速度
|
|
{
|
|
kx1_pitch=(newAngle_pitch-SecondOrder_angle_pitch)*(1-K2_pitch)*(1-K2_pitch);
|
|
ky1_pitch=ky1_pitch+kx1_pitch*dt;
|
|
kx2_pitch=ky1_pitch+2*(1-K2_pitch)*(newAngle_pitch-SecondOrder_angle_pitch)+newRate_pitch;
|
|
SecondOrder_angle_pitch=SecondOrder_angle_pitch+ kx2_pitch*dt;
|
|
return SecondOrder_angle_pitch;
|
|
}
|
|
|
|
float Pitch_A,Roll_A,Pitch_A_1,Roll_A_1,Yaw_A,Pitch_A_2,Roll_A_2; //滤波前
|
|
|
|
float ROLL_END,ROLL_END_1,PITCH_END,PITCH_END_1;
|
|
//float ROll_KAL_END, PITCH_KAL_END;
|
|
|
|
|
|
float Pitch_Kalman,Pitch_first,Pitch_second;
|
|
float Roll_Kalman,Roll_first,Roll_second;
|
|
|
|
u8 bufG[6],bufA[6]; //角速度、加速度数据缓冲区
|
|
int16_t GyroX,GyroY,GyroZ; //角速度
|
|
|
|
float GyroX_D,GyroY_D,GyroZ_D; //角速度
|
|
|
|
int16_t AccX,AccY,AccZ; //加速度
|
|
double AccX_D,AccY_D,AccZ_D; //加速度
|
|
uint32_t tem0,tem1; //存放系统时间，us
|
|
|
|
//获取系统时间，用于卡尔曼滤波
|
|
__STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
|
|
{
|
|
return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
|
|
}
|
|
static uint32_t getCurrentMicros(void)
|
|
{
|
|
/* Ensure COUNTFLAG is reset by reading SysTick control and status register */
|
|
GXT_SYSTICK_IsActiveCounterFlag();
|
|
uint32_t m = HAL_GetTick();
|
|
const uint32_t tms = SysTick->LOAD + 1;
|
|
__IO uint32_t u = tms - SysTick->VAL;
|
|
if (GXT_SYSTICK_IsActiveCounterFlag()) {
|
|
m = HAL_GetTick();
|
|
u = tms - SysTick->VAL;
|
|
}
|
|
return (m * 1000 + (u * 1000) / tms);
|
|
}
|
|
//获取系统时间，单位us
|
|
uint32_t micros(void)
|
|
{
|
|
return getCurrentMicros();
|
|
}
|
|
|
|
//初始化MPU6050
|
|
//返回值:1,成功
|
|
// 其他,错误代码
|
|
uint8_t GF_BSP_MPU_Init(void)
|
|
{
|
|
u8 res;
|
|
MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0X80); //复位MPU6050
|
|
HAL_Delay(500); //延时500ms
|
|
MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0X00); //唤醒MPU6050
|
|
HAL_Delay(500);
|
|
|
|
tem1 = micros();
|
|
|
|
MPU_Set_LPF(50); //设置MPU6050的数字低通滤波器
|
|
MPU_Set_Gyro_Fsr(2); //陀螺仪传感器,±1000dps
|
|
MPU_Set_Accel_Fsr(2); //加速度传感器,±8g
|
|
MPU_Set_Rate(1000); //设置采样率1000Hz
|
|
|
|
MPU_Write_Byte(MPU_ADDR, MPU_INT_EN_REG, 0X00); //关闭所有中断
|
|
MPU_Write_Byte(MPU_ADDR, MPU_USER_CTRL_REG, 0X00); //I2C主模式关闭
|
|
MPU_Write_Byte(MPU_ADDR, MPU_FIFO_EN_REG, 0X00); //关闭FIFO
|
|
MPU_Write_Byte(MPU_ADDR, MPU_INTBP_CFG_REG, 0X80); //INT引脚低电平有效
|
|
res = MPU_Read_Byte(MPU_ADDR, MPU_DEVICE_ID_REG);
|
|
if (res == MPU_WHO_AM_I_ADDR) //器件ID正确
|
|
{
|
|
MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0X01); //设置CLKSEL,PLL 1：X，2：Y，3：Z轴为参考
|
|
MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT2_REG, 0X00); //加速度与陀螺仪都工作
|
|
MPU_Set_Rate(1000); //设置采样率为1000Hz
|
|
return 1;
|
|
}
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
//设置MPU6050陀螺仪传感器满量程范围
|
|
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
|
|
//返回值:0,设置成功
|
|
// 其他,设置失败
|
|
u8 MPU_Set_Gyro_Fsr(u8 fsr)
|
|
{
|
|
return MPU_Write_Byte(MPU_ADDR, MPU_GYRO_CFG_REG, fsr << 3); //设置陀螺仪满量程范围
|
|
}
|
|
|
|
|
|
//设置MPU6050加速度传感器满量程范围
|
|
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
|
|
//返回值:0,设置成功
|
|
// 其他,设置失败
|
|
u8 MPU_Set_Accel_Fsr(u8 fsr)
|
|
{
|
|
return MPU_Write_Byte(MPU_ADDR, MPU_ACCEL_CFG_REG, fsr << 3); //设置加速度传感器满量程范围
|
|
}
|
|
|
|
|
|
//设置MPU6050的数字低通滤波器
|
|
//lpf:数字低通滤波频率(Hz)
|
|
//返回值:0,设置成功
|
|
// 其他,设置失败
|
|
u8 MPU_Set_LPF(u16 lpf)
|
|
{
|
|
u8 data = 0;
|
|
if (lpf >= 188) data = 1;
|
|
else if (lpf >= 98) data = 2;
|
|
else if (lpf >= 42) data = 3;
|
|
else if (lpf >= 20) data = 4;
|
|
else if (lpf >= 10) data = 5;
|
|
else data = 6;
|
|
return MPU_Write_Byte(MPU_ADDR, MPU_CFG_REG, data); //设置数字低通滤波器
|
|
}
|
|
//设置MPU6050的采样率(假定Fs=1KHz)
|
|
//rate:4~1000(Hz)
|
|
//返回值:0,设置成功
|
|
// 其他,设置失败
|
|
u8 MPU_Set_Rate(u16 rate)
|
|
{
|
|
u8 data;
|
|
if (rate > 1000) rate = 1000;
|
|
if (rate < 4) rate = 4;
|
|
data = 1000 / rate - 1;
|
|
data = MPU_Write_Byte(MPU_ADDR, MPU_SAMPLE_RATE_REG, data); //设置数字低通滤波器
|
|
return MPU_Set_LPF(rate / 2); //自动设置LPF为采样率的一半
|
|
}
|
|
|
|
//得到温度值
|
|
//返回值:温度值(扩大了100倍)
|
|
double MPU_Get_Temperature(void)
|
|
{
|
|
u8 buf[2];
|
|
short raw;
|
|
double temp;
|
|
MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
|
|
raw = ((u16)buf[0] << 8) | buf[1];
|
|
temp = 25 + ((double)raw) / 326.8;
|
|
return temp;
|
|
}
|
|
//得到陀螺仪值(原始值)
|
|
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
|
|
//返回值:0,成功
|
|
// 其他,错误代码
|
|
u8 MPU_Get_Gyroscope(short *gx, short *gy, short *gz)
|
|
{
|
|
u8 buf[6], res;
|
|
res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
|
|
if (res == 0)
|
|
{
|
|
*gx = ((u16)buf[0] << 8) | buf[1];
|
|
*gy = ((u16)buf[2] << 8) | buf[3];
|
|
*gz = ((u16)buf[4] << 8) | buf[5];
|
|
}
|
|
return res;
|
|
}
|
|
//得到加速度值(原始值)
|
|
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
|
|
//返回值:0,成功
|
|
// 其他,错误代码
|
|
u8 MPU_Get_Accelerometer(short *ax, short *ay, short *az)
|
|
{
|
|
u8 buf[6], res;
|
|
res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
|
|
if (res == 0)
|
|
{
|
|
*ax = ((u16)buf[0] << 8) | buf[1];
|
|
*ay = ((u16)buf[2] << 8) | buf[3];
|
|
*az = ((u16)buf[4] << 8) | buf[5];
|
|
}
|
|
return res;
|
|
}
|
|
|
|
//IIC连续写
|
|
//addr:器件地址
|
|
//reg:寄存器地址
|
|
//len:写入长度
|
|
//buf:数据区
|
|
//返回值:0,正常
|
|
// 其他,错误代码
|
|
u8 MPU_Write_Len(u8 addr, u8 reg, u8 len, u8 *buf)
|
|
{
|
|
u8 res;
|
|
res = HAL_I2C_Mem_Write(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
|
|
return res;
|
|
}
|
|
|
|
//IIC连续读
|
|
//addr:器件地址
|
|
//reg:要读取的寄存器地址
|
|
//len:要读取的长度
|
|
//buf:读取到的数据存储区
|
|
//返回值:0,正常
|
|
// 其他,错误代码
|
|
u8 MPU_Read_Len(u8 addr, u8 reg, u8 len, u8 *buf)
|
|
{
|
|
u8 res;
|
|
res = HAL_I2C_Mem_Read(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
|
|
return res;
|
|
}
|
|
|
|
//IIC写一个字节
|
|
//devaddr:器件IIC地址
|
|
//reg:寄存器地址
|
|
//data:数据
|
|
//返回值:0,正常
|
|
// 其他,错误代码
|
|
u8 MPU_Write_Byte(u8 addr, u8 reg, u8 data)
|
|
{
|
|
u8 res;
|
|
res = HAL_I2C_Mem_Write(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, &data, 1, 0xfff);
|
|
return res;
|
|
}
|
|
|
|
//IIC读一个字节
|
|
//reg:寄存器地址
|
|
//返回值:读到的数据
|
|
double Angle_End[2];float dt;
|
|
u8 MPU_Read_Byte(u8 addr, u8 reg)
|
|
{
|
|
u8 res;
|
|
HAL_I2C_Mem_Read(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, &res, 1, 0xfff);
|
|
return res;
|
|
}
|
|
|
|
float Angle_X_G, Angle_Y_G;
|
|
double ACC_TOTAL;
|
|
|
|
double XYZ_ACC[6];double XYZ_ACC_1[6];
|
|
|
|
void GF_BSP_GetAngle()
|
|
{
|
|
int GyroX_bias=29; int GyroY_bias=10;int GyroZ_bias=13;
|
|
tem0 = micros();
|
|
MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, bufG);
|
|
GyroX = (int16_t)(((int16_t)bufG[0] << 8) + bufG[1]);
|
|
GyroY = (int16_t)(((int16_t)bufG[2] << 8) + bufG[3]);
|
|
GyroZ = (int16_t)(((int16_t)bufG[4] << 8) + bufG[5]);
|
|
|
|
GyroX=GyroX-GyroX_bias;
|
|
GyroY=GyroY-GyroY_bias;
|
|
GyroZ=GyroZ-GyroZ_bias;
|
|
GyroX_D=(float)GyroX*0.030487804878049;
|
|
GyroY_D=(float)GyroY*0.030487804878049;
|
|
GyroZ_D=(float)GyroZ*0.030487804878049;
|
|
// GyroX = GyroX / 32.7 - 0 ;//gyroZ_OFF; // Convert to deg/s
|
|
|
|
MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, bufA);
|
|
AccX = (int16_t)(((int16_t)bufA[0] << 8) | bufA[1]);
|
|
AccY = (int16_t)(((int16_t)bufA[2] << 8) | bufA[3]);
|
|
AccZ = (int16_t)(((int16_t)bufA[4] << 8) | bufA[5]);
|
|
int AccX_Bis=-40; int AccY_Bis=-22; int AccZ_Bis=29;
|
|
AccX=AccX-AccX_Bis;
|
|
AccY=AccY-AccY_Bis;
|
|
AccZ=AccZ-AccZ_Bis;
|
|
|
|
AccX_D =1.0*(double)AccX *0.000244140625;
|
|
AccY_D =1.0*(double)AccY *0.000244140625;
|
|
AccZ_D =(double)AccZ *0.000244140625;
|
|
|
|
|
|
dt = (tem0-tem1)/1000000.0;
|
|
|
|
Roll_A_1=atan2(-AccY_D,AccX_D)*180/3.1415926;
|
|
ACC_TOTAL=sqrt(AccX_D*AccX_D+AccY_D*AccY_D+AccZ_D*AccZ_D);
|
|
|
|
|
|
Mean_filtering(GyroX_D,GyroY_D,GyroZ_D,AccX_D, AccY_D, AccZ_D,XYZ_ACC);
|
|
// Mean_filtering_1(GyroX_D,GyroY_D,GyroZ_D,AccX_D, AccY_D, AccZ_D,XYZ_ACC_1);
|
|
double GyroX_D_F,GyroY_D_F,GyroZ_D_F,AccX_D_F,AccY_D_F,AccZ_D_F; //角速度
|
|
GyroX_D_F=XYZ_ACC[0];
|
|
GyroY_D_F=XYZ_ACC[1];
|
|
GyroZ_D_F=XYZ_ACC[2];
|
|
|
|
AccX_D_F=XYZ_ACC[3];
|
|
AccY_D_F=XYZ_ACC[4];
|
|
AccZ_D_F=XYZ_ACC[5];
|
|
|
|
// 基于加速度计算欧拉角
|
|
|
|
Roll_A_2=atan2(-AccY_D_F,AccX_D_F)*180/3.1415926;//正确//正确//正确//正确//正确//正确//正确//正确//壁面安装计算方法，已和瑞芬对比
|
|
Pitch_A_2=-atan(AccZ_D_F/sqrt(AccY_D_F*AccY_D_F+AccX_D_F*AccX_D_F))*180/3.14159;//正确//正确//正确//正确//正确//壁面放置计算方法
|
|
//基于陀螺仪计算欧拉角
|
|
|
|
|
|
|
|
Kalman_BM_roll(Roll_A_2, GyroZ_D_F, dt, ACC_TOTAL);//修正后
|
|
//Kalman_BM_pitch(Pitch_A_2, GyroY_D, dt, ACC_TOTAL);//修正后
|
|
|
|
Mean_filter_Angle(Roll_kal_BM, 0,Angle_End);//,Pitch_kal_BM*100
|
|
Angle_End[0]=Angle_End[0]-1*0;
|
|
if(Angle_End[0]>180)
|
|
{
|
|
Angle_End[0]=Angle_End[0]-360;
|
|
}
|
|
else if(Angle_End[0]<=-180)
|
|
{
|
|
Angle_End[0]=Angle_End[0]+360;
|
|
}
|
|
|
|
Angle_End[0]=Angle_End[0]*100;
|
|
//MPU_Angle->MPU_Roll=(int32_t)(fabs(Roll_kal_BM*100));
|
|
|
|
|
|
MPU_Angle->MPU_Pitch=(int32_t)(Angle_End[0]);
|
|
|
|
//MPU_Angle->MPU_Pitch=(int32_t)(fabs(Pitch_kal_BM*100));
|
|
tem1= tem0;
|
|
}
|
|
|
|
|
|
void GetPitch(){
|
|
// pitch = &Pitch;
|
|
// pitch_kalman = &Pitch_Kalman;
|
|
// pitch_first = &Pitch_first;
|
|
// pitch_second = &Pitch_second;
|
|
|
|
//printf("%f,%f,%f,%f\r\n",Pitch,Pitch_Kalman,Pitch_first,Pitch_second);
|
|
}
|
|
void Judging_Symbols(double x,double y,double z,int *Symbol_Val)
|
|
{
|
|
if(x>0)
|
|
{
|
|
Symbol_Val[0]=1;
|
|
}
|
|
else
|
|
{
|
|
Symbol_Val[0]=-1;
|
|
}
|
|
if(y>0)
|
|
{
|
|
Symbol_Val[1]=1;
|
|
}
|
|
else
|
|
{
|
|
Symbol_Val[1]=-1;
|
|
}
|
|
if(z>0)
|
|
{
|
|
Symbol_Val[2]=1;
|
|
}
|
|
else
|
|
{
|
|
Symbol_Val[2]=-1;
|
|
}
|
|
|
|
}
|
|
|
|
float bias_roll = 0.0;
|
|
float rate_roll;
|
|
float P_roll[2][2]={{0.0,0.0},{0.0,0.0}};
|
|
float Q_angle_roll = 0.001;
|
|
|
|
float R_measure_roll = 0.03;
|
|
|
|
float Kalman_filter_roll(float newAngle_roll, float newRate_roll, float dt) {
|
|
//Pitch_Kalman = Kalman_filter(Pitch, GyroX, dt);
|
|
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
|
|
// Modified by Kristian Lauszus
|
|
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
|
|
|
|
// Discrete Kalman filter time update equations - Time Update ("Predict")
|
|
// Update xhat - Project the state ahead
|
|
/* Step 1 */
|
|
rate_roll = newRate_roll - bias_roll;
|
|
angle_roll += dt * rate_roll;
|
|
|
|
// Update estimation error covariance - Project the error covariance ahead
|
|
/* Step 2 */
|
|
P_roll[0][0] += dt * (dt*P_roll[1][1] - P_roll[0][1] - P_roll[1][0] + Q_angle_roll);
|
|
P_roll[0][1] -= dt * P_roll[1][1];
|
|
P_roll[1][0] -= dt * P_roll[1][1];
|
|
P_roll[1][1] += Q_bias_roll * dt;
|
|
|
|
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
|
|
// Calculate Kalman gain - Compute the Kalman gain
|
|
/* Step 4 */
|
|
float S_roll = P_roll[0][0] + R_measure_roll; // Estimate error
|
|
/* Step 5 */
|
|
float K_roll[2]; // Kalman gain - This is a 2x1 vector
|
|
K_roll[0] = P_roll[0][0] / S_roll;
|
|
K_roll[1] = P_roll[1][0] / S_roll;
|
|
|
|
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
|
|
/* Step 3 */
|
|
float y_roll = newAngle_roll - angle_roll; // Angle difference
|
|
/* Step 6 */
|
|
angle_roll += K_roll[0] * y_roll;
|
|
bias_roll += K_roll[1] * y_roll;
|
|
|
|
// Calculate estimation error covariance - Update the error covariance
|
|
/* Step 7 */
|
|
float P00_temp_roll = P_roll[0][0];
|
|
float P01_temp_roll = P_roll[0][1];
|
|
|
|
P_roll[0][0] -= K_roll[0] * P00_temp_roll;
|
|
P_roll[0][1] -= K_roll[0] * P01_temp_roll;
|
|
P_roll[1][0] -= K_roll[1] * P00_temp_roll;
|
|
P_roll[1][1] -= K_roll[1] * P01_temp_roll;
|
|
|
|
return angle_roll;
|
|
};
|
|
float K1_roll =0.1;
|
|
float FirstOrder_roll(float newAngle_roll, float newRate_roll, float dt)//采集后计算的角度和角加速度
|
|
{
|
|
FirstOrder_angle_roll = K1_roll * newAngle_roll + (1-K1_roll) * (angle_roll + newRate_roll * dt);
|
|
return FirstOrder_angle_roll;
|
|
}
|
|
|
|
float K2_roll =0.2; // 对加速度计取值的权重
|
|
float kx1_roll,kx2_roll,ky1_roll;
|
|
float SecondOrder_roll(float newAngle_roll, float newRate_roll, float dt)//采集后计算的角度和角加速度
|
|
{
|
|
kx1_roll=(newAngle_roll-SecondOrder_angle_roll)*(1-K2_roll)*(1-K2_roll);
|
|
ky1_roll=ky1_roll+kx1_roll*dt;
|
|
kx2_roll=ky1_roll+2*(1-K2_roll)*(newAngle_roll-SecondOrder_angle_roll)+newRate_roll;
|
|
SecondOrder_angle_roll=SecondOrder_angle_roll+ kx2_roll*dt;
|
|
return SecondOrder_angle_roll;
|
|
}
|
|
|
|
double GG_XX[NNN];
|
|
double GG_YY[NNN];
|
|
double GG_ZZ[NNN];
|
|
|
|
double AA_XX[NNN];
|
|
double AA_YY[NNN];
|
|
double AA_ZZ[NNN];
|
|
|
|
int Mean_Index=0;
|
|
void Mean_filtering(double G_XX, double G_YY, double G_ZZ,double A_XX, double A_YY, double A_ZZ,double *XYZ_Val)
|
|
{
|
|
GG_XX[Mean_Index]=G_XX;
|
|
GG_YY[Mean_Index]=G_YY;
|
|
GG_ZZ[Mean_Index]=G_ZZ;
|
|
|
|
|
|
AA_XX[Mean_Index]=A_XX;
|
|
AA_YY[Mean_Index]=A_YY;
|
|
AA_ZZ[Mean_Index]=A_ZZ;
|
|
|
|
double Sum_XYZ[7];
|
|
|
|
for (int i = 0; i < NNN; i++)
|
|
{
|
|
Sum_XYZ[0] += GG_XX[i];
|
|
Sum_XYZ[1] += GG_YY[i];
|
|
Sum_XYZ[2] += GG_ZZ[i];
|
|
|
|
Sum_XYZ[3] += AA_XX[i];
|
|
Sum_XYZ[4] += AA_YY[i];
|
|
Sum_XYZ[5] += AA_ZZ[i];
|
|
}
|
|
|
|
XYZ_Val[0]=Sum_XYZ[0]/NNN;
|
|
XYZ_Val[1]=Sum_XYZ[1]/NNN;
|
|
XYZ_Val[2]=Sum_XYZ[2]/NNN;
|
|
XYZ_Val[3]=Sum_XYZ[3]/NNN;
|
|
XYZ_Val[4]=Sum_XYZ[4]/NNN;
|
|
XYZ_Val[5]=Sum_XYZ[5]/NNN;
|
|
|
|
Mean_Index++;
|
|
|
|
if(Mean_Index>=NNN)
|
|
{
|
|
Mean_Index=0;
|
|
}
|
|
}
|
|
|
|
double Angle_RROOLL[NNN_1]={0};
|
|
double Angle_PITCH[NNN_1];
|
|
int Angle_Index=0;
|
|
double Sum_Angle[2]={0,0};
|
|
|
|
void Mean_filter_Angle(double G_XX_1, int Conversion_Flag,double *XY_Angle) //double G_YY_1,
|
|
{
|
|
double Old_GG_X= Angle_RROOLL[Angle_Index];
|
|
Angle_RROOLL[Angle_Index]=G_XX_1;
|
|
if(Sum_Angle[0]>135*NNN_1)
|
|
{
|
|
if(Angle_RROOLL[Angle_Index]<-160)
|
|
{
|
|
Angle_RROOLL[Angle_Index]=Angle_RROOLL[Angle_Index]+360;
|
|
}
|
|
}
|
|
else if(Sum_Angle[0]<-135*NNN_1)
|
|
{
|
|
if(Angle_RROOLL[Angle_Index]>160)
|
|
{
|
|
Angle_RROOLL[Angle_Index]=Angle_RROOLL[Angle_Index]-360;
|
|
}
|
|
}//角度转换完成
|
|
|
|
Sum_Angle[0]=Sum_Angle[0]+Angle_RROOLL[Angle_Index]- Old_GG_X;
|
|
XY_Angle[0]=Sum_Angle[0]/NNN_1;
|
|
if(XY_Angle[0]>180)
|
|
{
|
|
XY_Angle[0]=XY_Angle[0]-360;
|
|
for(int i=0;i<NNN_1;i++)
|
|
{
|
|
Angle_RROOLL[i]=XY_Angle[0];
|
|
}
|
|
Sum_Angle[0]=XY_Angle[0]*NNN_1;
|
|
}
|
|
else if(XY_Angle[0]<-180)
|
|
{
|
|
XY_Angle[0]=XY_Angle[0]+360;
|
|
for(int i=0;i<NNN_1;i++)
|
|
{
|
|
Angle_RROOLL[i]=XY_Angle[0];
|
|
}
|
|
Sum_Angle[0]=XY_Angle[0]*NNN_1;
|
|
}
|
|
|
|
Angle_Index++;
|
|
if(Angle_Index>=NNN_1)
|
|
{
|
|
Angle_Index=0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
double Sum_GX=0;
|
|
double Sum_GY=0;
|
|
double Sum_GZ=0;
|
|
|
|
double Sum_AX=0;
|
|
double Sum_AY=0;
|
|
double Sum_AZ=0;
|
|
int Mean_Index_0=0;
|
|
|
|
double GG_XX_X[NNN];
|
|
double GG_YY_X[NNN];
|
|
double GG_ZZ_X[NNN];
|
|
|
|
double AA_XX_X[NNN];
|
|
double AA_YY_X[NNN];
|
|
double AA_ZZ_X[NNN];
|
|
|
|
void Mean_filtering_1(double G_XX_1, double G_YY_1, double G_ZZ_1,double A_XX_1, double A_YY_1, double A_ZZ_1,double *XYZ_Val_1)
|
|
{
|
|
double Old_GG_X= GG_XX_X[Mean_Index_0];
|
|
double Old_GG_Y= GG_YY_X[Mean_Index_0];
|
|
double Old_GG_Z= GG_ZZ_X[Mean_Index_0];
|
|
|
|
double Old_AA_X= AA_XX_X[Mean_Index_0];
|
|
double Old_AA_Y= AA_YY_X[Mean_Index_0];
|
|
double Old_AA_Z= AA_ZZ_X[Mean_Index_0];
|
|
|
|
|
|
GG_XX_X[Mean_Index_0]=G_XX_1;
|
|
GG_YY_X[Mean_Index_0]=G_YY_1;
|
|
GG_ZZ_X[Mean_Index_0]=G_ZZ_1;
|
|
|
|
AA_XX_X[Mean_Index_0]=A_XX_1;
|
|
AA_YY_X[Mean_Index_0]=A_YY_1;
|
|
AA_ZZ_X[Mean_Index_0]=A_ZZ_1;
|
|
//数据已更新
|
|
|
|
Sum_GX=Sum_GX+GG_XX_X[Mean_Index_0]-Old_GG_X;//Sum Update
|
|
Sum_GY=Sum_GY+GG_YY_X[Mean_Index_0]-Old_GG_Y;//Sum Update
|
|
Sum_GZ=Sum_GZ+GG_ZZ_X[Mean_Index_0]-Old_GG_Z;//Sum Update
|
|
|
|
Sum_AX=Sum_AX+AA_XX_X[Mean_Index_0]-Old_AA_X;//Sum Update
|
|
Sum_AY=Sum_AY+AA_YY_X[Mean_Index_0]-Old_AA_Y;//Sum Update
|
|
Sum_AZ=Sum_AZ+AA_ZZ_X[Mean_Index_0]-Old_AA_Z;//Sum Update
|
|
|
|
XYZ_Val_1[0]=Sum_GX/NNN;
|
|
XYZ_Val_1[1]=Sum_GY/NNN;
|
|
XYZ_Val_1[2]=Sum_GZ/NNN;
|
|
|
|
XYZ_Val_1[3]=Sum_AX/NNN;
|
|
XYZ_Val_1[4]=Sum_AY/NNN;
|
|
XYZ_Val_1[5]=Sum_AZ/NNN;
|
|
|
|
Mean_Index_0++;
|
|
if(Mean_Index_0>=NNN)
|
|
{
|
|
Mean_Index_0=0;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|