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

Ubloxd: move gps parsing to function (#27122)

Browse Source 
move ublox gps parsing to function

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
pull/27161/head
Kurt Nistelberger 2 years ago committed by GitHub
parent e2f5b164bd
commit 549e4d9636
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 110 additions and 102 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. 210
      selfdrive/locationd/ublox_msg.cc
  2. 2
      selfdrive/locationd/ublox_msg.h

210
selfdrive/locationd/ublox_msg.cc
Unescape View File

@ -147,123 +147,129 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
// GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes
// We will first need to separate the data from the padding and parity
auto body = *msg->body();
assert(body.size() == 10);
std::string subframe_data;
subframe_data.reserve(30);
for (uint32_t word : body) {
word = word >> 6; // TODO: Verify parity
subframe_data.push_back(word >> 16);
subframe_data.push_back(word >> 8);
subframe_data.push_back(word >> 0);
}
if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) {
// GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes
// We will first need to separate the data from the padding and parity
assert(body.size() == 10);
std::string subframe_data;
subframe_data.reserve(30);
for (uint32_t word : body) {
word = word >> 6; // TODO: Verify parity
subframe_data.push_back(word >> 16);
subframe_data.push_back(word >> 8);
subframe_data.push_back(word >> 0);
// Collect subframes in map and parse when we have all the parts
{
kaitai::kstream stream(subframe_data);
gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
int sv_id = msg->sv_id();
uint64_t tow_counter = subframe.how()->tow_count();
bool clear_buffer = subframe_id == 1;
if (gps_sat_tow_count.count(sv_id) != 0) {
int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id];
clear_buffer |= counter_diff != 1 && counter_diff != -100798;
}
if (clear_buffer) gps_subframes[sv_id].clear();
// Collect subframes in map and parse when we have all the parts
gps_subframes[sv_id][subframe_id] = subframe_data;
gps_sat_tow_count[sv_id] = tow_counter;
}
if (gps_subframes[msg->sv_id()].size() == 5) {
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
eph.setSvId(msg->sv_id());
// Subframe 1
{
kaitai::kstream stream(subframe_data);
kaitai::kstream stream(gps_subframes[msg->sv_id()][1]);
gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
int sv_id = msg->sv_id();
uint64_t tow_counter = subframe.how()->tow_count();
bool clear_buffer = subframe_id == 1;
if (gps_sat_tow_count.count(sv_id) != 0) {
int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id];
clear_buffer |= counter_diff != 1 && counter_diff != -100798;
}
if (clear_buffer) gps_subframes[sv_id].clear();
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
eph.setGpsWeek(subframe_1->week_no());
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
eph.setToc(subframe_1->t_oc() * pow(2, 4));
eph.setAf2(subframe_1->af_2() * pow(2, -55));
eph.setAf1(subframe_1->af_1() * pow(2, -43));
eph.setAf0(subframe_1->af_0() * pow(2, -31));
eph.setSvHealth(subframe_1->sv_health());
}
gps_subframes[sv_id][subframe_id] = subframe_data;
gps_sat_tow_count[sv_id] = tow_counter;
// Subframe 2
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][2]);
gps_t subframe(&stream);
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
eph.setCrs(subframe_2->c_rs() * pow(2, -5));
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
eph.setCuc(subframe_2->c_uc() * pow(2, -29));
eph.setEcc(subframe_2->e() * pow(2, -33));
eph.setCus(subframe_2->c_us() * pow(2, -29));
eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0));
eph.setToe(subframe_2->t_oe() * pow(2, 4));
}
if (gps_subframes[msg->sv_id()].size() == 5) {
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
eph.setSvId(msg->sv_id());
// Subframe 1
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][1]);
gps_t subframe(&stream);
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
eph.setGpsWeek(subframe_1->week_no());
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
eph.setToc(subframe_1->t_oc() * pow(2, 4));
eph.setAf2(subframe_1->af_2() * pow(2, -55));
eph.setAf1(subframe_1->af_1() * pow(2, -43));
eph.setAf0(subframe_1->af_0() * pow(2, -31));
eph.setSvHealth(subframe_1->sv_health());
}
// Subframe 3
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
gps_t subframe(&stream);
gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
eph.setCic(subframe_3->c_ic() * pow(2, -29));
eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
eph.setCis(subframe_3->c_is() * pow(2, -29));
eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
eph.setCrc(subframe_3->c_rc() * pow(2, -5));
eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
eph.setIode(subframe_3->iode());
eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
}
// Subframe 2
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][2]);
gps_t subframe(&stream);
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
eph.setCrs(subframe_2->c_rs() * pow(2, -5));
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
eph.setCuc(subframe_2->c_uc() * pow(2, -29));
eph.setEcc(subframe_2->e() * pow(2, -33));
eph.setCus(subframe_2->c_us() * pow(2, -29));
eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0));
eph.setToe(subframe_2->t_oe() * pow(2, 4));
// Subframe 4
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][4]);
gps_t subframe(&stream);
gps_t::subframe_4_t* subframe_4 = static_cast<gps_t::subframe_4_t*>(subframe.body());
// This is page 18, why is the page id 56?
if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) {
auto iono = static_cast<gps_t::subframe_4_t::ionosphere_data_t*>(subframe_4->body());
double a0 = iono->a0() * pow(2, -30);
double a1 = iono->a1() * pow(2, -27);
double a2 = iono->a2() * pow(2, -24);
double a3 = iono->a3() * pow(2, -24);
eph.setIonoAlpha({a0, a1, a2, a3});
double b0 = iono->b0() * pow(2, 11);
double b1 = iono->b1() * pow(2, 14);
double b2 = iono->b2() * pow(2, 16);
double b3 = iono->b3() * pow(2, 16);
eph.setIonoBeta({b0, b1, b2, b3});
}
}
// Subframe 3
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
gps_t subframe(&stream);
gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
eph.setCic(subframe_3->c_ic() * pow(2, -29));
eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
eph.setCis(subframe_3->c_is() * pow(2, -29));
eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
eph.setCrc(subframe_3->c_rc() * pow(2, -5));
eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
eph.setIode(subframe_3->iode());
eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
}
return capnp::messageToFlatArray(msg_builder);
}
return kj::Array<capnp::word>();
}
// Subframe 4
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][4]);
gps_t subframe(&stream);
gps_t::subframe_4_t* subframe_4 = static_cast<gps_t::subframe_4_t*>(subframe.body());
// This is page 18, why is the page id 56?
if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) {
auto iono = static_cast<gps_t::subframe_4_t::ionosphere_data_t*>(subframe_4->body());
double a0 = iono->a0() * pow(2, -30);
double a1 = iono->a1() * pow(2, -27);
double a2 = iono->a2() * pow(2, -24);
double a3 = iono->a3() * pow(2, -24);
eph.setIonoAlpha({a0, a1, a2, a3});
double b0 = iono->b0() * pow(2, 11);
double b1 = iono->b1() * pow(2, 14);
double b2 = iono->b2() * pow(2, 16);
double b3 = iono->b3() * pow(2, 16);
eph.setIonoBeta({b0, b1, b2, b3});
}
}
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);
default:
return kj::Array<capnp::word>();
}
return kj::Array<capnp::word>();
}
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {

2
selfdrive/locationd/ublox_msg.h
Unescape View File

@ -102,6 +102,8 @@ class UbloxMsgParser {
inline bool valid();
inline bool valid_so_far();
kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
std::unordered_map<int, uint64_t> gps_sat_tow_count;

Write Preview
Loading…
Cancel
Save