MPU6050原始数据计算加速度、角速度、倾斜角度 |
您所在的位置:网站首页 › 计算倾斜角 › MPU6050原始数据计算加速度、角速度、倾斜角度 |
一、简介
依据MPU6050六轴运动传感器的原始数据,计算设备当前状态的加速度、角速度和倾斜角度。本项目的主控芯片是nRF52832(SDK:Nordic SDK 17.0.2.),但算法通用,读取原始数据的完整工程来自艾克姆,已上传个人主页。 二、MPU6050初始化和寄存器配置一般在唤醒MPU6050之前都会用MCU读一下设备地址以检测设备以及通信是否正常: // !< WHO_AM_I register identifies the device. Expected value is 0x68. #define ADDRESS_WHO_AM_I (0x75U) bool mpu6050_verify_product_id(void) { uint8_t who_am_i; if (mpu6050_register_read(ADDRESS_WHO_AM_I, &who_am_i, 1)) { if (who_am_i != MPU6050_WHO_AM_I) { return false; } else { return true; } } else { return false; } }上述函数返回true后,才可以开始配置寄存器初始化: #define MPU_PWR_MGMT1_REG 0x6B //电源管理寄存器1 #define MPU_SAMPLE_RATE_REG 0x19 //采样频率分频器 #define MPU_CFG_REG 0x1A //配置寄存器 #define MPU_INT_EN_REG 0x38 //中断使能寄存器 #define MPU_GYRO_CFG_REG 0x1B //陀螺仪配置寄存器 #define MPU_ACCEL_CFG_REG 0x1C //加速度计配置寄存器 //唤醒MPU6050 (void)mpu6050_register_write(MPU_PWR_MGMT1_REG , 0x00); //设置采样率(Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)):1KHz (void)mpu6050_register_write(MPU_SAMPLE_RATE_REG , 0x07); //设置低通滤波器,截止频率是1K,带宽是5K (void)mpu6050_register_write(MPU_CFG_REG , 0x06); //关闭中断 (void)mpu6050_register_write(MPU_INT_EN_REG, 0x00); //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s) (void)mpu6050_register_write(MPU_GYRO_CFG_REG , 0x18); //配置加速度传感器量程 +-2G s,不自检 (void)mpu6050_register_write(MPU_ACCEL_CFG_REG,0x00); 三、加速度、角速度计算公式 1.加速度某个轴的加速度 a = Range * G * ACC / 32768 . 其中G是当地重力加速度,ACC是该轴的加速度原始数据,Range是由配置加速度量程时写入的参数决定的,关系如下:
根据初始化时写入的数据,不自检,AFS_SEL = 00 = 0,这里Range = 2: //配置加速度传感器量程 +-2G s,不自检 (void)mpu6050_register_write(MPU_ACCEL_CFG_REG,0x00); 2.角速度某个轴的角速度 g = Range * Gyro / 32768 . Gyro是这个轴的角速度原始数据,Range是由配置角速度量程时写入的参数决定的,关系如下:
根据初始化时写入的数据,不自检,FS_SEL = 11 = 3,这里Range = 2000: //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s) (void)mpu6050_register_write(MPU_GYRO_CFG_REG , 0x18); 四、倾斜角度公式倾斜角度依据加速度原始数据计算,公式如下: #define x 0 #define z 1 Angle[x] = atan2(AccValue.X, AccValue.Y)*180/3.14f; Angle[z] = atan2(AccValue.Z, AccValue.Y)*180/3.14f;该公式适用的初始姿态是竖直的,也就是MPU6050的Y轴竖直向上时是初始状态,此时的角度为0。Angle[x] 是x轴与竖直平面的夹角度数,Angle[z] 是z轴与水平平面的夹角度数。 参考:(16条消息) 关于MPU6050的数据获取、分析与处理_acktomas的博客-CSDN博客_mpu6050读取 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |