stm32f103c8t6+mpu6050四元数姿态解算法，通过移植网上的例程 -

标题: 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/)