Added a few items to the control law for pitch control in waypoint nav
mode.
Merged navigation code with local commits