• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

AP_GPS_Auto.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 //
00003 // Auto-detecting pseudo-GPS driver
00004 //
00005 
00006 #include "AP_GPS.h"             // includes AP_GPS_Auto.h
00007 #include <wiring.h>
00008 
00009 static unsigned int     baudrates[] = {38400U, 57600U, 9600U, 4800U};
00010 
00011 AP_GPS_Auto::AP_GPS_Auto(FastSerial *port, GPS **gps)  : 
00012         GPS(port),
00013         _FSport(port),  // do we need this, or can we cast _port up?
00014         _gps(gps)
00015 {
00016 }
00017 
00018 
00019 // Do nothing at init time - it may be too early to try detecting the GPS
00020 void
00021 AP_GPS_Auto::init(void)
00022 {
00023 }
00024 
00025 //
00026 // Called the first time that a client tries to kick the GPS to update.
00027 //
00028 // We detect the real GPS, then update the pointer we have been called through
00029 // and return.
00030 bool
00031 AP_GPS_Auto::read(void)
00032 {
00033         GPS             *gps;
00034         int             i;
00035 
00036         // loop trying to find a GPS
00037         for (;;) {
00038                 // loop through possible baudrates
00039                 for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
00040                         Serial.printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
00041                         _FSport->begin(baudrates[i]);
00042                         if (NULL != (gps = _detect())) {
00043                                 // make the detected GPS the default
00044                                 *_gps = gps;
00045                                 
00046                                 // configure the detected GPS and run one update
00047                                 gps->print_errors = true;       // XXX
00048                                 gps->init();
00049                                 gps->update();
00050 
00051                                 // Drop back to our caller - subsequent calls through
00052                                 // _gps will not come here.
00053                                 return false;
00054                         }
00055                 }
00056         }
00057 }
00058 
00059 //
00060 // Perform one iteration of the auto-detection process.
00061 //
00062 GPS *
00063 AP_GPS_Auto::_detect(void)
00064 {
00065         unsigned long then;
00066         int             fingerprint[4];
00067         int             tries;
00068         GPS             *gps;
00069 
00070         //
00071         // Loop attempting to detect a recognised GPS
00072         //
00073         gps = NULL;
00074         for (tries = 0; tries < 2; tries++) {
00075 
00076                 //
00077                 // Empty the serial buffer and wait for 50ms of quiet.
00078                 //
00079                 // XXX We can detect babble by counting incoming characters, but
00080                 //     what would we do about it?
00081                 //
00082                 Serial.println("draining and waiting");
00083                 _port->flush();
00084                 then = millis();
00085                 do {
00086                         if (_port->available()) {
00087                                 then = millis();
00088                                 _port->read();
00089                         }
00090                 } while ((millis() - then) < 50);
00091                         
00092                 //
00093                 // Collect four characters to fingerprint a device
00094                 //
00095                 Serial.println("collecting fingerprint");
00096                 fingerprint[0] = _getc();
00097                 fingerprint[1] = _getc();
00098                 fingerprint[2] = _getc();
00099                 fingerprint[3] = _getc();
00100                 Serial.printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
00101                            fingerprint[0],
00102                            fingerprint[1],
00103                            fingerprint[2],
00104                            fingerprint[3]);
00105 
00106                 //
00107                 // u-blox or MTK in DIYD binary mode (whose smart idea was
00108                 // it to make the MTK look sort-of like it was talking UBX?)
00109                 //
00110                 if ((0xb5 == fingerprint[0]) &&
00111                         (0x62 == fingerprint[1]) && 
00112                         (0x01 == fingerprint[2])) {
00113 
00114                         // message 5 is MTK pretending to talk UBX
00115                         if (0x05 == fingerprint[3]) {
00116                                 Serial.printf("detected MTK in binary mode\n");
00117                                 gps = new AP_GPS_MTK(_port);
00118                                 break;
00119                         }
00120 
00121                         // any other message is u-blox
00122                         Serial.printf("detected u-blox in binary mode\n");
00123                         gps = new AP_GPS_UBLOX(_port);
00124                         break;
00125                 } 
00126 
00127                 //
00128                 // MTK v1.6
00129                 //
00130                 if ((0xd0 == fingerprint[0]) &&
00131                         (0xdd == fingerprint[1]) && 
00132                         (0x20 == fingerprint[2])) {
00133                         Serial.printf("detected MTK v1.6\n");
00134                         gps = new AP_GPS_MTK16(_port);
00135                         break;
00136                 }
00137 
00138                 //
00139                 // SIRF in binary mode
00140                 //
00141                 if ((0xa0 == fingerprint[0]) &&
00142                         (0xa2 == fingerprint[1])) {
00143                         Serial.printf("detected SIRF in binary mode\n");
00144                         gps = new AP_GPS_SIRF(_port);
00145                         break;
00146                 }
00147 
00148                 //
00149                 // If we haven't spammed the various init strings, send them now
00150                 // and retry to avoid a false-positive on the NMEA detector.
00151                 //
00152                 if (0 == tries) {
00153                         Serial.printf("sending setup strings and trying again\n");
00154                         _port->println(MTK_SET_BINARY);
00155                         _port->println(UBLOX_SET_BINARY);
00156                         _port->println(SIRF_SET_BINARY);
00157                         continue;
00158                 }
00159 
00160                 //
00161                 // Something talking NMEA
00162                 //
00163                 if (('$' == fingerprint[0]) &&
00164                         (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
00165 
00166                         // XXX this may be a bit presumptive, might want to give the GPS a couple of
00167                         //     iterations around the loop to react to init strings?
00168                         Serial.printf("detected NMEA\n");
00169                         gps = new AP_GPS_NMEA(_port);
00170                         break;
00171                 }
00172         }
00173         return(gps);
00174 }
00175 
00176 int
00177 AP_GPS_Auto::_getc(void)
00178 {
00179         while (0 == _port->available())
00180                 ;
00181         return(_port->read());
00182 }

Generated for ArduPilot Libraries by doxygen