ArduPilot · tridge · Dec 16, 2023 · Nov 30, 2023 · Nov 30, 2023 · Nov 30, 2023
diff --git a/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/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
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
@@ -3279,6 +3279,10 @@ def MicroStrainEAHRS7(self):
         '''Test MicroStrain EAHRS series 7 support'''
         self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt")
 
+    def InertialLabsEAHRS(self):
+        '''Test InertialLabs EAHRS support'''
+        self.fly_external_AHRS("ILabs", 5, "ap1.txt")
+
     def get_accelvec(self, m):
         return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81
 
@@ -5358,6 +5362,7 @@ def tests(self):
             self.VectorNavEAHRS,
             self.MicroStrainEAHRS5,
             self.MicroStrainEAHRS7,
+            self.InertialLabsEAHRS,
             self.Deadreckoning,
             self.DeadreckoningNoAirSpeed,
             self.EKFlaneswitch,

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -220,12 +220,12 @@ void AP_AHRS::init()
     }
 #if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
     if (_ekf_type.get() == 2) {
-        _ekf_type.set(3);
+        _ekf_type.set(EKFType::THREE);
         EKF3.set_enable(true);
     }
 #elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE
     if (_ekf_type.get() == 3) {
-        _ekf_type.set(2);
+        _ekf_type.set(EKFType::TWO);
         EKF2.set_enable(true);
     }
 #endif
@@ -234,7 +234,7 @@ void AP_AHRS::init()
     // a special case to catch users who had AHRS_EKF_TYPE=2 saved and
     // updated to a version where EK2_ENABLE=0
     if (_ekf_type.get() == 2 && !EKF2.get_enable() && EKF3.get_enable()) {
-        _ekf_type.set(3);
+        _ekf_type.set(EKFType::THREE);
     }
 #endif
 

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -421,6 +421,29 @@ class AP_AHRS {
         return _ekf_type;
     }
 
+    enum class EKFType : uint8_t {
+#if AP_AHRS_DCM_ENABLED
+        DCM = 0,
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+        THREE = 3,
+#endif
+#if HAL_NAVEKF2_AVAILABLE
+        TWO = 2,
+#endif
+#if AP_AHRS_SIM_ENABLED
+        SIM = 10,
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+        EXTERNAL = 11,
+#endif
+    };
+
+    // set the selected ekf type, for RC aux control
+    void set_ekf_type(EKFType ahrs_type) {
+        _ekf_type.set(ahrs_type);
+    }
+
     // these are only out here so vehicles can reference them for parameters
 #if HAL_NAVEKF2_AVAILABLE
     NavEKF2 EKF2;
@@ -668,7 +691,7 @@ class AP_AHRS {
      */
     AP_Int8 _wind_max;
     AP_Int8 _board_orientation;
-    AP_Int8 _ekf_type;
+    AP_Enum<EKFType> _ekf_type;
 
     /*
      * DCM-backend parameters; it takes references to these
@@ -683,23 +706,6 @@ class AP_AHRS {
     AP_Enum<GPSUse> _gps_use;
     AP_Int8 _gps_minsats;
 
-    enum class EKFType {
-#if AP_AHRS_DCM_ENABLED
-        DCM = 0,
-#endif
-#if HAL_NAVEKF3_AVAILABLE
-        THREE = 3,
-#endif
-#if HAL_NAVEKF2_AVAILABLE
-        TWO = 2,
-#endif
-#if AP_AHRS_SIM_ENABLED
-        SIM = 10,
-#endif
-#if HAL_EXTERNAL_AHRS_ENABLED
-        EXTERNAL = 11,
-#endif
-    };
     EKFType active_EKF_type(void) const { return state.active_EKF; }
 
     bool always_use_EKF() const {

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -101,6 +101,22 @@ AP_AHRS_DCM::update()
 
     // remember the last origin for fallback support
     IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
+
+#if HAL_LOGGING_ENABLED
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_log_ms >= 100) {
+        // log DCM at 10Hz
+        last_log_ms = now_ms;
+        AP::logger().WriteStreaming("DCM", "TimeUS,Roll,Pitch,Yaw",
+                                    "sddd",
+                                    "F000",
+                                    "Qfff",
+                                    AP_HAL::micros64(),
+                                    degrees(roll),
+                                    degrees(pitch),
+                                    wrap_360(degrees(yaw)));
+    }
+#endif // HAL_LOGGING_ENABLED
 }
 
 void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -286,6 +286,8 @@ class AP_AHRS_DCM : public AP_AHRS_Backend {
     // pre-calculated trig cache:
     float _sin_yaw;
     float _cos_yaw;
+
+    uint32_t last_log_ms;
 };
 
 #endif  // AP_AHRS_DCM_ENABLED
diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp
@@ -52,6 +52,7 @@
 #include "AP_Airspeed_DroneCAN.h"
 #include "AP_Airspeed_NMEA.h"
 #include "AP_Airspeed_MSP.h"
+#include "AP_Airspeed_External.h"
 #include "AP_Airspeed_SITL.h"
 extern const AP_HAL::HAL &hal;
 
@@ -433,6 +434,11 @@ void AP_Airspeed::allocate()
         case TYPE_MSP:
 #if AP_AIRSPEED_MSP_ENABLED
             sensor[i] = new AP_Airspeed_MSP(*this, i, 0);
+#endif
+            break;
+        case TYPE_EXTERNAL:
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+            sensor[i] = new AP_Airspeed_External(*this, i);
 #endif
             break;
         }
@@ -731,6 +737,24 @@ void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
 }
 #endif 
 
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+/*
+  handle airspeed airspeed data
+ */
+void AP_Airspeed::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt)
+{
+    if (!lib_enabled()) {
+        return;
+    }
+
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        if (param[i].type == TYPE_EXTERNAL && sensor[i]) {
+            sensor[i]->handle_external(pkt);
+        }
+    }
+}
+#endif 
+
 // @LoggerMessage: HYGR
 // @Description: Hygrometer data
 // @Field: TimeUS: Time since system startup

diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h
@@ -10,6 +10,9 @@
 #if AP_AIRSPEED_MSP_ENABLED
 #include <AP_MSP/msp.h>
 #endif
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+#endif
 
 class AP_Airspeed_Backend;
 
@@ -187,6 +190,7 @@ class AP_Airspeed
         TYPE_NMEA_WATER=13,
         TYPE_MSP=14,
         TYPE_I2C_ASP5033=15,
+        TYPE_EXTERNAL=16,
         TYPE_SITL=100,
     };
 
@@ -208,6 +212,10 @@ class AP_Airspeed
     void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
 #endif
 
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+    void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt);
+#endif
+
     enum class CalibrationState {
         NOT_STARTED,
         IN_PROGRESS,

diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
@@ -49,6 +49,9 @@ class AP_Airspeed_Backend {
     virtual bool get_airspeed(float& airspeed) {return false;}
 
     virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {}
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+    virtual void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) {}
+#endif
 
 #if AP_AIRSPEED_HYGROMETER_ENABLE
     // optional hygrometer support

diff --git a/libraries/AP_Airspeed/AP_Airspeed_External.cpp b/libraries/AP_Airspeed/AP_Airspeed_External.cpp
@@ -0,0 +1,58 @@
+#include "AP_Airspeed_External.h"
+
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+
+AP_Airspeed_External::AP_Airspeed_External(AP_Airspeed &_frontend, uint8_t _instance) :
+    AP_Airspeed_Backend(_frontend, _instance)
+{
+    set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,0,_instance,0));
+}
+
+// return the current differential_pressure in Pascal
+bool AP_Airspeed_External::get_differential_pressure(float &pressure)
+{
+    WITH_SEMAPHORE(sem);
+    if (press_count == 0) {
+        return false;
+    }
+    pressure = sum_pressure/press_count;
+    press_count = 0;
+    sum_pressure = 0;
+    return true;
+}
+
+// get last temperature
+bool AP_Airspeed_External::get_temperature(float &temperature)
+{
+    WITH_SEMAPHORE(sem);
+    if (temperature_count == 0) {
+        return false;
+    }
+    temperature = sum_temperature/temperature_count;
+    temperature_count = 0;
+    sum_temperature = 0;
+    return true;
+}
+
+void AP_Airspeed_External::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt)
+{
+    WITH_SEMAPHORE(sem);
+
+    sum_pressure += pkt.differential_pressure;
+    press_count++;
+    if (press_count > 100) {
+        // prevent overflow
+        sum_pressure /= 2;
+        press_count /= 2;
+    }
+
+    sum_temperature += pkt.temperature;
+    temperature_count++;
+    if (temperature_count > 100) {
+        // prevent overflow
+        sum_temperature /= 2;
+        temperature_count /= 2;
+    }
+}
+
+#endif // AP_AIRSPEED_EXTERNAL_ENABLED
diff --git a/libraries/AP_Airspeed/AP_Airspeed_External.h b/libraries/AP_Airspeed/AP_Airspeed_External.h
@@ -0,0 +1,38 @@
+/*
+  external AHRS airspeed backend
+ */
+#pragma once
+
+#include "AP_Airspeed_config.h"
+
+#if AP_AIRSPEED_EXTERNAL_ENABLED
+
+#include "AP_Airspeed_Backend.h"
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+
+class AP_Airspeed_External : public AP_Airspeed_Backend
+{
+public:
+    AP_Airspeed_External(AP_Airspeed &airspeed, uint8_t instance);
+
+    bool init(void) override {
+        return true;
+    }
+
+    void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) override;
+
+    // return the current differential_pressure in Pascal
+    bool get_differential_pressure(float &pressure) override;
+
+    // temperature not available via analog backend
+    bool get_temperature(float &temperature) override;
+
+private:
+    float sum_pressure;
+    uint8_t press_count;
+    float sum_temperature;
+    uint8_t temperature_count;
+};
+
+#endif // AP_AIRSPEED_EXTERNAL_ENABLED
+
diff --git a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
@@ -46,7 +46,7 @@ const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Airspeed type
     // @Description: Type of airspeed sensor
-    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL
+    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

diff --git a/libraries/AP_Airspeed/AP_Airspeed_config.h b/libraries/AP_Airspeed/AP_Airspeed_config.h
@@ -3,6 +3,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_MSP/AP_MSP_config.h>
+#include <AP_ExternalAHRS/AP_ExternalAHRS_config.h>
 
 #ifndef AP_AIRSPEED_ENABLED
 #define AP_AIRSPEED_ENABLED 1
@@ -66,3 +67,7 @@
 #ifndef AP_AIRSPEED_HYGROMETER_ENABLE
 #define AP_AIRSPEED_HYGROMETER_ENABLE (AP_AIRSPEED_ENABLED && BOARD_FLASH_SIZE > 1024)
 #endif
+
+#ifndef AP_AIRSPEED_EXTERNAL_ENABLED
+#define AP_AIRSPEED_EXTERNAL_ENABLED AP_AIRSPEED_ENABLED && HAL_EXTERNAL_AHRS_ENABLED
+#endif
diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp
@@ -61,7 +61,6 @@
 #define GAIN_CC50 20.0f   // LSB/uT
 #define GAIN_CC100 38.0f
 #define GAIN_CC200 75.0f
-#define UTESLA_TO_MGAUSS   10.0f // uT to mGauss conversion
 
 #define TMRC    0x94    // Update rate 150Hz
 #define CMM     0x71    // read 3 axes and set data ready if 3 axes are ready