diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b77549543c..5ce72a3c99 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -609,6 +609,16 @@ AP_GPS::first_unconfigured_gps(void) const return GPS_ALL_CONFIGURED; } +void +AP_GPS::broadcast_first_configuration_failure_reason(void) const { + uint8_t unconfigured = first_unconfigured_gps(); + if (drivers[unconfigured] == NULL) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1); + } else { + drivers[unconfigured]->broadcast_configuration_failure_reason(); + } +} + void AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 1e0a5048ea..dbcdf45395 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -344,6 +344,7 @@ public: // Returns the index of the first unconfigured GPS (returns GPS_ALL_CONFIGURED if all instances report as being configured) uint8_t first_unconfigured_gps(void) const; + void broadcast_first_configuration_failure_reason(void) const; private: struct GPS_timing { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index f3d9137aaa..314368a08d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1187,3 +1187,30 @@ AP_GPS_UBLOX::inject_data(uint8_t *data, uint8_t len) Debug("UBX: Not enough TXSPACE"); } } + + +static const char *reasons[] = {"navigation rate", + "posllh rate", + "status rate", + "solution rate", + "velned rate", + "dop rate", + "hw monitor rate", + "hw2 monitor rate", + "raw rate", + "version", + "navigation settings", + "GNSS settings", + "SBAS settings"}; + + +void +AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const { + for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) { + if (_unconfigured_messages & (1 << i)) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: u-blox %s configuration 0x%02x", + state.instance +1, reasons[i], _unconfigured_messages); + break; + } + } +} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index cd4a1ed19b..805c9b8db9 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -108,6 +108,7 @@ public: } } + void broadcast_configuration_failure_reason(void) const; private: // u-blox UBX protocol essentials struct PACKED ubx_header { diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index a6e019afc6..b230091b7c 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -49,6 +49,8 @@ public: virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan) { return ; } + virtual void broadcast_configuration_failure_reason(void) const { return ; } + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters)