diff options
| -rw-r--r-- | host/lib/usrp/gps_ctrl.cpp | 59 |
1 files changed, 44 insertions, 15 deletions
diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp index 275883443..3b440ba58 100644 --- a/host/lib/usrp/gps_ctrl.cpp +++ b/host/lib/usrp/gps_ctrl.cpp @@ -56,15 +56,42 @@ class gps_ctrl_parser { // <LEN> is 2 bytes (little-endian), length of <PAYLOAD> // <CRC> is 2 bytes + uint8_t ck_a = 0; + uint8_t ck_b = 0; + + if (gps_data_input.size() >= 8) { + uint8_t msg_class = gps_data_input[2]; + uint8_t msg_id = gps_data_input[3]; 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::cerr << "DATA: "; + for (size_t i=0; i < gps_data_input.size(); i++) { + uint8_t dat = gps_data_input[i]; + std::cerr << boost::format("%02x ") % (unsigned int)dat; + } + std::cerr << std::endl; + */ + + uint8_t ck_a_packet = gps_data_input[len+6]; + uint8_t ck_b_packet = gps_data_input[len+7]; + + // Range over which CRC is calculated is <CLASS><ID><LEN><PAYLOAD> + for (size_t i = 2; i < len+6; i++) { + ck_a += (uint8_t)(gps_data_input[i]); + ck_b += ck_a; + } + 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; + + if (ck_a == ck_a_packet and ck_b == ck_b_packet) { + return msg; + } } } @@ -100,23 +127,25 @@ class gps_ctrl_parser { std::string get_next_message() { - if (gps_data_input.size() >= 2) { - do { - char header1 = gps_data_input[0]; - char header2 = gps_data_input[1]; + while (gps_data_input.size() >= 2) { + 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(); - } + std::string parsed; + + if (header1 == '$' and header2 == 'G') { + parsed = parse_nmea(); + } + else if (header1 == '\xb5' and header2 == '\x62') { + parsed = parse_ubx(); + } + if (parsed.empty()) { gps_data_input.pop_front(); - if (gps_data_input.size() < 2) { - break; - } - } while (true); + } + else { + return parsed; + } } return std::string(); } |