PositionSensors.cpp

#include "Config.h"
#include "PositionSensors.h"
#include "MPU6050_9Axis_MotionApps41.h"

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

// Orientation/motion variables
Quaternion q;
VectorFloat gravity;
float euler[3];

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
	mpuInterrupt = true;

	// blink LED to indicate activity
	blinkState = !blinkState;
	digitalWrite(LED_PIN, blinkState);
}

void PositionSensors::init()
{
	// join I2C bus (I2Cdev library doesn't do this automatically)
	Wire.begin();

	DEBUG.println(F("Initializing I2C devices..."));
	mpu.initialize();

	DEBUG.println(F("Testing device connections..."));
	DEBUG.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	DEBUG.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if so)
	if (devStatus == 0) {
		DEBUG.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// enable Arduino interrupt detection
		DEBUG.println(F("Enabling interrupt detection ..."));
		pinMode(14, INPUT);
		attachInterrupt(14, dmpDataReady, RISING);
		mpuIntStatus = mpu.getIntStatus();

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		DEBUG.println(F("DMP ready! Waiting for first interrupt..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
	}
	else {
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		DEBUG.print(F("DMP Initialization failed (code "));
		DEBUG.print(devStatus);
		DEBUG.println(F(")"));
	}

	// configure LED for output
	pinMode(LED_PIN, OUTPUT);
}

void PositionSensors::getYawPitchRoll(float* ypr)
{
	const float RADIANS_TO_DEGREES = 57.2958; //180/3.14159

	if (mpuInterrupt || fifoCount >= packetSize){
		// reset interrupt flag and get INT_STATUS byte
		mpuInterrupt = false;
		mpuIntStatus = mpu.getIntStatus();

		// get current FIFO count
		fifoCount = mpu.getFIFOCount();

		// check for overflow (this should never happen unless our code is too inefficient)
		if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
			// reset so we can continue cleanly
			mpu.resetFIFO();
			DEBUG.println(F("FIFO overflow!"));

			// otherwise, check for DMP data ready interrupt (this should happen frequently)
		}
		else if (mpuIntStatus & 0x02) {
			// wait for correct available data length, should be a VERY short wait
			while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

			// read a packet from FIFO
			mpu.getFIFOBytes(fifoBuffer, packetSize);

			// track FIFO count here in case there is > 1 packet available
			// (this lets us immediately read more without waiting for an interrupt)
			fifoCount -= packetSize;

			// Obtain Euler angles from buffer
			//mpu.dmpGetQuaternion(&q, fifoBuffer);
			//mpu.dmpGetEuler(euler, &q);

			// Obtain YPR angles from buffer
			mpu.dmpGetQuaternion(&q, fifoBuffer);
			mpu.dmpGetGravity(&gravity, &q);
			mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
		}
	}
}

bool PositionSensors::dmpIsReady()
{
	return dmpReady;
}