From 92693e023793133e49a035daf37c14433e484778 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Oct 2024 14:18:39 +1100 Subject: [PATCH] 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 | 84 ++++++++++++++++++- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 1 + .../AP_ExternalAHRS_InertialLabs.cpp | 57 ++----------- .../AP_ExternalAHRS_InertialLabs.h | 2 +- .../AP_ExternalAHRS_MicroStrain5.cpp | 55 ++---------- .../AP_ExternalAHRS_MicroStrain5.h | 2 +- .../AP_ExternalAHRS_MicroStrain7.cpp | 63 ++------------ .../AP_ExternalAHRS_MicroStrain7.h | 2 +- .../AP_ExternalAHRS_VectorNav.cpp | 58 ++----------- .../AP_ExternalAHRS_VectorNav.h | 2 +- .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 9 +- 11 files changed, 123 insertions(+), 212 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 9abbfff0d1..07d875e57d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/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) { - backend->send_status_report(link); + float velVar, posVar, hgtVar, tasVar; + 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 } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index b617a18e05..c5913baa8c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/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(); diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index 0d2eac1629..fb057d000f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/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 -void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const +// get variances +bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { - // 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 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); + velVar = state2.kf_vel_covariance.length() * vel_gate_scale; + posVar = state2.kf_pos_covariance.xy().length() * pos_gate_scale; + hgtVar = state2.kf_pos_covariance.z * hgt_gate_scale; + tasVar = 0; + return true; } #endif // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index d2ffcf1717..0db51358e7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/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 { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index a5e4d41c4c..0e500bc403 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/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 - 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 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); - + velVar = gnss_data[gnss_instance].speed_accuracy * vel_gate_scale; + posVar = gnss_data[gnss_instance].horizontal_position_accuracy * pos_gate_scale; + hgtVar = gnss_data[gnss_instance].vertical_position_accuracy * hgt_gate_scale; + tasVar = 0; + return true; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h index 7d5c199eb1..a311af5089 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h +++ b/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 { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 43c961a57d..31082aea9b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/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 - 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 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); - + velVar = filter_data.ned_velocity_uncertainty.length() * vel_gate_scale; + posVar = filter_data.ned_position_uncertainty.xy().length() * pos_gate_scale; + hgtVar = filter_data.ned_position_uncertainty.z * hgt_gate_scale; + tasVar = 0; + return true; } bool AP_ExternalAHRS_MicroStrain7::times_healthy() const diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h index f278289d15..27fcd19d00 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h +++ b/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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 92e4147fe8..4a50262402 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/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 -void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const +// get variances +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; - 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, - pkt.velU / vel_gate, pkt.posU / pos_gate, pkt.posU / hgt_gate, - mag_var, 0, 0); + velVar = pkt.velU * vel_gate_scale; + posVar = pkt.posU * pos_gate_scale; + hgtVar = pkt.posU * hgt_gate_scale; + tasVar = 0; + return true; } #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index e500a0d8b3..c6665bb59b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/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 { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index e5a22a87cb..8ee9af82ce 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/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; };