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 <[email protected]>
ArduPilot · Mar 26, 2024 · dc2815e · dc2815e
1 parent 8fc76e7
commit dc2815e
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 7 deletions.
diff --git 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
@@ -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
@@ -77,15 +77,16 @@ class AP_MicroStrain
     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;