AP_ExternalAHRS: support backends with get_variances()

re-implement send_status_report in terms of get_variances and support
EKF failsafe with ExternalAHRS

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -289,6 +289,17 @@ void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const
     }
 }
 
+/*
+  get estimated variances, return false if not implemented
+ */
+bool AP_ExternalAHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
+{
+    if (backend != nullptr) {
+        return backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar);
+    }
+    return false;
+}
+
 bool AP_ExternalAHRS::get_gyro(Vector3f &gyro)
 {
     WITH_SEMAPHORE(state.sem);
@@ -312,9 +323,56 @@ bool AP_ExternalAHRS::get_accel(Vector3f &accel)
 // send an EKF_STATUS message to GCS
 void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const
 {
-    if (backend) {
+    float velVar, posVar, hgtVar, tasVar;
-        backend->send_status_report(link);
+    Vector3f magVar;
+    if (backend == nullptr || !backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar)) {
+        return;
     }
+
+    uint16_t flags = 0;
+    nav_filter_status filterStatus {};
+    get_filter_status(filterStatus);
+
+    if (filterStatus.flags.attitude) {
+        flags |= EKF_ATTITUDE;
+    }
+    if (filterStatus.flags.horiz_vel) {
+        flags |= EKF_VELOCITY_HORIZ;
+    }
+    if (filterStatus.flags.vert_vel) {
+        flags |= EKF_VELOCITY_VERT;
+    }
+    if (filterStatus.flags.horiz_pos_rel) {
+        flags |= EKF_POS_HORIZ_REL;
+    }
+    if (filterStatus.flags.horiz_pos_abs) {
+        flags |= EKF_POS_HORIZ_ABS;
+    }
+    if (filterStatus.flags.vert_pos) {
+        flags |= EKF_POS_VERT_ABS;
+    }
+    if (filterStatus.flags.terrain_alt) {
+        flags |= EKF_POS_VERT_AGL;
+    }
+    if (filterStatus.flags.const_pos_mode) {
+        flags |= EKF_CONST_POS_MODE;
+    }
+    if (filterStatus.flags.pred_horiz_pos_rel) {
+        flags |= EKF_PRED_POS_HORIZ_REL;
+    }
+    if (filterStatus.flags.pred_horiz_pos_abs) {
+        flags |= EKF_PRED_POS_HORIZ_ABS;
+    }
+    if (!filterStatus.flags.initalized) {
+        flags |= EKF_UNINITIALIZED;
+    }
+
+    const float mag_var = MAX(magVar.x, MAX(magVar.y, magVar.z));
+    mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
+                                       velVar,
+                                       posVar,
+                                       hgtVar,
+                                       mag_var, 0, 0);
 }
 
 void AP_ExternalAHRS::update(void)
@@ -357,6 +415,28 @@ void AP_ExternalAHRS::update(void)
                                     state.velocity.x, state.velocity.y, state.velocity.z,
                                     state.location.lat, state.location.lng, state.location.alt*0.01,
                                     filterStatus.value);
+
+        // @LoggerMessage: EAHV
+        // @Description: External AHRS variances
+        // @Field: TimeUS: Time since system startup
+        // @Field: Vel: velocity variance
+        // @Field: Pos: position variance
+        // @Field: Hgt: height variance
+        // @Field: MagX: magnetic variance, X
+        // @Field: MagY: magnetic variance, Y
+        // @Field: MagZ: magnetic variance, Z
+        // @Field: TAS: true airspeed variance
+
+        float velVar, posVar, hgtVar, tasVar;
+        Vector3f magVar;
+        if (backend != nullptr && backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar)) {
+            AP::logger().WriteStreaming("EAHV", "TimeUS,Vel,Pos,Hgt,MagX,MagY,MagZ,TAS",
+                                        "Qfffffff",
+                                        AP_HAL::micros64(),
+                                        velVar, posVar, hgtVar,
+                                        magVar.x, magVar.y, magVar.z,
+                                        tasVar);
+        }
     }
 #endif  // HAL_LOGGING_ENABLED
 }

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -120,6 +120,7 @@ public:
     bool get_gyro(Vector3f &gyro);
     bool get_accel(Vector3f &accel);
     void send_status_report(class GCS_MAVLINK &link) const;
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const;
 
     // update backend
     void update();

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp

@@ -1110,57 +1110,14 @@ void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status)
     status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL);
 }
 
-// send an EKF_STATUS message to GCS
+// get variances
-void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const
+bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
 {
-    // prepare flags
+    velVar = state2.kf_vel_covariance.length() * vel_gate_scale;
-    uint16_t flags = 0;
+    posVar = state2.kf_pos_covariance.xy().length() * pos_gate_scale;
-    nav_filter_status filterStatus;
+    hgtVar = state2.kf_pos_covariance.z * hgt_gate_scale;
-    get_filter_status(filterStatus);
+    tasVar = 0;
-    if (filterStatus.flags.attitude) {
+    return true;
-        flags |= EKF_ATTITUDE;
-    }
-    if (filterStatus.flags.horiz_vel) {
-        flags |= EKF_VELOCITY_HORIZ;
-    }
-    if (filterStatus.flags.vert_vel) {
-        flags |= EKF_VELOCITY_VERT;
-    }
-    if (filterStatus.flags.horiz_pos_rel) {
-        flags |= EKF_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.horiz_pos_abs) {
-        flags |= EKF_POS_HORIZ_ABS;
-    }
-    if (filterStatus.flags.vert_pos) {
-        flags |= EKF_POS_VERT_ABS;
-    }
-    if (filterStatus.flags.terrain_alt) {
-        flags |= EKF_POS_VERT_AGL;
-    }
-    if (filterStatus.flags.const_pos_mode) {
-        flags |= EKF_CONST_POS_MODE;
-    }
-    if (filterStatus.flags.pred_horiz_pos_rel) {
-        flags |= EKF_PRED_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.pred_horiz_pos_abs) {
-        flags |= EKF_PRED_POS_HORIZ_ABS;
-    }
-    if (!filterStatus.flags.initalized) {
-        flags |= EKF_UNINITIALIZED;
-    }
-
-    // send message
-    const float vel_gate = 5;
-    const float pos_gate = 5;
-    const float hgt_gate = 5;
-    const float mag_var = 0;
-    mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
-                                       state2.kf_vel_covariance.length()/vel_gate,
-                                       state2.kf_pos_covariance.xy().length()/pos_gate,
-                                       state2.kf_pos_covariance.z/hgt_gate,
-                                       mag_var, 0, 0);
 }
 
 #endif  // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h

@@ -37,7 +37,7 @@ public:
     bool initialised(void) const override;
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
     void get_filter_status(nav_filter_status &status) const override;
-    void send_status_report(class GCS_MAVLINK &link) const override;
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 
     // check for new data
     void update() override {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp

@@ -268,55 +268,14 @@ void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status)
     }
 }
 
-void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const
+// get variances
+bool AP_ExternalAHRS_MicroStrain5::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
 {
-    // prepare flags
+    velVar = gnss_data[gnss_instance].speed_accuracy * vel_gate_scale;
-    uint16_t flags = 0;
+    posVar = gnss_data[gnss_instance].horizontal_position_accuracy * pos_gate_scale;
-    nav_filter_status filterStatus;
+    hgtVar = gnss_data[gnss_instance].vertical_position_accuracy * hgt_gate_scale;
-    get_filter_status(filterStatus);
+    tasVar = 0;
-    if (filterStatus.flags.attitude) {
+    return true;
-        flags |= EKF_ATTITUDE;
-    }
-    if (filterStatus.flags.horiz_vel) {
-        flags |= EKF_VELOCITY_HORIZ;
-    }
-    if (filterStatus.flags.vert_vel) {
-        flags |= EKF_VELOCITY_VERT;
-    }
-    if (filterStatus.flags.horiz_pos_rel) {
-        flags |= EKF_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.horiz_pos_abs) {
-        flags |= EKF_POS_HORIZ_ABS;
-    }
-    if (filterStatus.flags.vert_pos) {
-        flags |= EKF_POS_VERT_ABS;
-    }
-    if (filterStatus.flags.terrain_alt) {
-        flags |= EKF_POS_VERT_AGL;
-    }
-    if (filterStatus.flags.const_pos_mode) {
-        flags |= EKF_CONST_POS_MODE;
-    }
-    if (filterStatus.flags.pred_horiz_pos_rel) {
-        flags |= EKF_PRED_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.pred_horiz_pos_abs) {
-        flags |= EKF_PRED_POS_HORIZ_ABS;
-    }
-    if (!filterStatus.flags.initalized) {
-        flags |= EKF_UNINITIALIZED;
-    }
-
-    // send message
-    const float vel_gate = 4; // represents hz value data is posted at
-    const float pos_gate = 4; // represents hz value data is posted at
-    const float hgt_gate = 4; // represents hz value data is posted at
-    const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
-    mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
-                                       gnss_data[gnss_instance].speed_accuracy/vel_gate, gnss_data[gnss_instance].horizontal_position_accuracy/pos_gate, gnss_data[gnss_instance].vertical_position_accuracy/hgt_gate,
-                                       mag_var, 0, 0);
-
 }
 
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h

@@ -42,7 +42,7 @@ public:
     bool initialised(void) const override;
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
     void get_filter_status(nav_filter_status &status) const override;
-    void send_status_report(class GCS_MAVLINK &link) const override;
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 
     // check for new data
     void update() override {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp

@@ -317,63 +317,14 @@ void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status)
     }
 }
 
-void AP_ExternalAHRS_MicroStrain7::send_status_report(GCS_MAVLINK &link) const
+// get variances
+bool AP_ExternalAHRS_MicroStrain7::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
 {
-    // prepare flags
+    velVar = filter_data.ned_velocity_uncertainty.length() * vel_gate_scale;
-    uint16_t flags = 0;
+    posVar = filter_data.ned_position_uncertainty.xy().length() * pos_gate_scale;
-    nav_filter_status filterStatus;
+    hgtVar = filter_data.ned_position_uncertainty.z * hgt_gate_scale;
-    get_filter_status(filterStatus);
+    tasVar = 0;
-    if (filterStatus.flags.attitude) {
+    return true;
-        flags |= EKF_ATTITUDE;
-    }
-    if (filterStatus.flags.horiz_vel) {
-        flags |= EKF_VELOCITY_HORIZ;
-    }
-    if (filterStatus.flags.vert_vel) {
-        flags |= EKF_VELOCITY_VERT;
-    }
-    if (filterStatus.flags.horiz_pos_rel) {
-        flags |= EKF_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.horiz_pos_abs) {
-        flags |= EKF_POS_HORIZ_ABS;
-    }
-    if (filterStatus.flags.vert_pos) {
-        flags |= EKF_POS_VERT_ABS;
-    }
-    if (filterStatus.flags.terrain_alt) {
-        flags |= EKF_POS_VERT_AGL;
-    }
-    if (filterStatus.flags.const_pos_mode) {
-        flags |= EKF_CONST_POS_MODE;
-    }
-    if (filterStatus.flags.pred_horiz_pos_rel) {
-        flags |= EKF_PRED_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.pred_horiz_pos_abs) {
-        flags |= EKF_PRED_POS_HORIZ_ABS;
-    }
-    if (!filterStatus.flags.initalized) {
-        flags |= EKF_UNINITIALIZED;
-    }
-
-    // send message
-    const float vel_gate = 4; // represents hz value data is posted at
-    const float pos_gate = 4; // represents hz value data is posted at
-    const float hgt_gate = 4; // represents hz value data is posted at
-    const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
-
-    const float velocity_variance {filter_data.ned_velocity_uncertainty.length() / vel_gate};
-    const float pos_horiz_variance {filter_data.ned_position_uncertainty.xy().length() / pos_gate};
-    const float pos_vert_variance {filter_data.ned_position_uncertainty.z / hgt_gate};
-    // No terrain alt sensor on MicroStrain7.
-    const float terrain_alt_variance {0};
-    // No airspeed sensor on MicroStrain7.
-    const float airspeed_variance {0};
-    mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
-                                       velocity_variance, pos_horiz_variance, pos_vert_variance,
-                                       mag_var, terrain_alt_variance, airspeed_variance);
-
 }
 
 bool AP_ExternalAHRS_MicroStrain7::times_healthy() const

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h

@@ -42,7 +42,7 @@ public:
     bool initialised(void) const override;
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
     void get_filter_status(nav_filter_status &status) const override;
-    void send_status_report(class GCS_MAVLINK &link) const override;
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 
     // check for new data
     void update() override

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp

@@ -788,59 +788,15 @@ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) con
     }
 }
 
-// send an EKF_STATUS message to GCS
+// get variances
-void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const
+bool AP_ExternalAHRS_VectorNav::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
 {
-    if (!latest_ins_ekf_packet) {
-        return;
-    }
-    // prepare flags
-    uint16_t flags = 0;
-    nav_filter_status filterStatus;
-    get_filter_status(filterStatus);
-    if (filterStatus.flags.attitude) {
-        flags |= EKF_ATTITUDE;
-    }
-    if (filterStatus.flags.horiz_vel) {
-        flags |= EKF_VELOCITY_HORIZ;
-    }
-    if (filterStatus.flags.vert_vel) {
-        flags |= EKF_VELOCITY_VERT;
-    }
-    if (filterStatus.flags.horiz_pos_rel) {
-        flags |= EKF_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.horiz_pos_abs) {
-        flags |= EKF_POS_HORIZ_ABS;
-    }
-    if (filterStatus.flags.vert_pos) {
-        flags |= EKF_POS_VERT_ABS;
-    }
-    if (filterStatus.flags.terrain_alt) {
-        flags |= EKF_POS_VERT_AGL;
-    }
-    if (filterStatus.flags.const_pos_mode) {
-        flags |= EKF_CONST_POS_MODE;
-    }
-    if (filterStatus.flags.pred_horiz_pos_rel) {
-        flags |= EKF_PRED_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.pred_horiz_pos_abs) {
-        flags |= EKF_PRED_POS_HORIZ_ABS;
-    }
-    if (!filterStatus.flags.initalized) {
-        flags |= EKF_UNINITIALIZED;
-    }
-
-    // send message
     const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)latest_ins_ekf_packet;
-    const float vel_gate = 5;
+    velVar = pkt.velU * vel_gate_scale;
-    const float pos_gate = 5;
+    posVar = pkt.posU * pos_gate_scale;
-    const float hgt_gate = 5;
+    hgtVar = pkt.posU * hgt_gate_scale;
-    const float mag_var = 0;
+    tasVar = 0;
-    mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
+    return true;
-                                       pkt.velU / vel_gate, pkt.posU / pos_gate, pkt.posU / hgt_gate,
-                                       mag_var, 0, 0);
 }
 
 #endif  // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h

@@ -37,7 +37,7 @@ public:
     bool initialised(void) const override;
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
     void get_filter_status(nav_filter_status &status) const override;
-    void send_status_report(class GCS_MAVLINK &link) const override;
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 
     // check for new data
     void update() override {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h

@@ -41,7 +41,7 @@ public:
     virtual bool initialised(void) const = 0;
     virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0;
     virtual void get_filter_status(nav_filter_status &status) const {}
-    virtual void send_status_report(class GCS_MAVLINK &link) const {}
+    virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { return false; }
 
     // Check for new data.
     // This is used when there's not a separate thread for EAHRS.
@@ -73,6 +73,13 @@ protected:
      */
     bool in_fly_forward(void) const;
 
+    /*
+      scale factors for get_variances() to return normalised values from SI units
+     */
+    const float vel_gate_scale = 0.2;
+    const float pos_gate_scale = 0.2;
+    const float hgt_gate_scale = 0.2;
+    
 private:
     AP_ExternalAHRS &frontend;
 };