From 2f69dcc085c87bcb90be4787e3c77f227bda073d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 5 Oct 2021 14:08:54 +1100
Subject: [PATCH] AP_AHRS: cache some trig results for yaw in DCM backend

---
 libraries/AP_AHRS/AP_AHRS_Backend.cpp | 4 ++--
 libraries/AP_AHRS/AP_AHRS_DCM.cpp     | 8 ++++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h       | 4 ++++
 3 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp
index afeb448e04..4f95e988d7 100644
--- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp
+++ b/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();
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index cbefae2f80..c15224b555 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/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();
 }
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 035c7ddc9c..7ba2a9689b 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/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;
 };