Weber Yang
For nucleoF103 change sda,scl to PB_7,PB_6
Dependents: AGV_0411 AGV_SMT_Sevro AGV_SMT
Fork of
MPU6050-DMP
by 
takaaki mastuzawa
Diff: MPU6050.h
- Revision:
- 8:473800bd51ac
- Parent:
- 3:25e1a5a10e53
--- a/MPU6050.h Thu Apr 12 01:11:08 2018 +0000
+++ b/MPU6050.h Thu May 10 08:15:44 2018 +0000
@@ -1,50 +1,21 @@
-//ported from arduino library: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
-//written by szymon gaertig (email: szymon@gaertig.com.pl)
-//
-//Changelog:
-//2013-01-08 - first beta release
-
-// I2Cdev library collection - MPU6050 I2C device class
-// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
-// 10/3/2011 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-// ... - ongoing debug release
-
-// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
-// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF
-// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING.
-
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
-
#ifndef _MPU6050_H_
#define _MPU6050_H_
#include "I2Cdev.h"
-#include "helper_3dmath.h"
+
+// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517
+// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
+
+#ifdef __AVR__
+#include <avr/pgmspace.h>
+#else
+#define PROGMEM /* empty */
+#define pgm_read_byte(x) (*(x))
+#define pgm_read_word(x) (*(x))
+#define pgm_read_float(x) (*(x))
+#define PSTR(STR) STR
+#endif
+
#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)
@@ -62,6 +33,10 @@
#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_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0]
#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
@@ -102,32 +77,20 @@
#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 MPU9150_RA_MAG_ADDRESS 0x0C
-#define MPU9150_RA_MAG_XOUT_L 0x03
-#define MPU9150_RA_MAG_XOUT_H 0x04
-#define MPU9150_RA_MAG_YOUT_L 0x05
-#define MPU9150_RA_MAG_YOUT_H 0x06
-#define MPU9150_RA_MAG_ZOUT_L 0x07
-#define MPU9150_RA_MAG_ZOUT_H 0x08
-
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
@@ -173,6 +136,26 @@
#define MPU6050_RA_FIFO_R_W 0x74
#define MPU6050_RA_WHO_AM_I 0x75
+#define MPU6050_SELF_TEST_XA_1_BIT 0x07
+#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03
+#define MPU6050_SELF_TEST_XA_2_BIT 0x05
+#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02
+#define MPU6050_SELF_TEST_YA_1_BIT 0x07
+#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03
+#define MPU6050_SELF_TEST_YA_2_BIT 0x03
+#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02
+#define MPU6050_SELF_TEST_ZA_1_BIT 0x07
+#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03
+#define MPU6050_SELF_TEST_ZA_2_BIT 0x01
+#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02
+
+#define MPU6050_SELF_TEST_XG_1_BIT 0x04
+#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05
+#define MPU6050_SELF_TEST_YG_1_BIT 0x04
+#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05
+#define MPU6050_SELF_TEST_ZG_1_BIT 0x04
+#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05
+
#define MPU6050_TC_PWR_MODE_BIT 7
#define MPU6050_TC_OFFSET_BIT 6
#define MPU6050_TC_OFFSET_LENGTH 6
@@ -415,9 +398,6 @@
// note: DMP code memory blocks defined at end of header file
class MPU6050 {
- private:
- I2Cdev i2Cdev;
- Serial debugSerial;
public:
MPU6050();
MPU6050(uint8_t address);
@@ -443,6 +423,15 @@
uint8_t getFullScaleGyroRange();
void setFullScaleGyroRange(uint8_t range);
+ // SELF_TEST registers
+ uint8_t getAccelXSelfTestFactoryTrim();
+ uint8_t getAccelYSelfTestFactoryTrim();
+ uint8_t getAccelZSelfTestFactoryTrim();
+
+ uint8_t getGyroXSelfTestFactoryTrim();
+ uint8_t getGyroYSelfTestFactoryTrim();
+ uint8_t getGyroZSelfTestFactoryTrim();
+
// ACCEL_CONFIG register
bool getAccelXSelfTest();
void setAccelXSelfTest(bool enabled);
@@ -617,6 +606,7 @@
uint32_t getExternalSensorDWord(int position);
// MOT_DETECT_STATUS register
+ uint8_t getMotionStatus();
bool getXNegMotionDetected();
bool getXPosMotionDetected();
bool getYNegMotionDetected();
@@ -695,22 +685,22 @@
// WHO_AM_I register
uint8_t getDeviceID();
void setDeviceID(uint8_t id);
-
+
// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
-
+
// XG_OFFS_TC register
uint8_t getOTPBankValid();
void setOTPBankValid(bool enabled);
- int8_t getXGyroOffset();
- void setXGyroOffset(int8_t offset);
+ int8_t getXGyroOffsetTC();
+ void setXGyroOffsetTC(int8_t offset);
// YG_OFFS_TC register
- int8_t getYGyroOffset();
- void setYGyroOffset(int8_t offset);
+ int8_t getYGyroOffsetTC();
+ void setYGyroOffsetTC(int8_t offset);
// ZG_OFFS_TC register
- int8_t getZGyroOffset();
- void setZGyroOffset(int8_t offset);
+ int8_t getZGyroOffsetTC();
+ void setZGyroOffsetTC(int8_t offset);
// X_FINE_GAIN register
int8_t getXFineGain();
@@ -737,23 +727,23 @@
void setZAccelOffset(int16_t offset);
// XG_OFFS_USR* registers
- int16_t getXGyroOffsetUser();
- void setXGyroOffsetUser(int16_t offset);
+ int16_t getXGyroOffset();
+ void setXGyroOffset(int16_t offset);
// YG_OFFS_USR* register
- int16_t getYGyroOffsetUser();
- void setYGyroOffsetUser(int16_t offset);
+ int16_t getYGyroOffset();
+ void setYGyroOffset(int16_t offset);
// ZG_OFFS_USR* register
- int16_t getZGyroOffsetUser();
- void setZGyroOffsetUser(int16_t offset);
-
+ int16_t getZGyroOffset();
+ void setZGyroOffset(int16_t offset);
+
// INT_ENABLE register (DMP functions)
bool getIntPLLReadyEnabled();
void setIntPLLReadyEnabled(bool enabled);
bool getIntDMPEnabled();
void setIntDMPEnabled(bool enabled);
-
+
// DMP_INT_STATUS
bool getDMPInt5Status();
bool getDMPInt4Status();
@@ -765,18 +755,18 @@
// INT_STATUS register (DMP functions)
bool getIntPLLReadyStatus();
bool getIntDMPStatus();
-
+
// USER_CTRL register (DMP functions)
bool getDMPEnabled();
void setDMPEnabled(bool enabled);
void resetDMP();
-
+
// BANK_SEL register
void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false);
-
+
// MEM_START_ADDR register
void setMemoryStartAddress(uint8_t address);
-
+
// MEM_R_W register
uint8_t readMemoryByte();
void writeMemoryByte(uint8_t data);
@@ -808,12 +798,12 @@
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);
-
+
// Register callbacks after a packet of FIFO data is processed
//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();
-
+
// Setup FIFO for various output
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
@@ -873,7 +863,7 @@
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
-
+
uint8_t dmpGetEuler(float *data, Quaternion *q);
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
@@ -909,12 +899,12 @@
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);
-
+
// Register callbacks after a packet of FIFO data is processed
//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();
-
+
// Setup FIFO for various output
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
@@ -975,7 +965,7 @@
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
-
+
uint8_t dmpGetEuler(float *data, Quaternion *q);
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);