p kj / Mbed 2 deprecated LPC824-BalanceCar

Microduino

Dependencies:   mbed

Fork of BalanceCar by Li Weiyi

RollPitch.h@1:620da20b810b, 2016-06-04 (annotated)

Committer:
lixianyu
Date:
Sat Jun 04 04:06:36 2016 +0000
Revision:
1:620da20b810b
Parent:
0:a4d8f5b3c546
Child:
2:99785a1007a4
sqrt???????float???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lixianyu 1:620da20b810b 1 #include "JJ_MPU6050_DMP_6Axis.h" // 与DMP工作库的修改版本（见注释内）typedef enum
lixianyu 0:a4d8f5b3c546 2
lixianyu 0:a4d8f5b3c546 3 #define I2C_SPEED 400000L //I2C速度
lixianyu 0:a4d8f5b3c546 4
lixianyu 0:a4d8f5b3c546 5 MPU6050 mpu;
lixianyu 0:a4d8f5b3c546 6 bool dmpReady = false;
lixianyu 0:a4d8f5b3c546 7 uint16_t packetSize;
lixianyu 0:a4d8f5b3c546 8
lixianyu 0:a4d8f5b3c546 9 bool dmpSetup()
lixianyu 0:a4d8f5b3c546 10 {
lixianyu 0:a4d8f5b3c546 11 #if 0
lixianyu 0:a4d8f5b3c546 12 Wire.begin();
lixianyu 0:a4d8f5b3c546 13 TWSR = 0;
lixianyu 0:a4d8f5b3c546 14 TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
lixianyu 0:a4d8f5b3c546 15 TWCR = 1 << TWEN;
lixianyu 0:a4d8f5b3c546 16 #endif
lixianyu 0:a4d8f5b3c546 17
lixianyu 0:a4d8f5b3c546 18 mpu.initialize();
lixianyu 0:a4d8f5b3c546 19 if(mpu.dmpInitialize() == 0) {
lixianyu 0:a4d8f5b3c546 20 mpu.setDMPEnabled(true);
lixianyu 0:a4d8f5b3c546 21 dmpReady = true;
lixianyu 0:a4d8f5b3c546 22 packetSize = mpu.dmpGetFIFOPacketSize();
lixianyu 0:a4d8f5b3c546 23 } else {
lixianyu 0:a4d8f5b3c546 24 dmpReady = false;
lixianyu 0:a4d8f5b3c546 25 }
lixianyu 0:a4d8f5b3c546 26 return dmpReady;
lixianyu 0:a4d8f5b3c546 27 }
lixianyu 0:a4d8f5b3c546 28 #define PI 3.1415926
lixianyu 0:a4d8f5b3c546 29 uint8_t dmpGetYPR(float *_ypr)
lixianyu 0:a4d8f5b3c546 30 {
lixianyu 0:a4d8f5b3c546 31 Quaternion q;
lixianyu 0:a4d8f5b3c546 32 VectorFloat gravity;
lixianyu 0:a4d8f5b3c546 33 uint8_t fifoBuffer[18];
lixianyu 0:a4d8f5b3c546 34 if(!dmpReady)
lixianyu 0:a4d8f5b3c546 35 return 0;
lixianyu 0:a4d8f5b3c546 36
lixianyu 0:a4d8f5b3c546 37 if(mpu.getFIFOCount() == 1024) {
lixianyu 0:a4d8f5b3c546 38 mpu.resetFIFO();
lixianyu 0:a4d8f5b3c546 39 } else {
lixianyu 0:a4d8f5b3c546 40 while(mpu.getFIFOCount() < packetSize);
lixianyu 0:a4d8f5b3c546 41 mpu.getFIFOBytes(fifoBuffer, packetSize);
lixianyu 0:a4d8f5b3c546 42 mpu.dmpGetQuaternion(&q, fifoBuffer);
lixianyu 0:a4d8f5b3c546 43 mpu.dmpGetGravity(&gravity, &q);
lixianyu 0:a4d8f5b3c546 44 mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
lixianyu 0:a4d8f5b3c546 45 for(int i=0; i<3; i++) {
lixianyu 0:a4d8f5b3c546 46 _ypr[i] *= 180/PI;
lixianyu 0:a4d8f5b3c546 47 }
lixianyu 0:a4d8f5b3c546 48 }
lixianyu 0:a4d8f5b3c546 49 return 0;
lixianyu 0:a4d8f5b3c546 50 }