diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6caafd467e..933d4326ed 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -83,14 +83,19 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = { }; /// Startup initialisation. -void AP_GPS::init(DataFlash_Class *dataflash) +void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager) { _DataFlash = dataflash; - hal.uartB->begin(38400UL, 256, 16); primary_instance = 0; + + // search for serial ports with gps protocol + AP_SerialManager::serial_state gps_serial; + if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, gps_serial)) { + _port[0] = gps_serial.uart; + } #if GPS_MAX_INSTANCES > 1 - if (hal.uartE != NULL) { - hal.uartE->begin(38400UL, 256, 16); + if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS2, gps_serial)) { + _port[1] = gps_serial.uart; } #endif } @@ -118,15 +123,19 @@ void AP_GPS::send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t */ void AP_GPS::send_blob_update(uint8_t instance) { + // exit immediately if no uart for this instance + if (_port[instance] == NULL) { + return; + } + // see if we can write some more of the initialisation blob if (initblob_state[instance].remaining > 0) { - AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE; - int16_t space = port->txspace(); + int16_t space = _port[instance]->txspace(); if (space > (int16_t)initblob_state[instance].remaining) { space = initblob_state[instance].remaining; } while (space > 0) { - port->write(pgm_read_byte(initblob_state[instance].blob)); + _port[instance]->write(pgm_read_byte(initblob_state[instance].blob)); initblob_state[instance].blob++; space--; initblob_state[instance].remaining--; @@ -143,7 +152,6 @@ void AP_GPS::detect_instance(uint8_t instance) { AP_GPS_Backend *new_gps = NULL; - AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE; struct detect_state *dstate = &detect_state[instance]; uint32_t now = hal.scheduler->millis(); @@ -152,12 +160,12 @@ 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_P(PSTR(" PX4 ")); - new_gps = new AP_GPS_PX4(*this, state[instance], port); + new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]); goto found_gps; } #endif - if (port == NULL) { + if (_port[instance] == NULL) { // UART not available return; } @@ -178,15 +186,15 @@ AP_GPS::detect_instance(uint8_t instance) dstate->last_baud = 0; } uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]); - port->begin(baudrate, 256, 16); + _port[instance]->begin(baudrate, 256, 16); dstate->last_baud_change_ms = now; send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } send_blob_update(instance); - while (port->available() > 0 && new_gps == NULL) { - uint8_t data = port->read(); + while (_port[instance]->available() > 0 && new_gps == NULL) { + uint8_t data = _port[instance]->read(); /* running a uBlox at less than 38400 will lead to packet corruption, as we can't receive the packets in the 200ms @@ -198,23 +206,23 @@ AP_GPS::detect_instance(uint8_t instance) pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { hal.console->print_P(PSTR(" ublox ")); - new_gps = new AP_GPS_UBLOX(*this, state[instance], port); + 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_P(PSTR(" MTK19 ")); - new_gps = new AP_GPS_MTK19(*this, state[instance], port); + 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_P(PSTR(" MTK ")); - new_gps = new AP_GPS_MTK(*this, state[instance], port); + new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]); } #if GPS_RTK_AVAILABLE else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { hal.console->print_P(PSTR(" SBP ")); - new_gps = new AP_GPS_SBP(*this, state[instance], port); + new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); } #endif // HAL_CPU_CLASS #if !defined(GPS_SKIP_SIRF_NMEA) @@ -222,7 +230,7 @@ AP_GPS::detect_instance(uint8_t instance) else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { hal.console->print_P(PSTR(" SIRF ")); - new_gps = new AP_GPS_SIRF(*this, state[instance], port); + new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); } else if (now - dstate->detect_started_ms > 5000) { // prevent false detection of NMEA mode in @@ -230,7 +238,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_P(PSTR(" NMEA ")); - new_gps = new AP_GPS_NMEA(*this, state[instance], port); + new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); } } #endif diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 25ecc2b839..d7d26d5514 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -26,6 +26,7 @@ #include #include #include "GPS_detect_state.h" +#include /** maximum number of GPS instances available on this platform. If more @@ -66,7 +67,7 @@ public: } /// Startup initialisation. - void init(DataFlash_Class *dataflash); + void init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager); /// Update GPS state based on possible bytes received from the module. /// This routine must be called periodically (typically at 10Hz or @@ -370,6 +371,7 @@ private: GPS_timing timing[GPS_MAX_INSTANCES]; GPS_State state[GPS_MAX_INSTANCES]; AP_GPS_Backend *drivers[GPS_MAX_INSTANCES]; + AP_HAL::UARTDriver *_port[GPS_MAX_INSTANCES]; /// primary GPS instance uint8_t primary_instance:2;