PID update for hdg hold problem
Other misc FC cleanups