diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 127c7e56b6..3ff768ba06 100644 --- 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> UbloxMsgParser::gen_msg() { switch (ubx_message.msg_type()) { case 0x0107: return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; - case 0x0213: + case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe) return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; - case 0x0215: + case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data) return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; case 0x0a09: return {"ubloxGnss", gen_mon_hw(static_cast(body))}; @@ -246,11 +261,144 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m return kj::Array(); } +kj::Array 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(); + } + + 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(); + } + + // 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(); + } + + 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(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(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(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(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(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 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(); } diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index f031313168..6988f20b74 100644 --- 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 parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); + kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); std::unordered_map> 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 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> glonass_strings; + std::unordered_map glonass_superframes; +}; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c787f4be2d..68675e7007 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -21b29c8a9eb9acd63e91cbda55657afb266a07f9 \ No newline at end of file +2beed04e654cdf84fac5842869f38c8cd55e9867 \ No newline at end of file