Headhold without Magnetometer

Minor changes to enable Headhold with Gyro only.

Heading drift over time, but not too bad.