NavEKF: support sending EKF_STATUS_REPORT

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -4569,6 +4569,37 @@ void  NavEKF::getFilterStatus(nav_filter_status &status) const
     status.flags.pred_horiz_pos_abs = gpsNavPossible; // we should be able to estimate an absolute position when we enter flight mode
 }
 
+// send an EKF_STATUS message to GCS
+void NavEKF::send_status_report(mavlink_channel_t chan)
+{
+    // get filter status
+    nav_filter_status filt_state;
+    getFilterStatus(filt_state);
+
+    // prepare flags
+    uint16_t flags = 0;
+    if (filt_state.flags.attitude) { flags |= EKF_ATTITUDE; }
+    if (filt_state.flags.horiz_vel) { flags |= EKF_VELOCITY_HORIZ; }
+    if (filt_state.flags.vert_vel) { flags |= EKF_VELOCITY_VERT; }
+    if (filt_state.flags.horiz_pos_rel) { flags |= EKF_POS_HORIZ_REL; }
+    if (filt_state.flags.horiz_pos_abs) { flags |= EKF_POS_HORIZ_ABS; }
+    if (filt_state.flags.vert_pos) { flags |= EKF_POS_VERT_ABS; }
+    if (filt_state.flags.terrain_alt) { flags |= EKF_POS_VERT_AGL; }
+    if (filt_state.flags.const_pos_mode) { flags |= EKF_CONST_POS_MODE; }
+    if (filt_state.flags.pred_horiz_pos_rel) { flags |= EKF_PRED_POS_HORIZ_REL; }
+    if (filt_state.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; }
+
+    // get variances
+    float velVar, posVar, hgtVar, tasVar;
+    Vector3f magVar;
+    Vector2f offset;
+    getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
+
+    // send message
+    mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), tasVar);
+
+}
+
 // Check arm status and perform required checks and mode changes
 void NavEKF::performArmingChecks()
 {

libraries/AP_NavEKF/AP_NavEKF.h

@@ -28,6 +28,7 @@
 #include <AP_Compass.h>
 #include <AP_Param.h>
 #include <AP_Nav_Common.h>
+#include <GCS_MAVLink.h>
 
 // #define MATH_CHECK_INDEXES 1
 
@@ -224,6 +225,9 @@ public:
     */
     void  getFilterStatus(nav_filter_status &status) const;
 
+    // send an EKF_STATUS_REPORT message to GCS
+    void send_status_report(mavlink_channel_t chan);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private: