     if (state != SBTSailbotModelStateCalibratingIMU && state != SBTSailbotModelStateNoIMU) {
       const RTVector3& vec = fusion.getFusionPose();
       float rawHeading = vec.z();
+      // Convert range to 0 to 2 * M_PI
       if (rawHeading < 0)
         heading = rawHeading + 2 * M_PI;
       else
         heading = rawHeading;
+      // Apply compass offset
+      if (heading > compassOffset)
+        heading -= compassOffset;
+      else
+        heading += compassOffset;
     }
-    char buffer[18];
+    char buffer[10];
     buffer[0] = SBTSailbotModelHeaderBoatState;
     buffer[1] = state;
     memcpy(&buffer[2], &heading, sizeof(heading));

      }
    case SBTSailbotModelHeaderManualControl: {
        remoteState = SBTSailbotModelStateManualControl;
        
        int remoteRudder;
        memcpy(&remoteRudder, &data[1], sizeof(remoteRudder));
        rudder = (float)remoteRudder;
        
        int remoteSheet;
        memcpy(&remoteSheet, &data[1 + 4], sizeof(remoteSheet));
        sheet = (float)remoteSheet;
        
        tillerServo.write(((rudder / 10.0) + M_PI / 2.0) * 180.0 / M_PI);
        sheetServo.write((sheet / 10.0) * 60 + 90);
        Serial.print("Manual control: "); Serial.println(rudder);

  Wire.begin();
  remoteState = SBTSailbotModelStateManualControl;
  windDirection = -1.0;
  compassOffset = M_PI;
  calibratedWind = false;
  imu = RTIMU::createIMU(&settings);                        // create the imu object

