   unsigned long now = millis();
 
   if (imu->IMURead()) {
+    failedIMUReadCount = 0;
     fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
-    if ((now - lastDataPacketSent) >= DATA_PACKET_SEND_INTERVAL) {
-      lastDataPacketSent = now;
-      if (!imu->IMUGyroBiasValid()) {
-        Serial.println("Calculating gyro bias - don't move IMU!");
 
-        char buffer[2];
-        buffer[0] = SBTSailbotModelHeaderState;
-        buffer[1] = SBTSailbotModelStateCalibratingIMU;
-        RFduinoBLE.send((char*)&buffer, 2);
-        return;
-      }
+    // Determine local state
+    if (!imu->IMUGyroBiasValid()) {
+      state = SBTSailbotModelStateCalibratingIMU;
+    } else {
+      state = remoteState;
+    }
 
+    // Update rudder and sheet
+    if (state == SBTSailbotModelStateAutomaticControl) {
+
+    } else if (state == SBTSailbotModelStateRecoveryMode) {
+
+    }
+  } else {
+    failedIMUReadCount++;
+    if (failedIMUReadCount > 50) {
+      state = SBTSailbotModelStateNoIMU;
+    }
+  }
+
+  // Communicate with remote
+  if ((now - lastDataPacketSent) >= DATA_PACKET_SEND_INTERVAL) {
+    lastDataPacketSent = now;
+    if (state == SBTSailbotModelStateCalibratingIMU) {
+      char buffer[2];
+      buffer[0] = SBTSailbotModelHeaderState;
+      buffer[1] = SBTSailbotModelStateCalibratingIMU;
+      RFduinoBLE.send((char*)&buffer, 2);
+    } else if (state == SBTSailbotModelStateNoIMU) {
+      char buffer[2];
+      buffer[0] = SBTSailbotModelHeaderState;
+      buffer[1] = SBTSailbotModelStateNoIMU;
+      RFduinoBLE.send((char*)&buffer, 2);
+    } else {
       const RTVector3& vec = fusion.getFusionPose();
-
       char buffer[10];
       buffer[0] = SBTSailbotModelHeaderBoatHeading;
       buffer[1] = state;

      memcpy(&buffer[2], &heading, sizeof(heading));
      RFduinoBLE.send((char*)&buffer, sizeof(float) + 2);
    }
  } else {
    char buffer[2];
    buffer[0] = SBTSailbotModelHeaderState;
    buffer[1] = SBTSailbotModelStateNoIMU;
    RFduinoBLE.send((char*)&buffer, 2);
  }
}


void RFduinoBLE_onConnect() {
  Serial.println("Bluetooth connect");

void setup() {
  Serial.begin(SERIAL_PORT_SPEED);
  Wire.begin();
  remoteState = SBTSailbotModelStateManualControl;
  imu = RTIMU::createIMU(&settings);                        // create the imu object

  Serial.print("ArduinoIMU starting using device ");
