/* run detection step for one GPS instance. If this finds a GPS then it will fill in drivers[instance] and change state[instance].status from NO_GPS to NO_FIX. */ void AP_GPS::detect_instance(uint8_t instance) { AP_GPS_Backend *new_gps = NULL; struct detect_state *dstate = &detect_state[instance]; uint32_t now = AP_HAL::millis(); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 if (_type[instance] == GPS_TYPE_PX4) { // 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 "); new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]); goto found_gps; } #endif #if CONFIG_HAL_BOARD == HAL_BOARD_QURT if (_type[instance] == GPS_TYPE_QURT) { hal.console->print(" QURTGPS "); new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]); goto found_gps; } #endif if (_port[instance] == NULL) { // UART not available return; } state[instance].instance = instance; state[instance].status = NO_GPS; state[instance].hdop = 9999; // by default the sbf/trimble gps outputs no data on its port, until configured. if (_type[instance] == GPS_TYPE_SBF) { hal.console->print(" SBF "); new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); } else if ((_type[instance] == GPS_TYPE_GSOF)) { hal.console->print(" GSOF "); new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); } // record the time when we started detection. This is used to try // to avoid initialising a uBlox as a NMEA GPS if (dstate->detect_started_ms == 0) { dstate->detect_started_ms = now; } if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { // try the next baud rate dstate->last_baud++; if (dstate->last_baud == ARRAY_SIZE(_baudrates)) { dstate->last_baud = 0; } uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]); _port[instance]->begin(baudrate); _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; #if UBLOX_RXM_RAW_LOGGING if(_raw_data != 0) send_blob_start(instance, _initialisation_raw_blob, sizeof(_initialisation_raw_blob)); else #endif send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } send_blob_update(instance); while (initblob_state[instance].remaining == 0 && _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 window for 5Hz fixes. The NMEA startup message should force the uBlox into 38400 no matter what rate it is configured for. */ if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { hal.console->print(" ublox "); 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 "); 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 "); 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 "); 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 "); 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)) { // prevent false detection of NMEA mode in // a MTK or UBLOX which has booted in NMEA mode if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { hal.console->print(" NMEA "); new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); } } } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_QURT found_gps: #endif if (new_gps != NULL) { state[instance].status = NO_FIX; drivers[instance] = new_gps; timing[instance].last_message_time_ms = now; } }
/* run detection step for one GPS instance. If this finds a GPS then it will fill in drivers[instance] and change state[instance].status from NO_GPS to NO_FIX. */ 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]; if (port == NULL) { // UART not available return; } uint32_t now = hal.scheduler->millis(); state[instance].instance = instance; state[instance].status = NO_GPS; // record the time when we started detection. This is used to try // to avoid initialising a uBlox as a NMEA GPS if (dstate->detect_started_ms == 0) { dstate->detect_started_ms = now; } if (now - dstate->last_baud_change_ms > 1200) { // try the next baud rate dstate->last_baud++; if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) { dstate->last_baud = 0; } uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]); port->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(); /* running a uBlox at less than 38400 will lead to packet corruption, as we can't receive the packets in the 200ms window for 5Hz fixes. The NMEA startup message should force the uBlox into 38400 no matter what rate it is configured for. */ if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && 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); } 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); } 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); } #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); } #endif // HAL_CPU_CLASS #if !defined( __AVR_ATmega1280__ ) // 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_P(PSTR(" SIRF ")); new_gps = new AP_GPS_SIRF(*this, state[instance], port); } else if (now - dstate->detect_started_ms > 5000) { // prevent false detection of NMEA mode in // a MTK or UBLOX which has booted in NMEA mode 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); } } #endif } if (new_gps != NULL) { state[instance].status = NO_FIX; drivers[instance] = new_gps; timing[instance].last_message_time_ms = now; } }
/* run detection step for one GPS instance. If this finds a GPS then it will fill in drivers[instance] and change state[instance].status from NO_GPS to NO_FIX. */ void AP_GPS::detect_instance(uint8_t instance) { AP_GPS_Backend *new_gps = NULL; struct detect_state *dstate = &detect_state[instance]; uint32_t now = hal.scheduler->millis(); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 if (_type[instance] == GPS_TYPE_PX4) { // 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[instance]); goto found_gps; } #endif if (_port[instance] == NULL) { // UART not available return; } state[instance].instance = instance; state[instance].status = NO_GPS; // record the time when we started detection. This is used to try // to avoid initialising a uBlox as a NMEA GPS if (dstate->detect_started_ms == 0) { dstate->detect_started_ms = now; } if (now - dstate->last_baud_change_ms > 1200) { // try the next baud rate dstate->last_baud++; if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) { dstate->last_baud = 0; } uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]); _port[instance]->begin(baudrate); _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } send_blob_update(instance); 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 window for 5Hz fixes. The NMEA startup message should force the uBlox into 38400 no matter what rate it is configured for. */ if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && 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[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[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[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[instance]); } #endif // HAL_CPU_CLASS #if !defined(GPS_SKIP_SIRF_NMEA) // 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_P(PSTR(" SIRF ")); 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 // a MTK or UBLOX which has booted in NMEA mode 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[instance]); } } #endif } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 found_gps: #endif if (new_gps != NULL) { state[instance].status = NO_FIX; drivers[instance] = new_gps; timing[instance].last_message_time_ms = now; } }