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