本作品由安信可社區(qū)用戶

lazy制作

設(shè)計初想

在不能關(guān)照老人與小孩兒的時候，可能會存在老人或小孩兒摔倒的情況。如果摔倒不及時處置可能造成比較嚴(yán)重的傷害。

這個小作品主要就是跌倒提醒的功能，網(wǎng)上也有類似的作品。這里本人使用安信可的Ai-M61-32su復(fù)刻一下。如果有跌倒?fàn)顟B(tài)時紅燈會亮起（默認(rèn)綠燈），小伙伴還可以考慮增加蜂鳴器等。

制作思路

摔倒檢測算法可以通過檢測加速度和角速度的變化來判斷物體是否發(fā)生了摔倒。在實現(xiàn)MPU6050摔倒檢測算法時，首先需要讀取MPU6050傳感器的原始數(shù)據(jù)，通過計算加速度和角速度的變化率來判斷物體是否摔倒，可以通過設(shè)置一個閾值來判斷加速度和角速度是否超過了正常范圍。

如果超過了閾值，則可認(rèn)為物體發(fā)生了摔倒。另外，還可以使用一些機器學(xué)習(xí)算法來提高摔倒檢測的準(zhǔn)確性。通過對大量的摔倒和非摔倒?fàn)顟B(tài)的數(shù)據(jù)進(jìn)行訓(xùn)練，可以建立一個模型來判斷物體是否發(fā)生了摔倒。這種方法可以更好地適應(yīng)不同物體和環(huán)境的情況，并提高摔倒檢測的靈敏度和準(zhǔn)確性。

一、MPU6050簡介

首先對設(shè)備所使用的主要硬件進(jìn)行介紹，【轉(zhuǎn)載】MPU6050簡介 - 魚芯工作室的文章 - 知乎

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

內(nèi)容如下：

MPU-60X0 是全球首例9 軸運動處理傳感器。它集成了3 軸MEMS 陀螺儀，3 軸MEMS加速度計，以及一個可擴(kuò)展的數(shù)字運動處理器DMP（Digital Motion Processor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。

擴(kuò)展之后就可以通過其I2C 或SPI 接口輸出一個9 軸的信號（SPI 接口僅在MPU-6000 可用）。MPU-60X0 也可以通過其I2C 接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。

MPU-60X0 對陀螺儀和加速度計分別用了三個16 位的ADC（0~65535），將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運動，傳感器的測量范圍都是用戶可控的，陀螺儀可測范圍為±250，±500，±1000，±2000°/秒（dps），加速度計可測范圍為±2，±4，±8，±16g。

芯片尺寸4×4×0.9mm，采用QFN 封裝（無引線方形封裝），可承受最大10000g 的沖擊，并有可編程的低通濾波器。

MPU6050的內(nèi)部框圖如下圖所示：

其中，需要了解的引腳有：SCL和 SDA是連接MCU的 IIC接口，MCU通過這個IIC 接口來控制MPU6050，另外還有一個 IIC 接口：AUX_CL和AUX_DA ，這個接口可用來連外部從設(shè)備比如磁力計，這樣就可以組成一個九軸傳感器。VLOGIC是IO口電壓，該引腳最低可以到1.8V電壓， 我們一般直接連VDD即可。AD0是從IIC 接口（接 MCU）的地址控制引腳，該引腳控制的是IIC 地址的最低位。如果接 GND ，則 MPU6050的IIC地址是：0X68，如果接VDD，則是0X69。

需要注意的是：這里的地址0x68和0x69是不包含用于數(shù)據(jù)傳輸?shù)淖畹臀坏模虼瞬⒉皇前宋粩?shù)據(jù)，如0x68表示的是110 1000，0x69表示的則是110 1001，通常最低位用于表示IIC主機的讀取數(shù)據(jù)/寫數(shù)據(jù)模式。self test為自檢，自檢的作用是可用來測試傳感器的機械和電氣結(jié)構(gòu)。也就是說通過自檢來測試芯片是否損壞。自檢啟動后，電路會使傳感器工作并且產(chǎn)生輸出信號。

關(guān)于自檢的具體說明，官方芯片手冊里有詳細(xì)描述。如下：

陀螺儀自檢

當(dāng)自檢激活時，車載電子設(shè)備將啟動相應(yīng)的傳感器。該致動將使傳感器的檢測質(zhì)量移動相當(dāng)于預(yù)定義科里奧利力的距離。該檢驗質(zhì)量位移導(dǎo)致傳感器輸出的變化，該變化反映在輸出信號中。輸出信號用于觀察自檢響應(yīng)。

自檢響應(yīng)定義如下：

自檢響應(yīng)=啟用自檢的傳感器輸出-未啟用自檢的檢波器輸出每個陀螺儀軸的自檢限值在MPU-6000/MPU-6050產(chǎn)品規(guī)范文件的電氣特性表中提供。當(dāng)自檢響應(yīng)的值在產(chǎn)品規(guī)格的最小/最大限值內(nèi)時，零件已通過自檢。當(dāng)自檢響應(yīng)超過文件中規(guī)定的最小/最大值時，該部件被視為自檢失敗。

加速計自檢

當(dāng)自檢激活時，車載電子設(shè)備將啟動相應(yīng)的傳感器。該動作模擬外力。被致動的傳感器反過來將產(chǎn)生相應(yīng)的輸出信號。輸出信號用于觀察自檢響應(yīng)。

自檢響應(yīng)定義如下：

自檢響應(yīng)=啟用自檢的傳感器輸出-未啟用自檢的檢波器輸出每個加速計軸的自檢限值在MPU-6000/MPU-6050產(chǎn)品規(guī)范文件的電氣特性表中提供。當(dāng)自檢響應(yīng)的值在產(chǎn)品規(guī)格的最小/最大限值內(nèi)時，零件已通過自檢。當(dāng)自檢響應(yīng)超過文件中規(guī)定的最小/最大值時，該部件被視為自檢失敗。

MPU6050相關(guān)寄存器

MPU6050官方的寄存器手冊上共介紹了40個寄存器的內(nèi)容和功能，在此只選取一些常用的和重要的寄存器作為了解。

1.采樣分頻寄存器 Sample Rate Divider

說明：該寄存器指定陀螺儀輸出速率的分頻器，用于為MPU-60X0生成采樣速率。

傳感器寄存器輸出，F(xiàn)IFO輸出，DMP采樣，運動檢測，靜止檢測和自由落體檢測都基于這個采樣頻率。

采樣頻率=陀螺儀輸出頻率/（1+SMPLRT_DIV）

當(dāng) DLPF（數(shù)字低通濾波器，見寄存器Configuration）禁用時（DLPF_CFG=0 or 7），陀螺輸出頻率=8kHz；當(dāng) DLPF 使能，陀螺儀輸出頻率=1KHz。注意：加速度計輸出頻率為 1KHz。這意味著，當(dāng)采樣頻率大于1KHZ時，同個加速度計采樣得到的數(shù)據(jù)，可能不止一次輸出到FIFO、DMP、傳感器寄存器。

2.配置寄存器 Configuration

說明：該寄存器為陀螺儀和加速度計配置外部幀同步(FSYNC) 管腳的采樣和數(shù)字低通濾波(DLPF)設(shè)置。

其中，數(shù)字低通濾波器DLPF由DLPF_CFG配置。根據(jù)下表所示的DLPF_CFG值對加速度計和陀螺儀進(jìn)行濾波。

其中，F(xiàn)S為陀螺儀輸出頻率。SMPLRT_DIV由預(yù)設(shè)定的采樣頻率根據(jù)上述的公式計算得出。一般情況下，DPLF濾波頻率為采樣頻率的一半，如設(shè)定采樣頻率為50Hz，由表可知當(dāng)FS為1kHz，SMPLRT_DIV的值為1000/50-1=19。

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

說明：該寄存器是用來觸發(fā)陀螺儀自檢和配置陀螺儀的滿量程范圍。

其中，XG_ST、YG_ST、ZG_ST分別用來設(shè)置陀螺儀X軸、Y軸、Z軸自檢，置0則不觸發(fā)自檢。FS_SEL[1:0]用于設(shè)置陀螺儀的滿量程，如下表：

我們一般設(shè)置為3，即滿量程為±2000°/s，由于采用16位ADC即0-65536，則靈敏度G=65536/4000=16.4LSB/（°/s），LSB表示最低有效位，即1°/s對應(yīng)的數(shù)字量為16.4。最終即可將陀螺儀輸出的數(shù)字量數(shù)據(jù)轉(zhuǎn)化為角速度。

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

說明：該寄存器是用來觸發(fā)加速度計自檢和配置加速度計的滿量程范圍。同時這個寄存器也可以用于配置數(shù)字高通濾波器（DHPF）。

其中，XA_ST、YA_ST、ZA_ST分別用來設(shè)置加速度計X軸、Y軸、Z軸自檢，置0則不觸發(fā)自檢。AFS_SEL[1:0]用于選擇加速度計的滿量程范圍，如下表：

我們一般設(shè)置為0，即滿量程為±2g，由于采用16位ADC即0-65536，則靈敏度G=65536/4=16384LSB/（g），LSB表示最低有效位，即1g對應(yīng)的數(shù)字量為16384。最終即可將加速度計輸出的數(shù)字量數(shù)據(jù)轉(zhuǎn)化為加速度。

5.加速度計測量值寄存器 Accelerometer

Measurements

說明：該寄存器存儲最近加速度計的測量值。加速度計根據(jù)采樣頻率（由采樣分頻寄存器寄存器設(shè)定 ）寫入到這些寄存器。即采樣頻率為50Hz，寫入數(shù)據(jù)的時間間隔為0.02s。加速度計測量值寄存器和溫度測量值寄存器，陀螺儀測量值寄存器，外部傳感器數(shù)據(jù)寄存器都是由兩組寄存器構(gòu)成：一個內(nèi)部寄存器集和一個用于用戶讀取的寄存器集。

加速度計傳感器的內(nèi)部寄存器集合里的數(shù)據(jù)根據(jù)采樣頻率更新。以此同時，每當(dāng)串行接口處于閑置狀態(tài)，面向用戶的讀取寄存器集合會復(fù)制內(nèi)部寄存器集合的數(shù)據(jù)值。這保證了突發(fā)讀取時傳感器寄存器可以讀到相同的采樣時刻的測量值。需要注意的是，如果沒有突發(fā)讀取，則用戶負(fù)責(zé)通過檢查數(shù)據(jù)就緒中斷（Data Ready interrupt）來確保瞬時的一組單字節(jié)讀取對應(yīng)于單字節(jié)的采樣數(shù)據(jù)。

參數(shù)：

ACCEL_XOUT ：

由 2部分組成的 16位數(shù)值存儲最近X 軸加速度計的測量值。

ACCEL_YOUT ：

由 2部分組成的 16位數(shù)值存儲最近Y 軸加速度計的測量值。

ACCEL_ZOUT ：由 2部分組成的 16位數(shù)值存儲最近Z 軸加速度計的測量值。

6.陀螺儀測量值寄存器 Gyroscope Measurements

說明：該寄存器存儲最近加陀螺儀的測量值。大致構(gòu)成與加速度計測量值寄存器相同，此處便不做敘述。參數(shù)分別為：GYRO_XOUT 、GYRO_YOUT 、GYRO_ZOUT 。

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

說明：該寄存器允許用戶配置電源模式和時鐘源，還提供了復(fù)位整個設(shè)備和禁用溫度傳感器的位。當(dāng)置SLEEP位為1時，MPU-60X0 可以進(jìn)入低功耗睡眠模式。當(dāng)SLEEP位禁用且 CYCLE位置 1時，MPU-60X0進(jìn)入循環(huán)模式（CycleMode）。在循環(huán)模式下，設(shè)備在休眠模式和喚醒之間循環(huán)，以LP_WAKE_CTRL（由電源管理2寄存器配置）確定的速率從active sensors（此處不知如何翻譯）獲取單個數(shù)據(jù)樣本。

該寄存器的最低三位用于設(shè)置系統(tǒng)的時鐘源選擇，默認(rèn)值是0（內(nèi)部8M RC振蕩），不過一般設(shè)置為1，選擇x軸陀螺PLL作為時鐘源，以獲得更高精度的時鐘。同時，使能角速度傳感器和加速度傳感器，這兩個操作通過電源管理寄存器2配置，設(shè)置對應(yīng)位為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.

其他參數(shù)：

DEVICE_RESET

該位置 1，重啟內(nèi)部寄存器到默認(rèn)值。復(fù)位完成后該位自動清0。

TEMP_DIS該位置 1，禁用溫度傳感器。

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

說明：該寄存器允許用戶在加速度計低功耗模式下配置喚醒頻率。也允許用戶讓加速度計和陀螺儀的個別軸進(jìn)入待機模式。

只讓MPU-60X0的加速度計進(jìn)入低功耗模式的步驟如下：

1.置 CYCLE位為 1

2.置 SLEEP位為 1

3.置 TEMP_DIS位為 1

4.置 STBY_XG,STBY_YG,STBY_ZG位為 1

在這種模式下，設(shè)備會關(guān)閉除了主 I2C接口外其他所有設(shè)備，加速度計只在固定的間隔喚醒并測量一次。喚醒的頻率可以通過配置 LP_WAKE_CTRL實現(xiàn)如下：

參數(shù)：

LP_WAKE_CTRL ：

2位無符號數(shù)值。指定加速度計在低功耗模式下的喚醒頻率。

STBY_XA ：

該位置 1，加速度計的 X軸進(jìn)入待機模式。

STBY_YA ：

該位置 1，加速度計的 Y軸進(jìn)入待機模式。

STBY_ZA ：

該位置 1，加速度計的 Z軸進(jìn)入待機模式。

STBY_XG ：

該位置 1，陀螺儀的 X軸進(jìn)入待機模式。

STBY_YG ：

該位置 1，陀螺儀的 Y軸進(jìn)入待機模式。

STBY_ZG ：該位置 1，陀螺儀的 Z軸進(jìn)入待機模式。

對于MPU 6050 進(jìn)行了簡單的介紹，也對這個模塊有了一些了解。

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請求即可實現(xiàn)消息推送。消息推送到微信可以參考:

#!/bin/sh

這個是做內(nèi)網(wǎng)穿透的時候，由于natapp免費版總是變地址，寫了個腳本，定時發(fā)送新的域名用的。日志經(jīng)過清理，只保留的INFO部分，基本不會太大。

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

審核編輯 黃宇