|
|
#include "IMU.H"
#include "mpu6050.h"
float q0=1,q1=0,q2=0,q3=0;
float exInt=0,eyInt=0,ezInt=0;
short Gyro_y=0,Gyro_x=0,Gyro_z=0; //Y軸陀螺儀數(shù)據(jù)暫存
short Accel_x=0,Accel_y=0,Accel_z=0; //X軸加速度值暫存
float Angle_ax=0.0,Angle_ay=0.0,Angle_az=0.0; //由加速度計算的加速度(弧度制)
float Angle_gy=0.0,Angle_gx=0.0,Angle_gz=0.0; //由角速度計算的角速率(角度制)
float gx; float gy; float gz; float ax; float ay; float az;
float pitch,roll,yaw; //歐拉角
short aacx,aacy,aacz; //加速度傳感器原始數(shù)據(jù)
short gyrox,gyroy,gyroz; //陀螺儀原始數(shù)據(jù)
#define Kp 0.8f
#define Ki 0.001f
#define halfT 0.004f
void shujuzh(float *gx, float *gy, float *gz, float *ax, float *ay, float *az)//取陀螺儀原始數(shù)值
{
MPU_Get_Gyroscope(&Gyro_x,&Gyro_y,&Gyro_z);
MPU_Get_Accelerometer(&Accel_x,&Accel_y,&Accel_z);
*ax = Accel_x/8192.0; //根據(jù)設(shè)置換算
*ay = Accel_y/8192.0;
*az = Accel_z/8192.0;
*gx = Gyro_x/65.5*0.0174533;//根據(jù)設(shè)置換算
*gy = Gyro_y/65.5*0.0174533;
*gz = Gyro_z/65.5*0.0174533;
}
void IMUupdate(float *pitch,float *roll)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
shujuzh(&gx, &gy, &gz, &ax, &ay, &az);
norm = sqrt(ax*ax + ay*ay + az*az); //把加速度計的三維向量轉(zhuǎn)成單維向量
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 下面是把四元數(shù)換算成《方向余弦矩陣》中的第三列的三個元素。
// 根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，正好是這三個元素
// 所以這里的vx vy vz，其實就是當(dāng)前的歐拉角（即四元數(shù)）的機(jī)體坐標(biāo)參照系上，換算出來的
// 重力單位向量。
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;
q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;
q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
*pitch = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰 換算成度
*roll = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 橫滾
}
#include "stm32f10x.h"
#include "sys.h"
#include "led.h"
#include "timer.h"
#include "usart.h"
#include "mpu6050.h"
#include "IMU.H"
//mpu6050 scl PB10 sda PB11
float Pitch,Roll,Yaw; //歐拉角
#define JTAG_SWD_DISABLE 0X02
#define SWD_ENABLE 0X01
#define JTAG_SWD_ENABLE 0X00
void JTAG_Set(u8 mode)
{
u32 temp;
temp=mode;
temp<<=25;
RCC->APB2ENR|=1<<0; //開啟輔助時鐘
AFIO->MAPR&=0XF8FFFFFF; //清除MAPR的[26:24]
AFIO->MAPR|=temp; //設(shè)置jtag模式
}
int main(void)
{
JTAG_Set(JTAG_SWD_DISABLE);
JTAG_Set(SWD_ENABLE);
delay_init();
uart_init(9600);
MPU_Init();
OLED_Init();
OLED_Clear();
LED_Init();
SYSTM=1;
while(1)
{
IMUupdate(&Pitch,&Roll);
OLED_Float(0,0,Pitch,12);
OLED_Num2(0,2,Roll);
}
}
全部資料51hei下載地址：
四元數(shù)法自己移植.7z
(249.77 KB, 下載次數(shù): 71)
2020-3-23 13:50 上傳
點(diǎn)擊文件名下載附件
|
評分
-
查看全部評分
|
