stc8a--mpu6050---dmp--x角度，y角度。z旋转角度不准确，缺少前面矫准环节程序。。

mpu6050.h

#ifndef __MPU6050_H
#define __MPU6050_H
#include "stc8a8k.h"
#include "config.h"
#include "math.h"




#define DMP			//ê1ó???í¨·?·¨?áè? 6050
sbit    SCL=P1^0;			//IICê±?óòy???¨ò?
sbit    SDA=P1^1;			//IICêy?Yòy???¨ò?

#define MPUDELAY 6		   
extern bit ack;	

extern void initDMPMPU6050(void);
extern unsigned char  getDeviceID(void);
extern unsigned char dmpInitialize(void);
extern void enableDMP(void);
// extern void read_dmp_angle(float *pangel,float *pgyrox);
  void read_dmp_angle(float *pangel,float *pangel2);	
#define	SMPLRT_DIV		0x19	//íó?Yò?2é?ù?ê￡?μ?Dí?μ￡o0x07(125Hz)
#define	CONFIG			0x1A	//μíí¨??2¨?μ?ê￡?μ?Dí?μ￡o0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//íó?Yò?×??ì?°2aá?·??§￡?μ?Dí?μ￡o0x18(2?×??ì￡?2000deg/s)
#define	ACCEL_CONFIG	0x1C	//?ó?ù??×??ì?￠2aá?·??§?°??í¨??2¨?μ?ê￡?μ?Dí?μ￡o0x01(2?×??ì￡?2G￡?5Hz)
#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40
#define	TEMP_OUT_H		0x41
#define	TEMP_OUT_L		0x42
#define	GYRO_XOUT_H		0x43
#define	GYRO_XOUT_L		0x44
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48
#define	USER_CTRL			0x6A	//ó??§???? ??′??÷?êDíó??§ê1?ü?òê1?ü?÷?ú￡?′??D?÷D?o?í¨μàoí′??D?÷??′??÷ò2?éò?ê1ó??a????′??÷?′???￡
#define	PWR_MGMT_1		0x6B	//μ??′1üàí￡?μ?Dí?μ￡o0x00(?y3￡??ó?)
#define	WHO_AM_I		0x75	//IICμ??·??′??÷(??è?êy?μ0x68￡????á)
#define	SlaveAddress	0xD0	//IICD′è?ê±μ?μ??·×??úêy?Y￡?+1?a?áè?
#endif

mpu6050.c

#include "MPU6050.H"
#ifdef DMP
code uchar dmpmemorydata[1929]= {
// 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
};
code unsigned char dmpcfgupddata[192] = {
//  dmp config
//  BANK    OFFSET  LENGTH  [DATA]
    0x03,   0x7B,   0x03,   0x4C, 0xCD, 0x6C,
    0x03,   0xAB,   0x03,   0x36, 0x56, 0x76,
    0x00,   0x68,   0x04,   0x02, 0xCB, 0x47, 0xA2,
    0x02,   0x18,   0x04,   0x00, 0x05, 0x8B, 0xC1,
    0x01,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x03,   0x7F,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
    0x03,   0x89,   0x03,   0x26, 0x46, 0x66,
    0x00,   0x6C,   0x02,   0x20, 0x00,
    0x02,   0x40,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x44,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x48,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x4C,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x50,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x54,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x58,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0x5C,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x02,   0xBC,   0x04,   0x00, 0x00, 0x00, 0x00,
    0x01,   0xEC,   0x04,   0x00, 0x00, 0x40, 0x00,
    0x03,   0x7F,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
    0x04,   0x02,   0x03,   0x0D, 0x35, 0x5D,
    0x04,   0x09,   0x04,   0x87, 0x2D, 0x35, 0x3D,
    0x00,   0xA3,   0x01,   0x00,
    0x00,   0x00,   0x00,   0x01, 	//?aà?ê??a??DMPμ?ì?êa?D??μ?
    //?-3ìDò?D′?DD′ú???a(?aà?2?ò??¨′í)
    //0x00,   0x00,   0x00,   0x01,  ?′LENGTH=0x00￡?óD′í

    0x07,   0x86,   0x01,   0xFE,
    0x07,   0x41,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38,
    0x07,   0x7E,   0x01,   0x30,
    0x07,   0x46,   0x01,   0x9A,
    0x07,   0x47,   0x04,   0xF1, 0x28, 0x30, 0x38,
    0x07,   0x6C,   0x04,   0xF1, 0x28, 0x30, 0x38,
    0x02,   0x16,   0x02,   0x00, 0x01,
    /* é?DD×?oóò???êy?Yμ÷??FIFO rate :0x01=100HZ,0x02=66HZ,0x03=50HZ ,0x04=40HZ,0x05=33.33HZ,
    // ?é′ó datasheet 1?ê?í???
    //dmp updates
        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*/
};
code unsigned char dmpUpdates[47]= {

    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

};
#endif
//IIC?ù±?D-òé$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
bit ack;	//ó|′e±ê???? 0:ack 1￡onak
//I^Cê±Dò?D?óê±éè??￡???ì?2????÷D???μ?êy?Yê?2á
void Delay_us(uchar cnt) {
    while(--cnt) {
        _nop_();;
    }
}
//**************************************
//I2C?eê?D?o?
//**************************************
void I2C_Start() {
    SDA = 1;                    //à-??êy?Y??
    SCL = 1;                    //à-??ê±?ó??
    _nop_();;         					//?óê±
    SDA = 0;                    //2úéú???μ??
    Delay_us(MPUDELAY);         				//?óê±
    SCL = 0;                    //à-μíê±?ó??
}
//**************************************
//I2Cí￡?1D?o?
//**************************************
void I2C_Stop() {
    SDA = 0;                    //à-μíêy?Y??
    SCL = 1;                    //à-??ê±?ó??
    Delay_us(MPUDELAY);         //?óê±
    SDA = 1;                    //2úéúé?éy??
    _nop_();
    _nop_();         	//?óê±
}

//**************************************
//?òI2C×ü??·￠?íò???×??úêy?Y
//**************************************
void I2C_SendByte(uchar dat) {
    uchar i;
    for (i=0; i<8; i++) {        //8????êy?÷
        dat <<= 1;              //ò?3?êy?Yμ?×?????
        SDA = CY;               //?íêy?Y?ú
        SCL = 1;                //à-??ê±?ó??
        Delay_us(MPUDELAY);     //?óê±
        SCL = 0;                //à-μíê±?ó??
    }
    //????ê??óê?′óéè±?·￠??μ?ó|′eD?o?
    SDA = 1;
    SCL = 1;
    Delay_us(MPUDELAY);         //?óê±
    ack = SDA;	//?óê?éè±?μ?ack	_n
    SCL = 0;
    SDA = 1;
}
//**************************************
//′óI2C×ü???óê?ò???×??úêy?Y
//**************************************
uchar I2C_RecvByte(bit _ack) {
    uchar i;
    uchar dat = 0;
    for (i=0; i<8; i++) {        //8????êy?÷
        dat <<= 1;
        SCL = 1;                //à-??ê±?ó??
        Delay_us(MPUDELAY);     //?óê±
        dat |= SDA;             //?áêy?Y
        SCL = 0;                //à-μíê±?ó??
        _nop_();
        _nop_();     //?óê±
    }
    //????ê±?÷???÷·￠?íACK?ò?òNAK
    SDA = _ack;	//ack?ò??nak
    SCL = 1;	//à-??ê±?ó2úéúμú9??ê±?ó
    Delay_us(MPUDELAY);     //?óê±
    SCL = 0;	//à-μ×ê±?ó
    SDA = 1;	//êí·?×ü??
    _nop_();
    _nop_();
    return dat;
}
//**************************************
//?òI2Céè±?D′è?ò???×??úêy?Y
//**************************************
uchar I2C_Single_Write(uchar REG_Address,uchar REG_data) {
    I2C_Start();                  //?eê?D?o?
    I2C_SendByte(SlaveAddress);   //·￠?íéè±?μ??·+D′D?o?
    if(ack) return 0;	//?D??ê?·?ê?ACKè?1?2?ê?·μ??0
    I2C_SendByte(REG_Address);    //?ú2???′??÷μ??·
    if(ack) return 0;	//?D??ê?·?ê?ACKè?1?2?ê?·μ??0
    I2C_SendByte(REG_data);       //?ú2???′??÷êy?Y￡?
    if(ack) return 0;	//?D??ê?·?ê?ACKè?1?2?ê?·μ??0
    I2C_Stop();                   //·￠?íí￡?1D?o?
    return 1;
}
//**************************************
//′óI2Céè±??áè?ò???×??úêy?Y
//**************************************
uchar I2C_Single_Read(uchar REG_Address) {
    uchar REG_data;
    I2C_Start();                   //?eê?D?o?
    I2C_SendByte(SlaveAddress);    //·￠?íéè±?μ??·+D′D?o?
    I2C_SendByte(REG_Address);     //·￠?í′?′￠μ￥?aμ??·￡?′ó0?aê?
    I2C_Start();                   //?eê?D?o?
    I2C_SendByte(SlaveAddress+1);  //·￠?íéè±?μ??·+?áD?o?
    REG_data=I2C_RecvByte(1);       //?á3???′??÷êy?Y

    I2C_Stop();                    //í￡?1D?o?
    return REG_data;
}
#ifdef DMP
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
/*?áè?8????′??÷?à??×??ú
 * @2?êy addr    I2C′ó?÷?t?ú2?μ??·
 * @2?êy length  D′è?êy?Y3¤?è
 * @2?êy Data    ±￡′?êy?Yμ?μ??·
 * @·μ???μ ·μ??×′ì? (1=3é1|)
 */
uchar I2C_reads(uchar addr,uchar length,uchar *Data) {
    uchar i;
    length --;
    I2C_Start();	//?eê?D?o?
    I2C_SendByte(SlaveAddress);	//éè±?μ??·+D′D?o?
    if(ack) return 0;
    I2C_SendByte(addr);	//éè±??ú2?μ??·
    if(ack) return 0;
    I2C_Start();	//?eê?D?o?
    I2C_SendByte(SlaveAddress+1);	//éè±?μ??·+?áD?o?
    if(ack) return 0;
    for(i=0; i<length; i++) {
        Data[i]= I2C_RecvByte(0);	//?áè?êy?Y,·￠?íACK
    }
    Data[length] = I2C_RecvByte(1);//?áè?êy?Y,·￠?íNAK
    I2C_Stop();	//í￡?1D?o?
    return 1;
}
/*?áè?8????′??÷ò???×??ú
  2?êy addr ??′??÷μ??·
  2?êy *Data êy?Y′?′￠μ??·
  ·μ???μ (1=3é1|,0=꧰ü)
*/
uchar I2C_read(uchar addr,uchar *Data) {
    return I2C_reads(addr,1,Data);
}

/**D′è?8????′??÷μ?ò??????￡
 * @2?êy addr    I2C′ó?÷?t?ú2?μ??·
 * @2?êy bitNum  D′è?μ?±èì???(0-7)
 * @2?êy data    D′è?êy?Y
 * @·μ???μ ·μ??×′ì? (1=3é1|)
 */
uchar I2C_WriteBit(uchar addr,uchar bitNum,uchar Data) {
    uchar b;
    if (I2C_read(addr,&b)) {
        b = (Data != 0) ? (b | (1 << bitNum)):(b & ~(1 << bitNum));
        return I2C_Single_Write(addr,b);	//D′è?êy?Y
    }
    else
        return 0;
}

/**D′è?8????′??÷μ??à?????￡
 * @2?êy addr     I2C′ó?÷?t?ú2?μ??·
 * @2?êy bitStart μúò???μ?D′è?????￡¨0-7￡?
 * @2?êy length   D′μ?±èì?êy(2?3?1y8)
 * @2?êy Data     D′è?êy?Y
 * @·μ???μ ·μ??×′ì? (1=3é1|)
 */
uchar I2C_WriteBits(uchar addr,uchar bitStart,uchar length,uchar Data) {
    //      010 òaD′è?μ??μ
    // 76543210 ±èì???
    //    xxx   args: bitStart=4, length=3
    // 00011100 ?ú??×??ú
    // 10101111 ?-ê??μ￡¨?ù±?￡?
    // 10100011 ?-ê??μ & ~?ú??
    // 10101011 ?ú?? | ?-ê??μ
    uchar b,mask=0;
    if (I2C_read(addr,&b))    {
        mask = (((1<<length) - 1) << (bitStart-length + 1));	//?ú??
        Data <<=(bitStart - length + 1);	//°?D′è?μ?êy?Yò??ˉμ???
        Data &= mask;
        b &= ~(mask);
        b |= Data;
        I2C_Single_Write(addr,b);	//D′è?êy?Y
        return 1;
    }
    else
        return 0;
}

/**?áè?ò?????′ó8???÷?tμ???′??÷?￡
* @2?êy addr    I2C′ó?÷?t?ú2?μ??·
* @2?êy bitNum  ??μ?????à′?áè?￡¨0-7￡?
* @2?êy *data   êy?Y′?′￠μ??·
* @·μ???μ￡¨1=3é1|￡?
*/
uchar I2C_readBit(uchar addr,uchar bitNum,uchar *Data) {
    uchar b;
    if(I2C_read(addr,&b)) {
        *Data = b & (1 << bitNum);
        return 1;
    }
    else  {
        return 0;
    }
}
/**?áè?8????′??÷μ??à?????￡
* @2?êy addr    I2C′ó?÷?t?ú2?μ??·
* @2?êy bitStartμúò???μ??????áè?￡¨0-7￡?
* @2?êy length  ???áè?@2?êy3¤?èêy￡¨2?3?1y8￡?
* @2?êy *data   êy?Y′?′￠μ??·￡¨?′'101'è?o?bitStart?????áè???μèóú0X05￡?
* @·μ???μ￡¨1=3é1|￡?
*/
uchar I2C_readBits(uchar addr,uchar bitStart,uchar length,uchar *Data) {
    // 01101001 ?áè?×??ú
    // 76543210 ±èì???
    //    xxx   args: bitStart=4, length=3
    //    010   masked
    //   -> 010 shifted
    uchar b,mask=0;
    if(I2C_read(addr,&b)) {
        mask = ((1 << length) - 1) << (bitStart - length + 1);
        b &= mask;
        b >>= (bitStart - length + 1);
        *Data = b;
        return 1;
    }
    else
        return 0;
}
#endif
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$

//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//??í¨μ??áè?MPU6050μ?·?·¨?￡
//**************************************
//3?ê??ˉMPU6050
//**************************************
#ifdef GENERAL
void InitMPU6050() {
    I2C_Single_Write(PWR_MGMT_1, 0x00);	//?a3yDY??×′ì?
    I2C_Single_Write(SMPLRT_DIV, 0x07);	//íó?Yò?2é?ù?ê	125hz
    I2C_Single_Write(CONFIG, 0x01);		//1?a184hz  4?a21HZ??2¨ ?óê±A8.5ms ′?′|è??μó|?àμ±×￠òa￡??óê±ó??μí3?ü?ú?à?ü?aò?
    I2C_Single_Write(GYRO_CONFIG, 0x08);	//???ù?è íó?Yò?500?è/S 65.536LSB ?′65.536?a1?è????
    I2C_Single_Write(ACCEL_CONFIG, 0x00);  //?ó?ù?è ê1ó?+-2gá?3ì	 16384LSB
}
//**************************************
//o?3éêy?Y
//**************************************
int GetData(uchar REG_Address) {
    char H,L;
    H=I2C_Single_Read(REG_Address);
    L=I2C_Single_Read(REG_Address+1);
    return (H<<8)+L;   //o?3éêy?Y
}
//??oa3μ ′?í3?￡ê? ?áè????Dò????áμ??ó?ù?è￡?????ó???????μ??D??￡?2￠ó???ó|μ????ù?è×??￥21??2¨?￡
//è??ú￡????è oí???ù?èμ?????
void read_hubu_data(float *pacc,float *pangle,float *pangular_speed) {
    int acc,gular;
    float curr_angle=0,car_value=0;
    acc  = GetData(ACCEL_XOUT_H)+ 0; //ACC_OFFSET;	//?ó?ù?è + DT?y?μ
    gular= GetData(GYRO_YOUT_H) -65; //GULAR_OFFSET;//???ù?è +DT?y?μ
    //???ù?è???? 	°′???ù?èá?3ì è?1?ê1ó?μ?ê? ￡??à2000 ?è/s?￡16?????è￡??ò?????μêy=2^16/4000=16.4 LSB/(?è/s)
    car_value=(float)gular/65.536;	 //500μ?á?3ì
    *pangular_speed=car_value;
    //???èμ????? ?úD????è?é????  óéóú?ü??μ?￡¨x?a???è￡?sin(x)=x ?ùò??±?óê1ó?xμ??μ 	?ù×a???a???è ???è*180/3.1415926=???è  180?￠3.1415926=57.2957795
    //?ó?ù?èá?3ì ê?ê1ó? +-2G ?ò 65536/4
    curr_angle=(float)acc/16384.00*57.2957795;
    *pacc=curr_angle;
    //?￥21??2¨￡????ó?ù?è??3?μ????è×???2¨
    *pangle=(0.9)*(*pangle+car_value*SYSTIMES/1000)+(0.1)*curr_angle;
}
#endif

#ifdef DMP
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
//dmp?à1?μ?oˉêy
void DelayUs2x(unsigned char t) {	//?óê±1
    while(--t);
}

void DelayMs(unsigned char t) {	//?óê±2
    while(t--) {
        //′ó???óê±1mS
        DelayUs2x(245);
        DelayUs2x(245);
    }
}
/*
  ?ó?? DMP′ú??μ?
  ·μ???μ  (1=3é1|,0=꧰ü)
*/
uchar loadfirmware(void) {
    uint datanum=0;	//DMP1ì?tD′è?±ê????
    uchar ye,i,j;
    uchar bank=0;	//??￡¨256??êy?Yò???￡?
    uchar addr=0;
    for(; bank<8; bank++) {
        if(bank == 7)	//?aà?μ?×÷ó??íê???·?×?oóò???êy?Y
            i = 8;
        else
            i = 16;
        for(ye=0; ye<i; ye++) {
            I2C_Single_Write(0x6d,bank);
            I2C_Single_Write(0x6e,addr);
            I2C_Start();	//?eê?D?o?
            I2C_SendByte(SlaveAddress);	//éè±?μ??·+D′D?o?
            I2C_SendByte(0x6f);	//éè±??ú2?μ??·
            for(j=0; j<16; j++) {
                I2C_SendByte(dmpmemorydata[datanum++]);	//D′è?DMPμ?êy?Y
                if(ack) return 0;
            }
            addr += 16;
            I2C_Stop();	//í￡?1D?o?
        }
    }
    I2C_Single_Write(0x6d,7);
    I2C_Single_Write(0x6e,addr);
    I2C_Start();	//?eê?D?o?
    I2C_SendByte(SlaveAddress);	//éè±?μ??·+D′D?o?
    I2C_SendByte(0x6f);	//éè±??ú2?μ??·
    for(i=0; i<9; i++) {
        I2C_SendByte(dmpmemorydata[datanum++]);	//D′è?DMPμ?êy?Y
        if(ack) return 0;
    }
    I2C_Stop();	//í￡?1D?o?
    return 1;
}
uchar loadcfgupd(void) {	//DMPéè??
    uchar line;	//ò?12DèòaD′è?30ì?éè??êy?Y
    uchar bank;	//ò3
    uchar datacounts=0;	//DMPéè??êy?Y±ê????
    uchar bytes2write;	//êy?Y3¤?è?￡
    uchar offset;	//??ò?μ??·
    uchar writingcounts;	//êy?YD′è?±ê??ó?bytes2writeò?í?ê1ó?
    uchar special;

    for (line=0; line<30; line++) {
        bank=dmpcfgupddata[datacounts++];
        offset=dmpcfgupddata[datacounts++];
        bytes2write=dmpcfgupddata[datacounts++];
        I2C_Single_Write(0x6d,bank);
        I2C_Single_Write(0x6e,offset);
        I2C_Start();	//?eê?D?o?
        I2C_SendByte(SlaveAddress);	//éè±?μ??·+D′D?o?
        I2C_SendByte(0x6f);	//éè±??ú2?μ??·
        for(writingcounts=0; writingcounts<bytes2write; writingcounts++) {
            I2C_SendByte(dmpcfgupddata[datacounts++]);	//D′è?DMP????êy?Y
            if(ack) return 0;
        }
        if(0 == bytes2write) {
            special=dmpcfgupddata[datacounts++];
            if(0x01 == special) {
                //éè??á????ˉ?D????ó?￡¨??￡?;
                //éè??FIFO?o3???ò?3???ó?￡¨??￡?;
                //éè??DMP??ó?￡¨??￡?;
                I2C_Single_Write(0x38,0x32);
            }
            else
                return 0;
        }
    }
    I2C_Stop();	//í￡?1D?o?
    return 1;
}
/*×?oó?üD?DMP*/
uchar xdmpUpdates(uchar datacounts) {
    uchar writingcounts,bank,offset,bytes2write;
    bank=dmpUpdates[datacounts++];
    offset=dmpUpdates[datacounts++];
    bytes2write=dmpUpdates[datacounts++];
    I2C_Single_Write(0x6d,bank);
    I2C_Single_Write(0x6e,offset);
    I2C_Start();	//?eê?D?o?
    I2C_SendByte(SlaveAddress);	//éè±?μ??·+D′D?o?
    I2C_SendByte(0x6f);	//éè±??ú2?μ??·
    for(writingcounts=0; writingcounts<bytes2write; writingcounts++) {
        I2C_SendByte(dmpUpdates[datacounts++]);	//D′è?DMP????êy?Y
        if(ack) return 0;
    }
    I2C_Stop();	//í￡?1D?o?
    return 0;
}
/*?áè? FIFO ??êy*/
uint getFIFOCount() {
    uchar i[2];
    I2C_reads(0x72,2,i);
    return ((i[0]<<8)+i[1]);
}
/*FIFOêy?Y?áè?
2?êy *Data	′?′￠êy?Yμ?μ??·
·μ???μ (1=?áè?3é1|,0?áè?꧰ü)
*/
uchar readdmp(uchar *Data) {
    return I2C_reads(0x74,42,Data);
}

//?ó??2￠???? DMP êy×????ˉ′|àíòy??
uchar dmpInitialize(void) {
    uchar hwRevision,otpValid,mpuIntStatus/*fifoBuffer[128]*/;
    uchar xgOffsetTC,ygOffsetTC,zgOffsetTC;
    uint fifoCount;
    I2C_WriteBit(PWR_MGMT_1,7,1);	//?′?? MPU6050
    DelayMs(30);
    I2C_WriteBit(PWR_MGMT_1,6,0);	//???1?ˉ???￡ê?
    I2C_Single_Write(0x6D,0x70);	//D′è?ò???×??úêy?Yμ?0x6d??′??÷(????ó??§ bank)
    I2C_Single_Write(0x6E,0x06);	//D′è?ò???×??úêy?Yμ?0x6e??′??÷(????′?′￠×??ú)
    I2C_read(0x6F,&hwRevision);	//?áè?
    I2C_Single_Write(0x6D,0);	//?????ú′? bank ????
    I2C_readBit(0x00,0,&otpValid);	//?áè? OTP bank óDD§±ê??
    I2C_readBits(0x00,6,6,&xgOffsetTC);	//?áíó?Y????TC?μ X
    I2C_readBits(0x01,6,6,&ygOffsetTC);	//?áíó?Y????TC?μ Y)
    I2C_readBits(0x02,6,6,&zgOffsetTC);	//?áíó?Y????TC?μ Z
    I2C_Single_Write(0x25,0x7f);	//éè??′ó0μ??· 0x7
    I2C_WriteBit(USER_CTRL,5,0);	//??ó?I2C?÷?￡ê?
    I2C_Single_Write(0x25,0x68);	//?aà??é?üòa???￡?1??óD?a?÷°×?aà?
    I2C_WriteBit(USER_CTRL,1,1);	//I2C×ü???÷???′??
    DelayMs(20);
    if((loadfirmware()) == 0 ) return 0;	//?ó?? DMP′ú??μ??ú′?
    if((loadcfgupd()) == 0 ) return 0;	//????DMP
    I2C_WriteBits(PWR_MGMT_1,2,3,0x03);	//éè??ê±?ó??3??′Zíó?Y
    I2C_Single_Write(0x38,0x12);	//éè??DMPoíFIFO_OFLOW??ó??D??
    I2C_Single_Write(0x19,4);	//éè??2é?ù?ê?a200 hz  (1khz / (1 + 4) = 200 Hz)
    I2C_WriteBits(CONFIG,5,3,0x1);	//éè??ía2???í?2?TEMP_OUT_L[0]
    I2C_WriteBits(CONFIG,2,3,0x03);	//éè??DLPF′??í42o?×è
    //I2C_WriteBits(GYRO_CONFIG,4,2,0x03);	//íó?Yáé???èéè???a+ / - 2000 deg/sec
    //I2C_WriteBits(GYRO_CONFIG,4,2,0x01);	//íó?Yáé???èéè???a+ / - 500 deg/sec
    I2C_WriteBits(GYRO_CONFIG,4,2,0x02);	//íó?Yáé???èéè???a+ / - 500 deg/sec
    I2C_Single_Write(0x70,0x03);	//éè??DMP????×??ú￡¨1|?ü?′?a￡?
    I2C_Single_Write(0x71,0x00);	//éè??DMP????×??ú￡¨1|?ü?′?a￡?
    I2C_WriteBit(0x00,0,0);	//??3yOTP Bank ±ê??
    I2C_WriteBits(0x00,6,6,xgOffsetTC);	//éè??X íó?Yμ???TCs???°μ??μ
    I2C_WriteBits(0x01,6,6,ygOffsetTC);	//éè??Y íó?Yμ???TCs???°μ??μ
    I2C_WriteBits(0x02,6,6,zgOffsetTC);	//éè??Z íó?Yμ???TCs???°μ??μ
    xdmpUpdates(0);	//×?oó?üD?1/7(oˉêy?′?a)dmpUpdatesêy×éμúò?DD
    xdmpUpdates(5);	//×?oó?üD?2/7(oˉêy?′?a)dmpUpdatesêy×éμú?tDD
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    fifoCount = getFIFOCount();	//?áè? FIFO ??êy
    //readdmp(fifoCount,fifoBuffer);	//?áè?FIFOà?μ?êy?Y
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO

    I2C_Single_Write(0x1F,2);	//???ˉ?ì2a?D?μéè???a2
    I2C_Single_Write(0x21,156);	//á????ˉ?ì2a?D?μ?a156
    I2C_Single_Write(0x20,80);	//éè?????ˉ?ì2a3?D?ê±???á80
    I2C_Single_Write(0x22,0);	//éè??á????ˉ?ì2aê±??0
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    I2C_WriteBit(USER_CTRL,6,1);	//ê1?ü FIFO
    I2C_WriteBit(USER_CTRL,7,1);	//ê1?ü DMP
    I2C_WriteBit(USER_CTRL,3,1);	//?′?? DMP
    xdmpUpdates(12);	//×?oó?üD?3/7(oˉêy?′?a)dmpUpdatesêy×éμúèyDD
    xdmpUpdates(17);	//×?oó?üD?4/7(oˉêy?′?a)dmpUpdatesêy×éμú??DD
    xdmpUpdates(28);	//×?oó?üD?5/7(oˉêy?′?a)dmpUpdatesêy×éμú??DD
    while((fifoCount = getFIFOCount()) < 3);	//μè′y FIFO ??êy > 2
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    //readdmp(fifoCount,fifoBuffer);	//?áè?FIFOà?μ?êy?Y
    I2C_read(0x3A,&mpuIntStatus);	//?áè??D??×′ì?
    xdmpUpdates(35);	//×?oó?üD?6/7(oˉêy?′?a)dmpUpdatesêy×éμúáùDD
    while((fifoCount = getFIFOCount()) < 3);	//μè′y FIFO ??êy > 2
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    //readdmp(fifoCount,fifoBuffer);	//?áè?FIFOà?μ?êy?Y
    I2C_read(0x3A,&mpuIntStatus);	//?áè??D??×′ì?
    xdmpUpdates(40);	//×?oó?üD?7/7(oˉêy?′?a)dmpUpdatesêy×éμú??DD
    I2C_WriteBit(USER_CTRL,7,0);	//??ó?DMP(é?oó?ú′ò?a?ü)
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    I2C_read(0x3A,&mpuIntStatus);
    return 1;
}
/*3?ê??ˉMPU6050*/
void initDMPMPU6050(void) {
    I2C_WriteBits(PWR_MGMT_1,2,3,0x01);	//μ??′1üàí
    //I2C_WriteBits(GYRO_CONFIG,4,2,0x00);	//éè??íó?Yò?á?3ì 250/s
    //I2C_WriteBits(GYRO_CONFIG,4,2,0x08);	//éè??íó?Yò?á?3ì 500/s
    I2C_WriteBits(GYRO_CONFIG,4,2,0x10);	//éè??íó?Yò?á?3ì 1000/s
    I2C_WriteBits(ACCEL_CONFIG,4,2,0x00);	//éè???ó?ù?èá?3ì 2G
    I2C_WriteBit(PWR_MGMT_1,6,1);	//μ??′1üàíMUP??è??ˉ???￡ê?
}
/* ê1?üDMP*/
void enableDMP(void) {
    I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    I2C_WriteBit(USER_CTRL,7,1);	//ê1?üDMP
}
/*?é?¤MPU6050á??ó*/
uchar getDeviceID(void) {
    uchar b=0;	//áùê±±?á?
    I2C_readBits(WHO_AM_I,6,6,&b);	//?áè?i2c1ì?¨μ??·￡?è￥μ?×?????oí×?μí???aá???êy?Y
    return b == 0x34;	//?D??Bê?·?μèóú0x34￡?è?1?μèóú·μ??1￡?2?μèóú·μ??0(?aμ?ê?0x38)
}

//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//只是读取一个角度extern void read_dmp_angle(float *pangel,float *pgyrox);
void read_dmp_angle(float *pangel,float *pangel2)	
 {	
    xdata uchar dmpdatas[42];	//DMPêy?Y
    xdata float quartf[4],quar_add; // ???aêy
    uchar zd,i;
    i=getFIFOCount();//?áè?FIFO??êy
    I2C_read(0x3A,&zd);	//?áè??D??×′ì?
    if((zd & 0x10)||i==1024) {	//?D??FIFOê?·?ò?3?
        I2C_WriteBit(USER_CTRL,2,1);	//?′?? FIFO
    }
    else if (zd & 0x02) {
        while(i<42) i=getFIFOCount();
        readdmp(dmpdatas);	//?áè?FIFOêy?Y
    }

    quartf[0] = (float)(((long)dmpdatas[0] << 24) | ((long)dmpdatas[1] << 16) | ((long)dmpdatas[2] << 8) | dmpdatas[3])/1073741824.0f;
    quartf[1] = (float)(((long)dmpdatas[4] << 24) | ((long)dmpdatas[5] << 16) | ((long)dmpdatas[6] << 8) | dmpdatas[7])/1073741824.0f;
    quartf[2] = (float)(((long)dmpdatas[8] << 24) | ((long)dmpdatas[9] << 16) | ((long)dmpdatas[10] << 8) | dmpdatas[11])/1073741824.0f;
    quartf[3] = (float)(((long)dmpdatas[12] << 24) | ((long)dmpdatas[13] << 16) | ((long)dmpdatas[14] << 8) | dmpdatas[15])/1073741824.0f;
    quar_add=quartf[0]*quartf[0]+quartf[1]*quartf[1]+quartf[2]*quartf[2]+quartf[3]*quartf[3];
    if(quar_add<0.99 || quar_add>1.01 ) return;
// 			q0 = q0 / norm;
// 	q1 = q1 / norm;
// 	q2 = q2 / norm;
// 	q3 = q3 / norm;
		quartf[0]=quartf[0]/quar_add;
			quartf[1]=quartf[1]/quar_add;
			quartf[2]=quartf[2]/quar_add;
			quartf[3]=quartf[3]/quar_add;
    *pangel=asin(-2 * quartf[1] * quartf[3] + 2 * quartf[0]* quartf[2])* 57.3;///2.4
    *pangel2=asin(2 * quartf[0] * quartf[1] + 2 * quartf[2]* quartf[3])* 57.3;///2.4

}
#endif