         //switch to manual mode
         altHold = false;
     } else if (altHold == false && gearCmd == true){
-        altitudeSetpoint = altitude.get();
-        altPID.train(throttleCurve.get(throttle));
-        velocity.set(0);
-        altPID.set(0);
+        altHoldInit(throttleOut);
         altHold = true;
+    }
+
+    if(altHold){
+        throttleOut = altHoldUpdate(throttle);
     }
 
     if(fabs(yawCmd) > 0.1){

    const float yawCmd   = ((float)getAPM2Radio(RADIO_YAW)-90)   / 90.0;
    const float throttle = ((float)getAPM2Radio(RADIO_THROTTLE)-25)/130.0;
    const bool  gearCmd  = (getAPM2Radio(4) > 90);

    if(getAPM2Radio(RADIO_THROTTLE) <= CHANNEL_MIN){
        horizon.standby();

const float GsToFeet = -32.17;
boolean calibrated = false;
boolean altHold    = false;
PIDcontroller altPID(&altHoldParams);
uint32_t calStartTime;
const uint8_t CHANNEL_MIN = 32;
