From 1426ff27325b6a892c2b3900b6147130c2196b5a Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 9 Feb 2016 13:59:12 -0700 Subject: [PATCH] AP_GPS: Broadcast what type of GPS is found and at what baud rate --- libraries/AP_GPS/AP_GPS.cpp | 33 ++++++++++++++++++++++++++++++++- libraries/AP_GPS/AP_GPS.h | 1 + 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 186473f50e..eccef4c609 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -19,6 +19,7 @@ #include #include #include +#include extern const AP_HAL::HAL &hal; @@ -200,6 +201,7 @@ AP_GPS::detect_instance(uint8_t instance) // check for explicitely chosen PX4 GPS beforehand // it is not possible to autodetect it, nor does it require a real UART hal.console->print(" PX4 "); + _broadcast_gps_type("PX4", instance, -1); // baud rate isn't valid new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]); goto found_gps; } @@ -208,6 +210,7 @@ AP_GPS::detect_instance(uint8_t instance) #if CONFIG_HAL_BOARD == HAL_BOARD_QURT if (_type[instance] == GPS_TYPE_QURT) { hal.console->print(" QURTGPS "); + _broadcast_gps_type("QURTGPS", instance, -1); // baud rate isn't valid new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]); goto found_gps; } @@ -225,9 +228,11 @@ AP_GPS::detect_instance(uint8_t instance) // by default the sbf/trimble gps outputs no data on its port, until configured. if (_type[instance] == GPS_TYPE_SBF) { hal.console->print(" SBF "); + _broadcast_gps_type("SBF", instance, -1); // baud rate isn't valid new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); } else if ((_type[instance] == GPS_TYPE_GSOF)) { hal.console->print(" GSOF "); + _broadcast_gps_type("GSOF", instance, -1); // baud rate isn't valid new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); } @@ -271,27 +276,32 @@ AP_GPS::detect_instance(uint8_t instance) _baudrates[dstate->last_baud] >= 38400 && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { hal.console->print(" ublox "); + _broadcast_gps_type("u-blox", instance, dstate->last_baud); new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]); } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) && AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) { hal.console->print(" MTK19 "); + _broadcast_gps_type("MTK19", instance, dstate->last_baud); new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]); } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) && AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) { hal.console->print(" MTK "); + _broadcast_gps_type("MTK", instance, dstate->last_baud); new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]); } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { hal.console->print(" SBP "); + _broadcast_gps_type("SBP", instance, dstate->last_baud); new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); } // save a bit of code space on a 1280 else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { hal.console->print(" SIRF "); + _broadcast_gps_type("SIRF", instance, dstate->last_baud); new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); } else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) { @@ -300,6 +310,7 @@ AP_GPS::detect_instance(uint8_t instance) if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { hal.console->print(" NMEA "); + _broadcast_gps_type("NMEA", instance, dstate->last_baud); new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); } } @@ -593,7 +604,8 @@ AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan) } uint8_t -AP_GPS::first_unconfigured_gps(void) const { +AP_GPS::first_unconfigured_gps(void) const +{ for(int i = 0; i < GPS_MAX_INSTANCES; i++) { if(_type[i] != GPS_TYPE_NONE && (drivers[i] == NULL || !drivers[i]->is_configured())) { return i; @@ -601,3 +613,22 @@ AP_GPS::first_unconfigured_gps(void) const { } return GPS_ALL_CONFIGURED; } + +void +AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index) +{ + char buffer[64]; + if (baud_index >= 0) { + hal.util->snprintf(buffer, sizeof(buffer), + "GPS %d: detected as %s at %d baud", + instance, + type, + _baudrates[baud_index]); + } else { + hal.util->snprintf(buffer, sizeof(buffer), + "GPS %d: detected as %s", + instance, + type); + } + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer); +} diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index b36025436e..b427c2a335 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -392,6 +392,7 @@ private: void detect_instance(uint8_t instance); void update_instance(uint8_t instance); + void _broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index); }; #define GPS_BAUD_TIME_MS 1200