AP_GPS: Add covariance output to AP_GPS matching ROS NavSatFix

* Zero every iteration in case GPS quality degrades or fix is lost
* Use float precision for now

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/AP_GPS/AP_GPS.cpp

@@ -525,6 +525,25 @@ bool AP_GPS::vertical_accuracy(uint8_t instance, float &vacc) const
     return false;
 }
 
+AP_GPS::CovarianceType AP_GPS::position_covariance(const uint8_t instance, Matrix3f& cov) const
+{
+    AP_GPS::CovarianceType cov_type = AP_GPS::CovarianceType::UNKNOWN;
+    cov.zero();
+    float hacc, vacc;
+    if (horizontal_accuracy(instance, hacc) && vertical_accuracy(instance, vacc))
+    {
+        cov_type = AP_GPS::CovarianceType::DIAGONAL_KNOWN;
+        const auto hacc_variance = hacc * hacc;
+        cov[0][0] = hacc_variance;
+        cov[1][1] = hacc_variance;
+        cov[2][2] = vacc * vacc;
+    }
+    // There may be a receiver that implements hdop+vdop but not accuracy
+    // If so, there could be a condition here to attempt to calculate it
+
+    return cov_type;
+}
+
 
 /**
    convert GPS week and milliseconds to unix epoch in milliseconds

libraries/AP_GPS/AP_GPS.h

@@ -20,6 +20,7 @@
 #include <AP_Common/Location.h>
 #include <AP_Param/AP_Param.h>
 #include "GPS_detect_state.h"
+#include <AP_Math/AP_Math.h>
 #include <AP_MSP/msp.h>
 #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
 #include <SITL/SIM_GPS.h>
@@ -168,6 +169,14 @@ public:
         GPS_ROLE_MB_ROVER,
     };
 
+    // GPS Covariance Types matching ROS2 sensor_msgs/msg/NavSatFix
+    enum class CovarianceType : uint8_t {
+        UNKNOWN = 0,  ///< The GPS does not support any accuracy metrics
+        APPROXIMATED = 1,  ///< The accuracy is approximated through metrics such as HDOP/VDOP
+        DIAGONAL_KNOWN = 2, ///< The diagonal (east, north, up) components of covariance are reported by the GPS
+        KNOWN = 3, ///< The full covariance array is reported by the GPS
+    };
+
     /*
       The GPS_State structure is filled in by the backend driver as it
       parses each message from the GPS.
@@ -327,6 +336,8 @@ public:
         return vertical_accuracy(primary_instance, vacc);
     }
 
+    CovarianceType position_covariance(const uint8_t instance, Matrix3f& cov) const WARN_IF_UNUSED;
+
     // 3D velocity in NED format
     const Vector3f &velocity(uint8_t instance) const {
         return state[instance].velocity;