Compass: remove the need to call calculate() on the compass object

the new AHRS code doesn't use calculate() and the compass.heading
attribute. Instead it works on the raw magnetometer vector. This
change removes the internal calculate state from the compass object
and instead adds calculate_heading() for use by older code that
doesn't go via AHRS.

This significantly reduces the calculation involved in compass updates

The null offsets enable/disable code is also removed, as it is not
needed now that compass offsets are not linked to the AHRS state.

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;
-}

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
     ///
-	virtual void calculate(const Matrix3f &dcm_matrix);
+    /// @returns heading in radians
+	///
+	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

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);