This is functionally the same as having a heading hold gyro in hardware.
Code is copied from the simple stabilisation routine and is a bit rough at the minute, but initial testing looks good.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1670 ebee16cc-31ac-478f-84a7-5cbb03baadba