Added some flight calculation

Still working on formula to decide motor power adjustments. No camera
control yet.