常用的六轴陀螺仪有MPU6050,icm-20602。MPU6050基本上只用软件IIC驱动，速率较慢，数据漂移也相对大一点。
以MPU6050为例。
软件IIC驱动 -> MPU6050寄存器基本配置 -> 读取原始数据 -> 将原始数据滤波后使用。
原始数据可以使用互补滤波，卡尔曼滤波，解算四元数来进行解算成欧拉角信息。
目前文章使用的是自结算四元数。
//内部使用，用户无需调用
static void mpu6050_simiic_start(void)
{
SDA_Out();
MPU6050_SDA_HIGH();
MPU6050_SCL_HIGH();
DELAY_US(4);
MPU6050_SDA_LOW();
DELAY_US(4);
MPU6050_SCL_LOW();
}
//内部使用，用户无需调用
static void mpu6050_simiic_stop(void)
{
SDA_Out();
MPU6050_SCL_LOW();
MPU6050_SDA_LOW();
DELAY_US(4);
MPU6050_SCL_HIGH();
MPU6050_SDA_HIGH();
DELAY_US(4);
}
//主应答(包含ack:SDA=1和no_ack:SDA=0)
//内部使用，用户无需调用
static void mpu6050_simiic_sendack(unsigned char ack_dat)
{
if(ack_dat)
{
MPU6050_SCL_LOW();
SDA_Out();
MPU6050_SDA_LOW();
DELAY_US(2);
MPU6050_SCL_HIGH();
DELAY_US(2);
MPU6050_SCL_LOW();
}
else
{
MPU6050_SCL_LOW();
SDA_Out();
MPU6050_SDA_HIGH();
DELAY_US(2);
MPU6050_SCL_HIGH();
DELAY_US(2);
MPU6050_SCL_LOW();
}
}
static bool mpu6050_sccb_waitack(void)
{
uint8_t waittime = 0;
SDA_In();
MPU6050_SDA_HIGH();
DELAY_US(1);
MPU6050_SCL_HIGH();
DELAY_US(1);
while(GET_MPU6050_SDA) //应答为高电平，异常，通信失败
{
waittime++;
if(waittime > 250)
{
mpu6050_simiic_stop();
return 1;
}
}
MPU6050_SCL_LOW();
return 0;
}
//字节发送程序
//发送c(可以是数据也可是地址)，送完后接收从应答
//不考虑从应答位
//内部使用，用户无需调用
static void mpu6050_send_ch(uint8_t c)
{
uint8_t i = 0;
SDA_Out();
MPU6050_SCL_LOW();//拉低时钟开始数据传输
for(i=0;i<8;i++)
{
if((( c >> 7) & 0x01) == 0x01)
{
MPU6050_SDA_HIGH();//SDA 输出数据
}
else
{
MPU6050_SDA_LOW();
}
c <<= 1;
DELAY_US(2);
MPU6050_SCL_HIGH(); //SCL 拉高，采集信号
DELAY_US(2);
MPU6050_SCL_LOW(); //SCL 时钟线拉低
DELAY_US(2);
}
//mpu6050_sccb_waitack();
}
//字节接收程序
//接收器件传来的数据，此程序应配合|主应答函数|使用
//内部使用，用户无需调用
static uint8_t mpu6050_read_ch(uint8_t ack_x)
{
uint8_t i;
uint8_t c = 0;
SDA_In();
//MPU6050_SCL_LOW(); //置时钟线为低，准备接收数据位
//DELAY_US(2);
//MPU6050_SCL_HIGH(); //置时钟线为高，使数据线上数据有效
for(i=0;i<8;i++)
{
MPU6050_SCL_LOW(); //置时钟线为低，准备接收数据位
DELAY_US(2);
MPU6050_SCL_HIGH(); //置时钟线为高，使数据线上数据有效
c <<= 1;
if(GET_MPU6050_SDA)
{
c++; //读数据位，将接收的数据存c
}
DELAY_US(2);
}
//MPU6050_SCL_LOW(); //置时钟线为低，准备接收数据位
//DELAY_US(2);
mpu6050_simiic_sendack(ack_x);
return c;
}//-------------------------------------------------------------------------------------------------------------------
// @brief 模拟IIC写数据到设备寄存器函数
// @param dev_add 设备地址(低七位地址)
// @param reg 寄存器地址
// @param dat 写入的数据
// @return void
// @since v1.0
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
static void mpu6050_simiic_write_reg(uint8_t dev_add, uint8_t reg, uint8_t dat)
{
//writeByteI2C(&soft_i2c,(dev_add<<1),reg,dat);
mpu6050_simiic_start();
mpu6050_send_ch( (dev_add<<1) | 0x00); //发送器件地址加写位
mpu6050_sccb_waitack();
mpu6050_send_ch( reg ); //发送从机寄存器地址
mpu6050_sccb_waitack();
mpu6050_send_ch( dat ); //发送需要写入的数据
mpu6050_sccb_waitack();
mpu6050_simiic_stop();
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 模拟IIC从设备寄存器读取数据
// @param dev_add 设备地址(低七位地址)
// @param reg 寄存器地址
// @param type 选择通信方式是IIC 还是 SCCB
// @return uint8 返回寄存器的数据
// @since v1.0
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
uint8_t mpu6050_simiic_read_reg(uint8_t dev_add, uint8_t reg)
{
uint8_t data;
//readByteI2C(&soft_i2c,dev_add,reg,&data);
mpu6050_simiic_start();
mpu6050_send_ch( (dev_add<<1) | 0x00); //发送器件地址加写位
mpu6050_sccb_waitack();
mpu6050_send_ch( reg ); //发送从机寄存器地址
mpu6050_sccb_waitack();
mpu6050_simiic_stop();
mpu6050_simiic_start();
mpu6050_send_ch( (dev_add<<1) | 0x01); //发送器件地址加读位
mpu6050_sccb_waitack();
data = mpu6050_read_ch(no_ack); //读取数据
mpu6050_simiic_stop();
return data;
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 模拟IIC读取多字节数据
// @param dev_add 设备地址(低七位地址)
// @param reg 寄存器地址
// @param dat_add 数据保存的地址指针
// @param num 读取字节数量
// @param type 选择通信方式是IIC 还是 SCCB
// @return uint8 返回寄存器的数据
// @since v1.0
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_simiic_read_regs(uint8_t dev_add, uint8_t reg, uint8_t *dat_add, uint8_t num)
{
mpu6050_simiic_start();
mpu6050_send_ch( (dev_add<<1) | 0x00); //发送器件地址加写位
mpu6050_sccb_waitack();
mpu6050_send_ch( reg ); //发送从机寄存器地址
mpu6050_sccb_waitack();
//mpu6050_simiic_stop();
mpu6050_simiic_start();
mpu6050_send_ch( (dev_add<<1) | 0x01); //发送器件地址加读位
mpu6050_sccb_waitack();
while(--num)
{
*dat_add = mpu6050_read_ch(ack); //读取数据
dat_add++;
}
*dat_add = mpu6050_read_ch(no_ack); //读取数据
mpu6050_simiic_stop();
}
//-------------------------------------------------------------------------------------------------------------------
// @brief MPU6050自检函数
// @param NULL
// @return void
// @since v1.0
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
static bool mpu6050_self1_check(void)
{
printf("ID:0x%x\n",mpu6050_simiic_read_reg(MPU6050_DEV_ADDR, WHO_AM_I));
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, PWR_MGMT_1, 0x00); //解除休眠状态
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, SMPLRT_DIV, 0x07); //125HZ采样率
if(0x07 != mpu6050_simiic_read_reg(MPU6050_DEV_ADDR, SMPLRT_DIV))
{
printf("mpu6050 init error. %d\r\n",mpu6050_simiic_read_reg(MPU6050_DEV_ADDR, SMPLRT_DIV));
return false;
//卡在这里原因有以下几点
//1 MPU6050坏了，如果是新的这样的概率极低
//2 接线错误或者没有接好
//3 可能你需要外接上拉电阻，上拉到3.3V
//4 可能没有调用模拟IIC的初始化函数
}
return true;
}
bool mpu6050_init(void)
{
DELAY_MS(800);
SysCtlPeripheralEnable(SDA_Clock);
//2ma，上拉
GPIOPadConfigSet(SDA_Port,SDA_Pin,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPU);
//输出
GPIODirModeSet(SDA_Port,SDA_Pin,GPIO_DIR_MODE_OUT);
SysCtlPeripheralEnable(SCL_Clock);
//2ma，开漏
GPIOPadConfigSet(SCL_Port,SCL_Pin,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_OD);
//输出
GPIODirModeSet(SCL_Port,SCL_Pin,GPIO_DIR_MODE_OUT);
if(mpu6050_self1_check() == false)
{
return false;
}
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, PWR_MGMT_1, 0x00); //解除休眠状态
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, SMPLRT_DIV, 0x07); //125HZ采样率
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, MPU6050_CONFIG, 0x04); //
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, GYRO_CONFIG, 0x18); //2000
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, ACCEL_CONFIG, 0x10); //8g
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, User_Control, 0x00);
mpu6050_simiic_write_reg(MPU6050_DEV_ADDR, INT_PIN_CFG, 0x02);
mpu6050_gyro_x=0;
mpu6050_gyro_y=0;
mpu6050_gyro_z=0;
//开启滤波参数
IIR_imu();
return true;
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 获取MPU6050加速度计数据
// @param NULL
// @return void
// @since v1.0
// Sample usage: 执行该函数后，直接查看对应的变量即可
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_get_accdata(void)
{
uint8_t dat[6];
mpu6050_simiic_read_regs(MPU6050_DEV_ADDR, ACCEL_XOUT_H, dat, 6);
mpu6050_acc_x = (int16_t)(((uint16_t)dat[0]<<8 | dat[1]));
mpu6050_acc_y = (int16_t)(((uint16_t)dat[2]<<8 | dat[3]));
mpu6050_acc_z = (int16_t)(((uint16_t)dat[4]<<8 | dat[5]));
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 获取MPU6050陀螺仪数据
// @param NULL
// @return void
// @since v1.0
// Sample usage: 执行该函数后，直接查看对应的变量即可
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_get_gyro(void)
{
//所有
uint8_t dat[6];
mpu6050_simiic_read_regs(MPU6050_DEV_ADDR, GYRO_XOUT_H, dat, 6);
mpu6050_gyro_x = (int16_t)(((uint16_t)dat[0]<<8 | dat[1]));
mpu6050_gyro_y = (int16_t)(((uint16_t)dat[2]<<8 | dat[3]));
mpu6050_gyro_z = (int16_t)(((uint16_t)dat[4]<<8 | dat[5]));
}
/*
* 函数名：get_iir_factor
* 描述 ：求取IIR滤波器的滤波因子
* 输入 ：out_factor滤波因子首地址，Time任务执行周期，Cut_Off滤波截止频率
* 返回 ：
*/
void get_iir_factor(float *out_factor,float Time, float Cut_Off)
{
*out_factor = Time /( Time + 1/(2.0f * PI * Cut_Off) );
}
/* 获取IIR低通滤波 */
void IIR_imu(void)
{
int8_t i;
for(i=0;i<=100;i++)
{
mpu6050_get_gyro();
set.gyro.x+=mpu6050_gyro_x;
set.gyro.y+=mpu6050_gyro_y;
set.gyro.z+=mpu6050_gyro_z;
}
set.gyro.x/= 100;
set.gyro.y/= 100;
set.gyro.z/= 100;
set.offset_flag = 1;
//printf("%d\n",set.offset_flag);
get_iir_factor(&imu.att_acc_factor,imu_Read_Time,15);
get_iir_factor(&imu.att_gyro_factor,imu_Read_Time,10);
}
/**
* @brief IIR低通滤波器
* @param *acc_in 输入三轴数据指针变量
* @param *acc_out 输出三轴数据指针变量
* @param lpf_factor 滤波因数
* @retval x
*/
float iir_lpf(float in,float out,float lpf_factor)
{
out = out + lpf_factor * (in - out);
return out;
}
// 快速开根号算法
float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
/*
* 函数名：mahony_update
* 描述 ：姿态解算
* 输入 ：陀螺仪三轴数据（单位：弧度/秒），加速度三轴数据（单位：g）
* 返回 ：
*/
//Gyroscope units are radians/second, accelerometer units are irrelevant as the vector is normalised.
void mahony_update(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
if(ax*ay*az==0)
return;
gx=gx*(PI / 180.0f);
gy=gy*(PI / 180.0f);
gz=gz*(PI / 180.0f);
//[ax,ay,az]是机体坐标系下加速度计测得的重力向量(竖直向下)
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//VectorA = MatrixC * VectorB
//VectorA ：参考重力向量转到在机体下的值
//MatrixC ：地理坐标系转机体坐标系的旋转矩阵
//VectorB ：参考重力向量（0,0,1）
//[vx,vy,vz]是地理坐标系重力分向量[0,0,1]经过DCM旋转矩阵(C(n->b))计算得到的机体坐标系中的重力向量(竖直向下)
vx = Mat.DCM_T[0][2];
vy = Mat.DCM_T[1][2];
vz = Mat.DCM_T[2][2];
//机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’
//利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
//实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
//但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
//两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
//加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
//也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。
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;
//姿态误差补偿到角速度上，修正角速度积分漂移，通过调节Kp、Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
gx = gx + kp*ex + exInt;
gy = gy + kp*ey + eyInt;
gz = gz + kp*ez + ezInt;
//一阶龙格库塔法更新四元数
q0 = q0 + (-q1*gx - q2*gy - q3*gz)* MahonyPERIOD * 0.0005f;
q1 = q1 + ( q0*gx + q2*gz - q3*gy)* MahonyPERIOD * 0.0005f;
q2 = q2 + ( q0*gy - q1*gz + q3*gx)* MahonyPERIOD * 0.0005f;
q3 = q3 + ( q0*gz + q1*gy - q2*gx)* MahonyPERIOD * 0.0005f;
//把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
//printf("%f,%f,%f,%f\n",q0,q1,q2,q3);
static float pitch_old=0, roll_old=0,yaw_old=0;
float temp = 0;
//四元素转欧拉角
imu.pitch = atan2f(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * (180.0f / PI);
imu.roll = -asinf(2.0f*(q0*q2 - q1*q3)) * (180.0f / PI);
//z轴角速度积分的偏航角
imu.yaw += imu.deg_s.z * MahonyPERIOD * 0.001f;
//防温漂
temp = imu.pitch-pitch_old;
if(temp<2 && temp > -2)
{
imu.pitch = pitch_old;
}
else
{
pitch_old = imu.pitch;
}
temp = imu.yaw-yaw_old;
if(temp<0.1f && temp > -0.1f)
{
imu.yaw = yaw_old;
}
else
{
yaw_old = imu.yaw;
}
/*
* 函数名：rotation_matrix
* 描述 ：旋转矩阵：机体坐标系 -> 地理坐标系
* 输入 ：
* 返回 ：
*/
void rotation_matrix(void)
{
Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);
Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);
Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}
/*
* 函数名：rotation_matrix_T
* 描述 ：旋转矩阵的转置矩阵：地理坐标系 -> 机体坐标系
* 输入 ：
* 返回 ：
*/
void rotation_matrix_T(void)
{
Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);
Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2);
Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);
Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}
/*
* 函数名：Matrix_ready
* 描述 ：矩阵更新准备，为姿态解算使用
* 输入 ：
* 返回 ：
*/
void Matrix_ready(void)
{
rotation_matrix(); //旋转矩阵更新
rotation_matrix_T(); //旋转矩阵的逆矩阵更新
}
/***********************************************************
函数名称：void IMU(void)
函数功能：获得姿态结算后的值
入口参数：无
出口参数：无
备 注：直接读取imu.pitch imu.roll imu.yaw
***********************************************************/
void IMU(void)
{
if(set.offset_flag)
{
/*获取X、Y的角速度和加速度*/
mpu6050_get_accdata();
mpu6050_get_gyro();
/*滤波算法*/
mpu6050_gyro_x-=set.gyro.x;
mpu6050_gyro_y-=set.gyro.y;
mpu6050_gyro_z-=set.gyro.z;
acc_x = iir_lpf(mpu6050_acc_x,acc_x,imu.att_acc_factor);
acc_y = iir_lpf(mpu6050_acc_y,acc_y,imu.att_acc_factor);
acc_z = iir_lpf(mpu6050_acc_z,acc_z,imu.att_acc_factor);
//printf("%.2f,%.2f,%.2f\n",acc_x,acc_y,acc_z);
gyro_x =iir_lpf(mpu6050_gyro_x,gyro_x,imu.att_gyro_factor);
gyro_y =iir_lpf(mpu6050_gyro_y,gyro_y,imu.att_gyro_factor);
gyro_z =iir_lpf(mpu6050_gyro_z,gyro_z,imu.att_gyro_factor);
//printf("%d,%d,%d\n",gyro_x,gyro_y,gyro_z);
//=================重力补偿版
/*acc_x = (float)mpu6050_acc_x * Acc_Gain * G;
acc_y = (float)mpu6050_acc_y * Acc_Gain * G;
acc_z = (float)mpu6050_acc_z * Acc_Gain * G;
gyro_x = (float)mpu6050_gyro_x * Gyro_Gain;
gyro_y = (float)mpu6050_gyro_y * Gyro_Gain;
gyro_z = (float)mpu6050_gyro_z * Gyro_Gain;
//-----------------IIR滤波
acc_x = acc_x*Kp_New + acc_x_old *Kp_Old;
acc_y = acc_y*Kp_New + acc_y_old *Kp_Old;
acc_z = acc_z*Kp_New + acc_z_old *Kp_Old;
acc_x_old = acc_x;
acc_y_old = acc_y;
acc_z_old = acc_z;
IMUupdate(acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,&imu);*/
//===============================
/*数据存储*/
imu.acc_g.x = (float)acc_x/4096; //加速度计量程为:±8g/4096, ±16g/2048, ±4g/8192, ±2g/16384
imu.acc_g.y = (float)acc_y/4096;
imu.acc_g.z = (float)acc_z/4096;
imu.deg_s.x = (float)mpu6050_gyro_x/16.4f;//陀螺仪量程为:±2000dps/16.4, ±1000dps/32.8, ±500 dps /65.6
imu.deg_s.y = (float)mpu6050_gyro_y/16.4f;//±250 dps/131.2, ±125 dps/262.4
imu.deg_s.z = (float)mpu6050_gyro_z/16.4f;
//卡尔曼姿态解算
//imu.roll = -Kalman_Filter_x(imu.acc_g.x,imu.deg_s.x);
//imu.pitch = -Kalman_Filter_y(imu.acc_g.y,imu.deg_s.y);
//imu.yaw = -Kalman_Filter_x(imu.acc_g.z,imu.deg_s.z);
/*姿态解算*/
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);
Matrix_ready();
}
}#define imu_Read_Time 0.01f//原始数据采集时间间隔 秒为单位 0.01秒 10ms
#define MahonyPERIOD 10.0f//姿态解算周期（ms）