 
     // update yaw target
     if(fabs(yawCmd) > 0.1){
-        yawTarget += yawCmd/160.0;
+        static uint32_t integrationTimer = micros();
+        float dt = (micros() - integrationTimer)/1e6;
+        yawTarget += yawCmd*dt;
         yawTarget = truncateRadian(yawTarget);
     }
 
-    altitudeCalculation();
     // calculate target throttle
     float throttleOut = (altHold)? altHoldUpdate(throttle) :
                                    throttleCurve.get(throttle);

    const float yawCmd   = ((float)APMRadio::get(RADIO_YAW)-90)   / 90.0;
    const float throttle = ((float)APMRadio::get(RADIO_THROTTLE)-25)/130.0;
    const bool  gearCmd  = (APMRadio::get(RADIO_GEAR) > 90);

    // check for low throttle standby mode
    if(APMRadio::get(RADIO_THROTTLE) <= CHANNEL_MIN){

    }

    return throttleOutput;
}

void sendTelemetry(){
