             if(calStartTime + 2000 < millis()) {
                 orientation.calibrate(false);
                 yawTarget = orientation.getYaw();
+                altitudeCalcInit();
                 output.standby();
                 setState(FLYING);
             }

//

void altHoldInit(float curThrottle){
    altitudeEst = baro.getAltitude();
    altitudeSetpoint = altitudeEst;
    hover = curThrottle;
    oldIntegral = 0.0;

        //adjust setpoint
        float th = (throttleCMD-0.5);
        if(fabs(th) > 0.1) altitudeSetpoint += th/4.0;

        float error = altitudeSetpoint-altitudeEst;
        float newIntegral = oldIntegral + error * dt;
        throttleOutput = (K0/10.0)*(error + K1*velocityEst + K2*newIntegral) + hover;

        oldIntegral = newIntegral;
    }

    return throttleOutput;
