first commit
This commit is contained in:
124
MPU6050/APP/imu.c
Normal file
124
MPU6050/APP/imu.c
Normal file
@ -0,0 +1,124 @@
|
||||
#include "imu.h"
|
||||
#include "mpu6050.h"
|
||||
#include <math.h>
|
||||
|
||||
const float M_PI = 3.1415926535;
|
||||
const float RtA = 57.2957795f;
|
||||
const float AtR = 0.0174532925f;
|
||||
const float Gyro_G = 0.03051756f*2; //<2F><><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-2000<30><30>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>1 / (65536 / 4000) = 0.03051756*2
|
||||
const float Gyro_Gr = 0.0005326f*2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>,ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD> 2*0.03051756 * 0.0174533f = 0.0005326*2
|
||||
|
||||
static float NormAcc;
|
||||
|
||||
/* <20><>Ԫ<EFBFBD><D4AA>ϵ<EFBFBD><CFB5> */
|
||||
typedef volatile struct {
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3;
|
||||
} Quaternion;
|
||||
Quaternion NumQ = {1, 0, 0, 0};
|
||||
|
||||
/* <20><><EFBFBD><EFBFBD><EFBFBD>ǻ<EFBFBD><C7BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
struct V{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
};
|
||||
volatile struct V GyroIntegError = {0};
|
||||
|
||||
_st_AngE Angle; //<2F><>ǰ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ̬
|
||||
/* <20><>Ԫ<EFBFBD><D4AA><EFBFBD>ⷨ<EFBFBD><E2B7A8>ʼ<EFBFBD><CABC> */
|
||||
void imu_rest(void)
|
||||
{
|
||||
NumQ.q0 =1;
|
||||
NumQ.q1 = 0;
|
||||
NumQ.q2 = 0;
|
||||
NumQ.q3 = 0;
|
||||
GyroIntegError.x = 0;
|
||||
GyroIntegError.y = 0;
|
||||
GyroIntegError.z = 0;
|
||||
Angle.pitch = 0;
|
||||
Angle.roll = 0;
|
||||
}
|
||||
|
||||
void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt)
|
||||
{
|
||||
volatile struct V Gravity,Acc,Gyro,AccGravity;
|
||||
|
||||
static float KpDef = 0.5f ;
|
||||
static float KiDef = 0.0001f;
|
||||
//static float KiDef = 0.00001f;
|
||||
|
||||
float q0_t,q1_t,q2_t,q3_t;
|
||||
//float NormAcc;
|
||||
float NormQuat;
|
||||
float HalfTime = dt * 0.5f;
|
||||
|
||||
//<2F><>ȡ<EFBFBD><C8A1>Ч<EFBFBD><D0A7>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
|
||||
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
|
||||
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
|
||||
// <20><><EFBFBD>ٶȹ<D9B6>һ<EFBFBD><D2BB>
|
||||
//printf("accX:%d\r\n",MPU6050.accX);
|
||||
NormAcc = 1/sqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
|
||||
//printf("NorAcc%f\r\n",NormAcc);
|
||||
// NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
|
||||
|
||||
Acc.x = pMpu->accX * NormAcc;
|
||||
Acc.y = pMpu->accY * NormAcc;
|
||||
Acc.z = pMpu->accZ * NormAcc;
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵ó<CBB5><C3B3><EFBFBD>ֵ
|
||||
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
|
||||
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
|
||||
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6><C8BB>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵIJ<C8B5><C4B2><EFBFBD>ֵ
|
||||
GyroIntegError.x += AccGravity.x * KiDef;
|
||||
GyroIntegError.y += AccGravity.y * KiDef;
|
||||
GyroIntegError.z += AccGravity.z * KiDef;
|
||||
|
||||
//<2F><><EFBFBD>ٶ<EFBFBD><D9B6>ںϼ<DABA><CFBC>ٶȻ<D9B6><C8BB>ֲ<EFBFBD><D6B2><EFBFBD>ֵ
|
||||
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
|
||||
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
|
||||
|
||||
// һ<><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
|
||||
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
|
||||
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
|
||||
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
|
||||
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
|
||||
|
||||
NumQ.q0 += q0_t;
|
||||
NumQ.q1 += q1_t;
|
||||
NumQ.q2 += q2_t;
|
||||
NumQ.q3 += q3_t;
|
||||
// <20><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
NormQuat = 1/sqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
|
||||
NumQ.q0 *= NormQuat;
|
||||
NumQ.q1 *= NormQuat;
|
||||
NumQ.q2 *= NormQuat;
|
||||
NumQ.q3 *= NormQuat;
|
||||
|
||||
// <20><>Ԫ<EFBFBD><D4AA>תŷ<D7AA><C5B7><EFBFBD><EFBFBD>
|
||||
{
|
||||
|
||||
#ifdef YAW_GYRO
|
||||
*(
|
||||
float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
|
||||
#else
|
||||
float yaw_G = pMpu->gyroZ * Gyro_G;
|
||||
if((yaw_G > 1.0f) || (yaw_G < -1.0f)) //<2F><><EFBFBD><EFBFBD>̫С<CCAB><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>Ǹ<EFBFBD><C7B8>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
pAngE->yaw += yaw_G * dt;
|
||||
// printf("Yaw:%f\r\n",pAngE->yaw);
|
||||
}
|
||||
#endif
|
||||
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
|
||||
|
||||
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA; //PITCH
|
||||
// printf("Pitch:%f;\r\n",pAngE->pitch);
|
||||
// printf("Roll:%f;\r\n",pAngE->roll);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user