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); }