From 8a35079907c8dbcc3dfbbe9a094e6fef7a9af697 Mon Sep 17 00:00:00 2001 From: "deweibel@gmail.com" Date: Tue, 21 Jun 2011 01:20:39 +0000 Subject: [PATCH] clean up idleTimeout git-svn-id: https://arducopter.googlecode.com/svn/trunk@2606 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS_406.cpp | 2 -- libraries/AP_GPS/AP_GPS_Auto.cpp | 1 - libraries/AP_GPS/AP_GPS_MTK.cpp | 2 -- libraries/AP_GPS/AP_GPS_MTK16.cpp | 1 - libraries/AP_GPS/AP_GPS_NMEA.cpp | 2 -- libraries/AP_GPS/AP_GPS_SIRF.cpp | 1 - libraries/AP_GPS/AP_GPS_UBLOX.cpp | 1 - libraries/AP_GPS/GPS.cpp | 2 +- libraries/AP_GPS/GPS.h | 21 +++++++++++++-------- 9 files changed, 14 insertions(+), 19 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_406.cpp b/libraries/AP_GPS/AP_GPS_406.cpp index 5e9cb9f675..379d096cdd 100644 --- a/libraries/AP_GPS/AP_GPS_406.cpp +++ b/libraries/AP_GPS/AP_GPS_406.cpp @@ -27,8 +27,6 @@ void AP_GPS_406::init(void) _configure_gps(); // Function to configure GPS, to output only the desired msg's AP_GPS_SIRF::init(); // let the superclass do anything it might need here - - idleTimeout = 1200; } // Private Methods ////////////////////////////////////////////////////////////// diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index b411fe408e..89c194983e 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -36,7 +36,6 @@ AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) : void AP_GPS_Auto::init(void) { - idleTimeout = 1200; } // Called the first time that a client tries to kick the GPS to update. diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 6375f61ab6..3f1a5cec46 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -33,8 +33,6 @@ AP_GPS_MTK::init(void) // set initial epoch code _epoch = TIME_OF_DAY; - - idleTimeout = 1200; } // Process bytes available from the stream diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index bf7180f112..47a8b86beb 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -36,7 +36,6 @@ AP_GPS_MTK16::init(void) _epoch = TIME_OF_DAY; _time_offset = 0; _offset_calculated = false; - idleTimeout = 1200; } // Process bytes available from the stream diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 9748d20743..119ead871c 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -116,8 +116,6 @@ void AP_GPS_NMEA::init(void) // send the ublox init strings bs->print_P((const prog_char_t *)_ublox_init_string); - - idleTimeout = 1200; } bool AP_GPS_NMEA::read(void) diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index 4b247153ca..65fbb25c41 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -40,7 +40,6 @@ AP_GPS_SIRF::init(void) // send SiRF binary setup messages _port->write(init_messages, sizeof(init_messages)); - idleTimeout = 1200; } // Process bytes available from the stream diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 74339f87de..ae1361ddd6 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -29,7 +29,6 @@ AP_GPS_UBLOX::init(void) _port->flush(); _epoch = TIME_OF_WEEK; - idleTimeout = 1200; } // Process bytes available from the stream diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 57502643da..bc1f702211 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -13,7 +13,7 @@ GPS::update(void) // if we did not get a message, and the idle timer has expired, re-init if (!result) { - if ((millis() - _idleTimer) > idleTimeout) { + if ((millis() - _idleTimer) > _idleTimeout) { _status = NO_GPS; init(); diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 02d5632b5f..80dab7fe47 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -85,6 +85,10 @@ public: /// already seen. bool new_data; + /// Update the idle timeout value + /// + void setIdleTimeout(unsigned long ms) { _idleTimeout = ms; } + // Deprecated properties bool fix; ///< true if we have a position fix (use ::status instead) bool valid_read; ///< true if we have seen data from the GPS (use ::status instead) @@ -96,14 +100,6 @@ public: virtual void setHIL(long time, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); - /// Time in milliseconds after which we will assume the GPS is no longer - /// sending us updates and attempt a re-init. - /// - /// 1200ms allows a small amount of slack over the worst-case 1Hz update - /// rate. - /// - unsigned long idleTimeout; - protected: Stream *_port; ///< port the GPS is attached to @@ -160,6 +156,15 @@ private: /// Last time that the GPS driver got a good packet from the GPS /// unsigned long _idleTimer; + + /// Time in milliseconds after which we will assume the GPS is no longer + /// sending us updates and attempt a re-init. + /// + /// 1200ms allows a small amount of slack over the worst-case 1Hz update + /// rate. + /// + static unsigned long _idleTimeout; + /// Our current status GPS_Status _status;