From 9aea7812485259a039accf960b010d282f8ff169 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 4 Jan 2014 11:30:32 +1100
Subject: [PATCH] GCS_MAVLink: moved send_ahrs2 to common code

---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 19 +++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 328aa8da6c..34b9e06b31 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -181,6 +181,7 @@ public:
     // common send functions
     void send_meminfo(void);
     void send_power_status(void);
+    void send_ahrs2(AP_AHRS &ahrs);
 
 private:
     void        handleMessage(mavlink_message_t * msg);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 64a0e501bb..e3ef6ca540 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -18,6 +18,7 @@
  */
 
 #include <GCS.h>
+#include <AP_AHRS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -159,3 +160,21 @@ void GCS_MAVLINK::send_power_status(void)
                                   hal.analogin->power_status_flags());
 #endif
 }
+
+// report AHRS2 state
+void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
+{
+#if AP_AHRS_NAVEKF_AVAILABLE
+    Vector3f euler;
+    struct Location loc;
+    if (ahrs.get_secondary_attitude(euler) && ahrs.get_secondary_position(loc)) {
+        mavlink_msg_ahrs2_send(chan,
+                               euler.x,
+                               euler.y,
+                               euler.z,
+                               loc.alt*1.0e-2f,
+                               loc.lat,
+                               loc.lng);
+    }
+#endif
+}