diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index ddbcf93f81..7a96b6cf76 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -22,7 +22,6 @@ Compass::Compass(void) :
     _learn(1),
     _use_for_yaw(1),
     _auto_declination(1),
-    _null_enable(false),
     _null_init_done(false)
 {
 }
@@ -66,9 +65,7 @@ Compass::set_initial_location(long latitude, long longitude)
     // the declination based on the initial GPS fix
 	if (_auto_declination) {
 		// Set the declination based on the lat/lng from GPS
-		null_offsets_disable();
 		_declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
-		null_offsets_enable();
 	}
 }
 
@@ -85,8 +82,8 @@ Compass::get_declination()
 }
 
 
-void
-Compass::calculate(float roll, float pitch)
+float
+Compass::calculate_heading(float roll, float pitch)
 {
 //  Note - This function implementation is deprecated
 //  The alternate implementation of this function using the dcm matrix is preferred
@@ -96,6 +93,8 @@ Compass::calculate(float roll, float pitch)
     float sin_roll;
     float cos_pitch;
     float sin_pitch;
+    float heading;
+
     cos_roll = cos(roll);
 	sin_roll = sin(roll);
     cos_pitch = cos(pitch);
@@ -118,18 +117,18 @@ Compass::calculate(float roll, float pitch)
             heading += (2.0 * M_PI);
     }
 
-    // Optimization for external DCM use. Calculate normalized components
-    heading_x = cos(heading);
-    heading_y = sin(heading);
+    return heading;
 }
 
 
-void
-Compass::calculate(const Matrix3f &dcm_matrix)
+float
+Compass::calculate_heading(const Matrix3f &dcm_matrix)
 {
     float headX;
     float headY;
     float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
+    float heading;
+
 	// sin(pitch) = - dcm_matrix(3,1)
 	// cos(pitch)*sin(roll) = - dcm_matrix(3,2)
 	// cos(pitch)*cos(roll) = - dcm_matrix(3,3)
@@ -138,7 +137,7 @@ Compass::calculate(const Matrix3f &dcm_matrix)
         // we are pointing straight up or down so don't update our
         // heading using the compass. Wait for the next iteration when
         // we hopefully will have valid values again.
-        return;
+        return 0;
     }
 
     // Tilt compensated magnetic field X component:
@@ -159,23 +158,7 @@ Compass::calculate(const Matrix3f &dcm_matrix)
             heading += (2.0 * M_PI);
     }
 
-    // Optimization for external DCM use. Calculate normalized components
-    heading_x = cos(heading);
-    heading_y = sin(heading);
-
-#if 0
-    if (isnan(heading_x) || isnan(heading_y)) {
-        Serial.printf("COMPASS: c.x %f c.y %f c.z %f cos_pitch %f mag_x %d mag_y %d mag_z %d headX %f headY %f heading %f heading_x %f heading_y %f\n",
-                      dcm_matrix.c.x,
-                      dcm_matrix.c.y,
-                      dcm_matrix.c.x,
-                      cos_pitch,
-                      (int)mag_x, (int)mag_y, (int)mag_z,
-                      headX, headY,
-                      heading,
-                      heading_x, heading_y);
-    }
-#endif
+    return heading;
 }
 
 
@@ -202,7 +185,7 @@ Compass::calculate(const Matrix3f &dcm_matrix)
 void
 Compass::null_offsets(void)
 {
-    if (_null_enable == false || _learn == 0) {
+    if (_learn == 0) {
         // auto-calibration is disabled
         return;
     }
@@ -274,18 +257,3 @@ Compass::null_offsets(void)
     // set the new offsets
     _offset.set(_offset.get() - diff);
 }
-
-
-void
-Compass::null_offsets_enable(void)
-{
-	_null_init_done = false;
-	_null_enable = true;
-}
-
-void
-Compass::null_offsets_disable(void)
-{
-	_null_init_done = false;
-	_null_enable = false;
-}
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 8adc3ede13..f02f78c833 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -20,9 +20,6 @@ public:
 	int16_t         mag_x;          ///< magnetic field strength along the X axis
 	int16_t         mag_y;          ///< magnetic field strength along the Y axis
 	int16_t         mag_z;          ///< magnetic field strength along the Z axis
-	float           heading;        ///< compass heading in radians
-	float           heading_x;      ///< compass vector X magnitude
-	float           heading_y;      ///< compass vector Y magnitude
 	uint32_t        last_update;    ///< micros() time of last update
 	bool			healthy;        ///< true if last read OK
 
@@ -46,13 +43,17 @@ public:
 	/// @param  roll                The current airframe roll angle.
 	/// @param  pitch               The current airframe pitch angle.
 	///
-	virtual void calculate(float roll, float pitch);
+    /// @returns heading in radians
+    ///
+	virtual float calculate_heading(float roll, float pitch);
 
 	/// Calculate the tilt-compensated heading_ variables.
 	///
 	/// @param dcm_matrix			The current orientation rotation matrix
+    ///
+    /// @returns heading in radians
 	///
-	virtual void calculate(const Matrix3f &dcm_matrix);
+	virtual float calculate_heading(const Matrix3f &dcm_matrix);
 
 	/// Set the compass orientation matrix, used to correct for
 	/// various compass mounting positions.
@@ -100,16 +101,6 @@ public:
 	///
 	void null_offsets(void);
 
-
-	/// Enable/Start automatic offset updates 
-	///
-	void null_offsets_enable(void);
-
-
-	/// Disable/Stop automatic offset updates
-	///
-	void null_offsets_disable(void);
-
     /// return true if the compass should be used for yaw calculations
     bool use_for_yaw(void) { return healthy && _use_for_yaw; }
 
@@ -130,7 +121,6 @@ protected:
     AP_Int8             _use_for_yaw;           ///<enable use for yaw calculation
     AP_Int8             _auto_declination;      ///<enable automatic declination code
 
-	bool                _null_enable;        	///< enabled flag for offset nulling
 	bool                _null_init_done;        ///< first-time-around flag used by offset nulling
 
     ///< used by offset correction
diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
index fae4dd6124..35890bcc53 100644
--- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
+++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
@@ -66,12 +66,13 @@ void loop()
     timer = micros();
     compass.read();
     unsigned long read_time = micros() - timer;
+    float heading;
 
     if (!compass.healthy) {
 	    Serial.println("not healthy");
 	    return;
     }
-    compass.calculate(0,0);  // roll = 0, pitch = 0 for this example
+    heading = compass.calculate_heading(0,0);  // roll = 0, pitch = 0 for this example
 
     // capture min
     if( compass.mag_x < min[0] )
@@ -96,7 +97,7 @@ void loop()
 
     // display all to user
     Serial.printf("Heading: %.2f (%3u,%3u,%3u) ",
-		  ToDeg(compass.heading),
+		  ToDeg(heading),
 		  compass.mag_x,
 		  compass.mag_y,
 		  compass.mag_z);