   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;

  Serial.println("Bluetooth disconnect");
}

void RFduinoBLE_onReceive(char *data, int len) {

  SBTSailbotModelHeader command = (SBTSailbotModelHeader)data[0];

  switch (command) {
    case SBTSailbotModelHeaderAutomaticControl: {
        state = SBTSailbotModelStateAutomaticControl;
        int heading;
        memcpy(&heading, &data[1], sizeof(heading));
        Serial.print("Selected heading "); Serial.println(heading);
        break;
      }
    case SBTSailbotModelHeaderManualControl: {
        state = SBTSailbotModelStateManualControl;
        int rudder;
        memcpy(&rudder, &data[1], sizeof(rudder));
        int sheet;

  SBTSailbotModelStateRecoveryMode,
};

#define DATA_PACKET_SEND_INTERVAL  500
#define  SERIAL_PORT_SPEED  9600

unsigned long lastDataPacketSent;

char state;
char remoteState;
int failedIMUReadCount;

Servo tillerServo;
Servo sheetServo;
