一、簡(jiǎn)介
根據(jù)六軸運(yùn)動(dòng)傳感的原始數(shù)據(jù)，估算設(shè)備當(dāng)前狀態(tài)的加速度、角速率和傾斜角度。本項(xiàng)目的主控芯片是（SDK：SDK17.0.2.），但算法通用，讀取原始數(shù)據(jù)的完整工程來(lái)自艾克姆，已上傳個(gè)人主頁(yè)。
二、初始化和寄存器配置
通常在喚起之前就會(huì)用MCU讀一下設(shè)備地址以監(jiān)測(cè)設(shè)備以及通訊是否正常：
// !< 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;
}
}上述函數(shù)返回true后加速度的五個(gè)公式，才可以開(kāi)始配置寄存器初始化：
#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 //加速度計(jì)配置寄存器
//喚醒MPU6050
(void)mpu6050_register_write(MPU_PWR_MGMT1_REG , 0x00);
//設(shè)置采樣率(Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))：1KHz
(void)mpu6050_register_write(MPU_SAMPLE_RATE_REG , 0x07);
//設(shè)置低通濾波器，截止頻率是1K，帶寬是5K
(void)mpu6050_register_write(MPU_CFG_REG , 0x06);
//關(guān)閉中斷
(void)mpu6050_register_write(MPU_INT_EN_REG, 0x00);
//陀螺儀自檢及測(cè)量范圍，典型值：0x18(不自檢，2000deg/s)
(void)mpu6050_register_write(MPU_GYRO_CFG_REG , 0x18);
//配置加速度傳感器量程 +-2G s，不自檢
(void)mpu6050_register_write(MPU_ACCEL_CFG_REG,0x00); 三、加速度、角速率估算公式1.加速度
某個(gè)軸的加速度a=Range*G*ACC/32768.其中G是當(dāng)?shù)刂亓铀俣龋珹CC是該軸的加速度原始數(shù)據(jù)，Range是由配置加速度阻值時(shí)寫入的參數(shù)決定的，關(guān)系如下：
按照初始化時(shí)寫入的數(shù)據(jù)，不自檢，=00=0，這兒Range=2：
//配置加速度傳感器量程 +-2G s，不自檢
(void)mpu6050_register_write(MPU_ACCEL_CFG_REG,0x00); 2.角速率
某個(gè)軸的角速率g=Range*Gyro/32768.Gyro是這個(gè)軸的角速率原始數(shù)據(jù)，Range是由配置角速率阻值時(shí)寫入的參數(shù)決定的，關(guān)系如下：
按照初始化時(shí)寫入的數(shù)據(jù)，不自檢，=11=3，這兒Range=2000：
//陀螺儀自檢及測(cè)量范圍，典型值：0x18(不自檢，2000deg/s)
(void)mpu6050_register_write(MPU_GYRO_CFG_REG , 0x18);四、傾斜角度公式
傾斜角度根據(jù)加速度原始數(shù)據(jù)估算，公式如下：
#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;該公式適用的初始姿態(tài)是豎直的，也就是的Y軸豎直向下時(shí)是初始狀態(tài)加速度的五個(gè)公式，此時(shí)的角度為0。Angle[x]是x軸與豎直平面的傾角度數(shù)，Angle[z]是z軸與水平平面的傾角度數(shù)。
參考：(16條消息)關(guān)于的數(shù)據(jù)獲取、分析與處理的博客-CSDN博客讀取