标题: stm32f103c8t6+mpu6050四元数姿态解算法,通过移植网上的例程 [打印本页]

作者: 遨游天空    时间: 2020-3-23 12:48
标题: stm32f103c8t6+mpu6050四元数姿态解算法,通过移植网上的例程
#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)



作者: ljjakey    时间: 2020-4-4 13:44
谢谢分享
作者: hominidclint    时间: 2020-4-9 21:06
学习学习,感谢分享,




欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1