AP_Compass: use const on more functions and remove old calculate_heading()

the calculate_heading() based on roll/pitch is not needed anywhere

libraries/AP_Compass/Compass.cpp

@@ -168,54 +168,16 @@ Compass::set_declination(float radians, bool save_to_eeprom)
 }
 
 float
-Compass::get_declination()
+Compass::get_declination() const
 {
     return _declination.get();
 }
 
 
 float
-Compass::calculate_heading(float roll, float pitch)
+Compass::calculate_heading(const Matrix3f &dcm_matrix) const
 {
-//  Note - This function implementation is deprecated
-//  The alternate implementation of this function using the dcm matrix is preferred
-    float headX;
-    float headY;
-    float cos_roll;
-    float sin_roll;
-    float cos_pitch;
-    float sin_pitch;
-    float heading;
-
-    cos_roll = cosf(roll);
-    sin_roll = sinf(roll);
-    cos_pitch = cosf(pitch);
-    sin_pitch = sinf(pitch);
-
-    // Tilt compensated magnetic field X component:
-    headX = mag_x*cos_pitch + mag_y*sin_roll*sin_pitch + mag_z*cos_roll*sin_pitch;
     // Tilt compensated magnetic field Y component:
-    headY = mag_y*cos_roll - mag_z*sin_roll;
-    // magnetic heading
-    heading = atan2f(-headY,headX);
-
-    // Declination correction (if supplied)
-    if( fabsf(_declination) > 0.0f )
-    {
-        heading = heading + _declination;
-        if (heading > PI)    // Angle normalization (-180 deg, 180 deg)
-            heading -= (2.0f * PI);
-        else if (heading < -PI)
-            heading += (2.0f * PI);
-    }
-
-    return heading;
-}
-
-
-float
-Compass::calculate_heading(const Matrix3f &dcm_matrix)
-{
     float headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y;
 
     // Tilt compensated magnetic field X component:

libraries/AP_Compass/Compass.h

@@ -64,22 +64,13 @@ public:
     /// possible
     virtual void accumulate(void) = 0;
 
-    /// Calculate the tilt-compensated heading_ variables.
-    ///
-    /// @param  roll                The current airframe roll angle.
-    /// @param  pitch               The current airframe pitch angle.
-    ///
-    /// @returns heading in radians
-    ///
-    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
     ///
-    float calculate_heading(const Matrix3f &dcm_matrix);
+    float calculate_heading(const Matrix3f &dcm_matrix) const;
 
     /// Sets the compass offset x/y/z values.
     ///
@@ -122,7 +113,7 @@ public:
     void null_offsets(void);
 
     /// return true if the compass should be used for yaw calculations
-    bool use_for_yaw(void) {
+    bool use_for_yaw(void) const {
         return healthy && _use_for_yaw;
     }
 
@@ -132,7 +123,7 @@ public:
     /// @param save_to_eeprom       true to save to eeprom (false saves only to memory)
     ///
     void set_declination(float radians, bool save_to_eeprom = true);
-    float get_declination();
+    float get_declination() const;
 
     // set overall board orientation
     void set_board_orientation(enum Rotation orientation) {
@@ -152,7 +143,7 @@ public:
     }
 
     /// get the motor compensation value.
-    uint8_t motor_compensation_type() {
+    uint8_t motor_compensation_type() const {
         return _motor_comp_type;
     }
 
@@ -177,7 +168,7 @@ public:
     ///
     /// @returns                    The current compass offsets.
     ///
-    Vector3f &get_motor_offsets() { return _motor_offset; }
+    const Vector3f &get_motor_offsets() const { return _motor_offset; }
 
     /// Set the throttle as a percentage from 0.0 to 1.0
     /// @param thr_pct              throttle expressed as a percentage from 0 to 1.0

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -76,7 +76,10 @@ void loop()
             hal.console->println("not healthy");
             return;
         }
-        heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0 for this example
+	Matrix3f dcm_matrix;
+	// use roll = 0, pitch = 0 for this example
+	dcm_matrix.from_euler(0, 0, 0);
+        heading = compass.calculate_heading(dcm_matrix);
         compass.null_offsets();
 
         // capture min