     mx_pavg = mx_avg; my_pavg = my_avg; mz_pavg = mz_avg;
     delay(5);
   }while(!stable);
-
+  
+  compass.update(int(mx_avg), int(my_avg), int(mz_avg), int(ax_avg), int(ay_avg), int(az_avg));
+  compass.init(compass.yaw);
   accel.setRestingPosition(0.0, 0.0, 1.0);
-  //accel.setRestingPosition(233.5522003, 255.8198547, 8202.2744141);
+  //accel.setRestingPosition(int(ax_avg), int(ay_avg), int(az_avg)); // only use if craft is perfectly level
   accel.update(int(ax_avg), int(ay_avg), int(az_avg));
-  gyro.init(gx_avg, gy_avg, gz_avg, accel.x, accel.y, accel.z); // gyro offset xyz, accel starting rotation xyz
+  gyro.init(gx_avg, gy_avg, gz_avg, accel.x, accel.y, 0.0); // gyro offset xyz, accel starting rotation xyz
   #ifdef DEBUG
-    Serial.printf("\tresting accel vector %0.3f %0.3f %0.3f \n", ax_avg, ay_avg, az_avg);
-    Serial.printf("\tcurrent accel angle %0.3f %0.3f %0.3f \n", accel.x, accel.y, accel.z);
-    Serial.printf("\tgyro offset %0.3f %0.3f %0.3f \n", gx_avg, gy_avg, gz_avg);
+    Serial.printf("accel resting vector %0.3f %0.3f %0.3f\n", ax_avg, ay_avg, az_avg);
+    Serial.printf("accel current angle %0.3f %0.3f %0.3f\n", accel.x, accel.y, accel.z);
+    Serial.printf("gyro rate offset %0.3f %0.3f %0.3f\n", gx_avg, gy_avg, gz_avg);
+    Serial.printf("compass resting heading %0.3f\n", compass.yaw);
   #endif
-}
-
-void calibrateESC(){
+  print("IMU Calibration Complete\n");
+}
+
+void compass_calibrate(){
+  print("Compass Calibration...\n");
+  print("move sensor around in all orientations, monitor the min/max values, run until satisfied\n");
+  print("use results to set compass.config(Xoffset,Yoffset,Zoffset,Xscale,Yscale,Zscale);");
+  delay(2000);
+  float Xoffset, Yoffset, Zoffset;
+  float Xscale, Yscale, Zscale;
+  float longCord;
+  int16_t Xmax = -32760; 
+  int16_t Xmin = 32760;
+  int16_t Ymax = -32760; 
+  int16_t Ymin = 32760;
+  int16_t Zmax = -32760; 
+  int16_t Zmin = 32760;
+  while(true){
+    while(!imu.getMagReading( &mx, &my, &mz)){}
+    Xmax = mx > Xmax ? mx : Xmax;
+    Xmin = mx < Xmin ? mx : Xmin;
+    Ymax = my > Ymax ? my : Ymax;
+    Ymin = my < Ymin ? my : Ymin;
+    Zmax = mz > Zmax ? mz : Zmax;
+    Zmin = mz < Zmin ? mz : Zmin;
+    
+    Xoffset = (Xmax + Xmin)/2.0;
+    Yoffset = (Ymax + Ymin)/2.0;
+    Zoffset = (Zmax + Zmin)/2.0;
+
+    // find scale from largest chord length
+    longCord = float(max( max(Xmax - Xmin, Ymax - Ymin), Zmax - Zmin));
+    Xscale = longCord/(Xmax - Xmin);
+    Yscale = longCord/(Ymax - Ymin);
+    Zscale = longCord/(Zmax - Zmin);
+
+    #ifdef DEBUG
+        Serial.printf("X min %i max %i = offset %0.6f scale %0.6f\n", Xmin, Xmax, Xoffset, Xscale);
+        Serial.printf("Y min %i max %i = offset %0.6f scale %0.6f\n", Ymin, Ymax, Yoffset, Yscale);
+        Serial.printf("Z min %i max %i = offset %0.6f scale %0.6f\n", Zmin, Zmax, Zoffset, Zscale);
+    #endif
+  }
+  // use results to set compass.config(Xoffset,Yoffset,Zoffset,Xscale,Yscale,Zscale);
+  /*
+  // alternatively use scale from factory settings, see data sheet
+  int8_t rom_adjX, rom_adjY, rom_adjZ;
+  imu.getMagAdjustment(&rom_adjX, &rom_adjY, &rom_adjZ);
+  float Xscale = (((int(rom_adjX)-128)*0.5)/128.0)+1;
+  float Yscale = (((int(rom_adjY)-128)*0.5)/128.0)+1;
+  float Zscale = (((int(rom_adjZ)-128)*0.5)/128.0)+1;
+  */  
+}
+
+void esc_calibrate() {
     esc.update(motorPin_FL, throttleMax);
     esc.update(motorPin_FR, throttleMax);
     esc.update(motorPin_BR, throttleMax);

    //                  bool invX, invY, invZ)
    gyro.config(2000, 1000000, 0.01, false, true, true);
    accel.config(false, true, false);//args are for inverting axis

    // IMU initialize
    print("IMU Setup...");
    imu.initialize();

    radio.startRX();
    print("RX Setup Complete\n");

    pid_config();

    //GyroAngle.config( ushort FullScaleRangeDPS,
    //                  ulong deltaTime_UnitsPerSecond,
