Allow compass to work correctly

Ported from Peter Hofinger's work in the AICP project

http://gerrit.aicp-rom.com/#/c/1837
http://gerrit.aicp-rom.com/#/c/1838

Change-Id: I23bac4b937c1f4f52d34c27f88afd7cdb92fe9f4