 void setState(State s){
     state = s;
     comms.sendString(stateString[s]);
+    loopstart = micros();
+    loopcount = 0;
 }
 
 void loop() {
+    loopcount++;
     //always running updates
     loopQuad();
     sendTelemetry();

}

void sendTelemetry(){
    static auto timer = Interval::every(50);
    if(timer()){
        float voltage  = float((analogRead(67)/1024.l)*5.l*10.1f);
        float amperage = float((analogRead(66)/1024.l)*5.l*17.0f);

        using namespace Protocol;
        comms.sendTelem(LATITUDE   , state);
        comms.sendTelem(LONGITUDE  , ((float)APMRadio::get(RADIO_THROTTLE)-25)/130.0 );
        comms.sendTelem(HEADING    , toDeg(orientation.getYaw()));
        comms.sendTelem(PITCH      , toDeg(orientation.getPitch()));
        comms.sendTelem(ROLL       , toDeg(orientation.getRoll()));
        comms.sendTelem(GROUNDSPEED, profileTime(0));
        comms.sendTelem(VOLTAGE    , dVdA(voltage,amperage));
        //comms.sendTelem(AMPERAGE   , safe());
        //comms.sendTelem(ALTITUDE   , altitude.get());

        Serial.println();
        Serial.flush();

    throttleOutput = 0.0;
}

void altitudeCalcInit(){
    altitudeEst = baro.getAltitude();
}

void altitudeCalculation(){
    static auto timer = Interval::every(10);
    if(timer()){
        //float dt = (micros() - nextUpdate + UPDATE_MICROS) / 1e6;
        const float dt = 0.01;

        float C0 = AHP.C0;
        float C1 = AHP.C1;

        float barometer = baro.getAltitude();
        float altitude = barometer*C0 + altitudeEst*(1.0-C0);
        float newvelocity = (altitude-altitudeEst) / dt;
        float velocity = newvelocity*C1 + velocityEst*(1.0-C1);

        altitudeEst = altitude;
        velocityEst = velocity;
    }
}

float altHoldUpdate(float throttleCMD){
    static auto timer = Interval::every(10);
    if(timer()){
        //float dt = (micros() - nextUpdate + UPDATE_MICROS) / 1e6;
        const float dt = 0.01;

        float K0 = AHP.K0;
        float K1 = AHP.K1;
        float K2 = AHP.K2;
