sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Glonass support, ubloxd add ephemeris parsing (#27119)

Browse Source 
* add glonass kaitai parsing

* add kaita generated files

* remove glonass from build

* add string non immediate type

* fix kaitai bug

* cleanUp

* add patch file

* fix scons order

* make lookup const

* remove comments

* rename

* add to release files

* add signs

* final ublox parsing

* bump cereal

* update ref

---------

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
old-commit-hash: 4e27a7f22a
beeps
Kurt Nistelberger 2 years ago committed by GitHub
parent 7d2477e0af
commit 0c8cd2c07e
3 changed files with 162 additions and 4 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 152
      selfdrive/locationd/ublox_msg.cc
  2. 12
      selfdrive/locationd/ublox_msg.h
  3. 2
      selfdrive/test/process_replay/ref_commit

152
selfdrive/locationd/ublox_msg.cc
Unescape View File

@ -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>();
}

12
selfdrive/locationd/ublox_msg.h
Unescape View File

@ -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;
};

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
21b29c8a9eb9acd63e91cbda55657afb266a07f9
2beed04e654cdf84fac5842869f38c8cd55e9867
Write Preview
Loading…
Cancel
Save