 
     //Enable or disable manual control
     if (root.containsKey("manualControl")) {
-      manualControl = (boolean) root["manualControl"];
-      if (manualControl) {
-        sendJson("manualControl", true);
+      if (isCalibrated) {
+        manualControl = (boolean) root["manualControl"];
+        sendJson("manualControl", manualControl);
       } else {
-        sendJson("manualControl", false);
+        sendJson("error", "Receiver inputs require calibration");
+      }
+    }
+
+    //Enable or disable calibration mode
+    if (root.containsKey("calibrationMode")) {
+      calibrationMode = (boolean) root["calibrationMode"];
+      sendJson("calibrationMode", calibrationMode);
+      if (calibrationMode) {
+        resetCalibration();
+        isCalibrated = false;
+      } else {
+        if (calibrationComplete()) isCalibrated = true;
+        sendCalibrationJsonData();
       }
     }
 
     //Assigns pin numbers to the motor, servos, and receiver inputs based on the received JSON
     if (root.containsKey("config")) {
-      JsonObject& config = root["config"];
-      if (config.containsKey("aileron")) {
-        int aileronPin = (int) config["aileron"];
-        aileron.attach(aileronPin);
-      }
-      if (config.containsKey("elevator")) {
-        int elevatorPin = (int) config["elevator"];
-        elevator.attach(elevatorPin);
-      }
-      if (config.containsKey("rudder")) {
-        int rudderPin = (int) config["rudder"];
-        rudder.attach(rudderPin);
-      }
-      if (config.containsKey("throttle")) {
-        int throttlePin = (int) config["throttle"];
-        throttle.attach(throttlePin);
-      }
-      if (config.containsKey("aileronIn")) {
-        aileronIn = (int) config["aileronIn"];
-        pinMode(aileronIn, INPUT);
-      }
-      if (config.containsKey("elevatorIn")) {
-        elevatorIn = (int) config["elevatorIn"];
-        pinMode(elevatorIn, INPUT);
-      }
-      if (config.containsKey("rudderIn")) {
-        rudderIn = (int) config["rudderIn"];
-        pinMode(rudderIn, INPUT);
-      }
-      if (config.containsKey("throttleIn")) {
-        throttleIn = (int) config["throttleIn"];
-        pinMode(throttleIn, INPUT);
-      }
-      if (config.containsKey("cutoverIn")) {
-        cutoverIn = (int) config["cutoverIn"];
-        pinMode(cutoverIn, INPUT);
-      }
-      //Configures an interrupt service routine for listening for the cutover signal from the receiver
-      if (config.containsKey("cutoverInterrupt")) {
-        if (cutoverIn != -1) {
-          cutoverInterrupt = (boolean) config["cutoverInterrupt"];
-          attachInterrupt(digitalPinToInterrupt(cutoverIn), listenForCutover, CHANGE);
-        } else {
-          sendJson("error", "Unassigned cutover pin");
-        }
-      }
-    }
-
-    //If autopilot is enabled, set the servo values according to the received JSON
-    if (!manualControl) {
-      if (root.containsKey("aileron")) {
-        if (aileron.attached()) {
-          int aileronValue = root["aileron"];
-          aileron.write(aileronValue);
-          sendJson("aileron", aileronValue);
-        } else {
-          sendJson("error", "Unassigned aileron pin");
-        }
-      }
-
-      if (root.containsKey("elevator")) {
-        if (elevator.attached()) {
-          int elevatorValue = root["elevator"];
-          elevator.write(elevatorValue);
-          sendJson("elevator", elevatorValue);
-        } else {
-          sendJson("error", "Unassigned elevator pin");
-        }
-      }
-     
-      if (root.containsKey("rudder")) {
-        if (rudder.attached()) {
-          int rudderValue = root["rudder"];
-          rudder.write(rudderValue);
-          sendJson("rudder", rudderValue);
-        } else {
-          sendJson("error", "Unassigned rudder pin");
-        }
-      }
-
-      if (root.containsKey("throttle")) {
-        if (throttle.attached()) {
-          int throttleValue = root["throttle"];
-          throttle.write(throttleValue);
-          sendJson("throttle", throttleValue);
-        } else {
-          sendJson("error", "Unassigned throttle pin");
-        }
-      }
-    }
+      if (!manualControl) {
+        JsonObject& config = root["config"];
+        if (config.containsKey("aileron")) {
+          if (aileron.attached()) aileron.detach();
+          byte aileronOutPin = (byte) config["aileron"];
+          aileron.attach(aileronOutPin);
+        }
+        if (config.containsKey("elevator")) {
+          if (elevator.attached()) elevator.detach();
+          byte elevatorOutPin = (byte) config["elevator"];
+          elevator.attach(elevatorOutPin);
+        }
+        if (config.containsKey("rudder")) {
+          if (rudder.attached()) rudder.detach();
+          byte rudderOutPin = (byte) config["rudder"];
+          rudder.attach(rudderOutPin);
+        }
+        if (config.containsKey("throttle")) {
+          if (throttle.attached()) throttle.detach();
+          byte throttleOutPin = (byte) config["throttle"];
+          throttle.attach(throttleOutPin);
+        }
+        if (config.containsKey("aileronIn")) {
+          if (aileronInPin != -1) PCintPort::detachInterrupt(aileronInPin);
+          aileronInPin = (int) config["aileronIn"];
+          PCintPort::attachInterrupt(aileronInPin, listenForAileron, CHANGE);
+        }
+        if (config.containsKey("cutoverIn")) {
+          if (cutoverInPin != -1) PCintPort::detachInterrupt(cutoverInPin);
+          cutoverInPin = (int) config["cutoverIn"];
+          PCintPort::attachInterrupt(cutoverInPin, listenForCutover, CHANGE);
+        }
+        if (config.containsKey("elevatorIn")) {
+          if (elevatorInPin != -1) PCintPort::detachInterrupt(elevatorInPin);
+          elevatorInPin = (int) config["elevatorIn"];
+          PCintPort::attachInterrupt(elevatorInPin, listenForElevator, CHANGE);
+        }
+        if (config.containsKey("rudderIn")) {
+          if (rudderInPin != -1) PCintPort::detachInterrupt(rudderInPin);
+          rudderInPin = (int) config["rudderIn"];
+          PCintPort::attachInterrupt(rudderInPin, listenForRudder, CHANGE);
+        }
+        if (config.containsKey("throttleIn")) {
+          if (throttleInPin != -1) PCintPort::detachInterrupt(throttleInPin);
+          throttleInPin = (int) config["throttleIn"];
+          PCintPort::attachInterrupt(throttleInPin, listenForThrottle, CHANGE);
+        }
+      } else {
+        sendJson("error", "Manual control must be disabled to configure servos");
+      }
+    }
+
+    //Set the servo values according to the received JSON
+    if (root.containsKey("servos")) {
+      if (!manualControl) {
+        JsonObject& servos = root["servos"];
+        if (servos.containsKey("aileron")) {
+          if (aileron.attached()) {
+            byte aileronValue = root["aileron"];
+            aileron.write(aileronValue);
+            sendJson("aileron", aileronValue);
+          } else {
+            sendJson("error", "Unassigned aileron pin");
+          }
+        }
+        if (servos.containsKey("elevator")) {
+          if (elevator.attached()) {
+            byte elevatorValue = root["elevator"];
+            elevator.write(elevatorValue);
+            sendJson("elevator", elevatorValue);
+          } else {
+            sendJson("error", "Unassigned elevator pin");
+          }
+        }
+        if (servos.containsKey("rudder")) {
+          if (rudder.attached()) {
+            byte rudderValue = root["rudder"];
+            rudder.write(rudderValue);
+            sendJson("rudder", rudderValue);
+          } else {
+            sendJson("error", "Unassigned rudder pin");
+          }
+        }
+        if (servos.containsKey("throttle")) {
+          if (throttle.attached()) {
+            byte throttleValue = root["throttle"];
+            throttle.write(throttleValue);
+            sendJson("throttle", throttleValue);
+          } else {
+            sendJson("error", "Unassigned throttle pin");
+          }
+        }
+      } else {
+        sendJson("error", "Manual control must be disabled to use servos");
+      }
+    }
+    //Reset newData to false so we can read new serial input
     newData = false;
   }
 }
 
-//TODO prevent process from hanging if receiver is out of range and no interrupt is in place
-//Listens for cutover input to switch between manual (receiver) and autopilot (phone) mode
-void listenForCutover() {
-  if (cutoverIn != -1) {
-    long pulse = pulseIn(cutoverIn, HIGH, PULSE_TIMEOUT);
-    if (pulse != 0) {
-      if (pulse < CUTOVER_MIN) {
-        manualControl = false;
-        sendJson("manualControl", false);
-      } else if (pulse > CUTOVER_MAX) {
-        manualControl = true;
-        sendJson("manualControl", true);
-      }
-    }
-  }
-}
-
-//Reads receiver input and writes translated values to servos
-void readReceiverInput() {
-  if (!cutoverInterrupt) listenForCutover();  //If there is no cutover interrupt already running, listen for cutover without an interrupt
-  //Only read servo inputs if manual control is enabled
-  if (manualControl) {
-    if (throttle.attached()) {
-      if (throttleIn != -1) {
-        long pulse = pulseIn(throttleIn, HIGH, PULSE_TIMEOUT);
-        if (pulse != 0) {
-          int servoPosition = constrain(map(pulse, RECEIVER_MIN, RECEIVER_MAX, 0, 180), 0, 180);
-          throttle.write(servoPosition);
-          sendJson("rudder", servoPosition);         }
+//Processes input from the receiver
+void processReceiverInput() {
+  //Static variables are used because they maintain their value over multiple function calls
+  static byte servoInputFlagsLocal;
+  static uint16_t aileronInValueLocal;
+  static uint16_t elevatorInValueLocal;
+  static uint16_t rudderInValueLocal;
+  static uint16_t throttleInValueLocal;
+  static uint16_t cutoverInValueLocal;
+
+  //If any servo input byte flags were set in any of the ISR methods, process them here
+  if (servoInputFlags != 0) {
+    noInterrupts();
+    //While interrupts are paused, copy new servo input values to the corresponding local static variables
+    servoInputFlagsLocal = servoInputFlags;
+    if (servoInputFlagsLocal & FLAG_CUTOVER) cutoverInValueLocal = cutoverInValue;
+      if (manualControl || calibrationMode) {
+        if (servoInputFlagsLocal & FLAG_AILERON) aileronInValueLocal = aileronInValue;
+        if (servoInputFlagsLocal & FLAG_ELEVATOR) elevatorInValueLocal = elevatorInValue;
+        if (servoInputFlagsLocal & FLAG_RUDDER) rudderInValueLocal = rudderInValue;
+        if (servoInputFlagsLocal & FLAG_THROTTLE) throttleInValueLocal = throttleInValue;
+      }
+    servoInputFlags = 0;
+    interrupts();
+  }
+  //If the cutover switch is flipped, set manualControl accordingly
+  if (servoInputFlagsLocal & FLAG_CUTOVER) {
+    //Only use cutover values within the set range for receiver input
+    if (isValidReceiverInput(cutoverInValueLocal)) {
+      if (calibrationMode) {
+        if (cutoverInValueLocal > cutoverMax) cutoverMax = cutoverInValueLocal;
+        if (cutoverInValueLocal < cutoverMin) cutoverMin = cutoverInValueLocal;
+      } else if (isCalibrated) {
+        if (manualControl && cutoverInValueLocal > (cutoverMax - CUTOVER_BUFFER)) {
+          manualControl = false;
+          sendJson("manualControl", manualControl);
+        } else if (!manualControl && cutoverInValueLocal < (cutoverMin + CUTOVER_BUFFER)) {
+          manualControl = true;
+          sendJson("manualControl", manualControl);
+        }
       }
     } else {
-      sendJson("error", "Unassigned throttle pin");
-    }
-    
-    if (elevator.attached()) {
-      if (elevatorIn != -1) {
-        long pulse = pulseIn(elevatorIn, HIGH, PULSE_TIMEOUT);
-        if (pulse != 0) {
-          int servoPosition = constrain(map(pulse, RECEIVER_MIN, RECEIVER_MAX, 0, 180), 0, 180);
-          elevator.write(servoPosition);
-          sendJson("rudder", servoPosition);         }
+      sendJson("error", "Cutover input value out of range");
+    }
+  }
+  //Process input for each servo that received new input from the receiver
+  if (servoInputFlagsLocal & FLAG_AILERON) {
+    //Only use aileron values within the set range for receiver input
+    if (isValidReceiverInput(aileronInValueLocal)) {
+      if (calibrationMode) {
+        if (aileronInValueLocal > aileronMax) aileronMax = aileronInValueLocal;
+        if (aileronInValueLocal < aileronMin) aileronMin = aileronInValueLocal;
+      } else if (manualControl) {
+        processServoInput(aileron, aileronInValueLocal, "aileron", "Unassigned aileron pin");
       }
     } else {
-      sendJson("error", "Unassigned elevator pin");
-    }
-    
-    if (aileron.attached()) {
-      if (aileronIn != -1) {
-        long pulse = pulseIn(aileronIn, HIGH, PULSE_TIMEOUT);
-        if (pulse != 0) {
-          int servoPosition = constrain(map(pulse, RECEIVER_MIN, RECEIVER_MAX, 0, 180), 0, 180);
-          aileron.write(servoPosition);
-          sendJson("rudder", servoPosition);         }
+      sendJson("error", "Aileron input value out of range");
+    }
+  }
+  if (servoInputFlagsLocal & FLAG_ELEVATOR) {
+    //Only use elevator values within the set range for receiver input
+    if (isValidReceiverInput(elevatorInValueLocal)) {
+      if (calibrationMode) {
+        if (elevatorInValueLocal > elevatorMax) elevatorMax = elevatorInValueLocal;
+        if (elevatorInValueLocal < elevatorMin) elevatorMin = elevatorInValueLocal;
+      } else if (manualControl) {
+        processServoInput(elevator, elevatorInValueLocal, "elevator", "Unassigned elevator pin");
       }
     } else {
-      sendJson("error", "Unassigned aileron pin");
-    }
-
-    if (rudder.attached()) {
-      if (rudderIn != -1) {
-        long pulse = pulseIn(rudderIn, HIGH, PULSE_TIMEOUT);
-        if (pulse != 0) {
-          int servoPosition = constrain(map(pulse, RECEIVER_MIN, RECEIVER_MAX, 0, 180), 0, 180);
-          rudder.write(servoPosition);
-          sendJson("rudder", servoPosition); 
-        }
+      sendJson("error", "Elevator input value out of range");
+    }
+  }
+  if (servoInputFlagsLocal & FLAG_RUDDER) {
+  //Only use rudder values within the set range for receiver input
+    if (isValidReceiverInput(rudderInValueLocal)) {
+      if (calibrationMode) {
+        if (rudderInValueLocal > rudderMax) rudderMax = rudderInValueLocal;
+        if (rudderInValueLocal < rudderMin) rudderMin = rudderInValueLocal;
+      } else if (manualControl) {
+        processServoInput(rudder, rudderInValueLocal, "rudder", "Unassigned rudder pin");
       }
     } else {
-      sendJson("error", "Unassigned rudder pin");
+      sendJson("error", "Rudder input value out of range");
+    }
+  }
+  if (servoInputFlagsLocal & FLAG_THROTTLE) {
+    //Only use throttle values within the set range for receiver input
+    if (isValidReceiverInput(throttleInValueLocal)) {
+      if (calibrationMode) {
+        if (throttleInValueLocal > throttleMax) throttleMax = throttleInValueLocal;
+        if (throttleInValueLocal < throttleMin) throttleMin = throttleInValueLocal;
+      } else if (manualControl) {
+        processServoInput(throttle, throttleInValueLocal, "throttle", "Unassigned throttle pin");
+      }
+    } else {
+      sendJson("error", "Throttle input value out of range");
     }
   }
 }
 
 //Takes a key (text) value (boolean) pair and sends them as JSON over the serial interface
 void sendJson(const char key[], boolean value) {
-  StaticJsonBuffer<numChars> jsonBuffer;
+  StaticJsonBuffer<CHAR_ARRAY_SIZE> jsonBuffer;
   JsonObject& root = jsonBuffer.createObject();
   root[key] = value;
   Serial.print(startMarker);

      if (charIn != endMarker) {
        receivedJson[index] = charIn;
        index++;
        if (index >= numChars) {
          index = numChars - 1;
          sendJson("error", "Serial buffer overflow");
        }
      }

}

//Takes a key (text) value (int) pair and sends them as JSON over the serial interface
void sendJson(const char key[], byte value) {
  StaticJsonBuffer<CHAR_ARRAY_SIZE> jsonBuffer;
  JsonObject& root = jsonBuffer.createObject();
  root[key] = value;
  Serial.print(startMarker);
  root.printTo(Serial);
  Serial.println(endMarker);
}

//Sends all calibrated receiver input max and min values as JSON over the serial interface
void sendCalibrationJsonData() {
  StaticJsonBuffer<CHAR_ARRAY_SIZE> jsonBuffer;
  JsonObject& root = jsonBuffer.createObject();
  JsonObject &calibrationData = jsonBuffer.createObject();;
  calibrationData["aileronMin"] = aileronMin;
  calibrationData["aileronMax"] = aileronMax;
  calibrationData["elevatorMin"] = elevatorMin;
  calibrationData["elevatorMax"] = elevatorMax;
  calibrationData["rudderMin"] = rudderMin;
  calibrationData["rudderMax"] = rudderMax;
  calibrationData["throttleMin"] = throttleMin;
  calibrationData["throttleMax"] = throttleMax;
  calibrationData["cutoverMin"] = cutoverMin;
  calibrationData["cutoverMax"] = cutoverMax;
  root["calibrationData"] = calibrationData;
  Serial.print(startMarker);
  root.printTo(Serial);
  Serial.println(endMarker);
}

//Checks if the receiver inputs have been calibrated
//TODO use better verification method
boolean calibrationComplete() {
  if (aileronMax == RECEIVER_INPUT_DEFAULT) return false;
  if (aileronMin == RECEIVER_INPUT_DEFAULT) return false;
  if (elevatorMax == RECEIVER_INPUT_DEFAULT) return false;
  if (elevatorMin == RECEIVER_INPUT_DEFAULT) return false;
  if (rudderMax == RECEIVER_INPUT_DEFAULT) return false;
  if (rudderMin == RECEIVER_INPUT_DEFAULT) return false;
  if (throttleMax == RECEIVER_INPUT_DEFAULT) return false;
  if (throttleMin == RECEIVER_INPUT_DEFAULT) return false;
  if (cutoverMax == RECEIVER_INPUT_DEFAULT) return false;
  if (cutoverMin == RECEIVER_INPUT_DEFAULT) return false;
  return true;
}

//Reset all calibrated receiver input values to the default
void resetCalibration() {
  aileronMax = RECEIVER_INPUT_DEFAULT;
  aileronMin = RECEIVER_INPUT_DEFAULT;
  elevatorMax = RECEIVER_INPUT_DEFAULT;
  elevatorMin = RECEIVER_INPUT_DEFAULT;
  rudderMax = RECEIVER_INPUT_DEFAULT;
  rudderMin = RECEIVER_INPUT_DEFAULT;
  throttleMax = RECEIVER_INPUT_DEFAULT;
  throttleMin = RECEIVER_INPUT_DEFAULT;
  cutoverMax = RECEIVER_INPUT_DEFAULT;
  cutoverMin = RECEIVER_INPUT_DEFAULT;
}

//Interrupt Service Routine (ISR) for aileron receiver input
void listenForAileron() {
  static uint32_t aileronInStart;
  if (PCintPort::pinState) {
    aileronInStart = micros();
  } else {
    aileronInValue = (uint16_t) (micros() - aileronInStart);
    servoInputFlags |= FLAG_AILERON;
  }
}

//Interrupt Service Routine (ISR) for cutover input
void listenForCutover() {
  static uint32_t cutoverInStart;
  if (PCintPort::pinState) {
    cutoverInStart = micros();
  } else {
    cutoverInValue = (uint16_t) (micros() - cutoverInStart);
    servoInputFlags |= FLAG_CUTOVER;
  }
}

//Interrupt Service Routine (ISR) for elevator receiver input
void listenForElevator() {
  static uint32_t elevatorInStart;
  if (PCintPort::pinState) {
    elevatorInStart = micros();
  } else {
    elevatorInValue = (uint16_t) (micros() - elevatorInStart);
    servoInputFlags |= FLAG_ELEVATOR;
  }
}

//Interrupt Service Routine (ISR) for rudder receiver input
void listenForRudder() {
  static uint32_t rudderInStart;
  if (PCintPort::pinState) {
    rudderInStart = micros();
  } else {
    rudderInValue = (uint16_t) (micros() - rudderInStart);
    servoInputFlags |= FLAG_RUDDER;
  }
}

//Interrupt Service Routine (ISR) for throttle receiver input
void listenForThrottle() {
  static uint32_t throttleInStart;
  if (PCintPort::pinState) {
    throttleInStart = micros();
  } else {
    throttleInValue = (uint16_t) (micros() - throttleInStart);
    servoInputFlags |= FLAG_THROTTLE;
  }
}

//Determines whether a given value is valid receiver input
boolean isValidReceiverInput(uint16_t receiverInput) {
  if (receiverInput < RECEIVER_INPUT_MAX && receiverInput > RECEIVER_INPUT_MIN) return true;
  return false;
}

//Processes receiver input and writes the converted value out to the servo
void processServoInput(Servo servo, int receiverInput, const char servoType[], const char servoError[]) {
  if (servo.attached()) {
    //The servoInValue (in microseconds) is mapped to the min and max of the Servo.write() function
    byte convertedServoValue = map(receiverInput, getReceiverInputMin(servoType), getReceiverInputMax(servoType), SERVO_MIN, SERVO_MAX);
    servo.write(convertedServoValue);
    sendJson(servoType, convertedServoValue);
  } else {
    sendJson("error", servoError);
  }
}

//Takes a servoType and returns the corresponding calibrated receiver input max value
int getReceiverInputMax(const char servoType[]) {
  if (strcmp(servoType, "aileron") == 0) return aileronMax;
  if (strcmp(servoType, "elevator") == 0) return elevatorMax;
  if (strcmp(servoType, "rudder") == 0) return rudderMax;
  if (strcmp(servoType, "throttle") == 0) return throttleMax;
}

//Takes a servoType and returns the corresponding calibrated receiver input min value
int getReceiverInputMin(const char servoType[]) {
  if (strcmp(servoType, "aileron") == 0) return aileronMin;
  if (strcmp(servoType, "elevator") == 0) return elevatorMin;
  if (strcmp(servoType, "rudder") == 0) return rudderMin;
  if (strcmp(servoType, "throttle") == 0) return throttleMin;
}
