Go to the documentation of this file.00001
00002
00003
00004
00005
00006 #include "AP_GPS.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),
00014 _gps(gps)
00015 {
00016 }
00017
00018
00019
00020 void
00021 AP_GPS_Auto::init(void)
00022 {
00023 }
00024
00025
00026
00027
00028
00029
00030 bool
00031 AP_GPS_Auto::read(void)
00032 {
00033 GPS *gps;
00034 int i;
00035
00036
00037 for (;;) {
00038
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
00044 *_gps = gps;
00045
00046
00047 gps->print_errors = true;
00048 gps->init();
00049 gps->update();
00050
00051
00052
00053 return false;
00054 }
00055 }
00056 }
00057 }
00058
00059
00060
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
00072
00073 gps = NULL;
00074 for (tries = 0; tries < 2; tries++) {
00075
00076
00077
00078
00079
00080
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
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
00108
00109
00110 if ((0xb5 == fingerprint[0]) &&
00111 (0x62 == fingerprint[1]) &&
00112 (0x01 == fingerprint[2])) {
00113
00114
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
00122 Serial.printf("detected u-blox in binary mode\n");
00123 gps = new AP_GPS_UBLOX(_port);
00124 break;
00125 }
00126
00127
00128
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
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
00150
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
00162
00163 if (('$' == fingerprint[0]) &&
00164 (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
00165
00166
00167
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 }