From 47fa858181f417449263ace4284d52495b3ab778 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 23 Mar 2024 15:37:47 -0600 Subject: [PATCH] AP_ExternalAHRS: Use filter data to populate EKF status report * Variances vs uncertainties may still be off * Requires enabling the following packets in SensorConnect: * Position Uncertainty (LLH) * Velocity Uncertainty (NED) Signed-off-by: Ryan Friedman --- .../AP_ExternalAHRS_MicroStrain7.cpp | 13 +++++++--- .../AP_ExternalAHRS/MicroStrain_common.cpp | 26 +++++++++++++++++++ .../AP_ExternalAHRS/MicroStrain_common.h | 7 ++--- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index d4ee38811c..72da0d1e0c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -361,11 +361,16 @@ void AP_ExternalAHRS_MicroStrain7::send_status_report(GCS_MAVLINK &link) const 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 - // TODO fix to use NED filter speed accuracy instead of first gnss - // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_ned_vel_uncertainty.htm + 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, - gnss_data[0].speed_accuracy/vel_gate, gnss_data[0].horizontal_position_accuracy/pos_gate, gnss_data[0].vertical_position_accuracy/hgt_gate, - mag_var, 0, 0); + velocity_variance, pos_horiz_variance, pos_vert_variance, + mag_var, terrain_alt_variance, airspeed_variance); } diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp index 7ced1123aa..f6cfa02008 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp @@ -60,6 +60,10 @@ enum class FilterPacketField { NED_VELOCITY = 0x02, // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm ATTITUDE_QUAT = 0x03, + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/0x82/data/0x08.htm + LLH_POSITION_UNCERTAINTY = 0x08, + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/0x82/data/0x09.htm + NED_VELOCITY_UNCERTAINTY = 0x09, // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm GPS_TIMESTAMP = 0xD3, }; @@ -295,6 +299,28 @@ void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet) filter_data.attitude_quat.normalize(); break; } + case FilterPacketField::LLH_POSITION_UNCERTAINTY: { + const float north_pos_acc = be32tofloat_ptr(packet.payload, i+2); + const float east_pos_acc = be32tofloat_ptr(packet.payload, i+6); + const float down_pos_acc = be32tofloat_ptr(packet.payload, i+10); + filter_data.ned_position_uncertainty = Vector3f( + north_pos_acc, + east_pos_acc, + down_pos_acc + ); + break; + } + case FilterPacketField::NED_VELOCITY_UNCERTAINTY: { + const float north_vel_uncertainty = be32tofloat_ptr(packet.payload, i+2); + const float east_vel_uncertainty = be32tofloat_ptr(packet.payload, i+6); + const float down_vel_uncertainty = be32tofloat_ptr(packet.payload, i+10); + filter_data.ned_velocity_uncertainty = Vector3f( + north_vel_uncertainty, + east_vel_uncertainty, + down_vel_uncertainty + ); + break; + } case FilterPacketField::FILTER_STATUS: { filter_status.state = be16toh_ptr(&packet.payload[i+2]); filter_status.mode = be16toh_ptr(&packet.payload[i+4]); diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.h b/libraries/AP_ExternalAHRS/MicroStrain_common.h index 9792a4f2ca..0e6b35db20 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.h +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.h @@ -77,15 +77,16 @@ protected: struct { uint16_t week; uint32_t tow_ms; - float horizontal_position_accuracy; - float vertical_position_accuracy; + // 1-sigma position uncertainty in the NED local-level frame [meters]. + Vector3f ned_position_uncertainty; int32_t lon; int32_t lat; int32_t hae_altitude; float ned_velocity_north; float ned_velocity_east; float ned_velocity_down; - float speed_accuracy; + // 1-sigma velocity uncertainties in the NED local-level frame. + Vector3f ned_velocity_uncertainty; // 4x1 vector representation of the quaternion describing the orientation of the device with respect to the NED local-level frame. // NED [Qw, Qx, Qy, Qz] Quaternion attitude_quat;