1 files changed, 87 insertions, 29 deletions

diff --git a/host/lib/usrp/usrp2/gps_ctrl.cpp b/host/lib/usrp/usrp2/gps_ctrl.cpp
index 20d670f81..2273b2cd9 100644
--- a/host/lib/usrp/usrp2/gps_ctrl.cpp
+++ b/host/lib/usrp/usrp2/gps_ctrl.cpp
@@ -33,74 +33,130 @@ using namespace boost::algorithm;
  * A usrp2 GPS control for Jackson Labs devices
  */
 
-//TODO: multiple baud rate support (requires mboard_impl changes for poking UART registers), NMEA support, better autodetection
+//TODO: multiple baud rate support (requires mboard_impl changes for poking UART registers)
 class usrp2_gps_ctrl_impl : public usrp2_gps_ctrl{
 public:
   usrp2_gps_ctrl_impl(usrp2_iface::sptr iface){
     _iface = iface;
-    //do init here
-    //so the Jackson Labs Firefly (and Fury) don't acknowledge successful commands -- only invalid ones.
-    //first we test to see if there's a Firefly/Fury connected by sending an invalid packet and listening for the response
 
     std::string reply;
+    bool i_heard_some_nmea = false, i_heard_something_weird = false;
 
-    //TODO: try multiple baud rates (many GPS's are set up for 4800bps, you're fixed at 115200bps 8N1 right now)
-    //you have to poke registers in order to set baud rate, there's no dude/bro interface for it
-    _iface->read_uart(GPS_UART); //flush it out
-    _iface->write_uart(GPS_UART, "HAAAY GUYYYYS\n");
-    try {
-      reply = _iface->read_uart(GPS_UART);
-	//std::cerr << "Got reply from GPS: " << reply.c_str() << " with length = " << reply.length() << std::endl;
-    } catch (std::runtime_error err) {
-      if(err.what() != std::string("usrp2 no control response")) throw; //sorry can't cope with that
-      else { //we don't actually have a GPS installed
-        gps_type = GPS_TYPE_NONE;
+    gps_type = GPS_TYPE_NONE;
+
+//    set_uart_baud_rate(GPS_UART, 115200);
+    //first we look for a Jackson Labs Firefly (since that's what we sell with the USRP2+...)
+
+    _iface->read_uart(GPS_UART); //get whatever junk is in the rx buffer right now, and throw it away
+    _iface->write_uart(GPS_UART, "HAAAY GUYYYYS\n"); //to elicit a response from the Firefly
+
+    //then we loop until we either timeout, or until we get a response that indicates we're a JL device
+    int timeout = GPS_TIMEOUT_TRIES;
+    while(timeout--) {
+      reply = safe_gps_read();
+      if(trim_right_copy(reply) == "Command Error") {
+        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
     }
 
-    if(trim_right_copy(reply) == "Command Error") gps_type = GPS_TYPE_JACKSON_LABS;
-    else gps_type = GPS_TYPE_NONE; //we'll add NMEA support later
+    if((i_heard_some_nmea) && (gps_type != GPS_TYPE_JACKSON_LABS)) gps_type = GPS_TYPE_GENERIC_NMEA;
+
+    //otherwise, we can try some other common baud rates looking to see if a GPS is connected (todo, later)
+    if((gps_type == GPS_TYPE_NONE) && i_heard_something_weird) {
+      std::cout << "Invalid reply, possible incorrect baud rate" << std::endl;
+    }
+
+    bool found_gprmc = false;
 
     switch(gps_type) {
     case GPS_TYPE_JACKSON_LABS:
-      std::cerr << "Found a Jackson Labs GPS" << std::endl;
-      //issue some setup stuff so it quits spewing data out when not asked to
-      //none of these should issue replies so we don't bother looking for it
+      std::cout << "Found a Jackson Labs GPS" << std::endl;
+      //issue some setup stuff so it spits out the appropriate data
+      //none of these should issue replies so we don't bother looking for them
       //we have to sleep between commands because the JL device, despite not acking, takes considerable time to process each command.
-       boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+       boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
       _iface->write_uart(GPS_UART, "SYST:COMM:SER:ECHO OFF\n");
-       boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+       boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
       _iface->write_uart(GPS_UART, "SYST:COMM:SER:PRO OFF\n");
-       boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+       boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
       _iface->write_uart(GPS_UART, "GPS:GPGGA 0\n");
-       boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+       boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
       _iface->write_uart(GPS_UART, "GPS:GGAST 0\n");
-       boost::this_thread::sleep(boost::posix_time::milliseconds(200));
+       boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
       _iface->write_uart(GPS_UART, "GPS:GPRMC 1\n");
-       boost::this_thread::sleep(boost::posix_time::milliseconds(200));
-      break;
+       boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
+
+//      break;
 
     case GPS_TYPE_GENERIC_NMEA:
+      if(gps_type == GPS_TYPE_GENERIC_NMEA) std::cout << "Found a generic NMEA GPS device" << std::endl;
+      found_gprmc = false;
+      //here we loop around looking for a GPRMC packet. if we don't get one, we don't have a usable GPS.
+      timeout = GPS_TIMEOUT_TRIES;
+      while(timeout--) {
+        reply = safe_gps_read();
+        if(reply.substr(0, 6) == "$GPRMC") {
+          found_gprmc = true;
+          break;
+        }
+      }
+      if(!found_gprmc) {
+        if(gps_type == GPS_TYPE_JACKSON_LABS) std::cout << "Firefly GPS not locked or warming up." << std::endl;
+        else std::cout << "GPS does not output GPRMC packets. Cannot retrieve time." << std::endl;
+        gps_type = GPS_TYPE_NONE;
+      }
+      break;
+
     case GPS_TYPE_NONE:
     default:
-
       break;
+
     }
+
+
   }
 
   ~usrp2_gps_ctrl_impl(void){
 
   }
 
+  std::string safe_gps_read() {
+    std::string reply;
+    try {
+        reply = _iface->read_uart(GPS_UART);
+  	    //std::cerr << "Got reply from GPS: " << reply.c_str() << " with length = " << reply.length() << std::endl;
+    } catch (std::runtime_error err) {
+      if(err.what() != std::string("usrp2 no control response")) throw; //sorry can't cope with that
+      else { //we don't actually have a GPS installed
+        reply = std::string();
+      }
+    }
+    return reply;
+  }
+
   ptime get_time(void) {
     std::string reply;
     ptime now;
     boost::tokenizer<boost::escaped_list_separator<char> > tok(reply);
     std::vector<std::string> toked;
+    int timeout = GPS_TIMEOUT_TRIES;
+    bool found_gprmc = false;
     switch(gps_type) {
     case GPS_TYPE_JACKSON_LABS: //deprecated in favor of a single NMEA parser
     case GPS_TYPE_GENERIC_NMEA:
-      while(reply.length() == 0) reply = _iface->read_uart(GPS_UART); //loop until we hear something
+
+      while(timeout--) {
+        reply = safe_gps_read();
+        if(reply.substr(0, 6) == "$GPRMC") {
+          found_gprmc = true;
+          break;
+        }
+      }
+      UHD_ASSERT_THROW(found_gprmc);
+
       tok.assign(reply);
       toked.assign(tok.begin(), tok.end());
 
@@ -138,6 +194,8 @@ private:
   } gps_type;
 
   static const int GPS_UART = 2; //TODO: this should be plucked from fw_common.h or memory_map.h or somewhere in common with the firmware
+  static const int GPS_TIMEOUT_TRIES = 5;
+  static const int FIREFLY_STUPID_DELAY_MS = 200;
 
 };