From dc0491a8ff8e0e451219e1620914405e36267fd2 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Thu, 16 Jun 2022 23:42:39 +0100
Subject: [PATCH] AP_AHRS_view: add rotate method to move from AHRS to AHRS
 view referance frames

---
 libraries/AP_AHRS/AP_AHRS_View.cpp | 6 ++++++
 libraries/AP_AHRS/AP_AHRS_View.h   | 3 +++
 2 files changed, 9 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp
index 2adbd8d385..450567ce2c 100644
--- a/libraries/AP_AHRS/AP_AHRS_View.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_View.cpp
@@ -100,3 +100,9 @@ Vector2f AP_AHRS_View::body_to_earth2D(const Vector2f &bf) const
     return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw,
                     bf.x * trig.sin_yaw + bf.y * trig.cos_yaw);
 }
+
+// Rotate vector from AHRS reference frame to AHRS view reference frame
+void AP_AHRS_View::rotate(Vector3f &vec) const
+{
+    vec = rot_view * vec;
+}
diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h
index 2302f3c143..c1e1b3879d 100644
--- a/libraries/AP_AHRS/AP_AHRS_View.h
+++ b/libraries/AP_AHRS/AP_AHRS_View.h
@@ -195,6 +195,9 @@ public:
     // get pitch trim (deg)
     float get_pitch_trim() const { return _pitch_trim_deg; }
 
+    // Rotate vector from AHRS reference frame to AHRS view refences frame
+    void rotate(Vector3f &vec) const;
+
 private:
     const enum Rotation rotation;
     AP_AHRS &ahrs;