AP_ExternalAHRS_AdvancedNavigation

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

@@ -3083,6 +3083,10 @@ 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'''
         self.customise_SITL_commandline(["--serial4=sim:MicroStrain7"])
@@ -6115,6 +6119,7 @@ def tests(self):
             self.MicroStrainEAHRS5,
             self.MicroStrainEAHRS7,
             self.InertialLabsEAHRS,
+            self.AdvancedNavigationEAHRS,
             self.GpsSensorPreArmEAHRS,
             self.Deadreckoning,
             self.DeadreckoningNoAirSpeed,

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -26,6 +26,7 @@
 #include "AP_ExternalAHRS_MicroStrain5.h"
 #include "AP_ExternalAHRS_MicroStrain7.h"
 #include "AP_ExternalAHRS_InertialLabs.h"
+#include "AP_ExternalAHRS_AdvancedNavigation.h"
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -73,6 +74,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @DisplayName: External AHRS options
     // @Description: External AHRS options bitmask
     // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag.
+    // @Bitmask: 1:Provide airspeed aiding to ExternalAHRS device from airspeed sensors.
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0),
 
@@ -89,7 +91,14 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @Units: Hz
     // @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
 };
 
@@ -130,6 +139,11 @@ void AP_ExternalAHRS::init(void)
         return;
 #endif
 
+#if AP_EXTERNAL_AHRS_ADNAV_ENABLED
+    case DevType::AdNav:
+        backend = new AP_ExternalAHRS_AdvancedNavigation(this, state);
+        return;
+#endif
     }
 
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype));

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -34,6 +34,7 @@ class AP_ExternalAHRS {
 public:
     friend class AP_ExternalAHRS_backend;
     friend class AP_ExternalAHRS_VectorNav;
+    friend class AP_ExternalAHRS_AdvancedNavigation;
 
     AP_ExternalAHRS();
 
@@ -49,10 +50,12 @@ 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
-        // 3 reserved for AdNav
         // 4 reserved for CINS
         // 6 reserved for Trimble
 #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
@@ -173,6 +176,7 @@ 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; }
 
@@ -184,6 +188,7 @@ class AP_ExternalAHRS {
     AP_Int16         log_rate;
     AP_Int16         options;
     AP_Int16         sensors;
+    AP_Float         arsp_err_20ms;
 
     static AP_ExternalAHRS *_singleton;