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>
old-commit-hash: 549e4d9636
beeps
Kurt Nistelberger 2 years ago committed by GitHub
parent 0135d85a6d
commit 2e62b4f08c
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
Unified 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); return capnp::messageToFlatArray(msg_builder);
} }
kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(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(); 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) { // Collect subframes in map and parse when we have all the parts
// 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 kaitai::kstream stream(subframe_data);
assert(body.size() == 10); gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
std::string subframe_data; int sv_id = msg->sv_id();
subframe_data.reserve(30); uint64_t tow_counter = subframe.how()->tow_count();
for (uint32_t word : body) {
word = word >> 6; // TODO: Verify parity bool clear_buffer = subframe_id == 1;
subframe_data.push_back(word >> 16); if (gps_sat_tow_count.count(sv_id) != 0) {
subframe_data.push_back(word >> 8); int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id];
subframe_data.push_back(word >> 0); 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); gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id(); gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
int sv_id = msg->sv_id();
uint64_t tow_counter = subframe.how()->tow_count(); eph.setGpsWeek(subframe_1->week_no());
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
bool clear_buffer = subframe_id == 1; eph.setToc(subframe_1->t_oc() * pow(2, 4));
if (gps_sat_tow_count.count(sv_id) != 0) { eph.setAf2(subframe_1->af_2() * pow(2, -55));
int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id]; eph.setAf1(subframe_1->af_1() * pow(2, -43));
clear_buffer |= counter_diff != 1 && counter_diff != -100798; eph.setAf0(subframe_1->af_0() * pow(2, -31));
} eph.setSvHealth(subframe_1->sv_health());
if (clear_buffer) gps_subframes[sv_id].clear(); }
gps_subframes[sv_id][subframe_id] = subframe_data; // Subframe 2
gps_sat_tow_count[sv_id] = tow_counter; {
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) { // Subframe 3
MessageBuilder msg_builder; {
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
eph.setSvId(msg->sv_id()); gps_t subframe(&stream);
gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
// Subframe 1
{ eph.setCic(subframe_3->c_ic() * pow(2, -29));
kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
gps_t subframe(&stream); eph.setCis(subframe_3->c_is() * pow(2, -29));
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body()); eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
eph.setCrc(subframe_3->c_rc() * pow(2, -5));
eph.setGpsWeek(subframe_1->week_no()); eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
eph.setTgd(subframe_1->t_gd() * pow(2, -31)); eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
eph.setToc(subframe_1->t_oc() * pow(2, 4)); eph.setIode(subframe_3->iode());
eph.setAf2(subframe_1->af_2() * pow(2, -55)); eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
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 2 // Subframe 4
{ {
kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); kaitai::kstream stream(gps_subframes[msg->sv_id()][4]);
gps_t subframe(&stream); gps_t subframe(&stream);
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body()); gps_t::subframe_4_t* subframe_4 = static_cast<gps_t::subframe_4_t*>(subframe.body());
eph.setCrs(subframe_2->c_rs() * pow(2, -5)); // This is page 18, why is the page id 56?
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) {
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); auto iono = static_cast<gps_t::subframe_4_t::ionosphere_data_t*>(subframe_4->body());
eph.setCuc(subframe_2->c_uc() * pow(2, -29)); double a0 = iono->a0() * pow(2, -30);
eph.setEcc(subframe_2->e() * pow(2, -33)); double a1 = iono->a1() * pow(2, -27);
eph.setCus(subframe_2->c_us() * pow(2, -29)); double a2 = iono->a2() * pow(2, -24);
eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); double a3 = iono->a3() * pow(2, -24);
eph.setToe(subframe_2->t_oe() * pow(2, 4)); 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 return capnp::messageToFlatArray(msg_builder);
{ }
kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); return kj::Array<capnp::word>();
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 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) { 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();
inline bool valid_so_far(); 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, std::unordered_map<int, std::string>> gps_subframes;
std::unordered_map<int, uint64_t> gps_sat_tow_count; std::unordered_map<int, uint64_t> gps_sat_tow_count;

Write Preview
Loading…
Cancel
Save