AP_ExternalAHRS: Add Advanced Navigation Driver Support

Tools/autotest/ArduPlane_Tests/AdvancedNavigationEAHRS/ap1.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.090027	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	50.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.367333	149.163164	28.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366814	149.165878	28.000000	1
+5	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.362947	149.165179	0.000000	1

Tools/autotest/arduplane.py

@@ -3127,6 +3127,10 @@ def MicroStrainEAHRS7(self):
     def InertialLabsEAHRS(self):
         '''Test InertialLabs EAHRS support'''
         self.fly_external_AHRS("ILabs", 5, "ap1.txt")
+
+    def AdvancedNavigationEAHRS(self):
+        '''Test AdvancedNavigation EAHRS series 5 support'''
+        self.fly_external_AHRS("AdNav", 3, "ap1.txt")
 
     def GpsSensorPreArmEAHRS(self):
         '''Test pre-arm checks related to EAHRS_SENSORS using the MicroStrain7 driver'''
@@ -7310,6 +7314,7 @@ def tests1b(self):
             self.MicroStrainEAHRS5,
             self.MicroStrainEAHRS7,
             self.InertialLabsEAHRS,
+            self.AdvancedNavigationEAHRS,
             self.GpsSensorPreArmEAHRS,
             self.Deadreckoning,
             self.DeadreckoningNoAirSpeed,

libraries/AP_AHRS/AP_AHRS_External.cpp

@@ -137,4 +137,9 @@ void AP_AHRS_External::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVe
     ekfNavVelGainScaler = 0.5;
 }
 
+bool AP_AHRS_External::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
+{
+    return AP::externalAHRS().get_variances(velVar, posVar, hgtVar, magVar, tasVar);
+}
+
 #endif

libraries/AP_AHRS/AP_AHRS_External.h

@@ -87,6 +87,8 @@ class AP_AHRS_External : public AP_AHRS_Backend {
     void send_ekf_status_report(class GCS_MAVLINK &link) const override;
 
     void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
+
+    bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 };
 
 #endif

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -24,6 +24,7 @@
 #include "AP_ExternalAHRS_backend.h"
 #include "AP_ExternalAHRS_VectorNav.h"
 #include "AP_ExternalAHRS_MicroStrain5.h"
+#include "AP_ExternalAHRS_AdvancedNavigation.h"
 #include "AP_ExternalAHRS_MicroStrain7.h"
 #include "AP_ExternalAHRS_InertialLabs.h"
 
@@ -90,6 +91,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10),
 
+    // @Param: _ARSP_ERR_20MS
+    // @DisplayName: Airspeed error at 20m/s
+    // @Description: Estimate of error in airspeed when vehicle travelling at 20m/s
+    // @Units: m/s
+    // @User: Advanced
+    AP_GROUPINFO("_ASER_20MS", 6, AP_ExternalAHRS, arsp_err_20ms, 1.0),
+
     AP_GROUPEND
 };
 
@@ -118,6 +126,12 @@ void AP_ExternalAHRS::init(void)
         return;
 #endif
 
+#if AP_EXTERNAL_AHRS_ADNAV_ENABLED
+    case DevType::AdNav:
+        backend = NEW_NOTHROW AP_ExternalAHRS_AdvancedNavigation(this, state);
+        return;
+#endif
+
 #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
     case DevType::MicroStrain7:
         backend = NEW_NOTHROW AP_ExternalAHRS_MicroStrain7(this, state);

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -35,6 +35,7 @@ class AP_ExternalAHRS {
 public:
     friend class AP_ExternalAHRS_backend;
     friend class AP_ExternalAHRS_VectorNav;
+    friend class AP_ExternalAHRS_AdvancedNavigation;
 
     AP_ExternalAHRS();
 
@@ -50,6 +51,9 @@ class AP_ExternalAHRS {
 #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
         MicroStrain5 = 2,
 #endif
+#if AP_EXTERNAL_AHRS_ADNAV_ENABLED
+        AdNav = 3,
+#endif
 #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
         InertialLabs = 5,
 #endif
@@ -154,6 +158,9 @@ class AP_ExternalAHRS {
         float  ned_vel_north;
         float  ned_vel_east;
         float  ned_vel_down;
+        float yaw;                   // degrees
+        float yaw_accuracy;
+        bool has_yaw;
     } gps_data_message_t;
 
     typedef struct {
@@ -176,7 +183,9 @@ class AP_ExternalAHRS {
 
     enum class OPTIONS {
         VN_UNCOMP_IMU = 1U << 0,
+        ARSP_AID = 1U << 1,
     };
+
     bool option_is_set(OPTIONS option) const { return (options.get() & int32_t(option)) != 0; }
 
 private:
@@ -187,6 +196,7 @@ class AP_ExternalAHRS {
     AP_Int16         log_rate;
     AP_Int16         options;
     AP_Int16         sensors;
+    AP_Float         arsp_err_20ms;
 
     static AP_ExternalAHRS *_singleton;