diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index ccbaf93528..384d7daccc 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -10,4 +10,4 @@ #include "AP_GPS_MTK.h" #include "AP_GPS_IMU.h" #include "AP_GPS_None.h" - +#include "AP_GPS_Auto.h" diff --git a/libraries/AP_GPS/AP_GPS_406.h b/libraries/AP_GPS/AP_GPS_406.h index c69ed2180b..39b6e55164 100644 --- a/libraries/AP_GPS/AP_GPS_406.h +++ b/libraries/AP_GPS/AP_GPS_406.h @@ -21,7 +21,7 @@ class AP_GPS_406 : public AP_GPS_SIRF public: // Methods AP_GPS_406(Stream *port); - void init(); + void init(void); private: void _change_to_sirf_protocol(void); diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp new file mode 100644 index 0000000000..63ee4cc040 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -0,0 +1,147 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Auto-detecting pseudo-GPS driver +// + +#include "AP_GPS.h" +#include +#include +#include + +static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U}; + +GPS * +AP_GPS_Auto::detect(void) +{ + GPS *gps; + int i; + + // loop trying to find a GPS + for (;;) { + // loop through possible baudrates + for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { + printf("autodetect at %d:%u\n", i, baudrates[i]); + _port->begin(baudrates[i]); + if (NULL != (gps = _detect())) + return(gps); + } + } +} + +// +// Perform one iteration of the auto-detection process. +// +GPS * +AP_GPS_Auto::_detect(void) +{ + unsigned long then; + int fingerprint[4]; + int tries; + GPS *gps; + + // + // Loop attempting to detect a recognised GPS + // + 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? + // + _port->flush(); + then = millis(); + do { + if (_port->available()) { + then = millis(); + _port->read(); + } + } while ((millis() - then) < 50); + + // + // Collect four characters to fingerprint a device + // + fingerprint[0] = _getc(); + fingerprint[1] = _getc(); + fingerprint[2] = _getc(); + fingerprint[3] = _getc(); + printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n", + fingerprint[0], + fingerprint[1], + fingerprint[2], + fingerprint[3]); + + // + // u-blox 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]) { + printf("detected MTK in binary mode\n"); + gps = new AP_GPS_MTK(_port); + break; + } + + // any other message is u-blox + printf("detected u-blox in binary mode\n"); + gps = new AP_GPS_UBLOX(_port); + break; + } + + // + // SIRF in binary mode + // + if ((0xa0 == fingerprint[0]) && + (0xa2 == fingerprint[1])) { + printf("detected SIRF in binary mode\n"); + gps = new AP_GPS_SIRF(_port); + break; + } + + // + // If we haven't spammed the various init strings, send them now + // and retry to avoid a false-positive on the NMEA detector. + // + if (0 == tries) { + printf("sending setup strings and trying again\n"); + _port->println(MTK_SET_BINARY); + _port->println(UBLOX_SET_BINARY); + _port->println(SIRF_SET_BINARY); + continue; + } + + // + // 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? + printf("detected NMEA\n"); + gps = new AP_GPS_NMEA(_port); + break; + } + } + + // If we detected a GPS, call its init routine + if (NULL != gps) { + gps->print_errors = true; // XXX + gps->init(); + } + return(gps); +} + +int +AP_GPS_Auto::_getc(void) +{ + while (0 == _port->available()) + ; + return(_port->read()); +} diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h new file mode 100644 index 0000000000..72b42cbdd9 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -0,0 +1,43 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Auto-detecting pseudo-GPS driver +// + +#ifndef AP_GPS_Auto_h +#define AP_GPS_Auto_h + +#include +#include + +class AP_GPS_Auto +{ +public: + /// Constructor + /// + /// @note The stream is expected to be set up and configured for the + /// correct bitrate before ::init is called. + /// + /// @param port Stream connected to the GPS module. + /// @param ptr Pointer to a GPS * that will be fixed up by ::init + /// when the GPS type has been detected. + /// + AP_GPS_Auto(FastSerial *port = NULL) : _port(port) {}; + + /// Detect and initialise the attached GPS unit. Returns a + /// pointer to the allocated & initialised GPS driver. + /// + GPS *detect(void); + +private: + /// Serial port connected to the GPS. + FastSerial *_port; + + /// low-level auto-detect routine + /// + GPS *_detect(void); + + /// fetch a character from the port + /// + int _getc(void); +}; +#endif diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index aa1eebc225..dbe96f6352 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -22,8 +22,12 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_MTK::init(void) { + _port->flush(); // initialize serial port for binary protocol use + // XXX should assume binary, let GPS_AUTO handle dynamic config? _port->print(MTK_SET_BINARY); + + // set 4Hz update rate _port->print(MTK_OUTPUT_4HZ); } diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 5f032b62a6..a69dca60df 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -36,8 +36,8 @@ class AP_GPS_MTK : public GPS { public: AP_GPS_MTK(Stream *s); - void init(); - void update(); + virtual void init(void); + virtual void update(void); private: #pragma pack(1) diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h index f3a3d9b11a..23f242b59c 100644 --- a/libraries/AP_GPS/AP_GPS_None.h +++ b/libraries/AP_GPS/AP_GPS_None.h @@ -7,5 +7,7 @@ class AP_GPS_None : public GPS { + virtual void init(void) {}; + virtual void update(void) {}; }; #endif diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index 20280e92c4..f1fe5e9f5c 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -31,9 +31,13 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s) // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_SIRF::init(void) { - // For modules that default to something other than SiRF binary, - // the module-specific subclass should take care of switching to binary mode. + _port->flush(); + // For modules that default to something other than SiRF binary, + // the module-specific subclass should take care of switching to binary mode + // before calling us. + + // send SiRF binary setup messages _port->write(init_messages, sizeof(init_messages)); } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index c1ddeee5a9..ce3ff454be 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -13,6 +13,8 @@ #include +#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C" + class AP_GPS_SIRF : public GPS { public: AP_GPS_SIRF(Stream *s); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b49d01c376..cd2541d118 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -24,6 +24,8 @@ void AP_GPS_UBLOX::init(void) { // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the // right reporting configuration. + + _port->flush(); } // Process bytes available from the stream diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index dfbce36ca7..ce905a85e8 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -13,12 +13,14 @@ #include +#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26" + class AP_GPS_UBLOX : public GPS { public: // Methods - AP_GPS_UBLOX(Stream *s); - void init(); + AP_GPS_UBLOX(Stream *s = NULL); + void init(void); void update(); private: diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 011e40d77b..99f3ad2b86 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -4,16 +4,6 @@ #include "WProgram.h" #include -void -GPS::init(void) -{ -} - -void -GPS::update(void) -{ -} - void GPS::_setTime(void){ _lastTime = millis(); diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 8f48671b88..ec81b977ec 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -20,22 +20,27 @@ public: /// @note The stream is expected to be set up and configured for the /// correct bitrate before ::init is called. /// - /// @param port Stream connected to the GPS module. + /// @param s Stream connected to the GPS module. If NULL, assumed + /// to be set up at ::init time. Support for setting + /// the port in the ctor for backwards compatibility. /// - GPS(Stream *port = NULL) : _port(port) {}; + GPS(Stream *s = NULL) : _port(s) {}; /// Startup initialisation. /// /// This routine performs any one-off initialisation required to set the /// GPS up for use. /// - virtual void init(); + /// @param s Stream connected to the GPS module. If NULL, assumed to + /// have been set up at constructor time. + /// + virtual void init(void) = 0; /// Update GPS state based on possible bytes received from the module. /// /// This routine must be called periodically to process incoming data. /// - virtual void update(); + virtual void update(void) = 0; // Properties long time; ///< GPS time in milliseconds from the start of the week diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde new file mode 100644 index 0000000000..2491d4697a --- /dev/null +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -0,0 +1,52 @@ +// +// Test for AP_GPS_AUTO +// + +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +GPS *gps; +AP_GPS_Auto GPS(&Serial1); + +#define T6 1000000 +#define T7 10000000 + +void setup() +{ + Serial.begin(38400); + Serial1.begin(38400); + + Serial.println("GPS AUTO library test"); + gps = GPS.detect(); // GPS Initialization + delay(1000); +} +void loop() +{ + delay(20); + gps->update(); + if (gps->new_data){ + Serial.print("gps:"); + Serial.print(" Lat:"); + Serial.print((float)gps->latitude / T7, DEC); + Serial.print(" Lon:"); + Serial.print((float)gps->longitude / T7, DEC); + Serial.print(" Alt:"); + Serial.print((float)gps->altitude / 100.0, DEC); + Serial.print(" GSP:"); + Serial.print(gps->ground_speed / 100.0); + Serial.print(" COG:"); + Serial.print(gps->ground_course / 100.0, DEC); + Serial.print(" SAT:"); + Serial.print(gps->num_sats, DEC); + Serial.print(" FIX:"); + Serial.print(gps->fix, DEC); + Serial.print(" TIM:"); + Serial.print(gps->time, DEC); + Serial.println(); + gps->new_data = 0; + } +} +