From 8b53f1272a0b90acbc0863e5c338685e5083fc90 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 22 Aug 2012 12:14:39 +1000
Subject: [PATCH] AHRS: new functions to support APM_Control library

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

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index ee91374c57..cd801a3843 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -58,6 +58,10 @@ public:
         _airspeed = airspeed;
     }
 
+    IMU*            get_imu() { 
+	    return _imu; 
+    }
+
     // Methods
     virtual void update(void) = 0;
 
@@ -71,6 +75,15 @@ public:
     int32_t pitch_sensor;
     int32_t yaw_sensor;
 
+    float get_pitch_rate_earth(void) {
+	    Vector3f omega = get_gyro();
+	    return cos(roll) * omega.y - sin(roll) * omega.z;
+    }
+    float get_roll_rate_earth(void)  {
+	    Vector3f omega = get_gyro();
+	    return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll));
+    }
+
     // return a smoothed and corrected gyro vector
     virtual Vector3f get_gyro(void) = 0;