Improve LEA-M8F autodetection and poll NAV-SOL

3 files changed, 171 insertions, 43 deletions

diff --git a/host/include/uhd/usrp/gps_ctrl.hpp b/host/include/uhd/usrp/gps_ctrl.hpp
index bbccac8bb..27e202d9b 100644
--- a/host/include/uhd/usrp/gps_ctrl.hpp
+++ b/host/include/uhd/usrp/gps_ctrl.hpp
@@ -52,10 +52,40 @@ public:
    */
   virtual bool gps_detected(void) = 0;
 
+  /*!
+   * Tell if the GPSDO is a LEA-M8F
+   * \return true if the LEA-M8F has been detected
+   */
+  virtual bool gps_detected_lea_m8f(void) = 0;
+
   //TODO: other fun things you can do with a GPS.
 
 };
 
+/*! The UBX-NAV-SOL message structure from the
+ *  u-blox UBX protocol
+ */
+struct ubx_nav_sol_t
+{
+    uint32_t  itow;           // ms GPS Millisecond Time of Week
+    int32_t   frac;           // ns remainder of rounded ms above
+    int16_t   week;           // GPS week
+    uint8_t   GPSfix;         // GPSfix Type, range 0..6
+    uint8_t   Flags;          // Navigation Status Flags
+    int32_t   ECEF_X;         // cm ECEF X coordinate
+    int32_t   ECEF_Y;         // cm ECEF Y coordinate
+    int32_t   ECEF_Z;         // cm ECEF Z coordinate
+    int32_t   PAcc;           // cm 3D Position Accuracy Estimate
+    int32_t   ECEFVX;         // cm/s ECEF X velocity
+    int32_t   ECEFVY;         // cm/s ECEF Y velocity
+    int32_t   ECEFVZ;         // cm/s ECEF Z velocity
+    uint32_t  SAcc;           // cm/s Speed Accuracy Estimate
+    uint16_t  PDOP;           // 0.01 Position DOP
+    uint8_t   res1;           // reserved
+    uint8_t   numSV;          // Number of SVs used in navigation solution
+    uint32_t  res2;           // reserved
+} __attribute__((packed));
+
 } //namespace uhd
 
 #endif /* INCLUDED_GPS_CTRL_HPP */
diff --git a/host/lib/usrp/b200/b200_impl.cpp b/host/lib/usrp/b200/b200_impl.cpp
index 98141dbaa..55748129d 100644
--- a/host/lib/usrp/b200/b200_impl.cpp
+++ b/host/lib/usrp/b200/b200_impl.cpp
@@ -278,7 +278,7 @@ b200_impl::b200_impl(const device_addr_t &device_addr)
     // Create the GPSDO control
     ////////////////////////////////////////////////////////////////////
     _async_task_data->gpsdo_uart = b200_uart::make(_ctrl_transport, B200_TX_GPS_UART_SID);
-    _async_task_data->gpsdo_uart->set_baud_divider(B200_BUS_CLOCK_RATE/115200);
+    _async_task_data->gpsdo_uart->set_baud_divider(B200_BUS_CLOCK_RATE/9600);
     _async_task_data->gpsdo_uart->write_uart("\n"); //cause the baud and response to be setup
     boost::this_thread::sleep(boost::posix_time::seconds(1)); //allow for a little propagation
 
@@ -305,7 +305,7 @@ b200_impl::b200_impl(const device_addr_t &device_addr)
         else
         {
             UHD_MSG(status) << "not found" << std::endl;
-            _local_ctrl->poke32(TOREG(SR_CORE_GPSDO_ST), B200_GPSDO_ST_NONE);
+            //_local_ctrl->poke32(TOREG(SR_CORE_GPSDO_ST), B200_GPSDO_ST_NONE);
         }
     }
 
@@ -474,9 +474,12 @@ b200_impl::b200_impl(const device_addr_t &device_addr)
         UHD_MSG(status) << "Setting references to the internal GPSDO" << std::endl;
         _tree->access<std::string>(mb_path / "time_source" / "value").set("gpsdo");
         _tree->access<std::string>(mb_path / "clock_source" / "value").set("gpsdo");
-        UHD_MSG(status) << "Initializing time to the internal GPSDO" << std::endl;
-        const time_t tp = time_t(_gps->get_sensor("gps_time").to_int()+1);
-        _tree->access<time_spec_t>(mb_path / "time" / "pps").set(time_spec_t(tp));
+
+        if (not _gps->gps_detected_lea_m8f()) {
+            UHD_MSG(status) << "Initializing time to the internal GPSDO" << std::endl;
+            const time_t tp = time_t(_gps->get_sensor("gps_time").to_int()+1);
+            _tree->access<time_spec_t>(mb_path / "time" / "pps").set(time_spec_t(tp));
+        }
     }
 
 }
diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp
index 3bf0afd5c..1895b494a 100644
--- a/host/lib/usrp/gps_ctrl.cpp
+++ b/host/lib/usrp/gps_ctrl.cpp
@@ -57,51 +57,131 @@ private:
           return update_cached_sensors(sensor);
       }
     } catch(std::exception &e) {
-      UHD_MSG(warning) << "get_cached_sensor: " << e.what();
+      UHD_MSG(warning) << "get_cached_sensor: " << e.what() << std::endl;
     }
     return std::string();
   }
 
   std::string update_cached_sensors(const std::string sensor) {
-    if(not gps_detected() || !(gps_type == GPS_TYPE_INTERNAL_GPSDO || gps_type == GPS_TYPE_LEA_M8F)) {
-        UHD_MSG(error) << "get_stat(): unsupported GPS or no GPS detected";
-        return std::string();
-    }
-
-    std::list<std::string> list = boost::assign::list_of("GPGGA")("GPRMC")("SERVO");
-    const std::list<std::string> list_lea_m8f = boost::assign::list_of("GNGGA")("GNRMC");
+    if (gps_detected() && gps_type == GPS_TYPE_INTERNAL_GPSDO) {
 
-    if (gps_type == GPS_TYPE_LEA_M8F) {
-        list = list_lea_m8f;
-    }
+      const std::list<std::string> list = boost::assign::list_of("GPGGA")("GPRMC")("SERVO");
 
-    static const boost::regex status_regex("\\d\\d-\\d\\d-\\d\\d");
-    std::map<std::string,std::string> msgs;
+      static const boost::regex status_regex("\\d\\d-\\d\\d-\\d\\d");
+      std::map<std::string,std::string> msgs;
 
-    // Get all GPSDO messages available
-    // Creating a map here because we only want the latest of each message type
-    for (std::string msg = _recv(); msg.length() > 6; msg = _recv())
-    {
+      // Get all GPSDO messages available
+      // Creating a map here because we only want the latest of each message type
+      for (std::string msg = _recv(); msg.length() > 6; msg = _recv())
+      {
         // Look for SERVO message
         if (boost::regex_search(msg, status_regex, boost::regex_constants::match_continuous))
-            msgs["SERVO"] = msg;
+          msgs["SERVO"] = msg;
         else
-            msgs[msg.substr(1,5)] = msg;
-    }
+          msgs[msg.substr(1,5)] = msg;
+      }
 
-    boost::system_time time = boost::get_system_time();
+      boost::system_time time = boost::get_system_time();
 
-    // Update sensors with newly read data
-    BOOST_FOREACH(std::string key, list) {
+      // Update sensors with newly read data
+      BOOST_FOREACH(std::string key, list) {
         if (msgs[key].length())
-            sensors[key] = boost::make_tuple(msgs[key], time, !sensor.compare(key));
+          sensors[key] = boost::make_tuple(msgs[key], time, !sensor.compare(key));
+      }
+
+      // Return requested sensor if it was updated
+      if (msgs[sensor].length())
+        return msgs[sensor];
+
+      return std::string();
     }
+    else if (gps_detected() && gps_type == GPS_TYPE_LEA_M8F) {
+      const std::list<std::string> list = boost::assign::list_of("GNGGA")("GNRMC")("FIXTYPE");
+
+      // We try to receive the some UBX messages to find out if the clock is disciplined
+      std::string msg = _recv();
+
+      std::map<std::string,std::string> msgs;
+
+      // Get all GPSDO messages available
+      // Creating a map here because we only want the latest of each message type
+      for (std::string msg = _recv(); msg.length() > 6; msg = _recv())
+      {
+        std::stringstream ss;
+        ss << "Got message ";
+        for (size_t m = 0; m < msg.size(); m++) {
+          ss << std::hex << (unsigned int)(unsigned char)msg[m] << " " << std::dec;
+        }
+        UHD_MSG(warning) << ss.str() << ":" << std::endl << msg << std::endl;
+        // Get UBX-NAV-SOL
+        const uint8_t nav_sol_head[4] = {0xb5, 0x62, 0x01, 0x06};
+        const std::string nav_sol_head_str(reinterpret_cast<const char *>(nav_sol_head), 4);
+        if (msg.find(nav_sol_head_str) == 0) {
+          if (msg.length() > 52 + 8) {
+            ubx_nav_sol_t nav_sol;
+            memcpy(&nav_sol, msg.c_str(), sizeof(nav_sol));
+
+            UHD_MSG(warning) << "Got NAV-SOL " << nav_sol.itow << ", "
+                             << (int)nav_sol.numSV << " SVs, flags "
+                             << (int)nav_sol.Flags << std::endl;
+
+            std::string fixtype;
+
+            if (nav_sol.GPSfix == 0) {
+              fixtype = "no fix";
+            }
+            else if (nav_sol.GPSfix == 1) {
+              fixtype = "dead reckoning";
+            }
+            else if (nav_sol.GPSfix == 2) {
+              fixtype = "2d fix";
+            }
+            else if (nav_sol.GPSfix == 3) {
+              fixtype = "3d fix";
+            }
+            else if (nav_sol.GPSfix == 4) {
+              fixtype = "combined fix";
+            }
+            else if (nav_sol.GPSfix == 5) {
+              fixtype = "time-only fix";
+            }
+
+            if (not fixtype.empty()) {
+              msgs["FIXTYPE"] = fixtype;
+            }
 
-    // Return requested sensor if it was updated
-    if (msgs[sensor].length())
+            std::string next_msg = msg.substr(52+8);
+
+            UHD_MSG(warning) << "Next message " << next_msg << std::endl;
+            if (next_msg.find("$") == 0) {
+              msgs[next_msg.substr(1,5)] = next_msg;
+            }
+          }
+        }
+        else {
+          msgs[msg.substr(1,5)] = msg;
+        }
+      }
+
+      boost::system_time time = boost::get_system_time();
+
+      // Update sensors with newly read data
+      BOOST_FOREACH(std::string key, list) {
+        if (msgs[key].length()) {
+          sensors[key] = boost::make_tuple(msgs[key], time, !sensor.compare(key));
+        }
+      }
+
+      // Return requested sensor if it was updated
+      if (msgs[sensor].length())
         return msgs[sensor];
 
-    return std::string();
+      return std::string();
+    }
+    else {
+      UHD_MSG(error) << "get_stat(): unsupported GPS or no GPS detected" << std::endl;
+      return std::string();
+    }
   }
 
 public:
@@ -117,6 +197,9 @@ public:
     _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 GPSDO
 
+    // try to init LEA-M8F
+    init_lea_m8f();
+
     //wait for _send(...) to return
     sleep(milliseconds(GPSDO_STUPID_DELAY_MS));
 
@@ -124,14 +207,16 @@ public:
     const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
     while(boost::get_system_time() < comm_timeout) {
       reply = _recv();
+      UHD_MSG(warning) << "Received " << reply << std::endl;
       if(reply.find("Command Error") != std::string::npos) {
         gps_type = GPS_TYPE_INTERNAL_GPSDO;
         break;
       }
       else if(reply.substr(0, 3) == "$GP") i_heard_some_nmea = true; //but keep looking for that "Command Error" response
-      else if(reply.substr(0, 3) == "$GN") {
-          // The u-blox LEA-M8F outputs $GNGGA and $GNRMC messages
+      else if(reply.substr(0, 2) == "\xB5""\x62") {
+          // The u-blox LEA-M8F outputs UBX protocol messages
           gps_type = GPS_TYPE_LEA_M8F;
+          i_heard_some_nmea = false;
           break;
       }
       else if(reply.length() != 0) i_heard_something_weird = true; //probably wrong baud rate
@@ -180,7 +265,8 @@ public:
         ("gps_gnrmc")
         ("gps_time")
         ("gps_locked")
-        ("gps_servo");
+        ("gps_servo")
+        ("gps_fixtype");
     return ret;
   }
 
@@ -188,7 +274,8 @@ public:
     if(key == "gps_gpgga"
     or key == "gps_gprmc"
     or key == "gps_gngga"
-    or key == "gps_gnrmc") {
+    or key == "gps_gnrmc"
+    or key == "gps_fixtype" ) {
         return sensor_value_t(
                  boost::to_upper_copy(key),
                  get_cached_sensor(boost::to_upper_copy(key.substr(4,8)), GPS_NMEA_NORMAL_FRESHNESS, false, false),
@@ -229,7 +316,9 @@ private:
   }
 
   void init_lea_m8f(void) {
-      // Nothing much to do yet
+      // Enable the UBX-NAV-SOL message:
+      const uint8_t en_nav_sol[11] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4f};
+      _send(std::string(reinterpret_cast<const char *>(en_nav_sol), 11));
   }
 
   //retrieve a raw NMEA sentence
@@ -299,7 +388,7 @@ private:
             return gps_time;
 
         } catch(std::exception &e) {
-            UHD_MSG(warning) << "get_time: " << e.what();
+            UHD_MSG(warning) << "get_time: " << e.what() << std::endl;
             _flush();
             error_cnt++;
         }
@@ -313,6 +402,10 @@ private:
       return (get_time() - from_time_t(0)).total_seconds();
   }
 
+  bool gps_detected_lea_m8f(void) {
+    return (gps_type == GPS_TYPE_LEA_M8F);
+  }
+
   bool gps_detected(void) {
     return (gps_type != GPS_TYPE_NONE);
   }
@@ -323,16 +416,18 @@ private:
         try {
             std::string reply;
             if (gps_type == GPS_TYPE_LEA_M8F) {
-               reply = get_cached_sensor("GNGGA", GPS_LOCK_FRESHNESS, false, false);
+               reply = get_cached_sensor("FIXTYPE", GPS_LOCK_FRESHNESS, false, false);
+               UHD_MSG(warning) << "FIXTYPE is " << reply << std::endl;
+               return reply == "3d fix";
             }
             else {
                reply = get_cached_sensor("GPGGA", GPS_LOCK_FRESHNESS, false, false);
+               if(reply.size() <= 1) return false;
+               return (get_token(reply, 6) != "0");
             }
-            if(reply.size() <= 1) return false;
 
-            return (get_token(reply, 6) != "0");
         } catch(std::exception &e) {
-            UHD_MSG(warning) << "locked: " << e.what();
+            UHD_MSG(warning) << "locked: " << e.what() << std::endl;
             error_cnt++;
         }
     }
@@ -385,7 +480,7 @@ private:
     GPS_TYPE_NONE
   } gps_type;
 
-  static const int GPS_COMM_TIMEOUT_MS = 1300;
+  static const int GPS_COMM_TIMEOUT_MS = 2300;
   static const int GPS_NMEA_FRESHNESS = 10;
   static const int GPS_NMEA_LOW_FRESHNESS = 2500;
   static const int GPS_NMEA_NORMAL_FRESHNESS = 1000;