Skip to content
Browse files

Fix to include proper OUTPUT_READABLE_WORLDACCEL format

  • Loading branch information...
1 parent 59a8c95 commit 4b0253ea3c563ccbba38561490a000a85540b960 @jrowberg committed Jul 9, 2012
Showing with 7 additions and 7 deletions.
  1. +7 −7 Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino
View
14 Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino
@@ -319,19 +319,19 @@ void loop() {
Serial.println(aaReal.z);
#endif
- #ifdef OUTPUT_READABLE_FRAMEACCEL
- // display initial-frame acceleration, adjusted to remove gravity
+ #ifdef OUTPUT_READABLE_WORLDACCEL
+ // display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccelInWorld(&aaFrame, &aaReal, &q);
- Serial.print("aframe\t");
- Serial.print(aaFrame.x);
+ mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+ Serial.print("aworld\t");
+ Serial.print(aaWorld.x);
Serial.print("\t");
- Serial.print(aaFrame.y);
+ Serial.print(aaWorld.y);
Serial.print("\t");
- Serial.println(aaFrame.z);
+ Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT

4 comments on commit 4b0253e

@wxclb
wxclb commented on 4b0253e Mar 9, 2013

/*
DMP of MPU-60X0, adapted for use with MultiWii and Picopter
20120304 by Frank26080115

adapted from code "MPU-6050 6-axis DMP Demo v0.01" by Noah Zerkin in 2011
*/

#include "common.h"
#include "MPU6050.h"
#include "stm32f10x_i2c.h"
#include "math.h"
#define ROLL 0
#define PITCH 1
#define YAW 2

#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F

#define INV_MAX_NUM_ACCEL_SAMPLES (8)
#define DMP_REF_QUATERNION (0)
#define DMP_REF_GYROS (DMP_REF_QUATERNION + 4) // 4
#define DMP_REF_CONTROL (DMP_REF_GYROS + 3) // 7
#define DMP_REF_RAW (DMP_REF_CONTROL + 4) // 11
#define DMP_REF_RAW_EXTERNAL (DMP_REF_RAW + 8) // 19
#define DMP_REF_ACCEL (DMP_REF_RAW_EXTERNAL + 6) // 25
#define DMP_REF_QUANT_ACCEL (DMP_REF_ACCEL + 3) // 28
#define DMP_REF_QUATERNION_6AXIS (DMP_REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) // 36
#define DMP_REF_EIS (DMP_REF_QUATERNION_6AXIS + 4) // 40
#define DMP_REF_DMP_PACKET (DMP_REF_EIS + 3) // 43
#define DMP_REF_GARBAGE (DMP_REF_DMP_PACKET + 1) // 44
#define DMP_REF_LAST (DMP_REF_GARBAGE + 1) // 45

#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU_RA_XA_OFFS_L_TC 0x07
#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU_RA_YA_OFFS_L_TC 0x09
#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU_RA_ZA_OFFS_L_TC 0x0B
#define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU_RA_XG_OFFS_USRL 0x14
#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU_RA_YG_OFFS_USRL 0x16
#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU_RA_ZG_OFFS_USRL 0x18
#define MPU_RA_SMPLRT_DIV 0x19
#define MPU_RA_CONFIG 0x1A
#define MPU_RA_GYRO_CONFIG 0x1B
#define MPU_RA_ACCEL_CONFIG 0x1C
#define MPU_RA_FF_THR 0x1D
#define MPU_RA_FF_DUR 0x1E
#define MPU_RA_MOT_THR 0x1F
#define MPU_RA_MOT_DUR 0x20
#define MPU_RA_ZRMOT_THR 0x21
#define MPU_RA_ZRMOT_DUR 0x22
#define MPU_RA_FIFO_EN 0x23
#define MPU_RA_I2C_MST_CTRL 0x24
#define MPU_RA_I2C_SLV0_ADDR 0x25
#define MPU_RA_I2C_SLV0_REG 0x26
#define MPU_RA_I2C_SLV0_CTRL 0x27
#define MPU_RA_I2C_SLV1_ADDR 0x28
#define MPU_RA_I2C_SLV1_REG 0x29
#define MPU_RA_I2C_SLV1_CTRL 0x2A
#define MPU_RA_I2C_SLV2_ADDR 0x2B
#define MPU_RA_I2C_SLV2_REG 0x2C
#define MPU_RA_I2C_SLV2_CTRL 0x2D
#define MPU_RA_I2C_SLV3_ADDR 0x2E
#define MPU_RA_I2C_SLV3_REG 0x2F
#define MPU_RA_I2C_SLV3_CTRL 0x30
#define MPU_RA_I2C_SLV4_ADDR 0x31
#define MPU_RA_I2C_SLV4_REG 0x32
#define MPU_RA_I2C_SLV4_DO 0x33
#define MPU_RA_I2C_SLV4_CTRL 0x34
#define MPU_RA_I2C_SLV4_DI 0x35
#define MPU_RA_I2C_MST_STATUS 0x36
#define MPU_RA_INT_PIN_CFG 0x37
#define MPU_RA_INT_ENABLE 0x38
#define MPU_RA_DMP_INT_STATUS 0x39
#define MPU_RA_INT_STATUS 0x3A
#define MPU_RA_ACCEL_XOUT_H 0x3B
#define MPU_RA_ACCEL_XOUT_L 0x3C
#define MPU_RA_ACCEL_YOUT_H 0x3D
#define MPU_RA_ACCEL_YOUT_L 0x3E
#define MPU_RA_ACCEL_ZOUT_H 0x3F
#define MPU_RA_ACCEL_ZOUT_L 0x40
#define MPU_RA_TEMP_OUT_H 0x41
#define MPU_RA_TEMP_OUT_L 0x42
#define MPU_RA_GYRO_XOUT_H 0x43
#define MPU_RA_GYRO_XOUT_L 0x44
#define MPU_RA_GYRO_YOUT_H 0x45
#define MPU_RA_GYRO_YOUT_L 0x46
#define MPU_RA_GYRO_ZOUT_H 0x47
#define MPU_RA_GYRO_ZOUT_L 0x48
#define MPU_RA_EXT_SENS_DATA_00 0x49
#define MPU_RA_MOT_DETECT_STATUS 0x61
#define MPU_RA_I2C_SLV0_DO 0x63
#define MPU_RA_I2C_SLV1_DO 0x64
#define MPU_RA_I2C_SLV2_DO 0x65
#define MPU_RA_I2C_SLV3_DO 0x66
#define MPU_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU_RA_SIGNAL_PATH_RESET 0x68
#define MPU_RA_MOT_DETECT_CTRL 0x69
#define MPU_RA_USER_CTRL 0x6A
#define MPU_RA_PWR_MGMT_1 0x6B
#define MPU_RA_PWR_MGMT_2 0x6C
#define MPU_RA_BANK_SEL 0x6D
#define MPU_RA_MEM_START_ADDR 0x6E
#define MPU_RA_MEM_R_W 0x6F
#define MPU_RA_DMP_CFG_1 0x70
#define MPU_RA_DMP_CFG_2 0x71
#define MPU_RA_FIFO_COUNTH 0x72
#define MPU_RA_FIFO_COUNTL 0x73
#define MPU_RA_FIFO_R_W 0x74
#define MPU_RA_WHO_AM_I 0x75

#define writeBit MPU6050_WriteBit
#define writeBits MPU6050_WriteBits

#define devAddr MPU6050_DEFAULT_ADDRESS

#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[]
#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[]
#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[]

typedef u8 byte;
typedef unsigned char boolean;
#define false 0
#define true 1

long dmp_lastRead = 0;
byte dmp_processed_packet[8];
byte dmp_received_packet[50];
byte dmp_temp = 0;
byte dmp_fifoCountL = 0;
byte dmp_fifoCountL2 = 0;
byte dmp_packetCount = 0x00;
boolean dmp_longPacket = false;
boolean dmp_firstPacket = true;

extern u8 fifoBuffer[42];
int16_t angle2[2];
int16_t gyroADC[3];

uint8_t *dmpPacketBuffer;
        uint16_t dmpPacketSize;


    uint8_t buffer[14];

//This 3D array contains the default DMP memory bank binary that gets loaded during initialization.
//In the Invensense UC3-A3 firmware this is uploaded in 128 byte tranmissions, but the Arduino Wire
//library only supports 32 byte transmissions, including the register address to which you're writing,
//so I broke it up into 16 byte transmission payloads which are sent in the dmp_init() function below.
//
//This was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
//directly from that code. That is true of all transmissions in this sketch, and any documentation has
//been added after the fact by referencing the Invensense code.

// this block of memory gets written to the MPU on start-up, and it seems
// to be volatile memory, so it has to be done each time (it only takes ~1
// second though)
const u8 dmpMemory[MPU6050_DMP_CODE_SIZE] = {
// bank 0, 256 bytes
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,

// bank 1, 256 bytes
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,

// bank 2, 256 bytes
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,

// bank 3, 256 bytes
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C,
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,

// bank 4, 256 bytes
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,

// bank 5, 256 bytes
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,

// bank 6, 256 bytes
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,

// bank 7, 138 bytes (remainder)
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3,
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC,
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF

};

// thanks to Noah Zerkin for piecing this stuff together!
const u8 dmpConfig[MPU6050_DMP_CONFIG_SIZE] = {
// BANK OFFSET LENGTH [DATA]
0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration
0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration
0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration
0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration
0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration
0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration
0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02
0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21
0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update
0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone
// SPECIAL 0x01 = enable interrupts
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt
0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate

// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

// It is important to make sure the host processor can keep up with reading and processing
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.

};

const u8 dmpUpdates[MPU6050_DMP_UPDATES_SIZE] = {
0x01, 0xB2, 0x02, 0xFF, 0xFF,
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
0x01, 0x6A, 0x02, 0x06, 0x00,
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
0x01, 0x62, 0x02, 0x00, 0x00,
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
};

void delay(u16 i)
{
u16 j ;
for(;i>0;i--)
{
for(j=0;j<1000;j++);
}
}

void dmp_resetFifo(){
byte ctrl;
// ctrl = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_USER_CTRL);
MPU6050_I2C_BufferRead(MPU6050_DEFAULT_ADDRESS, &ctrl, MPU_RA_USER_CTRL, 1);
ctrl|= 0x4;
//i2c_writeReg(MPU60X0_I2CADDR, MPU_RA_USER_CTRL, ctrl);
MPU6050_I2C_ByteWrite(MPU6050_DEFAULT_ADDRESS,&ctrl,MPU_RA_USER_CTRL);
}

void writeByte(u8 slaveAddr,u8 writeAddr, u8 pBuffer) {
MPU6050_I2C_ByteWrite( slaveAddr, &pBuffer, writeAddr);
}

void readBytes(u8 slaveAddr, u8 readAddr,u16 NumByteToRead,u8* buffer)
{
MPU6050_I2C_BufferRead(slaveAddr, buffer, readAddr, NumByteToRead);
}

void readByte(u8 slaveAddr, u8 readAddr,u8* buffer)
{
MPU6050_I2C_BufferRead(slaveAddr, buffer, readAddr, 1);
}

void getFIFOBytes(uint8_t *data, uint8_t length) {
readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
}

void resetFIFO() {
writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
}

void setMemoryBank(u8 bank, u8 prefetchEnabled, u8 userBank) {
bank &= 0x1F;
if (userBank) bank |= 0x20;
if (prefetchEnabled) bank |= 0x40;

 MPU6050_I2C_ByteWrite(MPU6050_DEFAULT_ADDRESS,&bank,MPU6050_RA_BANK_SEL);

}

void setSlaveAddress(u8 num, u8 address) {
if (num > 3) return;
writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address);

}

void readBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
{
MPU6050_ReadBits(slaveAddr, regAddr, bitStart,length, data) ;
}

void setMemoryStartAddress(uint8_t address) {
writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
}

 void writeBytes(u8 slaveAddr, u8 writeAddr,u8  chunkSize, u8* pBuffer){

int i;
for(i=0;i< chunkSize;i++){
MPU6050_I2C_ByteWrite(slaveAddr, pBuffer+i, writeAddr);
}
}

bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
uint8_t vb1[MPU6050_DMP_MEMORY_CHUNK_SIZE],vb2[MPU6050_DMP_MEMORY_CHUNK_SIZE];
uint8_t chunkSize;
uint8_t *verifyBuffer;
uint8_t *progBuffer;
uint16_t i;
uint8_t j;
setMemoryBank(bank,0,0);
setMemoryStartAddress(address);
if (verify) verifyBuffer = vb1;
if (useProgMem) progBuffer = vb2;
for (i = 0; i < dataSize;) {
// determine correct chunk size according to bank position and data size
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

    // make sure we don't go past the data size
    if (i + chunkSize > dataSize) chunkSize = dataSize - i;

    // make sure this chunk doesn't go past the bank boundary (256 bytes)
    if (chunkSize > 256 - address) chunkSize = 256 - address;


        // write the chunk of data as specified
        progBuffer = (uint8_t *)data + i;


    writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);




    // verify data if needed
    if (verify && verifyBuffer) {
        setMemoryBank(bank,0,0);
        setMemoryStartAddress(address);
        readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);

        if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
            /*Serial.print("Block write verification error, bank ");
            Serial.print(bank, DEC);
            Serial.print(", address ");
            Serial.print(address, DEC);
            Serial.print("!\nExpected:");
            for (j = 0; j < chunkSize; j++) {
                Serial.print(" 0x");
                if (progBuffer[j] < 16) Serial.print("0");
                Serial.print(progBuffer[j], HEX);
            }
            Serial.print("\nReceived:");
            for (uint8_t j = 0; j < chunkSize; j++) {
                Serial.print(" 0x");
                if (verifyBuffer[i + j] < 16) Serial.print("0");
                Serial.print(verifyBuffer[i + j], HEX);
            }
            Serial.print("\n");*/

            return false; // uh oh.
        }
    }

    // increase byte index by [chunkSize]
    i += chunkSize;

    // uint8_t automatically wraps to 0 at 256
    address += chunkSize;

    // if we aren't done, update bank (if necessary) and address
    if (i < dataSize) {
        if (address == 0) bank++;
        setMemoryBank(bank,0,0);
        setMemoryStartAddress(address);
    }
}

return true;

}

bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
return writeMemoryBlock(data, dataSize, bank, address, verify, false);
}

int8_t getXGyroOffset() {

readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];

}
void setXGyroOffset(int8_t offset) {
writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}

// YG_OFFS_TC register

int8_t getYGyroOffset() {
readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void setYGyroOffset(int8_t offset) {
writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}

// ZG_OFFS_TC register

int8_t getZGyroOffset() {
readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void setZGyroOffset(int8_t offset) {
writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}

boolean dmp_fifoReady(){
u8 temp;
// i2c_rep_start(MPU60X0_I2CADDR);
// i2c_write(MPU_RA_FIFO_COUNTH);
// i2c_rep_start(MPU60X0_I2CADDR + 1);
MPU6050_I2C_BufferRead(MPU6050_DEFAULT_ADDRESS, &temp, MPU_RA_FIFO_COUNTH, 1);
// byte fifoCountH = i2c_readAck();
// dmp_fifoCountL = i2c_readNak();
// if(dmp_fifoCountL == 42 || dmp_fifoCountL == 44){
// return 1;
// }

// else
if(temp==1)
return 1;
return 0;
}

uint16_t getFIFOCount() {
u8 bf[2];
readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, bf);
return (((uint16_t)bf[0]) << 8) | bf[1];
}

void dmp_bank_sel(byte bank){
/*
i2c_rep_start(MPU60X0_I2CADDR);
i2c_write(MPU_RA_BANK_SEL);
i2c_write(bank);
i2c_stop();
*/

MPU6050_I2C_ByteWrite(MPU6050_DEFAULT_ADDRESS,&bank,MPU_RA_BANK_SEL);
}

uint8_t getIntStatus() {

readByte(devAddr, MPU6050_RA_INT_STATUS, buffer);
return buffer[0];

}

void setDMPEnabled(bool enabled) {
writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
}

void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {

uint8_t chunkSize;
uint16_t i;
setMemoryBank(bank,0,0);
setMemoryStartAddress(address);
for (i = 0; i < dataSize;) {
    // determine correct chunk size according to bank position and data size
    chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

    // make sure we don't go past the data size
    if (i + chunkSize > dataSize) chunkSize = dataSize - i;

    // make sure this chunk doesn't go past the bank boundary (256 bytes)
    if (chunkSize > 256 - address) chunkSize = 256 - address;

    // read the chunk of data as specified
    readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);

    // increase byte index by [chunkSize]
    i += chunkSize;

    // uint8_t automatically wraps to 0 at 256
    address += chunkSize;

    // if we aren't done, update bank (if necessary) and address
    if (i < dataSize) {
        if (address == 0) bank++;
        setMemoryBank(bank,0,0);
        setMemoryStartAddress(address);
    }
}

}

bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
uint8_t *progBuffer, success, special;
uint16_t i, j;

// config set data is a long string of blocks with the following structure:
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
uint8_t bank, offset, length;
for (i = 0; i < dataSize;) {
       delay(5);
        bank = data[i++];
        offset = data[i++];
        length = data[i++];


    // write data or perform special action
    if (length > 0) {
        // regular block of data to write
        /*Serial.print("Writing config block to bank ");
        Serial.print(bank);
        Serial.print(", offset ");
        Serial.print(offset);
        Serial.print(", length=");
        Serial.println(length);*/

            progBuffer = (uint8_t *)data + i;

        success = writeMemoryBlock(progBuffer, length, bank, offset, true,0);
        i += length;
    } else {
        // special instruction
        // NOTE: this kind of behavior (what and when to do certain things)
        // is totally undocumented. This code is in here based on observed
        // behavior only, and exactly why (or even whether) it has to be here
        // is anybody's guess for now.
        if (useProgMem) {
            special =(uint8_t) data + i++;
        } else {
            special = data[i++];
        }
        /*Serial.print("Special command code ");
        Serial.print(special, HEX);
        Serial.println(" found...");*/
        if (special == 0x01) {
            // enable DMP-related interrupts

            //setIntZeroMotionEnabled(true);
            //setIntFIFOBufferOverflowEnabled(true);
            //setIntDMPEnabled(true);
            writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32);  // single operation

            success = true;
        } else {
            // unknown special command
            success = false;
        }
    }

    if (!success) {
        if (useProgMem) free(progBuffer);
        return false; // uh oh
    }
}

return true;

}
bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
return writeDMPConfigurationSet(data, dataSize, false);
}

void writeWord(uint8_t slaveAddr, uint8_t regAddr, uint16_t data){
writeByte(slaveAddr, regAddr, data>>8);
writeByte(slaveAddr, regAddr+1, data);
}

u8 dmp_init() {
unsigned char tmp,dmp_temp;
u8 xgOffset,ygOffset,zgOffset;
u16 fifoCount;

  u8 mpuIntStatus ;
    uint8_t otpValid;

u8 dmpUpdate[16], j;
u16 pos = 0;
static u8 ttt[100];
writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);// device reset
delay(300); // wait after reset

// disable sleep mode
writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false);



   // check OTP bank valid

// DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));

// MPU6050_ReadBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, &otpValid);
// DEBUG_PRINT(F("OTP bank is "));
// DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));

// get X/Y/Z gyro offsets
// DEBUG_PRINTLN(F("Reading gyro offset values..."));
xgOffset = getXGyroOffset();
ygOffset = getYGyroOffset();
zgOffset = getZGyroOffset();

// setup weird slave stuff (?)
// ////DEBUG_PRINTLLN(F("Setting slave 0 address to 0x7F..."));
setSlaveAddress(0, 0x7F);
// ////DEBUG_PRINTLLN(F("Disabling I2C Master mode..."));

writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, false);

// ////DEBUG_PRINTLLN(F("Setting slave 0 address to 0x68 (self)..."));
setSlaveAddress(0, 0x68);
// ////DEBUG_PRINTLLN(F("Resetting I2C Master control..."));
writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
delay(20);

  // load DMP code into memory banks
////DEBUG_PRINTL(F("Writing DMP code to MPU memory banks ("));
////DEBUG_PRINTL(MPU6050_DMP_CODE_SIZE);
////DEBUG_PRINTLLN(F(" bytes)"));
if(writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE,0,0,1)) 
 {
    ////DEBUG_PRINTLLN(F("Success! DMP code written and verified."));

    // write DMP configuration
    //DEBUG_PRINTL(F("Writing DMP configuration to MPU memory banks ("));
    //DEBUG_PRINTL(MPU6050_DMP_CONFIG_SIZE);
    ////DEBUG_PRINTLLN(F(" bytes in config def)"));
    delay(10);
    if(writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE))
      {
        ////DEBUG_PRINTLLN(F("Success! DMP configuration written and verified."));

        ////DEBUG_PRINTLLN(F("Setting clock source to Z Gyro..."));
        writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO);
        ////DEBUG_PRINTLLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
        writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x12);
        ////DEBUG_PRINTLLN(F("Setting sample rate to 200Hz..."));
        writeByte(devAddr, MPU6050_RA_SMPLRT_DIV,4); // 1khz / (1 + 4) = 200 Hz

        ////DEBUG_PRINTLLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
        writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L);

/** Get digital low-pass filter configuration.获取数字低通滤波器的配置。

  • The DLPF_CFG parameter sets the digital low pass filter configuration. It
  • also determines the internal sampling rate used by the device as shown in
  • the table below. *
  • 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. *
  • | ACCELEROMETER | GYROSCOPE
  • DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate
  • ---------+-----------+--------+-----------+--------+-------------
  • 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz
  • 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz
  • 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz
  • 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz
  • 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz
  • 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz
  • 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz
  • 7 | -- Reserved
    */

    ////DEBUG_PRINTLLN(F("Setting DLPF bandwidth to 42Hz..."));

        writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42);
        ////DEBUG_PRINTLLN(F("Setting gyro sensitivity to +/- 250 deg/sec..."));
        writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000);
    
        ////DEBUG_PRINTLLN(F("Setting DMP configuration bytes (function unknown)..."));
        writeByte(devAddr, MPU6050_RA_DMP_CFG_1, 0x03);
        writeByte(devAddr, MPU6050_RA_DMP_CFG_2, 0x00);
    
        ////DEBUG_PRINTLLN(F("Clearing OTP Bank flag..."));
        writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT,false);
        ////DEBUG_PRINTLLN(F("Setting X/Y/Z gyro offsets to previous values..."));
        setXGyroOffset(xgOffset); 
        setYGyroOffset(ygOffset);
        setZGyroOffset(zgOffset);
    
        ////DEBUG_PRINTLLN(F("Setting X/Y/Z gyro user offsets to zero..."));
        writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, 12);
        writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, 5);
        writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, -3);
    
    
       ////DEBUG_PRINTLLN(F("Writing final memory update 1/7 (function unknown)..."));
    
        for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
        writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],0,0);
    
        ////DEBUG_PRINTLLN(F("Writing final memory update 2/7 (function unknown)..."));
        for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
        writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],0,0);
    
        ////DEBUG_PRINTLLN(F("Resetting FIFO..."));
        resetFIFO();
    
        ////DEBUG_PRINTLLN(F("Reading FIFO count..."));
    

    // fifoCount=getFIFOCount();

    //DEBUG_PRINTL(F("Current FIFO count="));
    ////DEBUG_PRINTLLN(fifoCount);
    // getFIFOBytes(fifoBuffer, fifoCount);
    ////DEBUG_PRINTLLN(F("Setting motion detection threshold to 2..."));

          writeByte(devAddr, MPU6050_RA_MOT_THR, 2);
    
          ////DEBUG_PRINTLLN(F("Setting zero-motion detection threshold to 156..."));
          writeByte(devAddr, MPU6050_RA_ZRMOT_THR, 156);
    
          ////DEBUG_PRINTLLN(F("Setting motion detection duration to 80..."));
          writeByte(devAddr, MPU6050_RA_MOT_DUR, 80);
    
    
          ////DEBUG_PRINTLLN(F("Setting zero-motion detection duration to 0..."));
          writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, 0);
    
          ////DEBUG_PRINTLLN(F("Resetting FIFO..."));
          resetFIFO();
    
          ////DEBUG_PRINTLLN(F("Enabling FIFO..."));
          writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, true);
    
          ////DEBUG_PRINTLLN(F("Enabling DMP..."));
          setDMPEnabled(true);
    
          ////DEBUG_PRINTLLN(F("Resetting DMP..."));
          writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
    
          ////DEBUG_PRINTLLN(F("Writing final memory update 3/7 (function unknown)..."));
          for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
          writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],0,0);
    
          ////DEBUG_PRINTLLN(F("Writing final memory update 4/7 (function unknown)..."));
          for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
          writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],0,0);
    
          ////DEBUG_PRINTLLN(F("Writing final memory update 5/7 (function unknown)..."));
          for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
          writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],0,0);
    
          ////DEBUG_PRINTLLN(F("Waiting for FIFO count > 2..."));
          while ((fifoCount=getFIFOCount()) < 3) ;
    
    
    
          //DEBUG_PRINTL(F("Current FIFO count="));
          ////DEBUG_PRINTLLN(fifoCount);
          ////DEBUG_PRINTLLN(F("Reading FIFO data..."));
          getFIFOBytes(fifoBuffer, 42);
    
          ////DEBUG_PRINTLLN(F("Reading interrupt status..."));
           mpuIntStatus = getIntStatus();
    
          //DEBUG_PRINTL(F("Current interrupt status="));
          ////DEBUG_PRINTLLNF(mpuIntStatus, HEX);
    
          ////DEBUG_PRINTLLN(F("Reading final memory update 6/7 (function unknown)..."));
          for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
          readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
    
          ////DEBUG_PRINTLLN(F("Waiting for FIFO count > 2..."));
           while ((fifoCount=getFIFOCount()) < 3);
    
          //DEBUG_PRINTL(F("Current FIFO count="));
          ////DEBUG_PRINTLLN(fifoCount);
    
          ////DEBUG_PRINTLLN(F("Reading FIFO data..."));
          getFIFOBytes(fifoBuffer, fifoCount);
              delay(10);
    
    
          ////DEBUG_PRINTLLN(F("Reading interrupt status..."));
          mpuIntStatus = getIntStatus();
    
          //DEBUG_PRINTL(F("Current interrupt status="));
          ////DEBUG_PRINTLLNF(mpuIntStatus, HEX);
    
          ////DEBUG_PRINTLLN(F("Writing final memory update 7/7 (function unknown)..."));
          for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] =dmpUpdates[pos];
          writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],0,0);
    
          ////DEBUG_PRINTLLN(F("DMP is good to go! Finally."));
    
          ////DEBUG_PRINTLLN(F("Disabling DMP (you turn it on later)..."));
    

    // setDMPEnabled(false);

        ////DEBUG_PRINTLLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
        dmpPacketSize = 42;
        /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
            return 3; // TODO: proper error code for no memory
        }*/
    

    dmpPacketBuffer=fifoBuffer;
    ////DEBUG_PRINTLLN(F("Resetting FIFO and clearing INT status one last time..."));
    resetFIFO();
    getIntStatus();

    } else {
        ////DEBUG_PRINTLLN(F("ERROR! DMP configuration verification failed."));
        return 2; // configuration block loading failed
    }
    

    } else {
    ////DEBUG_PRINTLLN(F("ERROR! DMP code verification failed."));
    return 1; // main binary block loading failed
    }
    return 0; // success
    }

uint8_t dmpGetAccel(int16_t data, const uint8_t packet) {
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
if (packet == 0) packet = dmpPacketBuffer;
data[0] = (packet[28] << 8) + packet[29];
data[1] = (packet[32] << 8) + packet[33];
data[2] = (packet[36] << 8) + packet[37];
return 0;
}

uint8_t dmpGetGyro(int16_t data, const uint8_t packet) {
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
if (packet == 0) packet = dmpPacketBuffer;
data[0] = (packet[16] << 8) + packet[17];
data[1] = (packet[20] << 8) + packet[21];
data[2] = (packet[24] << 8) + packet[25];
return 0;
}

uint8_t dmpGetQuaternion(Quaternion q,const uint8_t packet) {
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
#define DVNB 32168.0f
int16_t w, x, y,z;

float m;

w = ((packet[0] << 8) + packet[1]);
x = ((packet[4] << 8) + packet[5]);
y = ((packet[8] << 8) + packet[9]);
z = ((packet[12] << 8) + packet[13]);

 m=sqrt(w*w + x*x + y*y + z*z) ;
   q->w = (float)w / m;
   q->x = (float)x / m;
   q->y = (float)y / m;
   q->z = (float)z / m;

return 0;

}

bool dmpPacketAvailable() {
return getFIFOCount() >= 42;
}

uint8_t dmpGetEuler(float data, Quaternion *q) {
data[0] = atan2(2
q->xq->y - 2q->wq->z, 2q->wq->w + 2q->xq->x - 1); // psi
data[1] = -asin(2
q->xq->z + 2q->wq->y); // theta
data[2] = atan2(2
q->yq->z - 2q->wq->x, 2q->wq->w + 2q->z*q->z - 1); // phi
return 0;
}

uint8_t dmpGetGravity(VectorFloat v, Quaternion *q) {
v -> x = 2 * (q -> x
q -> z - q -> wq -> y);
v -> y = 2 * (q -> w
q -> x + q -> yq -> z);
v -> z = q -> w
q -> w - q -> xq -> x - q -> yq -> y + q -> z*q -> z;
return 0;
}

uint8_t dmpGetYawPitchRoll(float data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2
q ->xq -> y - 2q -> wq -> z, 2q -> wq -> w + 2q -> xq -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan(gravity -> x / sqrt(gravity -> y
gravity -> y + gravity -> zgravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan(gravity -> y / sqrt(gravity -> x
gravity -> x + gravity -> z*gravity -> z));
return 0;
}

/** 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 */

/** 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 MPU6050::setDHPFMode(uint8_t bandwidth) {
I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
}

*/

    //MPU6050 I2C library for ARM STM32F103xx Microcontrollers - Main source file

//Has bit, byte and buffer I2C R/W functions
// 23/05/2012 by Harinadha Reddy Chintalapalli harinath.ec@gmail.com
// Changelog:
// 2012-05-23 - initial release. Thanks to Jeff Rowberg jeff@rowberg.net for his AVR/Arduino
// based MPU6050 development which inspired me & taken as reference to develop this.
/* ============================================================================================
MPU6050 device I2C library code for ARM STM32F103xx is placed under the MIT license
Copyright (c) 2012 Harinadha Reddy Chintalapalli

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.

*/

/* Includes */
#include "MPU6050.h"
#include "stm32f10x_i2c.h"

/** @defgroup MPU6050_Library

  • @{ */

/** Power on and prepare for general usage.

  • This will activate the device and take it out of sleep mode (which must be done
  • after start-up). This function also sets both the accelerometer and the gyroscope
  • to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
  • the clock source to use the X Gyro for reference, which is slightly better than
  • the default internal clock source. */ void MPU6050_Initialize() { MPU6050_SetClockSource(MPU6050_CLOCK_PLL_ZGYRO); MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_500); MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_2); MPU6050_SetSleepModeStatus(DISABLE); }

/** Verify the I2C connection.

  • Make sure the device is connected and responds as expected.
  • @return True if connection is valid, FALSE otherwise */ bool MPU6050_TestConnection() { if(MPU6050_GetDeviceID() == 0x34) //0b110100; 8-bit representation in hex = 0x34 return TRUE; else return FALSE; } // WHO_AM_I register

/** Get Device ID.

  • This register is used to verify the identity of the device (0b110100).
  • @return Device ID (should be 0x68, 104 dec, 150 oct)
  • @see MPU6050_RA_WHO_AM_I
  • @see MPU6050_WHO_AM_I_BIT
  • @see MPU6050_WHO_AM_I_LENGTH / uint8_t MPU6050_GetDeviceID() { uint8_t tmp; MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &tmp); return tmp; } /* 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 MPU6050_GetClockSource()
  • @see MPU6050_RA_PWR_MGMT_1
  • @see MPU6050_PWR1_CLKSEL_BIT
  • @see MPU6050_PWR1_CLKSEL_LENGTH */ void MPU6050_SetClockSource(uint8_t source) { MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); }

/** Set full-scale gyroscope range.

  • @param range New full-scale gyroscope range value
  • @see MPU6050_GetFullScaleGyroRange()
  • @see MPU6050_GYRO_FS_250
  • @see MPU6050_RA_GYRO_CONFIG
  • @see MPU6050_GCONFIG_FS_SEL_BIT
  • @see MPU6050_GCONFIG_FS_SEL_LENGTH */ void MPU6050_SetFullScaleGyroRange(uint8_t range) { MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); }

// 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 MPU6050_GetFullScaleGyroRange() { uint8_t tmp; MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, &tmp); return tmp; } /* 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 MPU6050_GetFullScaleAccelRange() { uint8_t tmp; MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, &tmp); return tmp; } /* Set full-scale accelerometer range.
  • @param range New full-scale accelerometer range setting
  • @see MPU6050_GetFullScaleAccelRange() */ void MPU6050_SetFullScaleAccelRange(uint8_t range) { MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); }

/** 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 MPU6050_GetSleepModeStatus() { uint8_t tmp; MPU6050_ReadBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, &tmp); if(tmp == 0x00) return FALSE; else return TRUE;
    } /
    * Set sleep mode status.
  • @param enabled New sleep mode enabled status
  • @see MPU6050_GetSleepModeStatus()
  • @see MPU6050_RA_PWR_MGMT_1
  • @see MPU6050_PWR1_SLEEP_BIT */ void MPU6050_SetSleepModeStatus(FunctionalState NewState) { MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, NewState); }

/** Get raw 6-axis motion sensor readings (accel/gyro).

  • Retrieves all currently available motion sensor values.
  • @param AccelGyro 16-bit signed integer array of length 6
  • @see MPU6050_RA_ACCEL_XOUT_H / void MPU6050_GetRawAccelGyro(s16 AccelGyro) { int i; u8 tmpBuffer[14]; MPU6050_I2C_BufferRead(MPU6050_DEFAULT_ADDRESS, tmpBuffer, MPU6050_RA_ACCEL_XOUT_H, 14); /* Get acceleration / for( i=0; i<3; i++) AccelGyro[i]=((s16)((u16)tmpBuffer[2i] << 8) + tmpBuffer[2i+1]); / Get Angular rate / for( i=4; i<7; i++) AccelGyro[i-1]=((s16)((u16)tmpBuffer[2i] << 8) + tmpBuffer[2*i+1]);

}

/** Write multiple bits in an 8-bit device register.

  • @param slaveAddr I2C slave device address
  • @param regAddr Register regAddr to write to
  • @param bitStart First bit position to write (0-7)
  • @param length Number of bits to write (not more than 8)
  • @param data Right-aligned value to write / void MPU6050_WriteBits(uint8_t slaveAddr, 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 tmp; uint8_t mask; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1);
    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 tmp &= ~(mask); // zero all important bits in existing byte tmp |= data; // combine data with existing byte MPU6050_I2C_ByteWrite(slaveAddr,&tmp,regAddr);
    } /
    * write a single bit in an 8-bit device register.
  • @param slaveAddr I2C slave device address
  • @param regAddr Register regAddr to write to
  • @param bitNum Bit position to write (0-7)
  • @param value New bit value to write / void MPU6050_WriteBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { uint8_t tmp; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1);
    tmp = (data != 0) ? (tmp | (1 << bitNum)) : (tmp & ~(1 << bitNum)); MPU6050_I2C_ByteWrite(slaveAddr,&tmp,regAddr); } /
    * Read multiple bits from an 8-bit device register.
  • @param slaveAddr 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 readTimeout) */ void MPU6050_ReadBits(uint8_t slaveAddr, 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 tmp; uint8_t mask ; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1); mask = ((1 << length) - 1) << (bitStart - length + 1); tmp &= mask; tmp >>= (bitStart - length + 1); *data = tmp; }

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

  • @param slaveAddr 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 readTimeout) */ void MPU6050_ReadBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) { uint8_t tmp; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1);
    *data = tmp & (1 << bitNum); }

/**

  • @brief Initializes the I2C peripheral used to drive the MPU6050
  • @param None
  • @return None
    */
    void MPU6050_I2C_Init()
    {
    I2C_InitTypeDef I2C_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    /* Enable I2C and GPIO clocks */
    RCC_APB1PeriphClockCmd(MPU6050_I2C_RCC_Periph, ENABLE);
    RCC_APB2PeriphClockCmd(MPU6050_I2C_RCC_Port, ENABLE);

    /* Configure I2C pins: SCL and SDA */
    GPIO_InitStructure.GPIO_Pin = MPU6050_I2C_SCL_Pin | MPU6050_I2C_SDA_Pin;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_Init(MPU6050_I2C_Port, &GPIO_InitStructure);

    /* I2C configuration */
    I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
    I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
    I2C_InitStructure.I2C_OwnAddress1 = MPU6050_DEFAULT_ADDRESS; // MPU6050 7-bit adress = 0x68, 8-bit adress = 0xD0;
    I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
    I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
    I2C_InitStructure.I2C_ClockSpeed = MPU6050_I2C_Speed;

    /* Apply I2C configuration after enabling it /
    I2C_Init(MPU6050_I2C, &I2C_InitStructure);
    /
    I2C Peripheral Enable */

    I2C_Cmd(MPU6050_I2C, ENABLE);

}

/**

  • @brief Writes one byte to the MPU6050.
  • @param slaveAddr : slave address MPU6050_DEFAULT_ADDRESS
  • @param pBuffer : pointer to the buffer containing the data to be written to the MPU6050.
  • @param writeAddr : address of the register in which the data will be written
  • @return None
    /
    void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8
    pBuffer, u8 writeAddr)
    {
    // ENTR_CRT_SECTION();

    /* Send START condition */
    I2C_GenerateSTART(MPU6050_I2C, ENABLE);

    /* Test on EV5 and clear it */
    while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

    /* Send MPU6050 address for write */
    I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter);

    /* Test on EV6 and clear it */
    while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

    /* Send the MPU6050's internal address to write to */
    I2C_SendData(MPU6050_I2C, writeAddr);

    /* Test on EV8 and clear it */
    while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

    /* Send the byte to be written */
    I2C_SendData(MPU6050_I2C, *pBuffer);

    /* Test on EV8 and clear it */
    while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

    /* Send STOP condition */
    I2C_GenerateSTOP(MPU6050_I2C, ENABLE);

    // EXT_CRT_SECTION();

}

/**

  • @brief Reads a block of data from the MPU6050.
  • @param slaveAddr : slave address MPU6050_DEFAULT_ADDRESS
  • @param pBuffer : pointer to the buffer that receives the data read from the MPU6050.
  • @param readAddr : MPU6050's internal address to read from.
  • @param NumByteToRead : number of bytes to read from the MPU6050 ( NumByteToRead >1 only for the Mgnetometer readinf).
  • @return None */

void MPU6050_I2C_BufferRead(u8 slaveAddr, u8* pBuffer, u8 readAddr, u16 NumByteToRead)
{
// ENTR_CRT_SECTION();

/* While the bus is busy */
while(I2C_GetFlagStatus(MPU6050_I2C, I2C_FLAG_BUSY));

/* Send START condition */
I2C_GenerateSTART(MPU6050_I2C, ENABLE);

/* Test on EV5 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

/* Send MPU6050 address for write */
I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter);

/* Test on EV6 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

/* Clear EV6 by setting again the PE bit */
I2C_Cmd(MPU6050_I2C, ENABLE);

/* Send the MPU6050's internal address to write to */
I2C_SendData(MPU6050_I2C, readAddr);

/* Test on EV8 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

/* Send STRAT condition a second time */
I2C_GenerateSTART(MPU6050_I2C, ENABLE);

/* Test on EV5 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

/* Send MPU6050 address for read */
I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Receiver);

/* Test on EV6 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

/* While there is data to be read /
while(NumByteToRead)
{
if(NumByteToRead == 1)
{
/
Disable Acknowledgement */

@wxclb
wxclb commented on 4b0253e Mar 9, 2013

this code is for stm32f103 ,not for avr singlechip microcontroller

@jrowberg
Owner
@wxclb
Please sign in to comment.
Something went wrong with that request. Please try again.