基于nrf52832 mpu6050应用实例（7）

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,&register_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);  //DLPFmpu6050_register_write(MPU_ACCEL_CFG_REG,0x1C); //mpu6050_register_write(MPU_INTBP_CFG_REG,0X1C); //INTmpu6050_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);           //10msmpu6050_register_write(MPU_CFG_REG,0x04);  //DLPFmpu6050_register_write(MPU_ACCEL_CFG_REG,0x1C); //mpu6050_register_write(MPU_INTBP_CFG_REG,0X1C); //INTmpu6050_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);           //64mgmpu6050_register_write(MPU_ZRMOT_DUR_REG,0x20);           //32msmpu6050_register_write(MPU_CFG_REG,0x04);  //DLPFmpu6050_register_write(MPU_ACCEL_CFG_REG,0x1C); //mpu6050_register_write(MPU_INTBP_CFG_REG,0X1C); //INTmpu6050_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 pathsuint8_t reset_value = 0x04U | 0x02U | 0x01U; // Resets gyro, accelerometer and temperature sensor signal pathstransfer_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-bit4uint8_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-bit4uint8_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 IDtransfer_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
与大伙沟通交流，技术在于分享而进步