ICM20602 姿态解算

2019-07-20 23:34发布

请教一下大家，ICM20602没有DMP可以直接用来合成四元数。只能获取Acce，Gyro六个轴向的原始数据。请教一下大家，如何获取四元数？
我尝试过这个 https://www.cnblogs.com/dchipnau/p/5310088.html  这个链接里面的四元数法，一阶滤波和卡尔曼滤波，最后得到的数据都不稳定。
特别是Yaw轴向，数据一致在循环，如下图所示：  XY方向也不是很稳定。请问这个是为什么。下面是我的代码。
希望哪位高手大神们指点一下，谢谢！！！

#include "bsp_icm20602.h"
#include <stdio.h>
#include <math.h>

/*
* Brief       For read ICM20602 WHO AM I
* Parameters None
* Return None
* NOTE       None
*/
void ICM206XX_ReadWhoAmI(void)
{
unsigned char ICM206XX_ID;

ICM206XX_ReadReg(ICM20602_DEV_ADDR, ICM20602_WHO_AM_I, &ICM206XX_ID);

printf("WHO AM I is: 0x%x ",ICM206XX_ID);
}

/*
* Brief       For ICM206XX self-test
* Parameters None
* Return None
* NOTE       None
*/
void ICM206XX_Self1_Check(void)
{
unsigned char Read_ID;

while(0x12 != Read_ID)
{
      //Always self-test,until to read correct ID
      ICM206XX_ReadReg(ICM20602_DEV_ADDR, ICM20602_WHO_AM_I, &Read_ID);
}
}

void ICM206XX_Self_Test(void)
{
unsigned char selfTest_Gyro = 0xE0;
unsigned char selfTest_Acce = 0xE0;

ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_GYRO_CONFIG, &selfTest_Gyro);
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_ACCEL_CONFIG, &selfTest_Acce);
}

/*
* Brief       For ICM206XX init
* Parameters None
* Return None
* NOTE       None
*/
void ICM206XX_Init(void)
{
unsigned char pwrMgmt = 0x80;
unsigned char pwrMgmt_test;

unsigned char cfg_Pwr_mgnt_1       = 0x01;
unsigned char cfg_Pwr_mgnt_2       = 0x00;
unsigned char cfg_ICM20602_config = 0x01;
unsigned char cfg_Smplrv_div       = 0x07;
unsigned char cfg_Gyro_config    = 0x18;
unsigned char cfg_Acce_config_1    = 0x18;
unsigned char cfg_Acce_config_2    = 0x23;

ICM206XX_Self1_Check();  //self-test

//self_test

//Reset
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_PWR_MGMT_1, &pwrMgmt);
IIC_delay(IIC_DELAY_COUNT*10);

//Wait for reset complete, can also use do...while()
while(0x80 & pwrMgmt_test)
{
      ICM206XX_ReadReg(ICM20602_DEV_ADDR, ICM20602_PWR_MGMT_1, &pwrMgmt_test);
}

//Config ICM20602
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_PWR_MGMT_1,    &cfg_Pwr_mgnt_1);    //Setting clk
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_PWR_MGMT_2,    &cfg_Pwr_mgnt_2);    //start gyro and acce
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_CONFIG,          &cfg_ICM20602_config);  //set 176Hz 1KHz
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_SMPLRT_DIV,    &cfg_Smplrv_div);    //SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV)
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_GYRO_CONFIG,    &cfg_Gyro_config);    //¡à2000 dps
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_ACCEL_CONFIG,    &cfg_Acce_config_1); //¡à16g
ICM206XX_WriteReg(ICM20602_DEV_ADDR, ICM20602_ACCEL_CONFIG_2, &cfg_Acce_config_2); //Average 8 samples 44.8HZ
printf("Init Complete! ");
}

/*
* Brief       For ICM206XX init
* Parameters *Src_Acce: get Accelerate source data
* Return None
* NOTE       None
*/
void ICM206XX_GetAcceSrcData(int short *Src_Acce)
{
unsigned char temp_acce[6];

ICM206XX_MultiReadReg(ICM20602_DEV_ADDR, ICM20602_ACCEL_XOUT_H, temp_acce, 6);

IIC_delay(IIC_DELAY_COUNT*100);
Src_Acce[0] = (temp_acce[0] << 8) | temp_acce[1];
Src_Acce[1] = (temp_acce[2] << 8) | temp_acce[3];
Src_Acce[2] = (temp_acce[4] << 8) | temp_acce[5];
}

/*
* Brief       For ICM206XX init
* Parameters *Src_Acce: get Accelerate source data
* Return None
* NOTE       None
*/
void ICM206XX_GetGyroSrcData(int short *Src_Gyro)
{
unsigned char temp_gyro[6];

ICM206XX_MultiReadReg(ICM20602_DEV_ADDR, ICM20602_GYRO_XOUT_H, temp_gyro, 6);

IIC_delay(IIC_DELAY_COUNT*100);
Src_Gyro[0] = (temp_gyro[0] << 8) | temp_gyro[1];
Src_Gyro[1] = (temp_gyro[2] << 8) | temp_gyro[3];
Src_Gyro[2] = (temp_gyro[4] << 8) | temp_gyro[5];
}

#if 0

//====================================quaternion================================
#define Kp 100.0f                         // ±èàyÔöòæÖ§ÅäÂêêÕá2μ½¼óËù¶è¼Æ
#define Ki 0.002f                         // »y·ÖÔöòæÖ§ÅäÂêμÄíóÂYòÇÆ«¼ûμÄÏÎ½ó
#define halfT 0.001f                      // 2éÑùÖüÆúμÄò»°ë

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // ËÄÔaêyμÄÔaËØ￡¬′ú±í1à¼Æ·½Ïò
float exInt = 0, eyInt = 0, ezInt = 0;    // °′±èàyËõD¡»y·ÖÎó2î

float Yaw,Pitch,Roll;                      // Æ«o½½Ç￡¬¸©Ñö½Ç￡¬1ö×a½Ç

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;

// 2aá¿Õy3￡»ˉ
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;                //???
ay = ay / norm;
az = az / norm;

// 1à¼Æ·½ÏòμÄÖØá|
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// ′íÎóμÄáìóòoí·½Ïò′«¸DÆ÷2aá¿2Î¿¼·½ÏòÖ®¼äμÄ½»2æ3é¼¨μÄ×üoí
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);

// »y·ÖÎó2î±èày»y·ÖÔöòæ
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

// μ÷ÕûoóμÄíóÂYòÇ2aá¿
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

// ÕûoÏËÄÔaêyÂêoíÕy3￡»°
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;

// Õy3￡»°ËÄÔaêy
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 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,?????
Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;             //??????,??
}
//====================================quaternion================================
#endif

#if 1
//====================================ò»½×»￥21ÂË2¨==============================
#define Kp    10.0f                      // ÕaàïμÄKpóÃóúμ÷Õû¼óËù¶è¼ÆDTÕyíóÂYòÇμÄËù¶è
#define Ki    0.008f
#define halfT 0.001f                   // 2éÑùÖüÆúμÄò»°ë￡¬óÃóúÇó½aËÄÔaêyÎ￠·Ö·½3ìê±¼ÆËã½ÇÔöá¿
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // 3õê¼×Ëì¬ËÄÔaêy
float exInt = 0, eyInt = 0, ezInt = 0; //μ±Ç°¼ó¼Æ2aμÃμÄÖØá|¼óËù¶èÔúèyÖáéÏμÄ·Öá¿óëóÃμ±Ç°×Ëì¬¼ÆËãμÃà′μÄÖØá|ÔúèyÖáéÏμÄ·Öá¿μÄÎó2îμÄ»y·Ö

float Yaw,Pitch,Roll;                      // Æ«o½½Ç￡¬¸©Ñö½Ç￡¬1ö×a½Ç

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)//g±íê¾íóÂYòÇ￡¬a±íê¾¼ó¼Æ
{

  float q0temp,q1temp,q2temp,q3temp; //ËÄÔaêyÔY′æ±äá¿￡¬Çó½aÎ￠·Ö·½3ìê±òaóÃ
  float norm;                         //ê¸á¿μÄÄ￡»òÕßËÄÔaêyμÄ·¶êy
  float vx, vy, vz;                   //μ±Ç°×Ëì¬¼ÆËãμÃà′μÄÖØá|ÔúèyÖáéÏμÄ·Öá¿
  float ex, ey, ez;                   //μ±Ç°×Ëì¬¼ÆËãμÃà′μÄÖØá|¼óËù¶èÔúèyÖáéÏμÄ·Öá¿￡¬óëμ±Ç°×Ëì¬¼ÆËãμÃà′μÄÖØá|ÔúèyÖá·Öá¿μÄÎó2î¡￡

  // Ïè°ÑÕaD©óÃμÃμ½μÄÖμËãoÃ
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q1q1 = q1*q1;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;

  if(ax*ay*az==0)    // ¼ó¼Æ′|óú×ÔóéÂäìå×′ì¬ê±￡¬2»½øDD×Ëì¬½aËã￡¬òòÎa»á2úéú·ÖÄ¸ÎTÇî′óμÄÇé¿ö
      return;

  norm = sqrt(ax*ax + ay*ay + az*az); // μ￥Î»»ˉ¼óËù¼Æ
  ax = ax /norm;                      // ÕaÑù±ä¸üáËá¿3ìò22»DèòaDT¸ÄKp2Îêy￡¬òòÎaÕaàï1éò»»ˉáË
  ay = ay / norm;
  az = az / norm;

  //μ±Ç°×Ëì¬¼ÆËã3öÖØá|Ôúèy¸öÖáéÏμÄ·Öá¿
  //2Î¿¼×ø±ênÏμ×a»ˉμ½ÔØìå×ø±êbÏμμÄóÃËÄÔaêy±íê¾μÄ·½ÏòóàÏò¾ØÕóμúèyáD
  vx = 2*(q1q3 - q0q2);
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3;

  //¼ÆËã2aμÃÖØá|óë¼ÆËãμÃμ½μÄÖØá|¼äμÄÎó2î￡¬Ïòá¿ía»y¿éòÔ±íê¾Õaò»Îó2î
  //Ô-ê¼Îòàí½aêÇòòÎaÕaá½¸öÏòá¿êÇμ￥Î»Ïòá¿Çòsin0 = 0￡»
  //2»1yòaêÇ¼D½ÇêÇ180¶èÄØ~Õa¸ö»1Ã»àí½a
  ex = (ay*vz - az*vy) ;
  ey = (az*vx - ax*vz) ;
  ez = (ax*vy - ay*vx) ;

  exInt = exInt + ex * Ki;          //¶ÔÎó2î½øDD»y·Ö
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;  //½«Îó2&#238

Ioó213￥μ½íóÂYòÇ￡¬¼′213￥áãμãÆˉòÆ
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt; //ÕaàïμÄgzóéóúÃ»óD1Û2aÕß½øDDD￡Õy»á2úéúÆˉòÆ￡¬±íÏÖ3öà′μÄ¾íêÇ»y·Ö×ÔÔö»òÕß×Ô¼õ

  //ÏÂÃæ½øDD×Ëì¬μÄ¸üDÂ￡¬ò2¾íêÇËÄÔaêyÎ￠·Ö·½3ìμÄÇó½a
  q0temp=q0; //ÔY′æμ±Ç°ÖμóÃóú¼ÆËã
  q1temp=q1; //íøéÏ′«μÄËã·¨′ó¶àÃ»óD×￠òaÕa¸öÎêìa￡¬Ôú′Ë¸üÕy
  q2temp=q2;
  q3temp=q3;

  //2éóÃò»½×±è¿¨½a·¨￡¬Ïà1ØÖaê¶¿éòÔ2Î¼û¡¶1ßDÔÆ÷¼tóëμ¼o½Ïμí3¡·P212
  q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
  q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
  q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
  q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;

  //μ￥Î»»ˉËÄÔaêyÔú¿Õ¼äDy×aê±2»»áà-éì￡¬½öóDDy×a½Ç¶è￡¬ÕaààËÆóúÏßDÔ′úêyàïÃæμÄÕy½»±ä»ˉ
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  //ËÄÔaêyμ½Å·à-½ÇμÄ×a»»
  //ÆäÖDYAWo½Ïò½Çóéóú¼óËù¶è¼Æ¶ÔÆäÃ»óDDTÕy×÷óÃ￡¬òò′Ë′Ë′|Ö±½óóÃíóÂYòÇ»y·Ö′úìæ
  Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // yaw
  Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
  Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
#endif
//====================================ò»½×»￥21ÂË2¨==============================

/*
* Brief       For ICM206XX get angular velocity pitch roll yaw
* Parameters *Temp_GyroX: get X-axis angular velocity
*             *Temp_GyroY: get Y-axis angular velocity
*             *Temp_GyrpZ: get Z-axis angular velocity
* Return None
* NOTE       None
*/
void ICM206XX_GetGyroVelocity(float *Temp_GyroX, float *Temp_GyroY, float *Temp_GyroZ)
{
int short test_acce[3];
int short Gyro_Offset[3];
unsigned char offset_SrcData[6];
int Sensitivity_X = 131;    //Gyro_Sensitivity = 131 LSB/(deg/s)
int Sensitivity_Y = 131;    //Gyro_Sensitivity = 131 LSB/(deg/s)
int Sensitivity_Z = 131;    //Gyro_Sensitivity = 131 LSB/(deg/s)

ICM206XX_GetAcceSrcData(test_acce);

ICM206XX_MultiReadReg(ICM20602_DEV_ADDR, ICM20602_XG_OFFS_USRH, offset_SrcData, 6);

Gyro_Offset[0] = (offset_SrcData[0] << 8) | offset_SrcData[1];
Gyro_Offset[1] = (offset_SrcData[2] << 8) | offset_SrcData[3];
Gyro_Offset[2] = (offset_SrcData[4] << 8) | offset_SrcData[5];

*Temp_GyroX = (test_acce[0] - Gyro_Offset[0])/(Sensitivity_X);
*Temp_GyroY = (test_acce[1] - Gyro_Offset[1])/(Sensitivity_Y);
*Temp_GyroZ = (test_acce[2] - Gyro_Offset[2])/(Sensitivity_Z);
}

/*
* Brief       For ICM206XX get acce pitch roll yaw
* Parameters *Temp_AcceX: get X-axis Acce velocity
*             *Temp_AcceY: get Y-axis Acce velocity
*             *Temp_AcceZ: get Z-axis Acce velocity
* Return None
* NOTE       None
*/
void ICM206XX_GetAcceVelocity(float *Temp_AcceX, float *Temp_AcceY, float *Temp_AcceZ)
{
int short test_acce[3];

ICM206XX_GetAcceSrcData(test_acce);

*Temp_AcceX = test_acce[0]/8192.0;
*Temp_AcceY = test_acce[1]/8192.0;
*Temp_AcceZ = test_acce[2]/8192.0;
}
//=====================================Test=====================================
/*
* Brief       For ICM206XX Test
* Parameters None
* Return None
* NOTE       None
*/
void ICM206XX_Test(void)
{
// unsigned char i;
// int short test_acce[3];
// int short test_gyro[3];
float AcceX,AcceY,AcceZ;
float GyroX,GyroY,GyroZ;

//ICM206XX_ReadWhoAmI();

// ICM206XX_GetAcceSrcData(test_acce);
// ICM206XX_GetGyroSrcData(test_gyro);

// //¶¨Öμ2aêÔêy¾YêÇ·ñìø¶ˉ￡¬èô2»ìø¶ˉ￡¬ÔòËÄÔaêyËã·¨Ã»Îêìa
// GyroX = 0x50;
// GyroY = 0x50;
// GyroZ = 0x50;
// AcceX = 0.9;
// AcceY = 0.9;
// AcceZ = 0.9;

ICM206XX_GetGyroVelocity(&GyroX, &GyroY, &GyroZ);
ICM206XX_GetAcceVelocity(&AcceX, &AcceY, &AcceZ);

// printf("Gyro[0] = %+5.1f, Gyro[1] = %+5.1f, Gyro[2] = %+5.1f, Acce[0] = %+5.1f, Acce[1] = %+5.1f, Acce[2] = %+5.1f ",
//          GyroX,GyroY,GyroZ, AcceX,AcceY,AcceZ);

IMUupdate(GyroX,GyroY,GyroZ, AcceX,AcceY,AcceZ);
//IIC_delay(IIC_DELAY_COUNT*100000);
printf("

itch = %+5.1f, Roll = %+5.1f, Yaw = %5.1f ",Pitch, Roll, Yaw);
}

友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。

该问题目前已经被作者或者管理员关闭, 无法添加新回复

1条回答

MkOaK

1楼-- · 2019-07-21 00:50

精彩回答 2 元偷偷看……

ICM20602 姿态解算

一周热门更多>

相关问题

相关文章

ICM20602 姿态解算

打开微信“扫一扫”，打开网页后点击屏幕右上角分享按钮

一周热门 更多>

相关问题

相关文章

采纳回答

编辑标签

举报内容

检举类型

检举原因

检举说明(必填)

一周热门更多>