First version of AP_Compass.cpp