GCS_MAVLink: moved send_ahrs2 to common code

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);

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
+}