From 602b81a4d721975a15efa9059de16a74e8369ae1 Mon Sep 17 00:00:00 2001
From: floaledm <flomartel@gmail.com>
Date: Wed, 21 Sep 2016 15:16:57 -0500
Subject: [PATCH] AP_Frsky_Telem: add ekf_status check

---
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 44 ++++++++++++++++++++-
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.h   |  2 +
 2 files changed, 44 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
index 75a059a17e..01a32782ee 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
@@ -98,8 +98,10 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
             _passthrough.send_attiandrng = false; // next iteration, check if we should send something other
         } else { // check if there's other data to send
             _passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth
-            // build mavlink message queue for sensor_status_flags
+            // build message queue for sensor_status_flags
             check_sensor_status_flags();
+            // build message queue for ekf_status
+            check_ekf_status();
             // if there's any message in the queue, start sending them chunk by chunk; three times each chunk
             if (get_next_msg_chunk()) {
                 send_uint32(DIY_FIRST_ID, _msg_chunk.chunk);
@@ -479,7 +481,7 @@ void AP_Frsky_Telem::queue_message(MAV_SEVERITY severity, const char *text)
 }
 
 /*
- * add control_sensors information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link
+ * add sensor_status_flags information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
  */
 void AP_Frsky_Telem::check_sensor_status_flags(void)
@@ -528,6 +530,44 @@ void AP_Frsky_Telem::check_sensor_status_flags(void)
     }
 }
 
+/*
+ * add innovation variance information to message cue, normally passed as ekf_status_report mavlink messages to the GCS, for transmission through FrSky link
+ * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
+ */
+void AP_Frsky_Telem::check_ekf_status(void)
+{
+    // get variances
+    float velVar, posVar, hgtVar, tasVar;
+    Vector3f magVar;
+    Vector2f offset;
+    if (_ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset)) {
+        uint32_t now = AP_HAL::millis();
+        if ((now - check_ekf_status_timer) > 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed
+            // multiple errors can be reported at a time. Same setup as Mission Planner.
+            if (velVar >= 1) {
+                queue_message(MAV_SEVERITY_CRITICAL, "Error velocity variance");
+                check_ekf_status_timer = now;
+            }
+            if (posVar >= 1) {
+                queue_message(MAV_SEVERITY_CRITICAL, "Error pos horiz variance");
+                check_ekf_status_timer = now;
+            }
+            if (hgtVar >= 1) {
+                queue_message(MAV_SEVERITY_CRITICAL, "Error pos vert variance");
+                check_ekf_status_timer = now;
+            }
+            if (magVar.length() >= 1) {
+                queue_message(MAV_SEVERITY_CRITICAL, "Error compass variance");
+                check_ekf_status_timer = now;
+            }
+            if (tasVar >= 1) {
+                queue_message(MAV_SEVERITY_CRITICAL, "Error terrain alt variance");
+                check_ekf_status_timer = now;
+            }
+        }
+    }
+}
+      
 /*
  * prepare parameter data
  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
index c6951655ec..dcca473737 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
@@ -164,6 +164,7 @@ private:
     
     float _relative_home_altitude; // altitude in centimeters above home
     uint32_t check_sensor_status_timer;
+    uint32_t check_ekf_status_timer;
     uint8_t _paramID;
     
     struct
@@ -239,6 +240,7 @@ private:
     // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
     bool get_next_msg_chunk(void);
     void check_sensor_status_flags(void);
+    void check_ekf_status(void);
     uint32_t calc_param(void);
     uint32_t calc_gps_latlng(bool *send_latitude);
     uint32_t calc_gps_status(void);