AP_ExternalAHRS: Add pre-arm for misconfigured EAHRS_SENSORS and GPS_TYPE

Tools/autotest/arduplane.py

@@ -3169,7 +3169,7 @@ def TerrainLoiter(self):
         self.fly_home_land_and_disarm(240)
 
     def fly_external_AHRS(self, sim, eahrs_type, mission):
-        """Fly with external AHRS (VectorNav)"""
+        """Fly with external AHRS"""
         self.customise_SITL_commandline(["--serial4=sim:%s" % sim])
 
         self.set_parameters({
@@ -3293,6 +3293,55 @@ def InertialLabsEAHRS(self):
         '''Test InertialLabs EAHRS support'''
         self.fly_external_AHRS("ILabs", 5, "ap1.txt")
 
+    def GpsSensorPreArmEAHRS(self):
+        '''Test pre-arm checks related to EAHRS_SENSORS using the MicroStrain7 driver'''
+        self.customise_SITL_commandline(["--serial4=sim:MicroStrain7"])
+
+        self.set_parameters({
+            "EAHRS_TYPE": 7,
+            "SERIAL4_PROTOCOL": 36,
+            "SERIAL4_BAUD": 230400,
+            "GPS1_TYPE": 0, # Disabled (simulate user setup error)
+            "GPS2_TYPE": 0, # Disabled (simulate user setup error)
+            "AHRS_EKF_TYPE": 11,
+            "INS_GYR_CAL": 1,
+            "EAHRS_SENSORS": 13, # GPS is enabled
+        })
+        self.reboot_sitl()
+        self.delay_sim_time(5)
+        self.progress("Running accelcal")
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,
+            p5=4,
+            timeout=5,
+        )
+
+        self.assert_prearm_failure("ExternalAHRS: Incorrect number", # Cut short due to message limits.
+                                   timeout=30,
+                                   other_prearm_failures_fatal=False)
+
+        self.set_parameters({
+            "EAHRS_TYPE": 7,
+            "SERIAL4_PROTOCOL": 36,
+            "SERIAL4_BAUD": 230400,
+            "GPS1_TYPE": 1, # Auto
+            "GPS2_TYPE": 21, # EARHS
+            "AHRS_EKF_TYPE": 11,
+            "INS_GYR_CAL": 1,
+            "EAHRS_SENSORS": 13, # GPS is enabled
+        })
+        self.reboot_sitl()
+        self.delay_sim_time(5)
+        self.progress("Running accelcal")
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,
+            p5=4,
+            timeout=5,
+        )
+        # Check prearm success with MicroStrain when the first GPS is occupied by another GPS.
+        # This supports the use case of comparing MicroStrain dual antenna to another GPS.
+        self.wait_ready_to_arm()
+
     def get_accelvec(self, m):
         return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81
 
@@ -5408,6 +5457,7 @@ def tests(self):
             self.MicroStrainEAHRS5,
             self.MicroStrainEAHRS7,
             self.InertialLabsEAHRS,
+            self.GpsSensorPreArmEAHRS,
             self.Deadreckoning,
             self.DeadreckoningNoAirSpeed,
             self.EKFlaneswitch,

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -29,6 +29,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_GPS/AP_GPS.h>
 #include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL &hal;
@@ -238,6 +239,27 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
     if (!backend->pre_arm_check(failure_msg, failure_msg_len)) {
         return false;
     }
+    // Verify the user has configured the GPS to accept EAHRS data.
+    if (has_sensor(AvailableSensor::GPS)) {
+        const auto eahrs_gps_sensors = backend->num_gps_sensors();
+
+        const auto &gps = AP::gps();
+        uint8_t n_configured_eahrs_gps = 0;
+        for (uint8_t i = 0; i < GPS_MAX_INSTANCES; ++i) {
+            const auto gps_type = gps.get_type(i);
+            if (gps_type == AP_GPS::GPS_TYPE_EXTERNAL_AHRS) {
+                n_configured_eahrs_gps++;
+            }
+        }
+
+        // Once AP supports at least 3 GPS's, change to == and remove the second condition.
+        // At that point, enforce that all GPS's in EAHRS can report to AP_GPS.
+        if (n_configured_eahrs_gps < 1 && eahrs_gps_sensors >= 1) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: Incorrect number of GPS sensors configured for EAHRS");
+            return false;
+        }
+    }
+
     if (!state.have_origin) {
         hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin");
 	    return false;

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h

@@ -181,6 +181,12 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
     uint16_t buffer_ofs;
     uint8_t buffer[256]; // max for normal message set is 167+8
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return 1;
+    }
+
 private:
     AP_HAL::UARTDriver *uart;
     int8_t port_num;

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h

@@ -49,6 +49,12 @@ class AP_ExternalAHRS_MicroStrain5: public AP_ExternalAHRS_backend, public AP_Mi
         build_packet();
     };
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return 1;
+    }
+
 private:
 
     uint32_t baudrate;

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp

@@ -18,6 +18,7 @@
     param set AHRS_EKF_TYPE 11
     param set EAHRS_TYPE 7
     param set GPS1_TYPE 21
+    param set GPS2_TYPE 21
     param set SERIAL3_BAUD 115
     param set SERIAL3_PROTOCOL 36
   UDEV rules for repeatable USB connection:
@@ -269,28 +270,28 @@ bool AP_ExternalAHRS_MicroStrain7::initialised(void) const
 bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
 {
     if (!initialised()) {
-        hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "not initialised");
+        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "not initialised");
         return false;
     }
     if (!times_healthy()) {
-        hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "data is stale");
+        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "data is stale");
         return false;
     }
     if (!filter_healthy()) {
-        hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "filter is unhealthy");
+        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter is unhealthy");
         return false;
     }
     if (!healthy()) {
-        hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "unhealthy");
+        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "unhealthy");
         return false;
     }
     static_assert(NUM_GNSS_INSTANCES == 2, "This check only works if there are two GPS types.");
     if (gnss_data[0].fix_type < GPS_FIX_TYPE_3D_FIX && gnss_data[1].fix_type < GPS_FIX_TYPE_3D_FIX) {
-        hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "missing 3D GPS fix on either GPS");
+        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "missing 3D GPS fix on either GPS");
         return false;
     }
     if (!filter_state_healthy(FilterState(filter_status.state))) {
-        hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "filter not healthy");
+        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter not healthy");
         return false;
     }
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h

@@ -50,6 +50,13 @@ class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_Mi
         build_packet();
     };
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override
+    {
+        return AP_MicroStrain::NUM_GNSS_INSTANCES;
+    }
+
 private:
 
     // GQ7 Filter States

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h

@@ -47,6 +47,11 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
     // Get model/type name
     const char* get_name() const override;
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return 1;
+    }
 private:
     AP_HAL::UARTDriver *uart;
     int8_t port_num;

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h

@@ -48,6 +48,9 @@ class AP_ExternalAHRS_backend {
     // This can also copy interim state protected by locking.
     virtual void update() = 0;
 
+    // Return the number of GPS sensors sharing data to AP_GPS.
+    virtual uint8_t num_gps_sensors(void) const = 0;
+
 protected:
     AP_ExternalAHRS::state_t &state;
     uint16_t get_rate(void) const;