     lastShortTime = micros();
     lastShortTimeMS = millis();
     imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-    bool updatedMag = imu.getMagReading( &mx, &my, &mz);
+    bool freshMag = imu.getMagReading( &mx, &my, &mz);
     gyro.update(gx, gy, gz, elapsed_si);
-
-
     accel.update(ax, ay, az);
-
     gyro.correctXY(accel.x, accel.y);
-
-    if(updatedMag){ 
-        compass.update(mx, my, mz, ax, ay, az);
-
-        //gyro.correctZ(compass.yaw); 
-        gyro.correctZ(0.0);
-    }    
-
-
+    if(freshMag){
+        compass.update(mx, my, mz, ax, ay, az); 
+    }
+    gyro.correctZ(compass.yaw); 
     
     if (ctrl.live) {
         pid_update();
         motor_update();
     }else{
         motor_off();
+        // zero out yaw
+        gyro.zeroZ();
+        compass.zero();
     }
   }
 

void pid_update() {
  // update PID & scale to power
  //print("PID_R "); print(lastShortTimeMS); print(" "); print(ctrl.rollf); print(" ");
  switch (ctrl.navRotModeRoll) {
    case ctrl.FLY_STAB:
      op_roll = long(PID_roll.compute( ctrl.rollf, gyro.posX));
      //print(gyro.posX); print(" ");
      break;
    case ctrl.FLY_ACRO:
      op_roll = long(PID_roll.compute( ctrl.rollf, gyro.rateX));
      //print(gyro.rateX); print(" ");
      break;
  }
  //print(PID_roll.getError()); print(" "); print(ctrl.Kp_roll); print(" "); print(ctrl.Kp_roll * PID_roll.getError()); print(" ")
  //print(PID_roll.getIntegral()); print(" "); print(ctrl.Ki_roll); print(" "); print(ctrl.Ki_roll * PID_roll.getIntegral()); print(" ")
  //print(PID_roll.getDerivative()); print(" "); print(ctrl.Kd_roll); print(" "); print(ctrl.Kd_roll * PID_roll.getDerivative()); print(" ")
  //print(op_roll); print("\n");

  //print("PID_P "); print(lastShortTimeMS); print(" "); print(ctrl.pitchf); print(" ");
  switch (ctrl.navRotModePitch) {
    case ctrl.FLY_STAB:
      op_pitch = long(PID_pitch.compute( ctrl.pitchf, gyro.posY));
      //print(gyro.posY); print(" ");
      break;
    case ctrl.FLY_ACRO:
      op_pitch = long(PID_pitch.compute( ctrl.pitchf, gyro.rateY));
      //print(gyro.rateY); print(" ");
      break;
  }
  //print(PID_pitch.getError()); print(" "); print(ctrl.Kp_pitch); print(" "); print(ctrl.Kp_pitch * PID_pitch.getError()); print(" ")
  //print(PID_pitch.getIntegral()); print(" "); print(ctrl.Ki_pitch); print(" "); print(ctrl.Ki_pitch * PID_pitch.getIntegral()); print(" ")
  //print(PID_pitch.getDerivative()); print(" "); print(ctrl.Kd_pitch); print(" "); print(ctrl.Kd_pitch * PID_pitch.getDerivative()); print(" ")
  //print(op_pitch); print("\n");

  //print("PID_Y "); print(lastShortTimeMS); print(" "); print(ctrl.yawf); print(" ");
  switch (ctrl.navRotModeYaw) {
    case ctrl.FLY_STAB: // use angle position for "stable" mode
      op_yaw = long(PID_yaw.compute( ctrl.yawf, gyro.posZ));
      //print(gyro.posZ); print(" ");
      break;
    case ctrl.FLY_ACRO: // use angle rate for "acrobatic" mode
      op_yaw = long(PID_yaw.compute( ctrl.yawf, gyro.rateZ));
      //print(gyro.rateZ); print(" ");
      break;
  }
  //print(PID_yaw.getError()); print(" "); print(ctrl.Kp_yaw); print(" "); print(ctrl.Kp_yaw * PID_yaw.getError()); print(" ")
  //print(PID_yaw.getIntegral()); print(" "); print(ctrl.Ki_yaw); print(" "); print(ctrl.Ki_yaw * PID_yaw.getIntegral()); print(" ")
  //print(PID_yaw.getDerivative()); print(" "); print(ctrl.Kd_yaw); print(" "); print(ctrl.Kd_yaw * PID_yaw.getDerivative()); print(" ")

        //print("Kill\n");
        return true;
      case ctrl.NAV_CODE: // navigation control packet
        print("Nav "); print(lastMidTime); print(" "); print(ctrl.rollf); print(" "); print(ctrl.pitchf); print(" "); print(ctrl.yawf); print(" "); print(ctrl.throttlef); print("\n");
        return true;
      case ctrl.PID_YAW_CODE: // PID Yaw settings
        print("Pid_Yaw "); print(ctrl.Kp_yaw); print(" "); print(ctrl.Ki_yaw); print(" "); print(ctrl.Kd_yaw); print("\n");
        pid_config();
        return true;
      case ctrl.PID_PITCH_CODE: // PID Pitch settings
        print("Pid_Pitch "); print(ctrl.Kp_pitch); print(" "); print(ctrl.Ki_pitch); print(" "); print(ctrl.Kd_pitch); print("\n");
        pid_config();
        return true;
      case ctrl.PID_ROLL_CODE: // PID Roll settings
        print("Pid_Roll "); print(ctrl.Kp_roll); print(" "); print(ctrl.Ki_roll); print(" "); print(ctrl.Kd_roll); print("\n");
        pid_config();
        return true;
      default: // unknown control code
