// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file AP_GPS_Auto.cpp /// @brief Simple GPS auto-detection logic. #include #include #include "AP_GPS.h" // includes AP_GPS_Auto.h // Define this to add NMEA to the auto-detection cycle. // // Note that there is a potential race where NMEA data may overlap with // the commands that switch a GPS out of NMEA mode that can cause // the GPS to switch to binary mode at the same time that this code // detects it as being in NMEA mode. // //#define WITH_NMEA_MODE 1 static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U}; const prog_char AP_GPS_Auto::_mtk_set_binary[] PROGMEM = MTK_SET_BINARY; const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY; AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) : GPS(s), _fs(s), _gps(gps) { } // Do nothing at init time - it may be too early to try detecting the GPS // void AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting) { idleTimeout = 1200; if (callback == NULL) callback = delay; _nav_setting = nav_setting; } // Called the first time that a client tries to kick the GPS to update. // // We detect the real GPS, then update the pointer we have been called through // and return. // /// @todo This routine spends a long time trying to detect a GPS. That's not strictly /// desirable; it might be a good idea to rethink the logic here to make it /// more asynchronous, so that other parts of the system can get a chance /// to run while GPS detection is in progress. /// bool AP_GPS_Auto::read(void) { GPS *gps; uint8_t i; uint32_t then; // Loop through possible baudrates trying to detect a GPS at one of them. // // Note that we need to have a FastSerial rather than a Stream here because // Stream has no idea of line speeds. FastSerial is quite OK with us calling // ::begin any number of times. // for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { // ensure the serial port has a large enough buffer for any protocol _fs->begin(baudrates[i], 256, 16); if (NULL != (gps = _detect())) { // configure the detected GPS and give it a chance to listen to its device gps->init(_nav_setting); then = millis(); while ((millis() - then) < 1200) { // if we get a successful update from the GPS, we are done gps->new_data = false; gps->update(); if (gps->new_data) { Serial.println_P(PSTR("OK")); *_gps = gps; return true; } } // GPS driver failed to parse any data from GPS, // delete the driver and continue the process. Serial.println_P(PSTR("failed, retrying")); delete gps; } } return false; } // // Perform one iteration of the auto-detection process. // GPS * AP_GPS_Auto::_detect(void) { uint32_t then; uint8_t fingerprint[4]; uint8_t tries; uint16_t charcount; GPS *gps; // // Loop attempting to detect a recognized GPS // Serial.print('G'); gps = NULL; for (tries = 0; tries < 2; tries++) { // // Empty the serial buffer and wait for 50ms of quiet. // // XXX We can detect babble by counting incoming characters, but // what would we do about it? // charcount = 0; _port->flush(); then = millis(); do { if (_port->available()) { then = millis(); _port->read(); charcount++; } } while ((millis() - then) < 50 && charcount < 5000); if (tries == 0) { // write configuration strings to put the GPS into the preferred // mode _write_progstr_block(_fs, _mtk_set_binary, sizeof(_mtk_set_binary)); _write_progstr_block(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size); _write_progstr_block(_fs, _sirf_set_binary, sizeof(_sirf_set_binary)); // ensure its all been written while (_fs->tx_pending()) { callback(10); } // give the GPS time to react to the settings callback(100); } // // Collect four characters to fingerprint a device // // If we take more than 1200ms to receive four characters, abort. // This will normally only be the case where there is no GPS attached. // while (_port->available() < 4) { callback(1); if ((millis() - then) > 1200) { Serial.print('!'); return NULL; } } fingerprint[0] = _port->read(); fingerprint[1] = _port->read(); fingerprint[2] = _port->read(); fingerprint[3] = _port->read(); // // ublox or MTK in DIYD binary mode (whose smart idea was // it to make the MTK look sort-of like it was talking UBX?) // if ((0xb5 == fingerprint[0]) && (0x62 == fingerprint[1]) && (0x01 == fingerprint[2])) { // message 5 is MTK pretending to talk UBX if (0x05 == fingerprint[3]) { gps = new AP_GPS_MTK(_port); Serial.print_P(PSTR(" MTK1.4 ")); break; } // any other message is ublox gps = new AP_GPS_UBLOX(_port); Serial.print_P(PSTR(" ublox ")); break; } // new style 3DR UBlox (April 2012)x if (0xb5 == fingerprint[0] && 0x62 == fingerprint[1] && 0x0d == fingerprint[2] && 0x01 == fingerprint[3]) { // new style Ublox gps = new AP_GPS_UBLOX(_port); Serial.print_P(PSTR(" ublox ")); break; } // // MTK v1.6 // if ((0xd0 == fingerprint[0]) && (0xdd == fingerprint[1]) && (0x20 == fingerprint[2])) { gps = new AP_GPS_MTK16(_port); Serial.print_P(PSTR(" MTK1.6 ")); break; } // // SIRF in binary mode // if ((0xa0 == fingerprint[0]) && (0xa2 == fingerprint[1])) { gps = new AP_GPS_SIRF(_port); Serial.print_P(PSTR(" SiRF ")); break; } #if WITH_NMEA_MODE // // Something talking NMEA // if (('$' == fingerprint[0]) && (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) { // XXX this may be a bit presumptive, might want to give the GPS a couple of // iterations around the loop to react to init strings? gps = new AP_GPS_NMEA(_port); break; } #endif Serial.printf("?"); } return(gps); }