p kj
/
LPC824-BalanceCar
Microduino
Fork of BalanceCar by
RollPitch.h
- Committer:
- lixianyu
- Date:
- 2016-06-04
- Revision:
- 0:a4d8f5b3c546
- Child:
- 1:620da20b810b
File content as of revision 0:a4d8f5b3c546:
#include <JJ_MPU6050_DMP_6Axis.h> // 与DMP工作库的修改版本(见注释内)typedef enum #define I2C_SPEED 400000L //I2C速度 MPU6050 mpu; bool dmpReady = false; uint16_t packetSize; bool dmpSetup() { #if 0 Wire.begin(); TWSR = 0; TWBR = ((16000000L / I2C_SPEED) - 16) / 2; TWCR = 1 << TWEN; #endif mpu.initialize(); if(mpu.dmpInitialize() == 0) { mpu.setDMPEnabled(true); dmpReady = true; packetSize = mpu.dmpGetFIFOPacketSize(); } else { dmpReady = false; } return dmpReady; } #define PI 3.1415926 uint8_t dmpGetYPR(float *_ypr) { Quaternion q; VectorFloat gravity; uint8_t fifoBuffer[18]; if(!dmpReady) return 0; if(mpu.getFIFOCount() == 1024) { mpu.resetFIFO(); } else { while(mpu.getFIFOCount() < packetSize); mpu.getFIFOBytes(fifoBuffer, packetSize); mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity); for(int i=0; i<3; i++) { _ypr[i] *= 180/PI; } } return 0; }