devbox
小蓝xlanll
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

陀螺仪及四元数解算_陀螺仪四元数

作者：小蓝xlanll | 2024-06-08 08:25:01

赞

踩

陀螺仪四元数

陀螺仪的介绍

常用的六轴陀螺仪有MPU6050,icm-20602。MPU6050基本上只用软件IIC驱动，速率较慢，数据漂移也相对大一点。

陀螺仪的使用

以MPU6050为例。
软件IIC驱动 -> MPU6050寄存器基本配置 -> 读取原始数据 -> 将原始数据滤波后使用。
原始数据可以使用互补滤波，卡尔曼滤波，解算四元数来进行解算成欧拉角信息。
目前文章使用的是自结算四元数。

代码示例

  1. //内部使用，用户无需调用
  2. static void mpu6050_simiic_start(void)
  3. {
  4.     SDA_Out();
  5. 	MPU6050_SDA_HIGH();
  6. 	MPU6050_SCL_HIGH();
  7. 	DELAY_US(4);
  8. 	MPU6050_SDA_LOW();
  9. 	DELAY_US(4);
  10. 	MPU6050_SCL_LOW();
  11. }
  12.  
  13. //内部使用，用户无需调用
  14. static void mpu6050_simiic_stop(void)
  15. {
  16.     SDA_Out();
  17.     MPU6050_SCL_LOW();
  18.     MPU6050_SDA_LOW();
  19.     DELAY_US(4);
  20. 	MPU6050_SCL_HIGH();
  21. 	MPU6050_SDA_HIGH();
  22. 	DELAY_US(4);
  23. }
  24.  
  25. //主应答(包含ack:SDA=1和no_ack:SDA=0)
  26. //内部使用，用户无需调用
  27. static void mpu6050_simiic_sendack(unsigned char ack_dat)
  28. {
  29. 	if(ack_dat)
  30.     {
  31.         MPU6050_SCL_LOW();
  32.         SDA_Out();
  33.         MPU6050_SDA_LOW();
  34.         DELAY_US(2);
  35.         MPU6050_SCL_HIGH();
  36.         DELAY_US(2);
  37.         MPU6050_SCL_LOW();
  38.     } 
  39.     else    	
  40.     {
  41.         MPU6050_SCL_LOW();
  42.         SDA_Out();
  43.         MPU6050_SDA_HIGH();
  44.         DELAY_US(2);
  45.         MPU6050_SCL_HIGH();
  46.         DELAY_US(2);
  47.         MPU6050_SCL_LOW();
  48.     }
  49. }
  50.  
  51.  
  52. static bool mpu6050_sccb_waitack(void)
  53. {
  54.     uint8_t waittime = 0;
  55.  
  56.     SDA_In();
  57.  
  58.     MPU6050_SDA_HIGH();
  59.     DELAY_US(1);
  60.     MPU6050_SCL_HIGH();
  61.     DELAY_US(1);
  62.  
  63.     while(GET_MPU6050_SDA)           //应答为高电平，异常，通信失败
  64.     {
  65.         waittime++;
  66.         if(waittime > 250)
  67.         {
  68.             mpu6050_simiic_stop();
  69.             return 1;
  70.         }
  71.     }
  72.     MPU6050_SCL_LOW();
  73.     return 0;
  74. }
  75.  
  76. //字节发送程序
  77. //发送c(可以是数据也可是地址)，送完后接收从应答
  78. //不考虑从应答位
  79. //内部使用，用户无需调用
  80. static void mpu6050_send_ch(uint8_t c)
  81. {
  82. 	uint8_t i = 0;
  83.     SDA_Out();
  84.     MPU6050_SCL_LOW();//拉低时钟开始数据传输
  85.  
  86.     for(i=0;i<8;i++)
  87.     {
  88.         if((( c >> 7) & 0x01) == 0x01) 
  89.         {
  90.             MPU6050_SDA_HIGH();//SDA 输出数据
  91.         }
  92.         else			
  93.         {
  94.             MPU6050_SDA_LOW();
  95.         }
  96.         c <<= 1;
  97.         DELAY_US(2);
  98.         MPU6050_SCL_HIGH();                //SCL 拉高，采集信号
  99.         DELAY_US(2);
  100.         MPU6050_SCL_LOW();                //SCL 时钟线拉低
  101.         DELAY_US(2);
  102.     }
  103. 	//mpu6050_sccb_waitack();
  104. }
  105.  
  106.  
  107. //字节接收程序
  108. //接收器件传来的数据，此程序应配合|主应答函数|使用
  109. //内部使用，用户无需调用
  110. static uint8_t mpu6050_read_ch(uint8_t ack_x)
  111. {
  112.     uint8_t i;
  113.     uint8_t c = 0;
  114.     SDA_In();   
  115.     
  116.     //MPU6050_SCL_LOW();         //置时钟线为低，准备接收数据位
  117.     //DELAY_US(2);
  118.     //MPU6050_SCL_HIGH();         //置时钟线为高，使数据线上数据有效     
  119.  
  120.     for(i=0;i<8;i++)
  121.     {
  122.         MPU6050_SCL_LOW();         //置时钟线为低，准备接收数据位
  123.         DELAY_US(2);
  124.         MPU6050_SCL_HIGH();         //置时钟线为高，使数据线上数据有效
  125.         c <<= 1;
  126.  
  127.         if(GET_MPU6050_SDA) 
  128.         {
  129.             c++;   //读数据位，将接收的数据存c
  130.         }
  131.         DELAY_US(2);
  132.     }
  133.     //MPU6050_SCL_LOW();         //置时钟线为低，准备接收数据位
  134.     //DELAY_US(2);
  135. 	mpu6050_simiic_sendack(ack_x);
  136.     return c;
  137. }

MPU6050模块初始化部分

  1. //-------------------------------------------------------------------------------------------------------------------
  2. //  @brief      模拟IIC写数据到设备寄存器函数
  3. //  @param      dev_add			设备地址(低七位地址)
  4. //  @param      reg				寄存器地址
  5. //  @param      dat				写入的数据
  6. //  @return     void						
  7. //  @since      v1.0
  8. //  Sample usage:				
  9. //-------------------------------------------------------------------------------------------------------------------
  10. static void mpu6050_simiic_write_reg(uint8_t dev_add, uint8_t reg, uint8_t dat)
  11. {
  12.     //writeByteI2C(&soft_i2c,(dev_add<<1),reg,dat);
  13.     
  14. 	mpu6050_simiic_start();
  15.     mpu6050_send_ch( (dev_add<<1) | 0x00);   //发送器件地址加写位
  16.     mpu6050_sccb_waitack();
  17. 	mpu6050_send_ch( reg );   				 //发送从机寄存器地址
  18.     mpu6050_sccb_waitack();
  19. 	mpu6050_send_ch( dat );   				 //发送需要写入的数据
  20.     mpu6050_sccb_waitack();
  21. 	mpu6050_simiic_stop();
  22. }
  23.  
  24. //-------------------------------------------------------------------------------------------------------------------
  25. //  @brief      模拟IIC从设备寄存器读取数据
  26. //  @param      dev_add			设备地址(低七位地址)
  27. //  @param      reg				寄存器地址
  28. //  @param      type			选择通信方式是IIC  还是 SCCB
  29. //  @return     uint8			返回寄存器的数据			
  30. //  @since      v1.0
  31. //  Sample usage:				
  32. //-------------------------------------------------------------------------------------------------------------------
  33. uint8_t mpu6050_simiic_read_reg(uint8_t dev_add, uint8_t reg)
  34. {
  35. 	uint8_t data;
  36.     //readByteI2C(&soft_i2c,dev_add,reg,&data);
  37. 	mpu6050_simiic_start();
  38.     mpu6050_send_ch( (dev_add<<1) | 0x00);  //发送器件地址加写位
  39.     mpu6050_sccb_waitack();
  40. 	mpu6050_send_ch( reg );   				//发送从机寄存器地址
  41. 	mpu6050_sccb_waitack();
  42.     mpu6050_simiic_stop();
  43.  
  44. 	mpu6050_simiic_start();
  45. 	mpu6050_send_ch( (dev_add<<1) | 0x01);  //发送器件地址加读位
  46.     mpu6050_sccb_waitack();
  47. 	data = mpu6050_read_ch(no_ack);   				//读取数据
  48. 	mpu6050_simiic_stop();
  49. 	return data;
  50. }
  51.  
  52. //-------------------------------------------------------------------------------------------------------------------
  53. //  @brief      模拟IIC读取多字节数据
  54. //  @param      dev_add			设备地址(低七位地址)
  55. //  @param      reg				寄存器地址
  56. //  @param      dat_add			数据保存的地址指针
  57. //  @param      num				读取字节数量
  58. //  @param      type			选择通信方式是IIC  还是 SCCB
  59. //  @return     uint8			返回寄存器的数据			
  60. //  @since      v1.0
  61. //  Sample usage:				
  62. //-------------------------------------------------------------------------------------------------------------------
  63. void mpu6050_simiic_read_regs(uint8_t dev_add, uint8_t reg, uint8_t *dat_add, uint8_t num)
  64. {
  65. 	mpu6050_simiic_start();
  66.     mpu6050_send_ch( (dev_add<<1) | 0x00);  //发送器件地址加写位
  67.     mpu6050_sccb_waitack();
  68. 	mpu6050_send_ch( reg );   				//发送从机寄存器地址
  69.     mpu6050_sccb_waitack();
  70.     //mpu6050_simiic_stop();
  71. 	
  72. 	mpu6050_simiic_start();
  73. 	mpu6050_send_ch( (dev_add<<1) | 0x01);  //发送器件地址加读位
  74.     mpu6050_sccb_waitack();
  75.     while(--num)
  76.     {
  77.         *dat_add = mpu6050_read_ch(ack); //读取数据
  78.         dat_add++;
  79.     }
  80.     *dat_add = mpu6050_read_ch(no_ack); //读取数据
  81. 	mpu6050_simiic_stop();
  82. }
  83.  
  84. //-------------------------------------------------------------------------------------------------------------------
  85. //  @brief      MPU6050自检函数
  86. //  @param      NULL
  87. //  @return     void					
  88. //  @since      v1.0
  89. //  Sample usage:				
  90. //-------------------------------------------------------------------------------------------------------------------
  91. static bool mpu6050_self1_check(void)
  92. {
  93.     printf("ID:0x%x\n",mpu6050_simiic_read_reg(MPU6050_DEV_ADDR, WHO_AM_I));
  94.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, PWR_MGMT_1, 0x00);	//解除休眠状态
  95.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, SMPLRT_DIV, 0x07);   //125HZ采样率
  96.     if(0x07 != mpu6050_simiic_read_reg(MPU6050_DEV_ADDR, SMPLRT_DIV))
  97.     {
  98. 		printf("mpu6050 init error. %d\r\n",mpu6050_simiic_read_reg(MPU6050_DEV_ADDR, SMPLRT_DIV));
  99. 		return false;
  100.         //卡在这里原因有以下几点
  101.         //1 MPU6050坏了，如果是新的这样的概率极低
  102.         //2 接线错误或者没有接好
  103.         //3 可能你需要外接上拉电阻，上拉到3.3V
  104. 		//4 可能没有调用模拟IIC的初始化函数
  105.     }
  106. 	return true;
  107. }
  108.  
  109. bool mpu6050_init(void)
  110. {
  111.     DELAY_MS(800);
  112.  
  113.     SysCtlPeripheralEnable(SDA_Clock);
  114.     //2ma，上拉
  115.     GPIOPadConfigSet(SDA_Port,SDA_Pin,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPU);
  116.     //输出
  117.     GPIODirModeSet(SDA_Port,SDA_Pin,GPIO_DIR_MODE_OUT);
  118.  
  119.     SysCtlPeripheralEnable(SCL_Clock);
  120.     //2ma，开漏
  121.     GPIOPadConfigSet(SCL_Port,SCL_Pin,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_OD);
  122.     //输出
  123.     GPIODirModeSet(SCL_Port,SCL_Pin,GPIO_DIR_MODE_OUT);
  124.  
  125.  
  126.     if(mpu6050_self1_check() == false)
  127. 	{
  128. 		return false;
  129. 	}
  130.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, PWR_MGMT_1, 0x00);	//解除休眠状态
  131.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, SMPLRT_DIV, 0x07);   //125HZ采样率
  132.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, MPU6050_CONFIG, 0x04);       //
  133.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, GYRO_CONFIG, 0x18);  //2000
  134.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, ACCEL_CONFIG, 0x10); //8g
  135. 	mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, User_Control, 0x00);
  136.     mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, INT_PIN_CFG, 0x02);
  137.     mpu6050_gyro_x=0;
  138.     mpu6050_gyro_y=0;
  139.     mpu6050_gyro_z=0;
  140.  
  141.     //开启滤波参数
  142.     IIR_imu();
  143.     return true;
  144. }
  145.  
  146. //-------------------------------------------------------------------------------------------------------------------
  147. //  @brief      获取MPU6050加速度计数据
  148. //  @param      NULL
  149. //  @return     void
  150. //  @since      v1.0
  151. //  Sample usage:				执行该函数后，直接查看对应的变量即可
  152. //-------------------------------------------------------------------------------------------------------------------
  153. void mpu6050_get_accdata(void)
  154. {
  155.     uint8_t dat[6];
  156.  
  157.     mpu6050_simiic_read_regs(MPU6050_DEV_ADDR, ACCEL_XOUT_H, dat, 6);
  158.     mpu6050_acc_x = (int16_t)(((uint16_t)dat[0]<<8 | dat[1]));
  159.     mpu6050_acc_y = (int16_t)(((uint16_t)dat[2]<<8 | dat[3]));
  160.     mpu6050_acc_z = (int16_t)(((uint16_t)dat[4]<<8 | dat[5]));
  161.  
  162. }
  163.  
  164. //-------------------------------------------------------------------------------------------------------------------
  165. //  @brief      获取MPU6050陀螺仪数据
  166. //  @param      NULL
  167. //  @return     void
  168. //  @since      v1.0
  169. //  Sample usage:				执行该函数后，直接查看对应的变量即可
  170. //-------------------------------------------------------------------------------------------------------------------
  171. void mpu6050_get_gyro(void)
  172. {
  173.  
  174.     //所有
  175.     uint8_t dat[6];
  176.     mpu6050_simiic_read_regs(MPU6050_DEV_ADDR, GYRO_XOUT_H, dat, 6);
  177.     mpu6050_gyro_x = (int16_t)(((uint16_t)dat[0]<<8 | dat[1]));
  178.     mpu6050_gyro_y = (int16_t)(((uint16_t)dat[2]<<8 | dat[3]));
  179.     mpu6050_gyro_z = (int16_t)(((uint16_t)dat[4]<<8 | dat[5]));
  180. }

原始数据解算四元数

  1.  
  2. /*
  3.  * 函数名：get_iir_factor
  4.  * 描述  ：求取IIR滤波器的滤波因子
  5.  * 输入  ：out_factor滤波因子首地址，Time任务执行周期，Cut_Off滤波截止频率
  6.  * 返回  ：
  7.  */
  8. void get_iir_factor(float *out_factor,float Time, float Cut_Off)
  9. {
  10.     *out_factor = Time /( Time + 1/(2.0f * PI * Cut_Off) );
  11. }
  12. /* 获取IIR低通滤波 */
  13. void IIR_imu(void)
  14. {
  15. 	int8_t i;
  16.     for(i=0;i<=100;i++)
  17.     {
  18.         mpu6050_get_gyro();
  19.         set.gyro.x+=mpu6050_gyro_x;
  20.         set.gyro.y+=mpu6050_gyro_y;
  21.         set.gyro.z+=mpu6050_gyro_z;
  22.     }
  23.     set.gyro.x/= 100;
  24.     set.gyro.y/= 100;
  25.     set.gyro.z/= 100;
  26.     set.offset_flag = 1;
  27.     //printf("%d\n",set.offset_flag);
  28.     get_iir_factor(&imu.att_acc_factor,imu_Read_Time,15);
  29.     get_iir_factor(&imu.att_gyro_factor,imu_Read_Time,10);
  30. }
  31. /**
  32.   * @brief   IIR低通滤波器
  33.   * @param   *acc_in 输入三轴数据指针变量
  34.   * @param   *acc_out 输出三轴数据指针变量
  35.   * @param   lpf_factor 滤波因数
  36.   * @retval  x
  37.   */
  38. float iir_lpf(float in,float out,float lpf_factor)
  39. {
  40.     out = out + lpf_factor * (in - out);
  41.     return out;
  42. }
  43. // 快速开根号算法
  44. float invSqrt(float x)
  45. {
  46.     float halfx = 0.5f * x;
  47.     float y = x;
  48.     long i = *(long*)&y;
  49.     i = 0x5f3759df - (i>>1);
  50.     y = *(float*)&i;
  51.     y = y * (1.5f - (halfx * y * y));
  52.     return y;
  53. }
  54. /*
  55.  * 函数名：mahony_update
  56.  * 描述  ：姿态解算
  57.  * 输入  ：陀螺仪三轴数据（单位：弧度/秒），加速度三轴数据（单位：g）
  58.  * 返回  ：
  59.  */
  60. //Gyroscope units are radians/second, accelerometer  units are irrelevant as the vector is normalised.
  61. void mahony_update(float gx, float gy, float gz, float ax, float ay, float az)
  62. {
  63.     float norm;
  64.     float vx, vy, vz;
  65.     float ex, ey, ez;
  66.  
  67.     if(ax*ay*az==0)
  68.         return;
  69.     gx=gx*(PI / 180.0f);
  70.     gy=gy*(PI / 180.0f);
  71.     gz=gz*(PI / 180.0f);
  72.     //[ax,ay,az]是机体坐标系下加速度计测得的重力向量(竖直向下)
  73.     norm = invSqrt(ax*ax + ay*ay + az*az);
  74.     ax = ax * norm;
  75.     ay = ay * norm;
  76.     az = az * norm;
  77.  
  78.     //VectorA = MatrixC * VectorB
  79.     //VectorA ：参考重力向量转到在机体下的值
  80.     //MatrixC ：地理坐标系转机体坐标系的旋转矩阵
  81.     //VectorB ：参考重力向量（0,0,1）
  82.     //[vx,vy,vz]是地理坐标系重力分向量[0,0,1]经过DCM旋转矩阵(C(n->b))计算得到的机体坐标系中的重力向量(竖直向下)
  83.  
  84.     vx = Mat.DCM_T[0][2];
  85.     vy = Mat.DCM_T[1][2];
  86.     vz = Mat.DCM_T[2][2];
  87.  
  88.     //机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’
  89.     //利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
  90.     //实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
  91.     //但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
  92.     //两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
  93.     //加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
  94.     //也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。
  95.  
  96.     ex = ay*vz - az*vy;
  97.     ey = az*vx - ax*vz;
  98.     ez = ax*vy - ay*vx;
  99.  
  100.     //对误差向量进行积分
  101.     exInt = exInt + ex*ki;
  102.     eyInt = eyInt + ey*ki;
  103.     ezInt = ezInt + ez*ki;
  104.  
  105.     //姿态误差补偿到角速度上，修正角速度积分漂移，通过调节Kp、Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
  106.     gx = gx + kp*ex + exInt;
  107.     gy = gy + kp*ey + eyInt;
  108.     gz = gz + kp*ez + ezInt;
  109.  
  110.     //一阶龙格库塔法更新四元数
  111.     q0 = q0 + (-q1*gx - q2*gy - q3*gz)* MahonyPERIOD * 0.0005f;
  112.     q1 = q1 + ( q0*gx + q2*gz - q3*gy)* MahonyPERIOD * 0.0005f;
  113.     q2 = q2 + ( q0*gy - q1*gz + q3*gx)* MahonyPERIOD * 0.0005f;
  114.     q3 = q3 + ( q0*gz + q1*gy - q2*gx)* MahonyPERIOD * 0.0005f;
  115.  
  116.     //把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
  117.     norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  118.     q0 = q0 * norm;
  119.     q1 = q1 * norm;
  120.     q2 = q2 * norm;
  121.     q3 = q3 * norm;
  122.     //printf("%f,%f,%f,%f\n",q0,q1,q2,q3);
  123.  
  124.     static float pitch_old=0, roll_old=0,yaw_old=0;
  125.     float temp = 0;
  126.     //四元素转欧拉角
  127.     imu.pitch =   atan2f(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * (180.0f / PI);
  128.     imu.roll  =   -asinf(2.0f*(q0*q2 - q1*q3)) * (180.0f / PI);
  129.     //z轴角速度积分的偏航角
  130.     imu.yaw += imu.deg_s.z  * MahonyPERIOD * 0.001f;
  131.  
  132.     //防温漂
  133.     temp = imu.pitch-pitch_old;
  134.     if(temp<2 && temp > -2)
  135.     {
  136.         imu.pitch = pitch_old;
  137.     }
  138.     else
  139.     {
  140.         pitch_old = imu.pitch;
  141.     }
  142.  
  143.     temp = imu.yaw-yaw_old;
  144.     if(temp<0.1f && temp > -0.1f)
  145.     {
  146.         imu.yaw = yaw_old;
  147.     }
  148.     else
  149.     {
  150.         yaw_old = imu.yaw;
  151.     }
  152. /*
  153.  * 函数名：rotation_matrix
  154.  * 描述  ：旋转矩阵：机体坐标系 -> 地理坐标系
  155.  * 输入  ：
  156.  * 返回  ：
  157.  */
  158. void rotation_matrix(void)
  159. {
  160.     Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
  161.     Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
  162.     Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);
  163.  
  164.     Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
  165.     Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
  166.     Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);
  167.  
  168.     Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
  169.     Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
  170.     Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
  171. }
  172. /*
  173.  * 函数名：rotation_matrix_T
  174.  * 描述  ：旋转矩阵的转置矩阵：地理坐标系 -> 机体坐标系
  175.  * 输入  ：
  176.  * 返回  ：
  177.  */
  178. void rotation_matrix_T(void)
  179. {
  180.     Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
  181.     Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);
  182.     Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2);
  183.  
  184.     Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
  185.     Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
  186.     Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);
  187.  
  188.     Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
  189.     Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
  190.     Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
  191. }
  192. /*
  193.  * 函数名：Matrix_ready
  194.  * 描述  ：矩阵更新准备，为姿态解算使用
  195.  * 输入  ：
  196.  * 返回  ：
  197.  */
  198. void Matrix_ready(void)
  199. {
  200.     rotation_matrix();                      //旋转矩阵更新
  201.     rotation_matrix_T();                    //旋转矩阵的逆矩阵更新
  202. }
  203. /***********************************************************
  204. 函数名称：void IMU(void)
  205. 函数功能：获得姿态结算后的值
  206. 入口参数：无
  207. 出口参数：无
  208. 备 注：直接读取imu.pitch  imu.roll  imu.yaw
  209. ***********************************************************/
  210. void IMU(void)
  211. {
  212.     if(set.offset_flag)
  213.     {
  214.         /*获取X、Y的角速度和加速度*/
  215.         mpu6050_get_accdata();
  216.         mpu6050_get_gyro();
  217.         /*滤波算法*/
  218.         mpu6050_gyro_x-=set.gyro.x;
  219.         mpu6050_gyro_y-=set.gyro.y;
  220.         mpu6050_gyro_z-=set.gyro.z;
  221.  
  222.         acc_x = iir_lpf(mpu6050_acc_x,acc_x,imu.att_acc_factor);
  223.         acc_y = iir_lpf(mpu6050_acc_y,acc_y,imu.att_acc_factor);
  224.         acc_z = iir_lpf(mpu6050_acc_z,acc_z,imu.att_acc_factor);
  225.         //printf("%.2f,%.2f,%.2f\n",acc_x,acc_y,acc_z);
  226.         gyro_x =iir_lpf(mpu6050_gyro_x,gyro_x,imu.att_gyro_factor);
  227.         gyro_y =iir_lpf(mpu6050_gyro_y,gyro_y,imu.att_gyro_factor);
  228.         gyro_z =iir_lpf(mpu6050_gyro_z,gyro_z,imu.att_gyro_factor);
  229.         //printf("%d,%d,%d\n",gyro_x,gyro_y,gyro_z);
  230.  
  231.         //=================重力补偿版
  232.         /*acc_x = (float)mpu6050_acc_x * Acc_Gain * G;
  233.         acc_y = (float)mpu6050_acc_y * Acc_Gain * G;
  234.         acc_z = (float)mpu6050_acc_z * Acc_Gain * G;
  235.         gyro_x = (float)mpu6050_gyro_x * Gyro_Gain;
  236.         gyro_y = (float)mpu6050_gyro_y * Gyro_Gain;
  237.         gyro_z = (float)mpu6050_gyro_z * Gyro_Gain;
  238.         //-----------------IIR滤波
  239.         acc_x = acc_x*Kp_New + acc_x_old *Kp_Old;
  240.         acc_y = acc_y*Kp_New + acc_y_old *Kp_Old;
  241.         acc_z = acc_z*Kp_New + acc_z_old *Kp_Old;
  242.         acc_x_old = acc_x;
  243.         acc_y_old = acc_y;
  244.         acc_z_old = acc_z;
  245.         IMUupdate(acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,&imu);*/
  246.         //===============================
  247.  
  248.         /*数据存储*/
  249.         imu.acc_g.x = (float)acc_x/4096; //加速度计量程为:±8g/4096, ±16g/2048, ±4g/8192, ±2g/16384 
  250.         imu.acc_g.y = (float)acc_y/4096;
  251.         imu.acc_g.z = (float)acc_z/4096;
  252.         imu.deg_s.x = (float)mpu6050_gyro_x/16.4f;//陀螺仪量程为:±2000dps/16.4, ±1000dps/32.8, ±500 dps /65.6
  253.         imu.deg_s.y = (float)mpu6050_gyro_y/16.4f;//±250 dps/131.2, ±125 dps/262.4
  254.         imu.deg_s.z = (float)mpu6050_gyro_z/16.4f;
  255.  
  256.         //卡尔曼姿态解算
  257.         //imu.roll = -Kalman_Filter_x(imu.acc_g.x,imu.deg_s.x);
  258.         //imu.pitch = -Kalman_Filter_y(imu.acc_g.y,imu.deg_s.y);
  259.         //imu.yaw = -Kalman_Filter_x(imu.acc_g.z,imu.deg_s.z);
  260.  
  261.         /*姿态解算*/
  262.         mahony_update(imu.deg_s.x,imu.deg_s.y,imu.deg_s.z,imu.acc_g.x,imu.acc_g.y,imu.acc_g.z);
  263.         Matrix_ready();
  264.     }
  265. }

注意部分

  1. #define imu_Read_Time   0.01f//原始数据采集时间间隔 秒为单位 0.01秒 10ms
  2. #define MahonyPERIOD    10.0f//姿态解算周期（ms）

  • 陀螺仪初始化时一定要放平！注意陀螺仪的通信地址~

  • 代码同步更新到本人的github仓库及gitee仓库中

  • 使用示例，仅做演示，注意调用时间间隔！

  • int main()
    {
        mpu6050_init();
        while(1)
        {
            Task_10ms();
        }
    }
    void Task_10ms(void)
    {
        IMU(); //读取姿态解算值
        printf("%.2f,%.2f,%.2f\n",imu.pitch,imu.roll,imu.yaw);
    }

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/689121
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号