Created
January 23, 2018 09:28
-
-
Save IOT-123/30c13a327cfa9e88d9d8a10111426320 to your computer and use it in GitHub Desktop.
A test sketch that logs basic PITCH/ROLL/YAW angle for the KY-521 module.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "I2Cdev.h" | |
#include "MPU6050_6Axis_MotionApps20.h" | |
#include "Wire.h" | |
MPU6050 mpu; | |
uint8_t mpuIntStatus; | |
uint16_t packetSize; | |
uint16_t fifoCount; | |
uint8_t fifoBuffer[64]; | |
Quaternion q; | |
VectorFloat gravity; | |
float ypr[3]; | |
volatile bool mpuInterrupt = false; | |
void dmpDataReady() {mpuInterrupt = true;} | |
void setup() { | |
Wire.begin(); | |
mpu.initialize(); | |
mpu.dmpInitialize(); | |
mpu.setDMPEnabled(true); | |
attachInterrupt(0, dmpDataReady, RISING); | |
mpuIntStatus = mpu.getIntStatus(); | |
packetSize = mpu.dmpGetFIFOPacketSize(); | |
Serial.begin(115200); | |
} | |
void loop() { | |
while (!mpuInterrupt && fifoCount < packetSize) {} | |
mpuInterrupt = false; | |
mpuIntStatus = mpu.getIntStatus(); | |
fifoCount = mpu.getFIFOCount(); | |
if ((mpuIntStatus & 0x10) || fifoCount == 1024) { | |
mpu.resetFIFO(); | |
Serial.println(F("FIFO overflow!")); | |
} | |
else if (mpuIntStatus & 0x02) { | |
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); | |
mpu.getFIFOBytes(fifoBuffer, packetSize); | |
fifoCount -= packetSize; | |
mpu.dmpGetQuaternion(&q, fifoBuffer); | |
mpu.dmpGetGravity(&gravity, &q); | |
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); | |
Serial.print("ypr\t"); | |
Serial.print(ypr[0]*180/M_PI); | |
Serial.print("\t"); | |
Serial.print(ypr[1]*180/M_PI); | |
Serial.print("\t"); | |
Serial.print(ypr[2]*180/M_PI); | |
Serial.println(); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment