Port PID control from Lua script.

- The quadcopter is still oscillating quite badly, eventually
  leading to a crash.  I've double checked the code and I haven't
  found any deviations from the original Lua source.  Perhaps there
  is a numerical or timing issue?