commaai · gast04 · Feb 3, 2023 · Jan 26, 2023 · Jan 26, 2023 · Jan 26, 2023
diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc
@@ -65,6 +65,21 @@ inline bool UbloxMsgParser::valid_so_far() {
   return true;
 }
 
+inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) {
+  // convert time to year (conversion from A3.1.3)
+  int J = 0;
+  if (1 <= Nt && Nt <= 366) {
+    J = 1;
+  } else if (367 <= Nt && Nt <= 731) {
+    J = 2;
+  } else if (732 <= Nt && Nt <= 1096) {
+    J = 3;
+  } else if (1097 <= Nt && Nt <= 1461) {
+    J = 4;
+  }
+  uint16_t year = 1996 + 4*(N4 -1) + (J - 1);
+  return year;
+}
 
 bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
   int needed = needed_bytes();
@@ -103,9 +118,9 @@ std::pair<std::string, kj::Array<capnp::word>> UbloxMsgParser::gen_msg() {
   switch (ubx_message.msg_type()) {
   case 0x0107:
     return {"gpsLocationExternal", gen_nav_pvt(static_cast<ubx_t::nav_pvt_t*>(body))};
-  case 0x0213:
+  case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe)
     return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
-  case 0x0215:
+  case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data)
     return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
   case 0x0a09:
     return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
@@ -246,11 +261,144 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
   return kj::Array<capnp::word>();
 }
 
+kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
+  if (msg->sv_id() == 255) {
+    // data can be decoded before identifying the SV number, in this case 255
+    // is returned, which means "unknown"  (ublox p32)
+    return kj::Array<capnp::word>();
+  }
+
+  auto body = *msg->body();
+  assert(body.size() == 4);
+  {
+    std::string string_data;
+    string_data.reserve(16);
+    for (uint32_t word : body) {
+      for (int i = 3; i >= 0; i--)
+        string_data.push_back(word >> 8*i);
+    }
+
+    kaitai::kstream stream(string_data);
+    glonass_t gl_string(&stream);
+
+    int string_number = gl_string.string_number();
+    if (string_number > 5 || gl_string.idle_chip()) {
+      // dont parse non immediate data, idle_chip == 0
+      return kj::Array<capnp::word>();
+    }
+
+    // immediate data is the same within one superframe
+    if (glonass_superframes[msg->sv_id()] != gl_string.superframe_number()) {
+      glonass_strings[msg->sv_id()].clear();
+      glonass_superframes[msg->sv_id()] = gl_string.superframe_number();
+    }
+    glonass_strings[msg->sv_id()][string_number] = string_data;
+  }
+
+  // publish if strings 1-5 have been collected
+  if (glonass_strings[msg->sv_id()].size() != 5) {
+    return kj::Array<capnp::word>();
+  }
+
+  MessageBuilder msg_builder;
+  auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
+  eph.setSvId(msg->sv_id());
+  uint16_t current_day = 0;
+
+  // string number 1
+  {
+    kaitai::kstream stream(glonass_strings[msg->sv_id()][1]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
+
+    eph.setP1(data->p1());
+    eph.setTk(data->t_k());
+    eph.setXVel(data->x_vel() * pow(2, -20));
+    eph.setXAccel(data->x_accel() * pow(2, -30));
+    eph.setX(data->x() * pow(2, -11));
+  }
+
+  // string number 2
+  {
+    kaitai::kstream stream(glonass_strings[msg->sv_id()][2]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_2_t* data = static_cast<glonass_t::string_2_t*>(gl_stream.data());
+
+    eph.setSvHealth(data->b_n()>>2); // MSB indicates health
+    eph.setP2(data->p2());
+    eph.setTb(data->t_b());
+    eph.setYVel(data->y_vel() * pow(2, -20));
+    eph.setYAccel(data->y_accel() * pow(2, -30));
+    eph.setY(data->y() * pow(2, -11));
+  }
+
+  // string number 3
+  {
+    kaitai::kstream stream(glonass_strings[msg->sv_id()][3]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_3_t* data = static_cast<glonass_t::string_3_t*>(gl_stream.data());
+
+    eph.setP3(data->p3());
+    eph.setGammaN(data->gamma_n() * pow(2, -40));
+    eph.setSvHealth(eph.getSvHealth() | data->l_n());
+    eph.setZVel(data->z_vel() * pow(2, -20));
+    eph.setZAccel(data->z_accel() * pow(2, -30));
+    eph.setZ(data->z() * pow(2, -11));
+  }
+
+  // string number 4
+  {
+    kaitai::kstream stream(glonass_strings[msg->sv_id()][4]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
+
+    current_day = data->n_t();
+    eph.setTauN(data->tau_n() * pow(2, -30));
+    eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
+    eph.setAge(data->e_n());
+    eph.setP4(data->p4());
+    eph.setSvURA(glonass_URA_lookup.at(data->f_t()));
+    if (msg->sv_id() != data->n()) {
+      LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n())
+    }
+    eph.setSvType(data->m());
+  }
+
+  // string number 5
+  {
+    kaitai::kstream stream(glonass_strings[msg->sv_id()][5]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
+
+    // string5 parsing is only needed to get the year, this can be removed and
+    // the year can be fetched later in laika (note rollovers and leap year)
+    uint8_t n_4 = data->n_4();
+    uint16_t year = get_glonass_year(n_4, current_day);
+
+    uint16_t last_leap_year = 1996 + 4*(n_4-1);
+    uint16_t days_till_this_year = (year - last_leap_year)*365;
+    if (days_till_this_year != 0) {
+      days_till_this_year++;
+    }
+
+    eph.setYear(year);
+    eph.setDayInYear(current_day - days_till_this_year);
+    eph.setHour((eph.getTk()>>7) & 0x1F);
+    eph.setMinute((eph.getTk()>>1) & 0x3F);
+    eph.setSecond((eph.getTk() & 0x1) * 30);
+  }
+
+  glonass_strings[msg->sv_id()].clear();
+  return capnp::messageToFlatArray(msg_builder);
+}
+
 
 kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
   switch (msg->gnss_id()) {
     case ubx_t::gnss_type_t::GNSS_TYPE_GPS:
       return parse_gps_ephemeris(msg);
+    case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS:
+      return parse_glonass_ephemeris(msg);
     default:
       return kj::Array<capnp::word>();
   }

diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h
@@ -10,6 +10,7 @@
 #include "cereal/messaging/messaging.h"
 #include "common/util.h"
 #include "selfdrive/locationd/generated/gps.h"
+#include "selfdrive/locationd/generated/glonass.h"
 #include "selfdrive/locationd/generated/ubx.h"
 
 using namespace std::string_literals;
@@ -101,13 +102,22 @@ class UbloxMsgParser {
     inline bool valid_cheksum();
     inline bool valid();
     inline bool valid_so_far();
+    inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt);
 
     kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
+    kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);
 
     std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
 
     size_t bytes_in_parse_buf = 0;
     uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
 
-};
+    // user range accuracy in meters
+    const std::unordered_map<uint8_t, float> glonass_URA_lookup =
+      {{ 0,  1}, { 1,   2}, { 2, 2.5}, { 3,   4}, { 4,  5}, {5, 7},
+       { 6, 10}, { 7,  12}, { 8,  14}, { 9,  16}, {10, 32},
+       {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}};
 
+    std::unordered_map<int, std::unordered_map<int, std::string>> glonass_strings;
+    std::unordered_map<int, int> glonass_superframes;
+};
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-21b29c8a9eb9acd63e91cbda55657afb266a07f9
+2beed04e654cdf84fac5842869f38c8cd55e9867
Original file line number	Diff line number	Diff line change
		@@ -1 +1 @@
		21b29c8a9eb9acd63e91cbda55657afb266a07f9
		2beed04e654cdf84fac5842869f38c8cd55e9867