gps_ctrl.cpp: Partially rewritten, to use caching instead of throwing

away the 'wrong' messages.

The old way to do this was when a sensor was requested, to wait for the
corresponding NMEA message to pass by, throwing away all the 'wrong'
ones in the meantime. However, this leads to problems if the user
requests combinations that are based on the same NMEA string.
To avoid this we now try to cache the strings for a 'freshness' period.

1 files changed, 109 insertions, 25 deletions

diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp
index f3bdded60..741e69397 100644
--- a/host/lib/usrp/gps_ctrl.cpp
+++ b/host/lib/usrp/gps_ctrl.cpp
@@ -26,6 +26,10 @@
 #include <boost/thread/thread.hpp>
 #include <boost/tokenizer.hpp>
 #include <boost/format.hpp>
+#include <boost/regex.hpp>
+
+#include "boost/tuple/tuple.hpp"
+#include "boost/foreach.hpp"
 
 using namespace uhd;
 using namespace boost::gregorian;
@@ -38,14 +42,69 @@ using namespace boost::this_thread;
  */
 
 class gps_ctrl_impl : public gps_ctrl{
+private:
+  std::map<std::string, boost::tuple<std::string, boost::system_time, bool> > sensors;
+
+  std::string get_cached_sensor(const std::string sensor, const int freshness, const bool once, const bool touch=true) {
+    boost::system_time time = boost::get_system_time();
+    try {
+      // this is nasty ...
+      //std::cout << boost::format("Requested %s - seen? ") % sensor << sensors[sensor].get<2>() << " once? " << once << std::endl;
+      if(time - sensors[sensor].get<1>() < milliseconds(freshness) && (!once or !sensors[sensor].get<2>())) {
+        sensors[sensor] = boost::make_tuple(sensors[sensor].get<0>(), sensors[sensor].get<1>(), touch);
+        return sensors[sensor].get<0>();
+      } else {
+          return update_cached_sensors(sensor);
+      }
+    } catch(std::exception &e) {
+      UHD_MSG(warning) << "get_cached_sensor: " << e.what();
+    }
+    return std::string();
+  }
+
+  std::string update_cached_sensors(const std::string sensor) {
+    if(not gps_detected() || (gps_type != GPS_TYPE_JACKSON_LABS)) {
+        UHD_MSG(error) << "get_stat(): unsupported GPS or no GPS detected";
+        return std::string();
+    }
+
+    std::string msg = _recv();
+    static const boost::regex status_regex("\\d\\d-\\d\\d-\\d\\d");
+    boost::system_time time = boost::get_system_time();
+    if(msg.size() < 6)
+      return std::string();
+
+    std::string nmea = msg.substr(1,5);
+    const std::list<std::string> list = boost::assign::list_of("GPGGA")("GPRMC");
+    BOOST_FOREACH(std::string key, list) {
+      // beginning matches one of the NMEA keys
+      if(!nmea.compare(key)) {
+        sensors[key] = boost::make_tuple(msg, time, !sensor.compare(key));
+        // if this was what we're looking for return it
+        return (!sensor.compare(key))? msg : std::string();
+      }
+    }
+
+     //We're still here so it's not one of the NMEA strings from above
+    if(boost::regex_search(msg, status_regex, boost::regex_constants::match_continuous)) {
+      trim(msg);
+      sensors["SERVO"] = boost::make_tuple(msg, time, false);
+      if(!sensor.compare("SERVO"))
+        return msg;
+      else
+        return std::string();
+    }
+    return std::string();
+  }
 public:
   gps_ctrl_impl(uart_iface::sptr uart){
     _uart = uart;
 
+
     std::string reply;
     bool i_heard_some_nmea = false, i_heard_something_weird = false;
     gps_type = GPS_TYPE_NONE;
-    
+
     //first we look for a Jackson Labs Firefly (since that's what we provide...)
     _flush(); //get whatever junk is in the rx buffer right now, and throw it away
     _send("HAAAY GUYYYYS\n"); //to elicit a response from the Firefly
@@ -60,7 +119,7 @@ public:
       if(reply.find("Command Error") != std::string::npos) {
         gps_type = GPS_TYPE_JACKSON_LABS;
         break;
-      } 
+      }
       else if(reply.substr(0, 3) == "$GP") i_heard_some_nmea = true; //but keep looking for that "Command Error" response
       else if(reply.length() != 0) i_heard_something_weird = true; //probably wrong baud rate
       sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
@@ -99,7 +158,8 @@ public:
         ("gps_gpgga")
         ("gps_gprmc")
         ("gps_time")
-        ("gps_locked");
+        ("gps_locked")
+        ("gps_servo");
     return ret;
   }
 
@@ -108,7 +168,7 @@ public:
     or key == "gps_gprmc") {
         return sensor_value_t(
                  boost::to_upper_copy(key),
-                 get_nmea(boost::to_upper_copy(key.substr(4,8))),
+                 get_cached_sensor(boost::to_upper_copy(key.substr(4,8)), GPS_NMEA_NORMAL_FRESHNESS, false, false),
                  "");
     }
     else if(key == "gps_time") {
@@ -117,6 +177,9 @@ public:
     else if(key == "gps_locked") {
         return sensor_value_t("GPS lock status", locked(), "locked", "unlocked");
     }
+    else if(key == "gps_servo") {
+        return sensor_value_t("GPS servo status", get_servo(), "");
+    }
     else {
         throw uhd::value_error("gps ctrl get_sensor unknown key: " + key);
     }
@@ -138,24 +201,25 @@ private:
      sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
     _send("GPS:GPRMC 1\n");
      sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
+    _send("SERV:TRAC 1\n");                        // enable servo trace message
+     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
   }
- 
+
   //retrieve a raw NMEA sentence
   std::string get_nmea(std::string msgtype) {
-    msgtype.insert(0, "$");
     std::string reply;
-    if(not gps_detected()) {
-        UHD_MSG(error) << "get_nmea(): unsupported GPS or no GPS detected";
-        return std::string();
-    }
-
-    _flush(); //flush all input before waiting for a message
 
     const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
     while(boost::get_system_time() < comm_timeout) {
-        reply = _recv();
-        if(reply.substr(0, 6) == msgtype)
-          return reply;
+        if(!msgtype.compare("GPRMC")) {
+          reply = get_cached_sensor(msgtype, GPS_NMEA_FRESHNESS, true);
+        }
+        else {
+          reply = get_cached_sensor(msgtype, GPS_NMEA_LOW_FRESHNESS, false);
+        }
+        if(reply.size()) {
+          if(reply.substr(1, 5) == msgtype) return reply;
+        }
         boost::this_thread::sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
     }
     throw uhd::value_error(str(boost::format("get_nmea(): no %s message found") % msgtype));
@@ -178,7 +242,7 @@ private:
   ptime get_time(void) {
     int error_cnt = 0;
     ptime gps_time;
-    while(error_cnt < 3) {
+    while(error_cnt < 10) {
         try {
             std::string reply = get_nmea("GPRMC");
 
@@ -188,29 +252,30 @@ private:
             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( 
+            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))) 
+                             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();
+            _flush();
             error_cnt++;
         }
     }
     throw uhd::value_error("Timeout after no valid message found");
-    
+
     return gps_time; //keep gcc from complaining
   }
-  
+
   time_t get_epoch_time(void) {
       return (get_time() - from_time_t(0)).total_seconds();
   }
@@ -223,7 +288,7 @@ private:
     int error_cnt = 0;
     while(error_cnt < 3) {
         try {
-            std::string reply = get_nmea("GPGGA");
+            std::string reply = get_cached_sensor("GPGGA", GPS_LOCK_FRESHNESS, false, false);
             if(reply.size() <= 1) return false;
 
             return (get_token(reply, 6) != "0");
@@ -236,6 +301,20 @@ private:
     return false;
   }
 
+  std::string get_servo(void) {
+    std::string reply;
+
+    const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
+    while(boost::get_system_time() < comm_timeout) {
+        reply = get_cached_sensor("SERVO", GPS_SERVO_FRESHNESS, false);
+        if(reply.size())
+          return reply;
+        boost::this_thread::sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
+    }
+    throw uhd::value_error("get_stat(): no servo message found");
+    return std::string();
+  }
+
   uart_iface::sptr _uart;
 
   void _flush(void){
@@ -258,7 +337,12 @@ private:
     GPS_TYPE_NONE
   } gps_type;
 
-  static const int GPS_COMM_TIMEOUT_MS = 1500;
+  static const int GPS_COMM_TIMEOUT_MS = 1300;
+  static const int GPS_NMEA_FRESHNESS = 10;
+  static const int GPS_NMEA_LOW_FRESHNESS = 2500;
+  static const int GPS_NMEA_NORMAL_FRESHNESS = 1000;
+  static const int GPS_SERVO_FRESHNESS = 2500;
+  static const int GPS_LOCK_FRESHNESS = 2500;
   static const int GPS_TIMEOUT_DELAY_MS = 200;
   static const int FIREFLY_STUPID_DELAY_MS = 200;
 };