1 基于平台
1.硬件平台：nRF52832
2:软件sdk：nRF5_SDK_14.2.0_17b948a
3：参考的博客链接
地址1
地址2
地址3
地址4
2 使用I2C接口驱动
参考之前博客，已编写完成且测试，可以直接移植调用
模拟I2C地址
3 MPU6050运动中断相关寄存器
由于这样或那样的原因，MPU6050的技术文档中省略了很多寄存器，而这些被刻意省略的寄存器往往涉及到各类高级应用，如较为常用的运动中断（自由落体中断、加速度中断和静止中断）、内部DMP数据融合和内部FIFO等。在此感谢运用反编译等技术手段整理出相关寄存器和DMP驱动的各位大神。
除基本寄存器（PWR_MGMT、CONFIG、ACCEL_CONFIG等）之外，运动中断应用所涉及到的相关内部寄存器主要包括：
1: 中断使能寄存器：INT_ENABLE ，寄存器地址0x38，为MPU6050所有中断输出的开关寄存器，用于使能运动中断、FIFO溢出中断和数据中断等；
2 FIFO控制寄存器：USER_CTRL，寄存器地址0x6A，该寄存器用于使能FIFO，并可控制MPU6050的I2C主机。应用运动中断功能时，应关闭FIFO和I2C主机；
3 FIFO使能寄存器：FIFO_EN，寄存器地址0x23，该寄存器用于使能各个FIFO功能，在应用运动中断功能时应关闭；
4 中断引脚配置寄存器：INT_PIN_CFG，寄存器地址0x37，该寄存器用于设置INT中断引脚的电平标准和驱动方式（推挽、开漏）等；
5 运动状态寄存器：MOT_DETECT_STATUS，寄存器地址0x61，该寄存器用于在触发运动中断（自由落体中断、加速度中断和静止中断）时标示中断的类型。
6 自由落体中断阈值寄存器：FF_THR，寄存器地址0x1D，当使能自由落体中断时，该寄存器的值决定中断的触发阈值，数值越高，对自由落体的检测越不灵敏；
7 自由落体中断时间寄存器：FF_DUR，寄存器地址0x1E，当使能自由落体中断时，该寄存器的值决定中断的持续时间阈值，当自由落体的持续时间达到设定阈值时触发中断。数值越高，对自由落体检测的时间阈值越长；
8 加速度中断阈值寄存器：MOT_THR，寄存器地址0x1F，当使能加速度中断时，该寄存器的值决定中断的触发阈值，数值越高，触发中断所需的加速度越大。
9 加速度中断时间寄存器：MOT_DUR，寄存器地址0x20，当使能加速度中断时，该寄存器的值决定中断的持续时间阈值，当加速度值持续时间达到设定阈值时触发中断。数值越高，触发中断所需的加速度持续时间越长；
10 静止中断阈值寄存器：ZRMOT_THR，寄存器地址0x21，当使能静止中断时，若当前加速度计输出的三轴值均小于静止中断阈值，则产生静止中断。数值越高，触发静止中断的要求越宽松。
11 静止中断时间寄存器：ZRMOT_DUR，寄存器地址0x22，当使能静止中断时，该寄存器的值决定触发静止中断所需的持续时间，数值越高，触发中断所需的静止持续时间越长。
除特殊说明外，各个寄存器均为逻辑1使能
中断使能寄存器：INT_ENABLE
FIFO控制寄存器：USER_CTRL
FIFO使能寄存器：FIFO_EN
中断引脚配置寄存器：INT_PIN_CFG
运动状态寄存器：MOT_DETECT_STATUS
自由落体中断阈值寄存器：FF_THR
4代码示例
.h文件
#ifndef mpu6050_h_
#define mpu6050_h_
#include "stdint.h"
#define MPU6050_SDA_PIN 16
#define MPU6050_SCL_PIN 15
#define MPU6050_ADDRESS_AD0_LOW 0xD0
#define MPU6050_ADDRESS_AD0_HIGH 0XD1
#define MPU_ADDR 0xD0
/*lint ++flb "Enter library region" */
#define ADDRESS_WHO_AM_I (0x75U) // !< WHO_AM_I register identifies the device. Expected value is 0x68.
#define ADDRESS_SIGNAL_PATH_RESET (0x68U) // !<
#define MPU6050_ADDRESS 0x69
#define MPU6050_GYRO_OUT 0x43
#define MPU6050_ACC_OUT 0x3B
#define MPU_SELF_TESTX_REG 0X0D //×Ô¼ì¼Ä´æÆ÷X
#define MPU_SELF_TESTY_REG 0X0E //×Ô¼ì¼Ä´æÆ÷Y
#define MPU_SELF_TESTZ_REG 0X0F //×Ô¼ì¼Ä´æÆ÷Z
#define MPU_SELF_TESTA_REG 0X10 //×Ô¼ì¼Ä´æÆ÷A
#define MPU_SAMPLE_RATE_REG 0X19 //²ÉÑùƵÂÊ·ÖƵÆ÷
#define MPU_CFG_REG 0X1A //ÅäÖüĴæÆ÷
#define MPU_GYRO_CFG_REG 0X1B //27:ÍÓÂÝÒÇÅäÖüĴæÆ÷
#define MPU_ACCEL_CFG_REG 0X1C //28¼ÓËٶȼÆÅäÖüĴæÆ÷
#define MPU_FF_THR 0X1D //×ÔÓÉÂäÌå¼ì²âÖжÏãÐÖµ
#define MPU_FF_DUR 0X1E //×ÔÓÉÂäÌåÖжÏʼþ
#define MPU_MOTION_DET_REG 0X1F //Ô˶¯¼ì²â·§ÖµÉèÖüĴæÆ÷
#define MPU_MOT_DUR_REG 0X20 //Ô˶¯ÖжÏʱ¼ä
#define MPU_ZRMOT_THR_REG 0X21 //¾²Ö¹¼ì²âãÐÖµ
#define MPU_ZRMOT_DUR_REG 0X22 //¾²Ö¹¼ì²âʱ¼ä
#define MPU_FIFO_EN_REG 0X23 //FIFOʹÄܼĴæÆ÷
#define MPU_I2CMST_CTRL_REG 0X24 //IICÖ÷»ú¿ØÖƼĴæÆ÷
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC´Ó»ú0Æ÷¼þµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV0_REG 0X26 //IIC´Ó»ú0Êý¾ÝµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC´Ó»ú0¿ØÖƼĴæÆ÷
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC´Ó»ú1Æ÷¼þµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV1_REG 0X29 //IIC´Ó»ú1Êý¾ÝµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC´Ó»ú1¿ØÖƼĴæÆ÷
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC´Ó»ú2Æ÷¼þµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV2_REG 0X2C //IIC´Ó»ú2Êý¾ÝµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC´Ó»ú2¿ØÖƼĴæÆ÷
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC´Ó»ú3Æ÷¼þµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV3_REG 0X2F //IIC´Ó»ú3Êý¾ÝµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC´Ó»ú3¿ØÖƼĴæÆ÷
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC´Ó»ú4Æ÷¼þµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV4_REG 0X32 //IIC´Ó»ú4Êý¾ÝµØÖ·¼Ä´æÆ÷
#define MPU_I2CSLV4_DO_REG 0X33 //IIC´Ó»ú4дÊý¾Ý¼Ä´æÆ÷
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC´Ó»ú4¿ØÖƼĴæÆ÷
#define MPU_I2CSLV4_DI_REG 0X35 //IIC´Ó»ú4¶ÁÊý¾Ý¼Ä´æÆ÷
//#define MPU_PWR_MGMT1_REG 0X6B //µçÔ´¹ÜÀí¼Ä´æÆ÷1
//#define MPU_PWR_MGMT2_REG 0X6C //µçÔ´¹ÜÀí¼Ä´æÆ÷2
#define MPU_I2CMST_STA_REG 0X36 //IICÖ÷»ú״̬¼Ä´æÆ÷
#define MPU_INTBP_CFG_REG 0X37 //ÖжÏ/ÅÔ·ÉèÖüĴæÆ÷
#define MPU_INT_EN_REG 0X38 //ÖжÏʹÄܼĴæÆ÷
#define MPU_INT_STA_REG 0X3A //ÖжÏ״̬¼Ä´æÆ÷
#define MPU_I2CMST_DELAY_REG 0X67 //IICÖ÷»úÑÓʱ¹ÜÀí¼Ä´æÆ÷
#define MPU_SIGPATH_RST_REG 0X68 //ÐźÅͨµÀ¸´Î»¼Ä´æÆ÷
#define MPU_MDETECT_CTRL_REG 0X69 //Ô˶¯¼ì²â¿ØÖƼĴæÆ÷
#define MPU_USER_CTRL_REG 0X6A //Óû§¿ØÖƼĴæÆ÷
#define MPU_PWR_MGMT1_REG 0X6B //µçÔ´¹ÜÀí¼Ä´æÆ÷1
#define MPU_PWR_MGMT2_REG 0X6C //µçÔ´¹ÜÀí¼Ä´æÆ÷2
#define MPU_FIFO_CNTH_REG 0X72 //FIFO¼ÆÊý¼Ä´æÆ÷¸ß°Ëλ
#define MPU_FIFO_CNTL_REG 0X73 //FIFO¼ÆÊý¼Ä´æÆ÷µÍ°Ëλ
#define MPU_FIFO_RW_REG 0X74 //FIFO¶Áд¼Ä´æÆ÷
#define MPU_DEVICE_ID_REG 0X75 //Æ÷¼þID¼Ä´æÆ÷
#define MPU_MOT_DETECT_STATUS 0X61 //Ô˶¯×´Ì¬¼Ä´æÆ÷
void mpu6050_init(void);
void task_read_MPU6050(void);
#endif
.c驱动代码
#include "mpu6050.h"
#include "iic_io.h"
#include "utility.h"
#include "bsp.h"
static uint8_t m_device_address; // !< Device address in bits [7:1]
/************************************************
* mpu6050_register_write
*
******************************************/
bool mpu6050_register_write(uint8_t register_address, uint8_t value)
{
uint8_t w2_data[2];
w2_data[0] = register_address;
w2_data[1] = value;
return hrs_iic_transfer(m_device_address, w2_data, 2, TWI_ISSUE_STOP);
}
/*****************************************************************************
*@function name: mpu6050_register_read
*@description : write and read reg
*@Para : register_address
*******************************************************************************/
bool mpu6050_register_read(uint8_t register_address, uint8_t * destination, uint8_t number_of_bytes)
{
bool transfer_succeeded;
transfer_succeeded=hrs_iic_transfer(m_device_address,®ister_address,1, TWI_DONT_ISSUE_STOP);
transfer_succeeded=hrs_iic_transfer(m_device_address|TWI_READ_BIT, destination, number_of_bytes, TWI_ISSUE_STOP);
return transfer_succeeded;
}
/***********************************************
* mpu6050_verify_product_id
*
***********************************/
const uint8_t expected_who_am_i = 0x68U; // !< Expected value to get from WHO_AM_I register.
bool mpu6050_verify_product_id(void)
{
uint8_t who_am_i=0;
if (mpu6050_register_read(ADDRESS_WHO_AM_I, &who_am_i, 1))
{
BSP_RTT("mpu6050_verify_product_id =0x%02x\r\n",who_am_i);
if (who_am_i != expected_who_am_i)
{
return false;
}
else
{
return true;
}
}
else
{
return false;
}
}
/*********************************************
* Ô˶¯¼ì²âÖжÏ
*
**************************************/
void Motion_Interrupt(void) //
{
mpu6050_register_write(MPU_MOTION_DET_REG,0x01);
mpu6050_register_write(MPU_MOT_DUR_REG,0x14);
mpu6050_register_write(MPU_CFG_REG,0x04); //DLPF
mpu6050_register_write(MPU_ACCEL_CFG_REG,0x1C); //
mpu6050_register_write(MPU_INTBP_CFG_REG,0X1C); //INT
mpu6050_register_write(MPU_INT_EN_REG,0x40); //
}
/*********************************************
* MPU_Fall_Init
*
****************************************************/
void MPU_Fall_Init(void) //
{
mpu6050_register_write(MPU_FF_THR,0x14); //20mg
mpu6050_register_write(MPU_FF_DUR,0x0A); //10ms
mpu6050_register_write(MPU_CFG_REG,0x04); //DLPF
mpu6050_register_write(MPU_ACCEL_CFG_REG,0x1C); //
mpu6050_register_write(MPU_INTBP_CFG_REG,0X1C); //INT
mpu6050_register_write(MPU_INT_EN_REG,0x40); //
}
/*********************************************
* MPU_Zero_Motion_Init
*
****************************************************/
void MPU_Zero_Motion_Init(void)
{
mpu6050_register_write(MPU_ZRMOT_THR_REG,0x20); //64mg
mpu6050_register_write(MPU_ZRMOT_DUR_REG,0x20); //32ms
mpu6050_register_write(MPU_CFG_REG,0x04); //DLPF
mpu6050_register_write(MPU_ACCEL_CFG_REG,0x1C); //
mpu6050_register_write(MPU_INTBP_CFG_REG,0X1C); //INT
mpu6050_register_write(MPU_INT_EN_REG,0x40); //
}
/**************************************
* mpu6050_init
*
*****************************/
// uint8_t inData[7]={0x00,
// 0x00,
// 0x03,
// 0x10,
// 0x00,
// 0x32,
// 0x01};
/****************************************************************************/
// bit2-bit0
/*
* * | ??????| ???
* * DLPF_CFG | ??| ??| ??| ??| ???
* ------------- + -------- + ------- + -------- + ------ + - -----------
* 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz
* 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz
* 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz
* 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz
* 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz
* 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz
* 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz
* 7 | ??| ??| ??
* */
void mpu6050_init(void)
{
bool transfer_succeeded = 0xff;
uint8_t device_address =0x68;
hrs_iic_init(MPU6050_SDA_PIN,MPU6050_SCL_PIN);
delay_ms(100);
m_device_address = (uint8_t)(device_address << 1);
// Do a reset on signal paths
uint8_t reset_value = 0x04U | 0x02U | 0x01U; // Resets gyro, accelerometer and temperature sensor signal paths
transfer_succeeded = mpu6050_register_write(ADDRESS_SIGNAL_PATH_RESET, reset_value);
//ÉèÖòÉÑùÂÊ
//rate:4~1000(Hz)
mpu6050_register_write(MPU_SAMPLE_RATE_REG ,0x00);// -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
//-- EXT_SYNC_SET 0 (½ûÓþ§ÕñÊäÈë½Å) ;
//default DLPF_CFG = 0 => (µÍͨÂ˲¨)ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
//0x06-5hz
mpu6050_register_write(MPU_CFG_REG ,0x06);//0x00); //CONFIG
//-- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
mpu6050_register_write(MPU_PWR_MGMT1_REG , 0x03); //PWR_MGMT_1
mpu6050_register_write(MPU_USER_CTRL_REG , 0x00); // 0x6AµÄ I2C_MST_EN ÉèÖóÉ0 ĬÈÏΪ0 6050 ʹÄÜÖ÷IIC
mpu6050_register_write(MPU_FIFO_EN_REG,0X00); //close fifo
/*****************************ÉèÖÃGYRO****************************************************/
//fsr:0,¡À250dps;1,¡À500dps;2,¡À1000dps;3,¡À2000dps bit3-bit4
uint8_t gyro_fsr =3;//
mpu6050_register_write(MPU_GYRO_CFG_REG,gyro_fsr<<3);// 0x10); //gyroÅäÖà Á¿³Ì 0-1000¶ÈÿÃë
/*****************************ÉèÖÃ ACC****************************************************/
//fsr:0,+-2G;1,+-4G;2,+-8G s ;3,+-16G bit3-bit4
uint8_t acc_fsr =0;
mpu6050_register_write(MPU_ACCEL_CFG_REG,acc_fsr<<3); //ACCÉèÖà Á¿³Ì +-2G s
/*********************************************************************************************/
// £¬¸ßµçƽÖжϣ¬Ò»Ö±Êä³ö¸ßµçƽֱµ½ÖжÏÇå³ý
mpu6050_register_write(MPU_INTBP_CFG_REG, 0x1C);//0x32);// INTµÍµçƽ
mpu6050_register_write(MPU_INT_EN_REG, 0X40);// 0x01);
// Read and verify product ID
transfer_succeeded &= mpu6050_verify_product_id();
Motion_Interrupt();
}
/*******************************************************
* MPU6050_ReadGyro
* ÍÓÂÝÒÇÖµ
*********************************/
void MPU6050_ReadGyro(int16_t *pGYRO_X , int16_t *pGYRO_Y , int16_t *pGYRO_Z )
{
uint8_t buf[6];
mpu6050_register_read(MPU6050_GYRO_OUT, buf, 6);
*pGYRO_X = (buf[0] << 8) | buf[1];
if(*pGYRO_X & 0x8000) *pGYRO_X-=65536;
*pGYRO_Y= (buf[2] << 8) | buf[3];
if(*pGYRO_Y & 0x8000) *pGYRO_Y-=65536;
*pGYRO_Z = (buf[4] << 8) | buf[5];
if(*pGYRO_Z & 0x8000) *pGYRO_Z-=65536;
}
/*******************************************************
* MPU6050_ReadAcc
* ¼ÓËÙ¶ÈÖµ
*********************************/
void MPU6050_ReadAcc( int16_t *pACC_X , int16_t *pACC_Y , int16_t *pACC_Z )
{
uint8_t buf[6];
mpu6050_register_read(MPU6050_ACC_OUT, buf, 6);
*pACC_X = (buf[0] << 8) | buf[1];
if(*pACC_X & 0x8000) *pACC_X-=65536;
*pACC_Y= (buf[2] << 8) | buf[3];
if(*pACC_Y & 0x8000) *pACC_Y-=65536;
*pACC_Z = (buf[4] << 8) | buf[5];
if(*pACC_Z & 0x8000) *pACC_Z-=65536;
}
void task_read_MPU6050(void)
{
int16_t AccValue[3],GyroValue[3];
MPU6050_ReadAcc( &AccValue[0], &AccValue[1] , &AccValue[2] );
MPU6050_ReadGyro(&GyroValue[0] , &GyroValue[1] , &GyroValue[2] );
BSP_RTT("6050-ACC: %d %d %d ",AccValue[0],AccValue[1],AccValue[2]);
BSP_RTT("6050-GYRO: %d %d %d \r\n",GyroValue[0],GyroValue[1],GyroValue[2]);
}
角度算法
// 变量定义
#define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角
//加速度单位g，陀螺仪rad/s
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm; //单位化
ay = ay / norm;
az = az / norm;
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
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;
// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 整合四元数率和正常化
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;
// 正常化四元
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; //偏移太大，等我找一个好用的
}
可以加入QQ群：687360507
与大伙沟通交流，技术在于分享而进步
扫一扫
3473
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
