Finish a working version.
TODO: PI -> PID
TODO: Altitude control.
TODO: Roslaunch file.