         **/
         case CALIBRATE:
             orientation.calibrate(true);
-            if(calStartTime + 2000 < millis()) {
+            if(calibrateEndTimer()) {
                 orientation.calibrate(false);
                 yawTarget = orientation.getYaw();
                 altitudeCalcInit();

// Flight State variables
boolean altHold = false;
float yawTarget = 0.0;
uint32_t calStartTime;

// Temporary test variables
uint32_t loopstart;

                   RADIO_YAW   = 3, RADIO_GEAR = 4, RADIO_AUX      = 5 };
const uint8_t CHANNEL_TRIGGER_MIN = 32;
const uint8_t CHANNEL_TRIGGER_MAX = 148;

// Time constants in milliseconds
const uint32_t ARMING_TIME = 2000;
const uint32_t CALIBRATING_TIME = 2000;
const uint32_t DISARMING_TIME = 750;

// State timer used to detect ARMING
StateTimer radioDownRight([](){
