AP_AHRS: cache roll_deg and friends

Author: peterbarker

libraries/AP_AHRS/AP_AHRS.h

@@ -561,9 +561,9 @@ class AP_AHRS {
     float get_yaw_rad() const { return yaw; }
 
     // roll/pitch/yaw euler angles, all in radians
-    float get_roll_deg() const { return roll_sensor * 0.01; }
-    float get_pitch_deg() const { return pitch_sensor * 0.01; }
-    float get_yaw_deg() const { return yaw_sensor * 0.01; }
+    float get_roll_deg() const { return roll_deg; }
+    float get_pitch_deg() const { return pitch_deg; }
+    float get_yaw_deg() const { return yaw_deg; }
 
     // helper trig value accessors
     float cos_roll() const  {
@@ -585,15 +585,10 @@ class AP_AHRS {
         return _sin_yaw;
     }
 
-    float roll_deg() const {
-        return roll_sensor * 0.01;
-    }
-    float pitch_deg() const {
-        return pitch_sensor * 0.01;
-    }
-    float yaw_deg() const {
-        return yaw_sensor * 0.01;
-    }
+    // floating point Euler angles (Degrees)
+    float roll_deg;
+    float pitch_deg;
+    float yaw_deg;
 
     // integer Euler angles (Degrees * 100)
     int32_t roll_sensor;

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -151,11 +151,16 @@ void AP_AHRS::update_trig(void)
  */
 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;
+    roll_deg = degrees(roll);
+    pitch_deg = degrees(pitch);
+    yaw_deg = degrees(yaw);
+    if (yaw_deg < 0) {
+        yaw_deg += 360;
+    }
+
+    roll_sensor  = roll_deg * 100;
+    pitch_sensor = pitch_deg * 100;
+    yaw_sensor   = yaw_deg * 100;
 }
 
 /*

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -70,12 +70,15 @@ void AP_AHRS_View::update()
 
     rot_body_to_ned.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;
+    roll_deg = degrees(roll);
+    pitch_deg = degrees(pitch);
+    yaw_deg = degrees(yaw);
+    if (yaw_deg < 0) {
+        yaw_deg += 360;
     }
+    roll_sensor  = roll_deg * 100;
+    pitch_sensor = pitch_deg * 100;
+    yaw_sensor   = yaw_deg * 100;
 
     ahrs.calc_trig(rot_body_to_ned,
                    trig.cos_roll, trig.cos_pitch, trig.cos_yaw,

libraries/AP_AHRS/AP_AHRS_View.h

@@ -181,6 +181,9 @@ class AP_AHRS_View
     float roll;
     float pitch;
     float yaw;
+    float roll_deg;
+    float pitch_deg;
+    float yaw_deg;
     int32_t roll_sensor;
     int32_t pitch_sensor;
     int32_t yaw_sensor;