     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 C0 = AHP.C0;
-        float C1 = AHP.C1;
         float K0 = AHP.K0;
         float K1 = AHP.K1;
         float K2 = AHP.K2;

        yawTarget = truncateRadian(yawTarget);
    }

    // calculate target throttle
    float throttleOut = (altHold)? altHoldUpdate(throttle) :
                                   throttleCurve.get(throttle);

    // set output targets
    comms.sendTelem(AMPERAGE, throttleOut);
    horizon.set(pitchCmd, rollCmd, yawTarget, throttleOut);
}


#include "platforms/Quadcopter.h"

boolean calibrated = false;
boolean altHold = false;
float outputThrottle;
uint32_t calStartTime;
const uint8_t CHANNEL_MIN = 32;
const uint8_t CHANNEL_MAX = 148;
