AP_ExternalAHRS: Add pre-arm for misconfigured EAHRS_SENSORS and GPS_TYPE

* This catches when there's a mismatch of GPSx_TYPE and EAHRS_SENSORS
  when GPS is enabled
* Before this pre-arm, failure to set GPS_TYPE2 to 21 (ExternalAHRS)
  resulted in silent rejection of the data in AP_GPS because the default
is off
* And fix a little logging bug

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

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 @@ public:
     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 @@ public:
         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 @@ public:
         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 @@ public:
     // 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 @@ public:
     // 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;