#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轴陀螺仪数据暂存
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; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
#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)//取陀螺仪原始数值
{
MPU_Get_Gyroscope(&Gyro_x,&Gyro_y,&Gyro_z);
MPU_Get_Accelerometer(&Accel_x,&Accel_y,&Accel_z);
*ax = Accel_x/8192.0; //根据设置换算
*ay = Accel_y/8192.0;
*az = Accel_z/8192.0;
*gx = Gyro_x/65.5*0.0174533;//根据设置换算
*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); //把加速度计的三维向量转成单维向量
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 下面是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
// 根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素
// 所以这里的vx vy vz，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的
// 重力单位向量。
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; //设置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下载地址：
四元数法自己移植.7z
(249.77 KB, 下载次数: 71)
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
