From 7d520c830736a60255f17fa478922debfe769c63 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 8 May 2020 07:23:48 +1000
Subject: [PATCH] AP_AHRS: added body/earth frame ops for Lua

---
 libraries/AP_AHRS/AP_AHRS.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 963e7407ea..f1b671a353 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -573,6 +573,16 @@ public:
         return _rsem;
     }
 
+    // convert a vector from body to earth frame
+    Vector3f body_to_earth(const Vector3f &v) const {
+        return v * get_rotation_body_to_ned();
+    }
+
+    // convert a vector from earth to body frame
+    Vector3f earth_to_body(const Vector3f &v) const {
+        return get_rotation_body_to_ned().mul_transpose(v);
+    }
+    
     // for holding parameters
     static const struct AP_Param::GroupInfo var_info[];