From aa115aea688ba9cc65b5bbacba1ce2c56512dd23 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sun, 19 Dec 2010 13:24:29 +0000 Subject: [PATCH] AP_GPS libs now recover from disconnect or failed initialization. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1188 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS_Auto.cpp | 2 +- libraries/AP_GPS/AP_GPS_Auto.h | 2 +- libraries/AP_GPS/AP_GPS_HIL.cpp | 2 +- libraries/AP_GPS/AP_GPS_HIL.h | 2 +- libraries/AP_GPS/AP_GPS_MTK.cpp | 3 ++- libraries/AP_GPS/AP_GPS_MTK.h | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 4 ++-- libraries/AP_GPS/AP_GPS_NMEA.h | 2 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 +- libraries/AP_GPS/AP_GPS_SIRF.h | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.h | 2 +- libraries/AP_GPS/GPS.cpp | 15 +++++++++++++++ libraries/AP_GPS/GPS.h | 5 ++++- 14 files changed, 33 insertions(+), 14 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index 727b1eac74..cd25ddd8af 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -21,7 +21,7 @@ AP_GPS_Auto::init(void) // We detect the real GPS, then update the pointer we have been called through // and return. void -AP_GPS_Auto::update(void) +AP_GPS_Auto::read(void) { GPS *gps; int i; diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index a259add7c0..910f87ed54 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -31,7 +31,7 @@ public: /// Detect and initialise the attached GPS unit. Returns a /// pointer to the allocated & initialised GPS driver. /// - void update(void); + void read(void); private: /// Serial port connected to the GPS. diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 7e153c86e7..f9da9cc628 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -24,7 +24,7 @@ void AP_GPS_HIL::init(void) { } -void AP_GPS_HIL::update(void) +void AP_GPS_HIL::read(void) { } diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index fd25cd06c2..8092366f79 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -18,7 +18,7 @@ class AP_GPS_HIL : public GPS { public: AP_GPS_HIL(Stream *s); void init(void); - void update(void); + void read(void); int status(void); /** * Hardware in the loop set function diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index db281390c0..2ff2247a35 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -22,6 +22,7 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_MTK::init(void) { + delay(1000); _port->flush(); // initialize serial port for binary protocol use // XXX should assume binary, let GPS_AUTO handle dynamic config? @@ -42,7 +43,7 @@ void AP_GPS_MTK::init(void) // The lack of a standard header length field makes it impossible to skip // unrecognised messages. // -void AP_GPS_MTK::update(void) +void AP_GPS_MTK::read(void) { byte data; int numc; diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index a69dca60df..2fd718ce21 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -37,7 +37,7 @@ class AP_GPS_MTK : public GPS { public: AP_GPS_MTK(Stream *s); virtual void init(void); - virtual void update(void); + virtual void read(void); private: #pragma pack(1) diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 0390552c0b..88e9cd2713 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -18,7 +18,7 @@ Methods: init() : GPS Initialization - update() : Call this funcion as often as you want to ensure you read the incomming gps data + read() : Call this funcion as often as you want to ensure you read the incomming gps data Properties: latitude : latitude * 10000000 (long value) @@ -63,7 +63,7 @@ AP_GPS_NMEA::init(void) // We can call this function on the main loop (50Hz loop) // If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. void -AP_GPS_NMEA::update(void) +AP_GPS_NMEA::read(void) { char c; int numc; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index b7b46bee15..b05e2734b5 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -37,7 +37,7 @@ class AP_GPS_NMEA : public GPS // Methods AP_GPS_NMEA(Stream *s); void init(); - void update(); + void read(); // Properties uint8_t quality; // GPS Signal quality diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index f1fe5e9f5c..eb46daa15d 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -50,7 +50,7 @@ void AP_GPS_SIRF::init(void) // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // -void AP_GPS_SIRF::update(void) +void AP_GPS_SIRF::read(void) { byte data; int numc; diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index ce3ff454be..0903b3bd38 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -20,7 +20,7 @@ public: AP_GPS_SIRF(Stream *s); virtual void init(); - virtual void update(); + virtual void read(); private: #pragma pack(1) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index cd2541d118..996b97c2f5 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -37,7 +37,7 @@ void AP_GPS_UBLOX::init(void) // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // -void AP_GPS_UBLOX::update(void) +void AP_GPS_UBLOX::read(void) { byte data; int numc; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index ce905a85e8..798011fb00 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -21,7 +21,7 @@ public: // Methods AP_GPS_UBLOX(Stream *s = NULL); void init(void); - void update(); + void read(); private: // u-blox UBX protocol essentials diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index f10564e6e7..f7a59ec6bf 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -20,3 +20,18 @@ GPS::_setTime(void) { _lastTime = millis(); } + +void +GPS::update(void) +{ + if (!status()) + { + Serial.println("Reinitializing GPS"); + init(); + _setTime(); + } + else + { + read(); + } +} diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 8ef306e41f..695012e6f3 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -44,7 +44,10 @@ public: /// /// Must be implemented by the GPS driver. /// - virtual void update(void) = 0; + void update(void); + + /// Implement specific routines for gps to receive a message. + virtual void read(void) = 0; /// Query GPS status ///