AP_AHRS: cache some trig results for yaw in DCM backend

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -89,7 +89,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
     if (gotAirspeed) {
         const Vector3f wind = wind_estimate();
         const Vector2f wind2d(wind.x, wind.y);
-        const Vector2f airspeed_vector{cosf(yaw) * airspeed, sinf(yaw) * airspeed};
+        const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed};
         gndVelADS = airspeed_vector + wind2d;
     }
 
@@ -132,7 +132,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
         // we have a rough airspeed, and we have a yaw. For
         // dead-reckoning purposes we can create a estimated
         // groundspeed vector
-        Vector2f ret(cosf(yaw), sinf(yaw));
+        Vector2f ret{_cos_yaw, _sin_yaw};
         ret *= airspeed;
         // adjust for estimated wind
         const Vector3f wind = wind_estimate();

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -101,6 +101,10 @@ AP_AHRS_DCM::update()
     _body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
     _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
+    // pre-calculate some trig for CPU purposes:
+    _cos_yaw = cosf(yaw);
+    _sin_yaw = sinf(yaw);
+
     backup_attitude();
 
     // remember the last origin for fallback support
@@ -231,6 +235,10 @@ AP_AHRS_DCM::reset(bool recover_eulers)
 
     }
 
+    // pre-calculate some trig for CPU purposes:
+    _cos_yaw = cosf(yaw);
+    _sin_yaw = sinf(yaw);
+
     _last_startup_ms = AP_HAL::millis();
 }
 

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -267,4 +267,8 @@ private:
     Vector2f _lp; // ground vector low-pass filter
     Vector2f _hp; // ground vector high-pass filter
     Vector2f _lastGndVelADS; // previous HPF input
+
+    // pre-calculated trig cache:
+    float _sin_yaw;
+    float _cos_yaw;
 };