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

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


float heading;
float rudder;
float sheet;
float compassOffset;

Servo tillerServo;
Servo sheetServo;
