 void setup() {
     setupQuad();
     horizon.standby();
+    setState(DISARMED);
+}
+
+void setState(State s){
+    state = s;
+    comms.sendString(stateString[s]);
 }
 
 void loop() {


    //flight mode state machine
    switch(state){
        case DISARMED:
            if(timeState<&radio_downRight>() > 5000) state = ARM;
            if(timeState<&radio_downLeftUpRight>() > 5000) state = CALIBRATE_MOTORS;
            break;
        case ARM:
            arm();
            state = FLYING;
            break;
        case CALIBRATE_MOTORS:
            calibrateESCs();
            state = FLYING;
            break;
        case OFF:
            output.disable();
            if(timeState<&radio_downRight>() > 5000) state = ENABLE;
            break;
        case ENABLE:
            if(calibrated) {
                output.enable();
                state = FLYING;
            } else {
                calStartTime = millis();
                state = CALIBRATE;
            }
            yawTarget = orientation.getYaw();
            break;
        case CALIBRATE:
            orientation.calibrate(true);
            if(calStartTime + 2000 < millis()) {
                output.enable();
                state = FLYING;
                orientation.calibrate(false);
                calibrated = true;
            }
            break;
        case FLYING:
            fly();
            if(timeState<&radio_downLeft>() > 750) state = OFF;
            break;
    }


const uint8_t CHANNEL_MIN = 32;
const uint8_t CHANNEL_MAX = 148;
float yawTarget = 0.0;
enum State {
    DISARMED,
    CALIBRATE,
    FLYING
};
const char* stateString[] = {
    [DISARMED] = "DISARMED", [CALIBRATE] = "CALIBRATE", [FLYING] = "FLYING" };
State state;

enum RadioChannel{
    RADIO_PITCH    = 0,
    RADIO_ROLL     = 1,
