用Ai-M61-32su做一个跌倒告警神器！

本作品由安信可社区用户

lazy制作

设计初想

在不能关照老人与小孩儿的时候，可能会存在老人或小孩儿摔倒的情况。如果摔倒不及时处置可能造成比较严重的伤害。

这个小作品主要就是跌倒提醒的功能，网上也有类似的作品。这里本人使用安信可的Ai-M61-32su复刻一下。如果有跌倒状态时红灯会亮起（默认绿灯），小伙伴还可以考虑增加蜂鸣器等。

制作思路

摔倒检测算法可以通过检测加速度和角速度的变化来判断物体是否发生了摔倒。在实现MPU6050摔倒检测算法时，首先需要读取MPU6050传感器的原始数据，通过计算加速度和角速度的变化率来判断物体是否摔倒，可以通过设置一个阈值来判断加速度和角速度是否超过了正常范围。

如果超过了阈值，则可认为物体发生了摔倒。另外，还可以使用一些机器学习算法来提高摔倒检测的准确性。通过对大量的摔倒和非摔倒状态的数据进行训练，可以建立一个模型来判断物体是否发生了摔倒。这种方法可以更好地适应不同物体和环境的情况，并提高摔倒检测的灵敏度和准确性。

一、MPU6050简介

首先对设备所使用的主要硬件进行介绍，【转载】MPU6050简介 - 鱼芯工作室的文章 - 知乎

https://zhuanlan.zhihu.com/p/346764320

内容如下：

MPU-60X0 是全球首例9 轴运动处理传感器。它集成了3 轴MEMS 陀螺仪，3 轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP（Digital Motion Processor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。

扩展之后就可以通过其I2C 或SPI 接口输出一个9 轴的信号（SPI 接口仅在MPU-6000 可用）。MPU-60X0 也可以通过其I2C 接口连接非惯性的数字传感器，比如压力传感器。

MPU-60X0 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。

芯片尺寸4×4×0.9mm，采用QFN 封装（无引线方形封装），可承受最大10000g 的冲击，并有可编程的低通滤波器。

MPU6050的内部框图如下图所示：

其中，需要了解的引脚有：SCL和 SDA是连接MCU的 IIC接口，MCU通过这个IIC 接口来控制MPU6050，另外还有一个 IIC 接口：AUX_CL和AUX_DA ，这个接口可用来连外部从设备比如磁力计，这样就可以组成一个九轴传感器。VLOGIC是IO口电压，该引脚最低可以到1.8V电压， 我们一般直接连VDD即可。AD0是从IIC 接口（接 MCU）的地址控制引脚，该引脚控制的是IIC 地址的最低位。如果接 GND ，则 MPU6050的IIC地址是：0X68，如果接VDD，则是0X69。

需要注意的是：这里的地址0x68和0x69是不包含用于数据传输的最低位的，因此并不是八位数据，如0x68表示的是110 1000，0x69表示的则是110 1001，通常最低位用于表示IIC主机的读取数据/写数据模式。self test为自检，自检的作用是可用来测试传感器的机械和电气结构。也就是说通过自检来测试芯片是否损坏。自检启动后，电路会使传感器工作并且产生输出信号。

关于自检的具体说明，官方芯片手册里有详细描述。如下：

陀螺仪自检

当自检激活时，车载电子设备将启动相应的传感器。该致动将使传感器的检测质量移动相当于预定义科里奥利力的距离。该检验质量位移导致传感器输出的变化，该变化反映在输出信号中。输出信号用于观察自检响应。

自检响应定义如下：

自检响应=启用自检的传感器输出-未启用自检的检波器输出每个陀螺仪轴的自检限值在MPU-6000/MPU-6050产品规范文件的电气特性表中提供。当自检响应的值在产品规格的最小/最大限值内时，零件已通过自检。当自检响应超过文件中规定的最小/最大值时，该部件被视为自检失败。

加速计自检

当自检激活时，车载电子设备将启动相应的传感器。该动作模拟外力。被致动的传感器反过来将产生相应的输出信号。输出信号用于观察自检响应。

自检响应定义如下：

自检响应=启用自检的传感器输出-未启用自检的检波器输出每个加速计轴的自检限值在MPU-6000/MPU-6050产品规范文件的电气特性表中提供。当自检响应的值在产品规格的最小/最大限值内时，零件已通过自检。当自检响应超过文件中规定的最小/最大值时，该部件被视为自检失败。

MPU6050相关寄存器

MPU6050官方的寄存器手册上共介绍了40个寄存器的内容和功能，在此只选取一些常用的和重要的寄存器作为了解。

1.采样分频寄存器 Sample Rate Divider

说明：该寄存器指定陀螺仪输出速率的分频器，用于为MPU-60X0生成采样速率。

传感器寄存器输出，FIFO输出，DMP采样，运动检测，静止检测和自由落体检测都基于这个采样频率。

采样频率=陀螺仪输出频率/（1+SMPLRT_DIV）

当 DLPF（数字低通滤波器，见寄存器Configuration）禁用时（DLPF_CFG=0 or 7），陀螺输出频率=8kHz；当 DLPF 使能，陀螺仪输出频率=1KHz。注意：加速度计输出频率为 1KHz。这意味着，当采样频率大于1KHZ时，同个加速度计采样得到的数据，可能不止一次输出到FIFO、DMP、传感器寄存器。

2.配置寄存器 Configuration

说明：该寄存器为陀螺仪和加速度计配置外部帧同步(FSYNC) 管脚的采样和数字低通滤波(DLPF)设置。

其中，数字低通滤波器DLPF由DLPF_CFG配置。根据下表所示的DLPF_CFG值对加速度计和陀螺仪进行滤波。

其中，FS为陀螺仪输出频率。SMPLRT_DIV由预设定的采样频率根据上述的公式计算得出。一般情况下，DPLF滤波频率为采样频率的一半，如设定采样频率为50Hz，由表可知当FS为1kHz，SMPLRT_DIV的值为1000/50-1=19。

3.陀螺仪配置寄存器 Gyroscope Configuration

说明：该寄存器是用来触发陀螺仪自检和配置陀螺仪的满量程范围。

其中，XG_ST、YG_ST、ZG_ST分别用来设置陀螺仪X轴、Y轴、Z轴自检，置0则不触发自检。FS_SEL[1:0]用于设置陀螺仪的满量程，如下表：

我们一般设置为3，即满量程为±2000°/s，由于采用16位ADC即0-65536，则灵敏度G=65536/4000=16.4LSB/（°/s），LSB表示最低有效位，即1°/s对应的数字量为16.4。最终即可将陀螺仪输出的数字量数据转化为角速度。

4.加速度计配置寄存器 Accelerometer Configuration

说明：该寄存器是用来触发加速度计自检和配置加速度计的满量程范围。同时这个寄存器也可以用于配置数字高通滤波器（DHPF）。

其中，XA_ST、YA_ST、ZA_ST分别用来设置加速度计X轴、Y轴、Z轴自检，置0则不触发自检。AFS_SEL[1:0]用于选择加速度计的满量程范围，如下表：

我们一般设置为0，即满量程为±2g，由于采用16位ADC即0-65536，则灵敏度G=65536/4=16384LSB/（g），LSB表示最低有效位，即1g对应的数字量为16384。最终即可将加速度计输出的数字量数据转化为加速度。

5.加速度计测量值寄存器 Accelerometer

Measurements

说明：该寄存器存储最近加速度计的测量值。加速度计根据采样频率（由采样分频寄存器寄存器设定 ）写入到这些寄存器。即采样频率为50Hz，写入数据的时间间隔为0.02s。加速度计测量值寄存器和温度测量值寄存器，陀螺仪测量值寄存器，外部传感器数据寄存器都是由两组寄存器构成：一个内部寄存器集和一个用于用户读取的寄存器集。

加速度计传感器的内部寄存器集合里的数据根据采样频率更新。以此同时，每当串行接口处于闲置状态，面向用户的读取寄存器集合会复制内部寄存器集合的数据值。这保证了突发读取时传感器寄存器可以读到相同的采样时刻的测量值。需要注意的是，如果没有突发读取，则用户负责通过检查数据就绪中断（Data Ready interrupt）来确保瞬时的一组单字节读取对应于单字节的采样数据。

参数：

ACCEL_XOUT ：

由 2部分组成的 16位数值存储最近X 轴加速度计的测量值。

ACCEL_YOUT ：

由 2部分组成的 16位数值存储最近Y 轴加速度计的测量值。

ACCEL_ZOUT ：由 2部分组成的 16位数值存储最近Z 轴加速度计的测量值。

6.陀螺仪测量值寄存器 Gyroscope Measurements

说明：该寄存器存储最近加陀螺仪的测量值。大致构成与加速度计测量值寄存器相同，此处便不做叙述。参数分别为：GYRO_XOUT 、GYRO_YOUT 、GYRO_ZOUT 。

7.电源管理寄存器1 Power Management 1

说明：该寄存器允许用户配置电源模式和时钟源，还提供了复位整个设备和禁用温度传感器的位。当置SLEEP位为1时，MPU-60X0 可以进入低功耗睡眠模式。当SLEEP位禁用且 CYCLE位置 1时，MPU-60X0进入循环模式（CycleMode）。在循环模式下，设备在休眠模式和唤醒之间循环，以LP_WAKE_CTRL（由电源管理2寄存器配置）确定的速率从active sensors（此处不知如何翻译）获取单个数据样本。

该寄存器的最低三位用于设置系统的时钟源选择，默认值是0（内部8M RC振荡），不过一般设置为1，选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器2配置，设置对应位为0即可开启。

附英文手册原文片段：Upon power up, the MPU-60X0 clock source defaults to

the internal oscillator. However, it is highly recommended that the device be configured to use one of the gyroscopes (or an ext ernal clock source) as the clock reference for improved stability.

其他参数：

DEVICE_RESET

该位置 1，重启内部寄存器到默认值。复位完成后该位自动清0。

TEMP_DIS该位置 1，禁用温度传感器。

8.电源管理寄存器2 Power Management 2

说明：该寄存器允许用户在加速度计低功耗模式下配置唤醒频率。也允许用户让加速度计和陀螺仪的个别轴进入待机模式。

只让MPU-60X0的加速度计进入低功耗模式的步骤如下：

1.置 CYCLE位为 1

2.置 SLEEP位为 1

3.置 TEMP_DIS位为 1

4.置 STBY_XG,STBY_YG,STBY_ZG位为 1

在这种模式下，设备会关闭除了主 I2C接口外其他所有设备，加速度计只在固定的间隔唤醒并测量一次。唤醒的频率可以通过配置 LP_WAKE_CTRL实现如下：

参数：

LP_WAKE_CTRL ：

2位无符号数值。指定加速度计在低功耗模式下的唤醒频率。

STBY_XA ：

该位置 1，加速度计的 X轴进入待机模式。

STBY_YA ：

该位置 1，加速度计的 Y轴进入待机模式。

STBY_ZA ：

该位置 1，加速度计的 Z轴进入待机模式。

STBY_XG ：

该位置 1，陀螺仪的 X轴进入待机模式。

STBY_YG ：

该位置 1，陀螺仪的 Y轴进入待机模式。

STBY_ZG ：该位置 1，陀螺仪的 Z轴进入待机模式。

对于MPU 6050 进行了简单的介绍，也对这个模块有了一些了解。

mpu6050.h

#ifndef __MPU6050_H

#define __MPU6050_H

#include

#include

#include

#include

#include

#include

#include //Added for uint_t

#include

#include "bflb_mtimer.h"

#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board

#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)

#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW

#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD

#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD

#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD

#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN

#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN

#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN

#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS

#define MPU6050_RA_XA_OFFS_L_TC 0x07

#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS

#define MPU6050_RA_YA_OFFS_L_TC 0x09

#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS

#define MPU6050_RA_ZA_OFFS_L_TC 0x0B

#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR

#define MPU6050_RA_XG_OFFS_USRL 0x14

#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR

#define MPU6050_RA_YG_OFFS_USRL 0x16

#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR

#define MPU6050_RA_ZG_OFFS_USRL 0x18

#define MPU6050_RA_SMPLRT_DIV 0x19

#define MPU6050_RA_CONFIG 0x1A

#define MPU6050_RA_GYRO_CONFIG 0x1B

#define MPU6050_RA_ACCEL_CONFIG 0x1C

#define MPU6050_RA_FF_THR 0x1D

#define MPU6050_RA_FF_DUR 0x1E

#define MPU6050_RA_MOT_THR 0x1F

#define MPU6050_RA_MOT_DUR 0x20

#define MPU6050_RA_ZRMOT_THR 0x21

#define MPU6050_RA_ZRMOT_DUR 0x22

#define MPU6050_RA_FIFO_EN 0x23

#define MPU6050_RA_I2C_MST_CTRL 0x24

#define MPU6050_RA_I2C_SLV0_ADDR 0x25

#define MPU6050_RA_I2C_SLV0_REG 0x26

#define MPU6050_RA_I2C_SLV0_CTRL 0x27

#define MPU6050_RA_I2C_SLV1_ADDR 0x28

#define MPU6050_RA_I2C_SLV1_REG 0x29

#define MPU6050_RA_I2C_SLV1_CTRL 0x2A

#define MPU6050_RA_I2C_SLV2_ADDR 0x2B

#define MPU6050_RA_I2C_SLV2_REG 0x2C

#define MPU6050_RA_I2C_SLV2_CTRL 0x2D

#define MPU6050_RA_I2C_SLV3_ADDR 0x2E

#define MPU6050_RA_I2C_SLV3_REG 0x2F

#define MPU6050_RA_I2C_SLV3_CTRL 0x30

#define MPU6050_RA_I2C_SLV4_ADDR 0x31

#define MPU6050_RA_I2C_SLV4_REG 0x32

#define MPU6050_RA_I2C_SLV4_DO 0x33

#define MPU6050_RA_I2C_SLV4_CTRL 0x34

#define MPU6050_RA_I2C_SLV4_DI 0x35

#define MPU6050_RA_I2C_MST_STATUS 0x36

#define MPU6050_RA_INT_PIN_CFG 0x37

#define MPU6050_RA_INT_ENABLE 0x38

#define MPU6050_RA_DMP_INT_STATUS 0x39

#define MPU6050_RA_INT_STATUS 0x3A

#define MPU6050_RA_ACCEL_XOUT_H 0x3B

#define MPU6050_RA_ACCEL_XOUT_L 0x3C

#define MPU6050_RA_ACCEL_YOUT_H 0x3D

#define MPU6050_RA_ACCEL_YOUT_L 0x3E

#define MPU6050_RA_ACCEL_ZOUT_H 0x3F

#define MPU6050_RA_ACCEL_ZOUT_L 0x40

#define MPU6050_RA_TEMP_OUT_H 0x41

#define MPU6050_RA_TEMP_OUT_L 0x42

#define MPU6050_RA_GYRO_XOUT_H 0x43

#define MPU6050_RA_GYRO_XOUT_L 0x44

#define MPU6050_RA_GYRO_YOUT_H 0x45

#define MPU6050_RA_GYRO_YOUT_L 0x46

#define MPU6050_RA_GYRO_ZOUT_H 0x47

#define MPU6050_RA_GYRO_ZOUT_L 0x48

#define MPU6050_RA_EXT_SENS_DATA_00 0x49

#define MPU6050_RA_EXT_SENS_DATA_01 0x4A

#define MPU6050_RA_EXT_SENS_DATA_02 0x4B

#define MPU6050_RA_EXT_SENS_DATA_03 0x4C

#define MPU6050_RA_EXT_SENS_DATA_04 0x4D

#define MPU6050_RA_EXT_SENS_DATA_05 0x4E

#define MPU6050_RA_EXT_SENS_DATA_06 0x4F

#define MPU6050_RA_EXT_SENS_DATA_07 0x50

#define MPU6050_RA_EXT_SENS_DATA_08 0x51

#define MPU6050_RA_EXT_SENS_DATA_09 0x52

#define MPU6050_RA_EXT_SENS_DATA_10 0x53

#define MPU6050_RA_EXT_SENS_DATA_11 0x54

#define MPU6050_RA_EXT_SENS_DATA_12 0x55

#define MPU6050_RA_EXT_SENS_DATA_13 0x56

#define MPU6050_RA_EXT_SENS_DATA_14 0x57

#define MPU6050_RA_EXT_SENS_DATA_15 0x58

#define MPU6050_RA_EXT_SENS_DATA_16 0x59

#define MPU6050_RA_EXT_SENS_DATA_17 0x5A

#define MPU6050_RA_EXT_SENS_DATA_18 0x5B

#define MPU6050_RA_EXT_SENS_DATA_19 0x5C

#define MPU6050_RA_EXT_SENS_DATA_20 0x5D

#define MPU6050_RA_EXT_SENS_DATA_21 0x5E

#define MPU6050_RA_EXT_SENS_DATA_22 0x5F

#define MPU6050_RA_EXT_SENS_DATA_23 0x60

#define MPU6050_RA_MOT_DETECT_STATUS 0x61

#define MPU6050_RA_I2C_SLV0_DO 0x63

#define MPU6050_RA_I2C_SLV1_DO 0x64

#define MPU6050_RA_I2C_SLV2_DO 0x65

#define MPU6050_RA_I2C_SLV3_DO 0x66

#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67

#define MPU6050_RA_SIGNAL_PATH_RESET 0x68

#define MPU6050_RA_MOT_DETECT_CTRL 0x69

#define MPU6050_RA_USER_CTRL 0x6A

#define MPU6050_RA_PWR_MGMT_1 0x6B

#define MPU6050_RA_PWR_MGMT_2 0x6C

#define MPU6050_RA_BANK_SEL 0x6D

#define MPU6050_RA_MEM_START_ADDR 0x6E

#define MPU6050_RA_MEM_R_W 0x6F

#define MPU6050_RA_DMP_CFG_1 0x70

#define MPU6050_RA_DMP_CFG_2 0x71

#define MPU6050_RA_FIFO_COUNTH 0x72

#define MPU6050_RA_FIFO_COUNTL 0x73

#define MPU6050_RA_FIFO_R_W 0x74

#define MPU6050_RA_WHO_AM_I 0x75

#define MPU6050_TC_PWR_MODE_BIT 7

#define MPU6050_TC_OFFSET_BIT 6

#define MPU6050_TC_OFFSET_LENGTH 6

#define MPU6050_TC_OTP_BNK_VLD_BIT 0

#define MPU6050_VDDIO_LEVEL_VLOGIC 0

#define MPU6050_VDDIO_LEVEL_VDD 1

#define MPU6050_CFG_EXT_SYNC_SET_BIT 5

#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3

#define MPU6050_CFG_DLPF_CFG_BIT 2

#define MPU6050_CFG_DLPF_CFG_LENGTH 3

#define MPU6050_EXT_SYNC_DISABLED 0x0

#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1

#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2

#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3

#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4

#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5

#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6

#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7

#define MPU6050_DLPF_BW_256 0x00

#define MPU6050_DLPF_BW_188 0x01

#define MPU6050_DLPF_BW_98 0x02

#define MPU6050_DLPF_BW_42 0x03

#define MPU6050_DLPF_BW_20 0x04

#define MPU6050_DLPF_BW_10 0x05

#define MPU6050_DLPF_BW_5 0x06

#define MPU6050_GCONFIG_FS_SEL_BIT 4

#define MPU6050_GCONFIG_FS_SEL_LENGTH 2

#define MPU6050_GYRO_FS_250 0x00

#define MPU6050_GYRO_FS_500 0x01

#define MPU6050_GYRO_FS_1000 0x02

#define MPU6050_GYRO_FS_2000 0x03

#define MPU6050_ACONFIG_XA_ST_BIT 7

#define MPU6050_ACONFIG_YA_ST_BIT 6

#define MPU6050_ACONFIG_ZA_ST_BIT 5

#define MPU6050_ACONFIG_AFS_SEL_BIT 4

#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2

#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2

#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3

#define MPU6050_ACCEL_FS_2 0x00

#define MPU6050_ACCEL_FS_4 0x01

#define MPU6050_ACCEL_FS_8 0x02

#define MPU6050_ACCEL_FS_16 0x03

#define MPU6050_DHPF_RESET 0x00

#define MPU6050_DHPF_5 0x01

#define MPU6050_DHPF_2P5 0x02

#define MPU6050_DHPF_1P25 0x03

#define MPU6050_DHPF_0P63 0x04

#define MPU6050_DHPF_HOLD 0x07

#define MPU6050_TEMP_FIFO_EN_BIT 7

#define MPU6050_XG_FIFO_EN_BIT 6

#define MPU6050_YG_FIFO_EN_BIT 5

#define MPU6050_ZG_FIFO_EN_BIT 4

#define MPU6050_ACCEL_FIFO_EN_BIT 3

#define MPU6050_SLV2_FIFO_EN_BIT 2

#define MPU6050_SLV1_FIFO_EN_BIT 1

#define MPU6050_SLV0_FIFO_EN_BIT 0

#define MPU6050_MULT_MST_EN_BIT 7

#define MPU6050_WAIT_FOR_ES_BIT 6

#define MPU6050_SLV_3_FIFO_EN_BIT 5

#define MPU6050_I2C_MST_P_NSR_BIT 4

#define MPU6050_I2C_MST_CLK_BIT 3

#define MPU6050_I2C_MST_CLK_LENGTH 4

#define MPU6050_CLOCK_DIV_348 0x0

#define MPU6050_CLOCK_DIV_333 0x1

#define MPU6050_CLOCK_DIV_320 0x2

#define MPU6050_CLOCK_DIV_308 0x3

#define MPU6050_CLOCK_DIV_296 0x4

#define MPU6050_CLOCK_DIV_286 0x5

#define MPU6050_CLOCK_DIV_276 0x6

#define MPU6050_CLOCK_DIV_267 0x7

#define MPU6050_CLOCK_DIV_258 0x8

#define MPU6050_CLOCK_DIV_500 0x9

#define MPU6050_CLOCK_DIV_471 0xA

#define MPU6050_CLOCK_DIV_444 0xB

#define MPU6050_CLOCK_DIV_421 0xC

#define MPU6050_CLOCK_DIV_400 0xD

#define MPU6050_CLOCK_DIV_381 0xE

#define MPU6050_CLOCK_DIV_364 0xF

#define MPU6050_I2C_SLV_RW_BIT 7

#define MPU6050_I2C_SLV_ADDR_BIT 6

#define MPU6050_I2C_SLV_ADDR_LENGTH 7

#define MPU6050_I2C_SLV_EN_BIT 7

#define MPU6050_I2C_SLV_BYTE_SW_BIT 6

#define MPU6050_I2C_SLV_REG_DIS_BIT 5

#define MPU6050_I2C_SLV_GRP_BIT 4

#define MPU6050_I2C_SLV_LEN_BIT 3

#define MPU6050_I2C_SLV_LEN_LENGTH 4

#define MPU6050_I2C_SLV4_RW_BIT 7

#define MPU6050_I2C_SLV4_ADDR_BIT 6

#define MPU6050_I2C_SLV4_ADDR_LENGTH 7

#define MPU6050_I2C_SLV4_EN_BIT 7

#define MPU6050_I2C_SLV4_INT_EN_BIT 6

#define MPU6050_I2C_SLV4_REG_DIS_BIT 5

#define MPU6050_I2C_SLV4_MST_DLY_BIT 4

#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5

#define MPU6050_MST_PASS_THROUGH_BIT 7

#define MPU6050_MST_I2C_SLV4_DONE_BIT 6

#define MPU6050_MST_I2C_LOST_ARB_BIT 5

#define MPU6050_MST_I2C_SLV4_NACK_BIT 4

#define MPU6050_MST_I2C_SLV3_NACK_BIT 3

#define MPU6050_MST_I2C_SLV2_NACK_BIT 2

#define MPU6050_MST_I2C_SLV1_NACK_BIT 1

#define MPU6050_MST_I2C_SLV0_NACK_BIT 0

#define MPU6050_INTCFG_INT_LEVEL_BIT 7

#define MPU6050_INTCFG_INT_OPEN_BIT 6

#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5

#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4

#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3

#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2

#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1

#define MPU6050_INTCFG_CLKOUT_EN_BIT 0

#define MPU6050_INTMODE_ACTIVEHIGH 0x00

#define MPU6050_INTMODE_ACTIVELOW 0x01

#define MPU6050_INTDRV_PUSHPULL 0x00

#define MPU6050_INTDRV_OPENDRAIN 0x01

#define MPU6050_INTLATCH_50USPULSE 0x00

#define MPU6050_INTLATCH_WAITCLEAR 0x01

#define MPU6050_INTCLEAR_STATUSREAD 0x00

#define MPU6050_INTCLEAR_ANYREAD 0x01

#define MPU6050_INTERRUPT_FF_BIT 7

#define MPU6050_INTERRUPT_MOT_BIT 6

#define MPU6050_INTERRUPT_ZMOT_BIT 5

#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4

#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3

#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2

#define MPU6050_INTERRUPT_DMP_INT_BIT 1

#define MPU6050_INTERRUPT_DATA_RDY_BIT 0

// TODO: figure out what these actually do

// UMPL source code is not very obivous

#define MPU6050_DMPINT_5_BIT 5

#define MPU6050_DMPINT_4_BIT 4

#define MPU6050_DMPINT_3_BIT 3

#define MPU6050_DMPINT_2_BIT 2

#define MPU6050_DMPINT_1_BIT 1

#define MPU6050_DMPINT_0_BIT 0

#define MPU6050_MOTION_MOT_XNEG_BIT 7

#define MPU6050_MOTION_MOT_XPOS_BIT 6

#define MPU6050_MOTION_MOT_YNEG_BIT 5

#define MPU6050_MOTION_MOT_YPOS_BIT 4

#define MPU6050_MOTION_MOT_ZNEG_BIT 3

#define MPU6050_MOTION_MOT_ZPOS_BIT 2

#define MPU6050_MOTION_MOT_ZRMOT_BIT 0

#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7

#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4

#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3

#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2

#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1

#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0

#define MPU6050_PATHRESET_GYRO_RESET_BIT 2

#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1

#define MPU6050_PATHRESET_TEMP_RESET_BIT 0

#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5

#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2

#define MPU6050_DETECT_FF_COUNT_BIT 3

#define MPU6050_DETECT_FF_COUNT_LENGTH 2

#define MPU6050_DETECT_MOT_COUNT_BIT 1

#define MPU6050_DETECT_MOT_COUNT_LENGTH 2

#define MPU6050_DETECT_DECREMENT_RESET 0x0

#define MPU6050_DETECT_DECREMENT_1 0x1

#define MPU6050_DETECT_DECREMENT_2 0x2

#define MPU6050_DETECT_DECREMENT_4 0x3

#define MPU6050_USERCTRL_DMP_EN_BIT 7

#define MPU6050_USERCTRL_FIFO_EN_BIT 6

#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5

#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4

#define MPU6050_USERCTRL_DMP_RESET_BIT 3

#define MPU6050_USERCTRL_FIFO_RESET_BIT 2

#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1

#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0

#define MPU6050_PWR1_DEVICE_RESET_BIT 7

#define MPU6050_PWR1_SLEEP_BIT 6

#define MPU6050_PWR1_CYCLE_BIT 5

#define MPU6050_PWR1_TEMP_DIS_BIT 3

#define MPU6050_PWR1_CLKSEL_BIT 2

#define MPU6050_PWR1_CLKSEL_LENGTH 3

#define MPU6050_CLOCK_INTERNAL 0x00

#define MPU6050_CLOCK_PLL_XGYRO 0x01

#define MPU6050_CLOCK_PLL_YGYRO 0x02

#define MPU6050_CLOCK_PLL_ZGYRO 0x03

#define MPU6050_CLOCK_PLL_EXT32K 0x04

#define MPU6050_CLOCK_PLL_EXT19M 0x05

#define MPU6050_CLOCK_KEEP_RESET 0x07

#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7

#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2

#define MPU6050_PWR2_STBY_XA_BIT 5

#define MPU6050_PWR2_STBY_YA_BIT 4

#define MPU6050_PWR2_STBY_ZA_BIT 3

#define MPU6050_PWR2_STBY_XG_BIT 2

#define MPU6050_PWR2_STBY_YG_BIT 1

#define MPU6050_PWR2_STBY_ZG_BIT 0

#define MPU6050_WAKE_FREQ_1P25 0x0

#define MPU6050_WAKE_FREQ_2P5 0x1

#define MPU6050_WAKE_FREQ_5 0x2

#define MPU6050_WAKE_FREQ_10 0x3

#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6

#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5

#define MPU6050_BANKSEL_MEM_SEL_BIT 4

#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5

#define MPU6050_WHO_AM_I_BIT 6

#define MPU6050_WHO_AM_I_LENGTH 6

#define MPU6050_DMP_MEMORY_BANKS 8

#define MPU6050_DMP_MEMORY_BANK_SIZE 256

#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16

// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")

#define I2CDEV_DEFAULT_READ_TIMEOUT 1000

void MPU6050();

// SMPLRT_DIV register

uint8_t getRate();

void setRate(uint8_t rate);

uint8_t getClockSource();

void setClockSource(uint8_t source);

// GYRO_CONFIG register

uint8_t getFullScaleGyroRange();

void setFullScaleGyroRange(uint8_t range);

uint8_t getFullScaleAccelRange();

void setFullScaleAccelRange(uint8_t range);

uint8_t getDHPFMode();

void setDHPFMode(uint8_t mode);

// PWR_MGMT_1 register

void reset();

bool getSleepEnabled();

void setSleepEnabled(bool enabled);

bool getTempSensorEnabled();

void setTempSensorEnabled(bool enabled);

uint8_t getMasterClockSpeed();

void setMasterClockSpeed(uint8_t speed);

int8_t writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);

int8_t writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);

int8_t writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);

int8_t writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);

int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);

int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);

int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);

int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);

int getState();

// WHO_AM_I register

uint8_t getDeviceID();

void getAcceleration(int16_t* x, int16_t* y, int16_t* z);

int16_t getAccelerationX();

int16_t getAccelerationY();

int16_t getAccelerationZ();

// TEMP_OUT_* registers

int16_t getTemperature();

// GYRO_*OUT_* registers

void getRotation(int16_t* x, int16_t* y, int16_t* z);

int16_t getRotationX();

int16_t getRotationY();

int16_t getRotationZ();

void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);

#endif

mpu6050.c

#include "Wire.h"

#include "MPU6050.h"

#include "log.h"

#define MPU6050_ADDR 0x68

uint8_t buffer[14];

int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {

beginTransmission(devAddr);

write_char(regAddr);

requestFrom(devAddr, length);

endTransmission();

int8_t count = 0;

while (available()) {

data[count] = readI2c();

count++;

}

LOG_E("=============================rn");

for (size_t i = 0; i < count; i++)

{

LOG_E("%2x data%d: %2x rn", regAddr, i, data[0]);

}

LOG_E("==============================rn");

return count;

}

int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {

return readBytes(devAddr, regAddr, 1, data);

}

/** Read a single bit from an 8-bit device register.

* @param devAddr I2C slave device address

* @param regAddr Register regAddr to read from

* @param bitNum Bit position to read (0-7)

* @param data Container for single bit value

* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)

* @return Status of read operation (true = success)

*/

int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {

uint8_t b;

uint8_t count = readByte(devAddr, regAddr, &b);

*data = b & (1 << bitNum);

return count;

}

/** Read multiple bits from an 8-bit device register.

* @param devAddr I2C slave device address

* @param regAddr Register regAddr to read from

* @param bitStart First bit position to read (0-7)

* @param length Number of bits to read (not more than 8)

* @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)

* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)

* @return Status of read operation (true = success)

*/

int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {

// 01101001 read byte

// 76543210 bit numbers

// xxx args: bitStart=4, length=3

// 010 masked

// -> 010 shifted

uint8_t count, b;

if ((count = readByte(devAddr, regAddr, &b)) != 0) {

uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);

b &= mask;

b >>= (bitStart - length + 1);

*data = b;

}

return count;

}

int8_t writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {

beginTransmission(devAddr);

write_char((uint8_t) regAddr); // send address

for (uint8_t i = 0; i < length; i++) {

write_char((uint8_t) data[i]);

}

endTransmission();

return 0;

}

int8_t writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {

return writeBytes(devAddr, regAddr, 1, &data);

}

int8_t writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {

// 010 value to write

// 76543210 bit numbers

// xxx args: bitStart=4, length=3

// 00011100 mask byte

// 10101111 original value (sample)

// 10100011 original & ~mask

// 10101011 masked | value

uint8_t b;

if(readByte(devAddr, regAddr, &b)!=0){

uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);

data <<= (bitStart - length + 1); // shift data into correct position

data &= mask; // zero all non-important bits in data

b &= ~(mask); // zero all important bits in existing byte

b |= data; // combine data with existing byte

return writeByte(devAddr, regAddr, b);

}

return 0;

}

int8_t writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {

uint8_t b;

readByte(devAddr, regAddr, &b);

b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));

return writeByte(devAddr, regAddr, b);

}

/** Get clock source setting.

* @return Current clock source setting

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_CLKSEL_BIT

* @see MPU6050_PWR1_CLKSEL_LENGTH

*/

uint8_t getClockSource() {

readBits(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer);

return buffer[0];

}

/** Set clock source setting.

* An internal 8MHz oscillator, gyroscope based clock, or external sources can

* be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator

* or an external source is chosen as the clock source, the MPU-60X0 can operate

* in low power modes with the gyroscopes disabled.

* Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.

* However, it is highly recommended that the device be configured to use one of

* the gyroscopes (or an external clock source) as the clock reference for

* improved stability. The clock source can be selected according to the following table:

*

* CLK_SEL | Clock Source

* --------+--------------------------------------

* 0 | Internal oscillator

* 1 | PLL with X Gyro reference

* 2 | PLL with Y Gyro reference

* 3 | PLL with Z Gyro reference

* 4 | PLL with external 32.768kHz reference

* 5 | PLL with external 19.2MHz reference

* 6 | Reserved

* 7 | Stops the clock and keeps the timing generator in reset

*

* @param source New clock source setting

* @see getClockSource()

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_CLKSEL_BIT

* @see MPU6050_PWR1_CLKSEL_LENGTH

*/

void setClockSource(uint8_t source) {

writeBits(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);

}

// GYRO_CONFIG register

/** Get full-scale gyroscope range.

* The FS_SEL parameter allows setting the full-scale range of the gyro sensors,

* as described in the table below.

*

* 0 = +/- 250 degrees/sec

* 1 = +/- 500 degrees/sec

* 2 = +/- 1000 degrees/sec

* 3 = +/- 2000 degrees/sec

*

* @return Current full-scale gyroscope range setting

* @see MPU6050_GYRO_FS_250

* @see MPU6050_RA_GYRO_CONFIG

* @see MPU6050_GCONFIG_FS_SEL_BIT

* @see MPU6050_GCONFIG_FS_SEL_LENGTH

*/

uint8_t getFullScaleGyroRange() {

readBits(MPU6050_ADDR, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer);

return buffer[0];

}

/** Set full-scale gyroscope range.

* @param range New full-scale gyroscope range value

* @see getFullScaleRange()

* @see MPU6050_GYRO_FS_250

* @see MPU6050_RA_GYRO_CONFIG

* @see MPU6050_GCONFIG_FS_SEL_BIT

* @see MPU6050_GCONFIG_FS_SEL_LENGTH

*/

void setFullScaleGyroRange(uint8_t range) {

writeBits(MPU6050_ADDR, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);

}

/** Get full-scale accelerometer range.

* The FS_SEL parameter allows setting the full-scale range of the accelerometer

* sensors, as described in the table below.

*

* 0 = +/- 2g

* 1 = +/- 4g

* 2 = +/- 8g

* 3 = +/- 16g

*

* @return Current full-scale accelerometer range setting

* @see MPU6050_ACCEL_FS_2

* @see MPU6050_RA_ACCEL_CONFIG

* @see MPU6050_ACONFIG_AFS_SEL_BIT

* @see MPU6050_ACONFIG_AFS_SEL_LENGTH

*/

uint8_t getFullScaleAccelRange() {

readBits(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer);

return buffer[0];

}

/** Set full-scale accelerometer range.

* @param range New full-scale accelerometer range setting

* @see getFullScaleAccelRange()

*/

void setFullScaleAccelRange(uint8_t range) {

writeBits(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);

}

/** Get the high-pass filter configuration.

* The DHPF is a filter module in the path leading to motion detectors (Free

* Fall, Motion threshold, and Zero Motion). The high pass filter output is not

* available to the data registers (see Figure in Section 8 of the MPU-6000/

* MPU-6050 Product Specification document).

* The high pass filter has three modes:

*

* Reset: The filter output settles to zero within one sample. This

* effectively disables the high pass filter. This mode may be toggled

* to quickly settle the filter.

* On: The high pass filter will pass signals above the cut off frequency.

* Hold: When triggered, the filter holds the present sample. The filter

* output will be the difference between the input sample and the held

* sample.

*

*

* ACCEL_HPF | Filter Mode | Cut-off Frequency

* ----------+-------------+------------------

* 0 | Reset | None

* 1 | On | 5Hz

* 2 | On | 2.5Hz

* 3 | On | 1.25Hz

* 4 | On | 0.63Hz

* 7 | Hold | None

*

* @return Current high-pass filter configuration

* @see MPU6050_DHPF_RESET

* @see MPU6050_RA_ACCEL_CONFIG

*/

uint8_t getDHPFMode() {

readBits(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer);

return buffer[0];

}

/** Set the high-pass filter configuration.

* @param bandwidth New high-pass filter configuration

* @see setDHPFMode()

* @see MPU6050_DHPF_RESET

* @see MPU6050_RA_ACCEL_CONFIG

*/

void setDHPFMode(uint8_t bandwidth) {

writeBits(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);

}

/** Get I2C master clock speed.

* I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the

* MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to

* the following table:

*

* I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider

* ------------+------------------------+-------------------

* 0 | 348kHz | 23

* 1 | 333kHz | 24

* 2 | 320kHz | 25

* 3 | 308kHz | 26

* 4 | 296kHz | 27

* 5 | 286kHz | 28

* 6 | 276kHz | 29

* 7 | 267kHz | 30

* 8 | 258kHz | 31

* 9 | 500kHz | 16

* 10 | 471kHz | 17

* 11 | 444kHz | 18

* 12 | 421kHz | 19

* 13 | 400kHz | 20

* 14 | 381kHz | 21

* 15 | 364kHz | 22

*

* @return Current I2C master clock speed

* @see MPU6050_RA_I2C_MST_CTRL

*/

uint8_t getMasterClockSpeed() {

readBits(MPU6050_ADDR, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer);

return buffer[0];

}

/** Set I2C master clock speed.

* @reparam speed Current I2C master clock speed

* @see MPU6050_RA_I2C_MST_CTRL

*/

void setMasterClockSpeed(uint8_t speed) {

writeBits(MPU6050_ADDR, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);

}

/** Get 3-axis accelerometer readings.

* These registers store the most recent accelerometer measurements.

* Accelerometer measurements are written to these registers at the Sample Rate

* as defined in Register 25.

* The accelerometer measurement registers, along with the temperature

* measurement registers, gyroscope measurement registers, and external sensor

* data registers, are composed of two sets of registers: an internal register

* set and a user-facing read register set.

* The data within the accelerometer sensors' internal register set is always

* updated at the Sample Rate. Meanwhile, the user-facing read register set

* duplicates the internal register set's data values whenever the serial

* interface is idle. This guarantees that a burst read of sensor registers will

* read measurements from the same sampling instant. Note that if burst reads

* are not used, the user is responsible for ensuring a set of single byte reads

* correspond to a single sampling instant by checking the Data Ready interrupt.

* Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS

* (Register 28). For each full scale setting, the accelerometers' sensitivity

* per LSB in ACCEL_xOUT is shown in the table below:

*

* AFS_SEL | Full Scale Range | LSB Sensitivity

* --------+------------------+----------------

* 0 | +/- 2g | 8192 LSB/mg

* 1 | +/- 4g | 4096 LSB/mg

* 2 | +/- 8g | 2048 LSB/mg

* 3 | +/- 16g | 1024 LSB/mg

*

* @param x 16-bit signed integer container for X-axis acceleration

* @param y 16-bit signed integer container for Y-axis acceleration

* @param z 16-bit signed integer container for Z-axis acceleration

* @see MPU6050_RA_GYRO_XOUT_H

*/

void getAcceleration(int16_t* x, int16_t* y, int16_t* z) {

readBytes(MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);

*x = (((int16_t)buffer[0]) << 8) | buffer[1];

*y = (((int16_t)buffer[2]) << 8) | buffer[3];

*z = (((int16_t)buffer[4]) << 8) | buffer[5];

}

/** Get X-axis accelerometer reading.

* @return X-axis acceleration measurement in 16-bit 2's complement format

* @see getMotion6()

* @see MPU6050_RA_ACCEL_XOUT_H

*/

int16_t getAccelerationX() {

readBytes(MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

/** Get Y-axis accelerometer reading.

* @return Y-axis acceleration measurement in 16-bit 2's complement format

* @see getMotion6()

* @see MPU6050_RA_ACCEL_YOUT_H

*/

int16_t getAccelerationY() {

readBytes(MPU6050_ADDR, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

/** Get Z-axis accelerometer reading.

* @return Z-axis acceleration measurement in 16-bit 2's complement format

* @see getMotion6()

* @see MPU6050_RA_ACCEL_ZOUT_H

*/

int16_t getAccelerationZ() {

readBytes(MPU6050_ADDR, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

// TEMP_OUT_* registers

/** Get current internal temperature.

* @return Temperature reading in 16-bit 2's complement format

* @see MPU6050_RA_TEMP_OUT_H

*/

int16_t getTemperature() {

readBytes(MPU6050_ADDR, MPU6050_RA_TEMP_OUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

// GYRO_*OUT_* registers

/** Get 3-axis gyroscope readings.

* These gyroscope measurement registers, along with the accelerometer

* measurement registers, temperature measurement registers, and external sensor

* data registers, are composed of two sets of registers: an internal register

* set and a user-facing read register set.

* The data within the gyroscope sensors' internal register set is always

* updated at the Sample Rate. Meanwhile, the user-facing read register set

* duplicates the internal register set's data values whenever the serial

* interface is idle. This guarantees that a burst read of sensor registers will

* read measurements from the same sampling instant. Note that if burst reads

* are not used, the user is responsible for ensuring a set of single byte reads

* correspond to a single sampling instant by checking the Data Ready interrupt.

* Each 16-bit gyroscope measurement has a full scale defined in FS_SEL

* (Register 27). For each full scale setting, the gyroscopes' sensitivity per

* LSB in GYRO_xOUT is shown in the table below:

*

* FS_SEL | Full Scale Range | LSB Sensitivity

* -------+--------------------+----------------

* 0 | +/- 250 degrees/s | 131 LSB/deg/s

* 1 | +/- 500 degrees/s | 65.5 LSB/deg/s

* 2 | +/- 1000 degrees/s | 32.8 LSB/deg/s

* 3 | +/- 2000 degrees/s | 16.4 LSB/deg/s

*

* @param x 16-bit signed integer container for X-axis rotation

* @param y 16-bit signed integer container for Y-axis rotation

* @param z 16-bit signed integer container for Z-axis rotation

* @see getMotion6()

* @see MPU6050_RA_GYRO_XOUT_H

*/

void getRotation(int16_t* x, int16_t* y, int16_t* z) {

readBytes(MPU6050_ADDR, MPU6050_RA_GYRO_XOUT_H, 6, buffer);

*x = (((int16_t)buffer[0]) << 8) | buffer[1];

*y = (((int16_t)buffer[2]) << 8) | buffer[3];

*z = (((int16_t)buffer[4]) << 8) | buffer[5];

}

/** Get X-axis gyroscope reading.

* @return X-axis rotation measurement in 16-bit 2's complement format

* @see getMotion6()

* @see MPU6050_RA_GYRO_XOUT_H

*/

int16_t getRotationX() {

readBytes(MPU6050_ADDR, MPU6050_RA_GYRO_XOUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

/** Get Y-axis gyroscope reading.

* @return Y-axis rotation measurement in 16-bit 2's complement format

* @see getMotion6()

* @see MPU6050_RA_GYRO_YOUT_H

*/

int16_t getRotationY() {

readBytes(MPU6050_ADDR, MPU6050_RA_GYRO_YOUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

/** Get Z-axis gyroscope reading.

* @return Z-axis rotation measurement in 16-bit 2's complement format

* @see getMotion6()

* @see MPU6050_RA_GYRO_ZOUT_H

*/

int16_t getRotationZ() {

readBytes(MPU6050_ADDR, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);

return (((int16_t)buffer[0]) << 8) | buffer[1];

}

// PWR_MGMT_1 register

/** Trigger a full device reset.

* A small delay of ~50ms may be desirable after triggering a reset.

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_DEVICE_RESET_BIT

*/

void reset() {

writeBit(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);

}

/** Get sleep mode status.

* Setting the SLEEP bit in the register puts the device into very low power

* sleep mode. In this mode, only the serial interface and internal registers

* remain active, allowing for a very low standby current. Clearing this bit

* puts the device back into normal mode. To save power, the individual standby

* selections for each of the gyros should be used if any gyro axis is not used

* by the application.

* @return Current sleep mode enabled status

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_SLEEP_BIT

*/

bool getSleepEnabled() {

readBit(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);

return buffer[0];

}

/** Set sleep mode status.

* @param enabled New sleep mode enabled status

* @see getSleepEnabled()

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_SLEEP_BIT

*/

void setSleepEnabled(bool enabled) {

writeBit(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);

}

/** Get temperature sensor enabled status.

* Control the usage of the internal temperature sensor.

* Note: this register stores the *disabled* value, but for consistency with the

* rest of the code, the function is named and used with standard true/false

* values to indicate whether the sensor is enabled or disabled, respectively.

* @return Current temperature sensor enabled status

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_TEMP_DIS_BIT

*/

bool getTempSensorEnabled() {

readBit(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);

return buffer[0] == 0; // 1 is actually disabled here

}

/** Set temperature sensor enabled status.

* Note: this register stores the *disabled* value, but for consistency with the

* rest of the code, the function is named and used with standard true/false

* values to indicate whether the sensor is enabled or disabled, respectively.

* @param enabled New temperature sensor enabled status

* @see getTempSensorEnabled()

* @see MPU6050_RA_PWR_MGMT_1

* @see MPU6050_PWR1_TEMP_DIS_BIT

*/

void setTempSensorEnabled(bool enabled) {

// 1 is actually disabled here

writeBit(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);

}

uint8_t getDeviceID() {

uint8_t buf[1];

readByte(MPU6050_ADDR, MPU6050_RA_WHO_AM_I, buf);

LOG_E("MPU6050 getDeviceID ：%2x!rn", buf[0]);

return buf[0];

}

// SMPLRT_DIV register

/** Get gyroscope output rate divider.

* The sensor register output, FIFO output, DMP sampling, Motion detection, Zero

* Motion detection, and Free Fall detection are all based on the Sample Rate.

* The Sample Rate is generated by dividing the gyroscope output rate by

* SMPLRT_DIV:

* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)

* where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or

* 7), and 1kHz when the DLPF is enabled (see Register 26).

* Note: The accelerometer output rate is 1kHz. This means that for a Sample

* Rate greater than 1kHz, the same accelerometer sample may be output to the

* FIFO, DMP, and sensor registers more than once.

* For a diagram of the gyroscope and accelerometer signal paths, see Section 8

* of the MPU-6000/MPU-6050 Product Specification document.

* @return Current sample rate

* @see MPU6050_RA_SMPLRT_DIV

*/

uint8_t getRate() {

readByte(MPU6050_ADDR, MPU6050_RA_SMPLRT_DIV, buffer);

return buffer[0];

}

/** Set gyroscope sample rate divider.

* @param rate New sample rate divider

* @see getRate()

* @see MPU6050_RA_SMPLRT_DIV

*/

void setRate(uint8_t rate) {

writeByte(MPU6050_ADDR, MPU6050_RA_SMPLRT_DIV, rate);

}

void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {

LOG_E("MPU6050 getMotion6!rn");

readBytes(MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);

*ax = (((int16_t)buffer[0]) << 8) | buffer[1];

*ay = (((int16_t)buffer[2]) << 8) | buffer[3];

*az = (((int16_t)buffer[4]) << 8) | buffer[5];

*gx = (((int16_t)buffer[8]) << 8) | buffer[9];

*gy = (((int16_t)buffer[10]) << 8) | buffer[11];

*gz = (((int16_t)buffer[12]) << 8) | buffer[13];

}

void MPU6050() {

begin();

// setClockSource(MPU6050_CLOCK_PLL_XGYRO);

// setFullScaleGyroRange(MPU6050_GYRO_FS_1000);

// setFullScaleAccelRange(MPU6050_ACCEL_FS_2);

// setSleepEnabled(false);

writeByte(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, 0x01);

writeByte(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_2, 0x00);

writeByte(MPU6050_ADDR, MPU6050_RA_GYRO_CONFIG, 0x18);

writeByte(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, 0x18);

}

消息推送目前还没时间完善，使用pushplus官方地址

https://www.pushplus.plus/，申请注册后拿到token使用http请求即可实现消息推送。消息推送到微信可以参考:

#!/bin/sh

这个是做内网穿透的时候，由于natapp免费版总是变地址，写了个脚本，定时发送新的域名用的。日志经过清理，只保留的INFO部分，基本不会太大。

可以直接将消息推送到微信。

审核编辑 黄宇