   ledStatus = true;
   lastBlink = micros();
 
+  lastPoll = lastIMUPoll = lastHumidityPoll = lastPressurePoll = micros();
+  timeout= (unsigned long) (10*(imu->IMUGetPollInterval() * 1000));
+
 } // setup
   
 void loop()
 {  
-    unsigned long currentTime = micros();
-    
     RTQuaternion rotatedGravity;
     RTQuaternion fusedConjugate;
     RTQuaternion qTemp;
     float residualsAlpha;
     float biasAlpha;
-    
+
+    //  poll at the rate recommended by the IMU
+    unsigned long currentTime = micros();
+    int pollDelay = ((imu->IMUGetPollInterval() * 1000) - (int)(currentTime - lastPoll));
+    if (pollDelay > 0) { delayMicroseconds(pollDelay); }
+    lastPoll = currentTime;
+
+    // check IMU stalled
+    if ( (currentTime - lastIMUPoll) > timeout ) {
+      // We have IMU stalled and need to reset it
+      Serial.println("!!!!!!!!!!!!!!!!!!!! IMU RESET: wait of data for too long !!!!!!!!!!!!!!!!!!!!\n");
+      imu->IMUInit();
+      lastPoll = micros();
+    }
+
     if (imu->IMURead()) {                       // get the latest data if any avail
         imuData = imu->getIMUData();
+        lastIMUPoll = micros();
+        
+        if ( (imuData.gyro.length() > 35.0) || (imuData.accel.length() > 16.0) || (imuData.compass.length() > 1000.0) ) {
+          // IMU Data Error
+          Serial.println("!!!!!!!!!!!!!!!!!!!! IMU RESET: Data out of range !!!!!!!!!!!!!!!!!!!!\n");
+          imu->IMUInit();
+          lastIMUPoll = micros();
+        }
+
         sampleCount++;
         dt=imuData.timestamp-lastTimestamp;
         lastTimestamp = imuData.timestamp;

    } // imuread
     
    ///////////////////////////////////////////////////////////////
    // Input Commands
    ///////////////////////////////////////////////////////////////
    if (INAVAIL) {

unsigned long lastTimestamp;
unsigned long motionstart;
unsigned long lastBlink;
unsigned long lastPoll;
unsigned long lastIMUPoll;
unsigned long lastHumidityPoll;
unsigned long lastPressurePoll;
unsigned long timeout;

int dt;       // time in between samples
int dtmotion; // time during which motion occured
