gps_ctrl: make GPSDO parsing more robust, add retries

1 files changed, 51 insertions, 32 deletions

diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp
index fb65549b5..342b0e2fe 100644
--- a/host/lib/usrp/gps_ctrl.cpp
+++ b/host/lib/usrp/gps_ctrl.cpp
@@ -26,6 +26,7 @@
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <boost/thread/thread.hpp>
 #include <boost/tokenizer.hpp>
+#include <boost/format.hpp>
 
 using namespace uhd;
 using namespace boost::gregorian;
@@ -77,13 +78,10 @@ public:
     case GPS_TYPE_JACKSON_LABS:
       UHD_MSG(status) << "Found a Jackson Labs GPS" << std::endl;
       init_firefly();
+      break;
 
     case GPS_TYPE_GENERIC_NMEA:
       if(gps_type == GPS_TYPE_GENERIC_NMEA) UHD_MSG(status) << "Found a generic NMEA GPS device" << std::endl;
-      if(get_time() == ptime()) {
-          UHD_MSG(status) << "No valid GPRMC packet found. Ignoring discovered GPS.";
-          gps_type = GPS_TYPE_NONE;
-      }
       break;
 
     case GPS_TYPE_NONE:
@@ -156,10 +154,9 @@ private:
         UHD_MSG(error) << "get_nmea(): unsupported GPS or no GPS detected";
         return std::string();
     }
-	//flush data to avoid reading outdated value (because NMEA string generated every second)
-	while (_recv() != ""){
-	  sleep(milliseconds(10));
-	}
+
+    while(_recv().size() != 0) sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
+
     int timeout = GPS_TIMEOUT_TRIES;
     while(timeout--) {
         reply = _recv();
@@ -167,7 +164,7 @@ private:
           return reply;
         boost::this_thread::sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
     }
-    UHD_MSG(error) << "get_nmea(): no " << msgtype << " message found";
+    throw uhd::value_error(str(boost::format("get_nmea(): no %s message found") % msgtype));
     return std::string();
   }
 
@@ -175,37 +172,49 @@ private:
   std::string get_token(std::string sentence, size_t offset) {
     boost::tokenizer<boost::escaped_list_separator<char> > tok(sentence);
     std::vector<std::string> toked;
-
-    tok.assign(sentence);
+    tok.assign(sentence); //this can throw
     toked.assign(tok.begin(), tok.end());
 
     if(toked.size() <= offset) {
-      UHD_MSG(error) << "get_token: too few tokens in reply " << sentence;
-      return std::string();
+        throw uhd::value_error(str(boost::format("Invalid response \"%s\"") % sentence));
     }
     return toked[offset];
   }
 
   ptime get_time(void) {
-    std::string reply = get_nmea("GPRMC");
+    int error_cnt = 0;
+    ptime gps_time;
+    while(error_cnt < 3) {
+        try {
+            std::string reply = get_nmea("GPRMC");
 
-    std::string datestr = get_token(reply, 9);
-    std::string timestr = get_token(reply, 1);
+            std::string datestr = get_token(reply, 9);
+            std::string timestr = get_token(reply, 1);
 
-    if(datestr.size() == 0 or timestr.size() == 0) {
-        return ptime();
+            if(datestr.size() == 0 or timestr.size() == 0) {
+                throw uhd::value_error(str(boost::format("Invalid response \"%s\"") % reply));
+            }
+            
+            //just trust me on this one
+            gps_time = ptime( date( 
+                             greg_year(boost::lexical_cast<int>(datestr.substr(4, 2)) + 2000),
+                             greg_month(boost::lexical_cast<int>(datestr.substr(2, 2))), 
+                             greg_day(boost::lexical_cast<int>(datestr.substr(0, 2))) 
+                           ),
+                          hours(  boost::lexical_cast<int>(timestr.substr(0, 2)))
+                        + minutes(boost::lexical_cast<int>(timestr.substr(2, 2)))
+                        + seconds(boost::lexical_cast<int>(timestr.substr(4, 2)))
+                     );
+            return gps_time;
+            
+        } catch(std::exception &e) {
+            UHD_MSG(warning) << "get_time: " << e.what();
+            error_cnt++;
+        }
     }
-
-    //just trust me on this one
-    return ptime( date( 
-                     greg_year(boost::lexical_cast<int>(datestr.substr(4, 2)) + 2000),
-                     greg_month(boost::lexical_cast<int>(datestr.substr(2, 2))), 
-                     greg_day(boost::lexical_cast<int>(datestr.substr(0, 2))) 
-                   ),
-                  hours(  boost::lexical_cast<int>(timestr.substr(0, 2)))
-                + minutes(boost::lexical_cast<int>(timestr.substr(2, 2)))
-                + seconds(boost::lexical_cast<int>(timestr.substr(4, 2)))
-             );
+    throw uhd::value_error("Timeout after no valid message found");
+    
+    return gps_time; //keep gcc from complaining
   }
   
   time_t get_epoch_time(void) {
@@ -217,10 +226,20 @@ private:
   }
 
   bool locked(void) {
-      std::string reply = get_nmea("GPGGA");
-      if(reply.size() <= 1) return false;
+    int error_cnt = 0;
+    while(error_cnt < 3) {
+        try {
+            std::string reply = get_nmea("GPGGA");
+            if(reply.size() <= 1) return false;
 
-      return (get_token(reply, 6) != "0");
+            return (get_token(reply, 6) != "0");
+        } catch(std::exception &e) {
+            UHD_MSG(warning) << "locked: " << e.what();
+            error_cnt++;
+        }
+    }
+    throw uhd::value_error("Timeout after no valid message found");
+    return false;
   }
 
   gps_send_fn_t _send;