From 0a94520682c70d18a94b6f64d5c25987af63026c Mon Sep 17 00:00:00 2001 From: "DrZiplok@gmail.com" Date: Sun, 17 Oct 2010 06:06:04 +0000 Subject: [PATCH] Add support for automatically detecting the connected GPS and configuring accordingly. Note that this is currently disabled as changes need to be made to APM to support it. Tested with MTK, u-blox and SiRF GPS'. git-svn-id: https://arducopter.googlecode.com/svn/trunk@671 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS.h | 2 +- libraries/AP_GPS/AP_GPS_406.h | 2 +- libraries/AP_GPS/AP_GPS_Auto.cpp | 147 ++++++++++++++++++ libraries/AP_GPS/AP_GPS_Auto.h | 43 +++++ libraries/AP_GPS/AP_GPS_MTK.cpp | 4 + libraries/AP_GPS/AP_GPS_MTK.h | 4 +- libraries/AP_GPS/AP_GPS_None.h | 2 + libraries/AP_GPS/AP_GPS_SIRF.cpp | 8 +- libraries/AP_GPS/AP_GPS_SIRF.h | 2 + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 + libraries/AP_GPS/AP_GPS_UBLOX.h | 6 +- libraries/AP_GPS/GPS.cpp | 10 -- libraries/AP_GPS/GPS.h | 13 +- .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 52 +++++++ 14 files changed, 275 insertions(+), 22 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_Auto.cpp create mode 100644 libraries/AP_GPS/AP_GPS_Auto.h create mode 100644 libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde 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; + } +} +