首页 > 其他分享 >陀螺仪的使用及四元数解算(MPU6050为例)

陀螺仪的使用及四元数解算(MPU6050为例)

时间:2023-08-06 17:44:23浏览次数:56  
标签:acc q1 q3 q2 为例 gyro 数解算 MPU6050 imu

陀螺仪的介绍

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

陀螺仪的使用

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

代码示例

模拟iic部分

//内部使用,用户无需调用
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;
}

MPU6050模块初始化部分

//-------------------------------------------------------------------------------------------------------------------
//  @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();
    }
}

注意部分!

解算四元数时,读取的时间间隔必须是一定的!需要修改自己的宏定义!最好是定时器立时间FLAG定时调用

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

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

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

标签:acc,q1,q3,q2,为例,gyro,数解算,MPU6050,imu
From: https://www.cnblogs.com/songmingze/p/17609666.html

相关文章

  • TwinCAT3 Database Server 模块的使用步骤(以MySQL为例)
    1.首先安装Mysql和Twincat3TF6420-Database-Server.exe2.在Mysql中创建数据库,以测试为目的,所以简单创建了两个 3.Twincat3可以在项目中添加,或者可以直接在菜单栏的Configurator中配置 连接的数据库的类型为NET_MySQL,由于拓扑是均基于一台IPC,所以可以选择localhost,数据库......
  • 贝叶斯网络python实战(以泰坦尼克号数据集为例,pgmpy库)
    贝叶斯网络python实战(以泰坦尼克号数据集为例,pgmpy库)leida_wt 2019-03-2423:05:36  16815  收藏 140分类专栏: 机器学习 文章标签: pgmpy 贝叶斯网络 泰坦尼克 机器学习 图网络版权 文章目录贝叶斯网络简介贝叶斯推断思路贝叶斯网络贝叶斯网络的实现应用步骤泰坦尼克......
  • 小米/红米关闭VoLTE图标(信号旁的HD),以K50为例
    原文:https://zhuanlan.zhihu.com/p/508684071  小米/红米关闭VoLTE图标(信号旁的HD),以K50为例行也思君夜深知雪重,时闻折竹声 45人赞同了该文章新版小米系统中应运营商要求已经隐藏了VoLTE功能的开关,如果使用卡2做为上网卡,HD图标会出现在......
  • 三维空间中的刚体运动、MPU6050、DMP姿态解算、卡尔曼滤波
    坐标系空间中三个正交的轴组成,构成线性空间的一组基($......
  • ModelWhale 推动数据开放、跨学科协同及产学研一体,以遥感领域为例
     2023年3月,科技部会同自然科学基金委启动“人工智能驱动的科学研究(AIforScience)”专项部署工作,布局“人工智能驱动的科学研究”前沿科技研发体系。对此,中国科学院院士、北京大学国际机器学习研究中心主任鄂维南认为,AIforScience是“以机器学习为代表的人工智能技术”与“科学......
  • 岩土工程振动在线监测:以道路桥梁基础为例
    岩土工程振动在线监测:以道路桥梁基础为例使用振弦传感器、采集仪和在线监测系统进行岩土工程监测:以道路桥梁基础振动监测为例一个应用振弦传感器、振弦采集仪和在线监测系统构成的岩土工程监测案例是道路桥梁基础的振动监测。 在道路桥梁基础的振动监测方面,振弦传感器可以用......
  • 基于瑞芯微平台cif接口dvp相机的视频接入(ov2640、rv1126为例)
    名词定义CIF,指RK芯片中的VIP模块,用以接收Sensor数据并保存到Memory中,仅转存数据,无ISP功能DVP,一种并行数据传输接口,即DigitalVideoPortHSYNC,指DVP接口的行同步信号PCLK,指Sensor输出PixelClockVSYNC,指DVP接口的场同步信号V4L2,即Video4Linux2,Linuxkernel的视频处理模块 ......
  • Linux下PAM认证详解(以centos7为例)
    Linux下PAM认证详解(以centos7为例)PAM简介(PluggableAuthenticationModules,可插拔认证模块) Sun公司于1995年开发的一种与认证相关的通用框架机制:PAM(可插拔认证模块)是实现认证工作的一个模块。     因为每个服务都用到不同的认证方式,所以就需要不同的认证库。  认......
  • 在本地搭建小程序服务器,以MacOS为例
    在本地搭建小程序服务器,以MacOS为例已知小程序的请求url需要为域名,并且为https。不过可以开启请求ip地址和只使用http(需要在小程序开发工具中开启“不校验合法域名。。。HTTPS证书。。。”)但是还是不能使用本机ip。想实现的效果:就和前后端分离一样,前端直接访问本地的后端服务......
  • Windows电脑为例,设置minio文件服务分布式部署
    前言关于分布式文件存储,之前我也是使用fastdfs,那为什么突然选择minio?1、它可以多平台部署2、搭建起来不是很复杂3、github近30K的star什么是minio?构建高性能的云原生数据机器学习,大数据分析,海量存储的基础架构MinIO支持各种应用程序数据工作负载在中国:阿里巴巴、腾讯、百......