   return false;
 }
 
-void updateMotor() {
-  // 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(op_yaw); print("\n");
-
-  op_throttle = ctrl.throttle;
-  
-  //    roll
-  // fl-----fr
-  // |  yaw  | pitch
-  // bl-----br
-  power_FL = op_throttle + op_pitch + op_roll - op_yaw;
-  power_FR = op_throttle + op_pitch - op_roll + op_yaw;
-  power_BR = op_throttle - op_pitch - op_roll - op_yaw;
-  power_BL = op_throttle - op_pitch + op_roll + op_yaw;
-  /*
-  power_FL = tt.interp(power_FL, FL_tt_x, FL_tt_y, FLTTLENGTH);
-  power_FR = tt.interp(power_FR, FR_tt_x, FR_tt_y, FRTTLENGTH);
-  power_BR = tt.interp(power_BR, BR_tt_x, BR_tt_y, BRTTLENGTH);
-  power_BL = tt.interp(power_BL, BL_tt_x, BL_tt_y, BLTTLENGTH);
-  */
-    
-  //print("MP "); print(lastShortTimeMS); print(" "); print(power_FL); print(" "); print(power_FR); print(" "); print(power_BR); print(" "); print(power_BL); print("\n");
-    
-  if (ctrl.live) {
-    esc.update(motorPin_FL, power_FL);
-    esc.update(motorPin_FR, power_FR);
-    esc.update(motorPin_BR, power_BR);
-    esc.update(motorPin_BL, power_BL);
-  } else {
-    esc.update(motorPin_FL, 0);
-    esc.update(motorPin_FR, 0);
-    esc.update(motorPin_BR, 0);
-    esc.update(motorPin_BL, 0);
-  }
-
-}
-
-void checkConnection() {
+void rx_watchdog() {
   // checked every 500ms
   if (validPacketCount < 10) { // should be ~22 per 500ms
     // set nav control to hover

        while (true) {}
    }
    print("IMU Setup Complete\n");

    print("Gyro Calibration...");
    calibrateIMU();
    print("Gyro Calibration Complete\n");

    // clear log
    //print("Clearing Log...");
    //logger.clear();

    while(ctrl.control_code != ctrl.NAV_CODE){
        if (millis() - lastMidTime > midPeriod) {
            lastMidTime = millis();
            rx_check();
        }
    }
    print("Connected\n");
