int DMP::getAttitude()
{
if (!dmpReady) return -1;
// wait for FIFO count > 42 bits
do {
fifoCount = mpu.getFIFOCount();
}while (fifoCount<42);
if (fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
return -1;
// otherwise, check for DMP data ready interrupt
//(this should happen frequently)
} else {
//read packet from fifo
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
for (int i=0;i<DIM;i++){
//offset removal
ypr[i]-=m_ypr_off[i];
//scaling for output in degrees
ypr[i]*=180/M_PI;
}
//printf(" %7.2f %7.2f %7.2f\n",ypr[0],ypr[1],ypr[2]);
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of ROLL, MPU is attached upside down
ypr[2]*=-1.0;
mpu.dmpGetGyro(g, fifoBuffer);
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
}
// printf("gyro %7.2f %7.2f %7.2f \n", (float)g[0]/131.0,
// (float)g[1]/131.0,
// (float)g[2]/131.0);
return 0;
}
}
void update_IMU()
{
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) mpu.resetFIFO();
// 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;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
mpu.dmpGetGyro(gyroRate,fifoBuffer);
gyro_rate_float[0] = (float)gyroRate[0]/2147483648*2000*0.41;
gyro_rate_float[1] = (float)gyroRate[1]/2147483648*2000*0.41;
gyro_rate_float[2] = (float)gyroRate[2]/2147483648*2000*0.41;
}
}
void DMP::initialize(){
//This routine waits for the yaw angle to stop
//drifting
if (!dmpReady) return;
printf("Initializing IMU...\n");
//float gyr_old = 10;
int n=0;
do {
// wait for FIFO count > 42 bits
do {
fifoCount = mpu.getFIFOCount();
}while (fifoCount<42);
if (fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt
//(this should happen frequently)
} else {
//save old yaw value
//gyr_old = gyro[ROLL];
//read packet from fifo
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetGyro(g, fifoBuffer);
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
}
// mpu.dmpGetQuaternion(&q, fifoBuffer);
// mpu.dmpGetGravity(&gravity, &q);
// mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// // printf("yaw = %f, pitch = %f, roll = %f\n",
// // ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
// // ypr[ROLL]*180/M_PI);
}
n++;
}while (fabs(gyro[ROLL]) + fabs(gyro[PITCH]) > 0.03 && n<5000);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
for (int i=0;i<DIM;i++) m_ypr_off[i] = ypr[i];
printf("IMU init done; offset values are :\n");
printf("yaw = %f, pitch = %f, roll = %f, n= %d\n\n",
ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
ypr[ROLL]*180/M_PI,n);
initialized = true;
}
void imu_read()
{
uint8_t count = 3;
// if programming failed, don't try to do anything
if (!dmpReady) return;
#ifdef IRQ_DEBUG
#ifdef __BOARD_YUN__
Console.println(F("DMP is ready!\nAwaiting for IRQ ready flag"));
#else
Serial.println(F("DMP is ready!\nAwaiting for IRQ ready flag"));
#endif
#endif
// wait for MPU interrupt or extra packet(s) available
/* while (!mpuInterrupt && fifoCount < packetSize) {
// Serial.println("Shouldn't get here...");
}
Serial.println("DMP flag OK");
*/
while(!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();
#ifdef __BOARD_YUN__
Console.println(F("FIFO overflow!"));
#else
Serial.println(F("FIFO overflow!"));
#endif
// 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;
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
#ifdef __USE_ANTILOCK__
if(lastQ == q)
{
if(--count)
{
#ifdef __BOARD_YUN__
Console.println(F("\nLOCKED\n"));
dmpReady = false;
Console.println(F("DMP is stalled, reinitializing..."));
#else
Serial.println(F("\nLOCKED\n"));
dmpReady = false;
Serial.println(F("DMP stalled, reinitializing..."));
#endif
mpu.reset();
if ((devStatus = mpu.dmpInitialize()) == 0)
{
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
#ifdef __BOARD_YUN__
Console.print(F("DMP reinitialization failed (code "));
Console.print(devStatus);
Console.println(F(")"));
#else
Serial.print(F("DMP reinitialization failed (code "));
Serial.print(devStatus);
Serial.println(")");
#endif
while (true) {
//delay(300);
//nilThdSleep(300);
sleep(300);
digitalWrite(SOL_LED, 0);
//delay(300);
//nilThdSleep(300);
sleep(300);
digitalWrite(SOL_LED, 1);
}
}
}
}
#endif
}
else {
#ifdef IRQ_DEBUG
#ifdef __BOARD_YUN__
Console.print(F("OK"));
#else
Serial.print(F("OK "));
#endif
#endif
count = 3;
lastQ = q;
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetGyro(gyro, fifoBuffer);
#ifdef DEBUG
#ifdef __BOARD_YUN__
Console.print(F("ypr gyro\t"));
Console.print(ypr[0] * 180/M_PI);
Console.print(F("\t"));
Console.print(ypr[1] * 180/M_PI);
Console.print(F("\t"));
Console.print(ypr[2] * 180/M_PI);
Console.print(F("\t"));
Console.println(gyro);
#else
Serial.print(F("ypr gyro\t"));
Serial.print(ypr[0] * 180/M_PI);
Serial.print(F("\t"));
Serial.print(ypr[1] * 180/M_PI);
Serial.print(F("\t"));
Serial.print(ypr[2] * 180/M_PI);
Serial.print(F("\t"));
Serial.println(gyro);
#endif
#endif
}
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(SOL_LED, blinkState);
//delay(1);
//nilThdSleep(1);
sleep(1);
}
