AP_AHRS: use a common function for updating the CD values

this ensures the wrapping of yaw is consistent between the 3 use cases

libraries/AP_AHRS/AP_AHRS.cpp

@@ -247,3 +247,15 @@ void AP_AHRS::update_trig(void)
     _sin_pitch = -temp.c.x;
     _sin_roll = temp.c.y / _cos_pitch;
 }
+
+/*
+  update the centi-degree values
+ */
+void AP_AHRS::update_cd_values(void)
+{
+    roll_sensor  = degrees(roll) * 100;
+    pitch_sensor = degrees(pitch) * 100;
+    yaw_sensor   = degrees(yaw) * 100;
+    if (yaw_sensor < 0)
+        yaw_sensor += 36000;
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -370,6 +370,9 @@ protected:
     //      should be called after _dcm_matrix is updated
     void update_trig(void);
 
+    // update roll_sensor, pitch_sensor and yaw_sensor
+    void update_cd_values(void);
+
     // pointer to compass object, if available
     Compass         * _compass;
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -837,12 +837,7 @@ AP_AHRS_DCM::euler_angles(void)
     _body_dcm_matrix.rotateXYinv(_trim);
     _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
-    roll_sensor     = degrees(roll)  * 100;
-    pitch_sensor    = degrees(pitch) * 100;
-    yaw_sensor      = degrees(yaw)   * 100;
-
-    if (yaw_sensor < 0)
-        yaw_sensor += 36000;
+    update_cd_values();
 }
 
 /* reporting of DCM state for MAVLink */

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -58,9 +58,7 @@ void AP_AHRS_NavEKF::update(void)
     roll = _dcm_attitude.x;
     pitch = _dcm_attitude.y;
     yaw = _dcm_attitude.z;
-    roll_sensor = degrees(roll)*100;
-    pitch_sensor = degrees(pitch)*100;
-    yaw_sensor = degrees(yaw)*100;
+    update_cd_values();
 
     AP_AHRS_DCM::update();
 
@@ -88,11 +86,8 @@ void AP_AHRS_NavEKF::update(void)
             roll  = eulers.x;
             pitch = eulers.y;
             yaw   = eulers.z;
-            roll_sensor  = degrees(roll) * 100;
-            pitch_sensor = degrees(pitch) * 100;
-            yaw_sensor   = degrees(yaw) * 100;
-            if (yaw_sensor < 0)
-                yaw_sensor += 36000;
+
+            update_cd_values();
             update_trig();
 
             // keep _gyro_bias for get_gyro_drift()