p kj / Mbed 2 deprecated LPC824-BalanceCar

Microduino

Dependencies:   mbed

Fork of BalanceCar by Li Weiyi

RollPitch.h@2:99785a1007a4, 2016-06-07 (annotated)

Committer:
lixianyu
Date:
Tue Jun 07 05:26:03 2016 +0000
Revision:
2:99785a1007a4
Parent:
1:620da20b810b 
Still not work......commit first.

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 2:99785a1007a4 8 //extern Serial mpc;
lixianyu 2:99785a1007a4 9 extern DigitalOut myled;
lixianyu 0:a4d8f5b3c546 10 bool dmpSetup()
lixianyu 0:a4d8f5b3c546 11 {
lixianyu 0:a4d8f5b3c546 12 #if 0
lixianyu 0:a4d8f5b3c546 13 Wire.begin();
lixianyu 0:a4d8f5b3c546 14 TWSR = 0;
lixianyu 0:a4d8f5b3c546 15 TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
lixianyu 0:a4d8f5b3c546 16 TWCR = 1 << TWEN;
lixianyu 0:a4d8f5b3c546 17 #endif
lixianyu 0:a4d8f5b3c546 18
lixianyu 0:a4d8f5b3c546 19 mpu.initialize();
lixianyu 0:a4d8f5b3c546 20 if(mpu.dmpInitialize() == 0) {
lixianyu 0:a4d8f5b3c546 21 mpu.setDMPEnabled(true);
lixianyu 0:a4d8f5b3c546 22 dmpReady = true;
lixianyu 0:a4d8f5b3c546 23 packetSize = mpu.dmpGetFIFOPacketSize();
lixianyu 0:a4d8f5b3c546 24 } else {
lixianyu 0:a4d8f5b3c546 25 dmpReady = false;
lixianyu 0:a4d8f5b3c546 26 }
lixianyu 2:99785a1007a4 27 //mpc.printf("packetSize = %d\r\n", packetSize);
lixianyu 0:a4d8f5b3c546 28 return dmpReady;
lixianyu 0:a4d8f5b3c546 29 }
lixianyu 2:99785a1007a4 30
lixianyu 0:a4d8f5b3c546 31 #define PI 3.1415926
lixianyu 0:a4d8f5b3c546 32 uint8_t dmpGetYPR(float *_ypr)
lixianyu 0:a4d8f5b3c546 33 {
lixianyu 0:a4d8f5b3c546 34 Quaternion q;
lixianyu 0:a4d8f5b3c546 35 VectorFloat gravity;
lixianyu 0:a4d8f5b3c546 36 uint8_t fifoBuffer[18];
lixianyu 0:a4d8f5b3c546 37 if(!dmpReady)
lixianyu 0:a4d8f5b3c546 38 return 0;
lixianyu 0:a4d8f5b3c546 39
lixianyu 2:99785a1007a4 40 //myled = !myled;
lixianyu 0:a4d8f5b3c546 41 if(mpu.getFIFOCount() == 1024) {
lixianyu 2:99785a1007a4 42 myled = 1;
lixianyu 0:a4d8f5b3c546 43 mpu.resetFIFO();
lixianyu 2:99785a1007a4 44 while(mpu.getFIFOCount() < packetSize);
lixianyu 2:99785a1007a4 45 mpu.getFIFOBytes(fifoBuffer, packetSize);
lixianyu 2:99785a1007a4 46 mpu.dmpGetQuaternion(&q, fifoBuffer);
lixianyu 2:99785a1007a4 47 mpu.dmpGetGravity(&gravity, &q);
lixianyu 2:99785a1007a4 48 mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
lixianyu 2:99785a1007a4 49 for(int i=0; i<3; i++) {
lixianyu 2:99785a1007a4 50 _ypr[i] *= 180/PI;
lixianyu 2:99785a1007a4 51 }
lixianyu 0:a4d8f5b3c546 52 } else {
lixianyu 0:a4d8f5b3c546 53 while(mpu.getFIFOCount() < packetSize);
lixianyu 0:a4d8f5b3c546 54 mpu.getFIFOBytes(fifoBuffer, packetSize);
lixianyu 0:a4d8f5b3c546 55 mpu.dmpGetQuaternion(&q, fifoBuffer);
lixianyu 0:a4d8f5b3c546 56 mpu.dmpGetGravity(&gravity, &q);
lixianyu 0:a4d8f5b3c546 57 mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
lixianyu 0:a4d8f5b3c546 58 for(int i=0; i<3; i++) {
lixianyu 0:a4d8f5b3c546 59 _ypr[i] *= 180/PI;
lixianyu 0:a4d8f5b3c546 60 }
lixianyu 2:99785a1007a4 61 myled = 0;
lixianyu 0:a4d8f5b3c546 62 }
lixianyu 0:a4d8f5b3c546 63 return 0;
lixianyu 0:a4d8f5b3c546 64 }