Here's how simple it can be using all 3 P,I,D:

loop

error = setpoint - measured_value

integral = integral + error*dt

derivative = (error - previous_error)/dt

output = Kp*error + Ki*integral + Kd*derivative

delay(dt)

While experimenting you can assign dt,gain to the value of R/C channel 5,6.

Let's start by controlling altitude only using the throttle channel, P term only:

void loop()

ft=get_altitude();

set_throttle((wasft-ft)*gain+wasthrottle); //gain is Kp

wasft=ft;

delay(dt);

A few more lines gives us I term:

const int NZEROSLOW=3;

const int NZEROSTOP=15;

ft=get_altitude();

chg=wasft-ft;

if last is in correct direction toward setpoint and j-lastj > NZEROSLOW then chg=0;

set_throttle(chg*gain+wasthrottle);

if chg then

last=chg;

lastj=j';

j++;

delay(dt);

It's not perfect, oversimplified a bit to make it easier to understand.

Another improvement in case it's moving too slow, not near the setpoint:

if ft-setpoint > 10ft and j-lastj > NZEROSTOP then chg= toward setpoint;

You can use the same method for pitch, roll, yaw using magnetic compass and GPS to correct towards setpoint.

Simple? Questions? Improvement? It works!

PS- Oh great library BTW! It's not needed for my solution here.