Fix heading reading. When trueHeading is invalid < 0, use magneticHeading