aboutsummaryrefslogtreecommitdiffstats
path: root/host/lib
diff options
context:
space:
mode:
Diffstat (limited to 'host/lib')
-rw-r--r--host/lib/usrp/gps_ctrl.cpp184
1 files changed, 136 insertions, 48 deletions
diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp
index b18382ede..1d3d240b5 100644
--- a/host/lib/usrp/gps_ctrl.cpp
+++ b/host/lib/usrp/gps_ctrl.cpp
@@ -39,17 +39,100 @@ using namespace boost::algorithm;
using namespace boost::this_thread;
/*!
- * A control for GPSDO devices
+ * A NMEA and UBX parser for the GPS serial interface.
*/
+class gps_ctrl_parser {
+ private:
+ std::deque<char> gps_data_input;
-gps_ctrl::~gps_ctrl(void){
- /* NOP */
-}
+ std::string parse_ubx()
+ {
+ // Assumptions:
+ // The deque now contains an UBX message in the format
+ // \xb6\x62<CLASS><ID><LEN><PAYLOAD><CRC>
+ // where
+ // <CLASS> is 1 byte
+ // <ID> is 1 byte
+ // <LEN> is 2 bytes (little-endian), length of <PAYLOAD>
+ // <CRC> is 2 bytes
+
+ if (gps_data_input.size() >= 8) {
+ uint8_t len_lo = gps_data_input[4];
+ uint8_t len_hi = gps_data_input[5];
+ size_t len = len_lo | (len_hi << 8);
+
+ if (gps_data_input.size() >= len + 8) {
+ std::string msg(gps_data_input.begin(), gps_data_input.begin() + (len + 8));
+ gps_data_input.erase(gps_data_input.begin(), gps_data_input.begin() + (len + 8));
+ return msg;
+ }
+ }
+
+ return std::string();
+ }
+
+ std::string parse_nmea()
+ {
+ // Assumptions:
+ // The deque now contains an NMEA message in the format
+ // $G.................*XX<CR><LF>
+ // the checksum XX is dropped from the message
+
+ std::deque<char>::iterator star;
+ star = std::find(gps_data_input.begin() + 2, gps_data_input.end(), '*');
+ if (star != gps_data_input.end()) {
+ std::string msg(gps_data_input.begin(), star);
+
+ // The parser will take care of the leftover *XX<CR><LF>
+ gps_data_input.erase(gps_data_input.begin(), star);
+ return msg;
+ }
+
+ return std::string();
+ }
+
+ public:
+ template <class InputIterator>
+ void push_data(InputIterator first, InputIterator last)
+ {
+ gps_data_input.insert(gps_data_input.end(), first, last);
+ }
+
+ std::string get_next_message()
+ {
+ if (gps_data_input.size() >= 2) {
+ do {
+ char header1 = gps_data_input[0];
+ char header2 = gps_data_input[1];
+
+ if (header1 == '$' and header2 == 'G') {
+ return parse_nmea();
+ }
+ else if (header1 == '\xb5' and header2 == '\x62') {
+ return parse_ubx();
+ }
+
+ gps_data_input.pop_front();
+ if (gps_data_input.size() < 2) {
+ break;
+ }
+ } while (true);
+ }
+ return std::string();
+ }
+
+ size_t size() { return gps_data_input.size(); }
+};
+/*!
+ * A control for GPSDO devices
+ */
class gps_ctrl_impl : public gps_ctrl{
private:
std::map<std::string, boost::tuple<std::string, boost::system_time, bool> > sensors;
+ gps_ctrl_parser gps_parser;
+
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 {
@@ -143,14 +226,19 @@ private:
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();
+ // Concatenate all incoming data into the deque
+ for (std::string msg = _recv();
+ msg.length() > 0 && gps_parser.size() < (2*115200);
+ msg = _recv())
+ {
+ gps_parser.push_data(msg.begin(), msg.end());
+ }
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())
+ for (std::string msg = gps_parser.get_next_message(); not msg.empty(); msg = gps_parser.get_next_message())
{
/*
std::stringstream ss;
@@ -160,54 +248,50 @@ private:
}
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;
+ if (msg.find(nav_sol_head_str) == 0 and msg.length() == 52 + 8) {
+ uint8_t gpsFix = msg[6 + 10]; // header size == 6, field offset == 10
- 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";
- }
+ std::string fixtype;
- if (not fixtype.empty()) {
- msgs["FIXTYPE"] = fixtype;
- }
-
- std::string next_msg = msg.substr(52+8);
+ if (gpsFix == 0) {
+ fixtype = "no fix";
+ }
+ else if (gpsFix == 1) {
+ fixtype = "dead reckoning";
+ }
+ else if (gpsFix == 2) {
+ fixtype = "2d fix";
+ }
+ else if (gpsFix == 3) {
+ fixtype = "3d fix";
+ }
+ else if (gpsFix == 4) {
+ fixtype = "combined fix";
+ }
+ else if (gpsFix == 5) {
+ fixtype = "time-only fix";
+ }
- //UHD_MSG(warning) << "Next message " << next_msg << std::endl;
- if (next_msg.find("$") == 0) {
- msgs[next_msg.substr(1,5)] = next_msg;
- }
+ if (not fixtype.empty()) {
+ msgs["FIXTYPE"] = fixtype;
}
}
- else {
+ else if (msg[0] == '$') {
msgs[msg.substr(1,5)] = msg;
}
+ else if (msg[0] == '\xb5' and msg[1] == '\x62') { /* Ignore unsupported UBX message */ }
+ else {
+ std::stringstream ss;
+ ss << "Unknown 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;
+ }
}
boost::system_time time = boost::get_system_time();
@@ -260,7 +344,8 @@ public:
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, 2) == "\xB5""\x62") {
+ else if(reply.substr(0, 2) == "\xB5""\x62" or
+ reply.substr(0, 3) == "$GN" ) {
// The u-blox LEA-M8F outputs UBX protocol messages
gps_type = GPS_TYPE_LEA_M8F;
i_heard_some_nmea = false;
@@ -362,9 +447,12 @@ private:
}
void init_lea_m8f(void) {
- // Enable the UBX-NAV-SOL message:
+ // Enable the UBX-NAV-SOL and the $GNRMC messages
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));
+ _send(std::string(reinterpret_cast<const char *>(en_nav_sol), sizeof(en_nav_sol)));
+
+ const uint8_t en_gnrmc[11] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xf0, 0x04, 0x01, 0xff, 0x18};
+ _send(std::string(reinterpret_cast<const char *>(en_gnrmc), sizeof(en_gnrmc)));
}
//retrieve a raw NMEA sentence
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-30 23:01:00 +0000