diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 315cc17b22765..7900bab8256e9 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -29,6 +29,7 @@ #include #include +#include #include 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; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index bee7d5c28e0cc..9de2c9526e931 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/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; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h index e6b2b9e6bdcb1..7d5c199eb1365 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h +++ b/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; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 901fff1c84cf5..6a576ab52cab2 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/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; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h index 5cff5af56a6dc..f278289d15fb4 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h +++ b/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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 3ce1fe7f5e2d5..c0e54c88bf2a4 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/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; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index ed987a01ced3a..e5a22a87cbe44 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/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;