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

Parse GLONASS ephemeris from mystery satellites (#27541)

Browse Source 
parse unknown glonass
old-commit-hash: 7d32554fe3
beeps
Harald Schäfer 2 years ago committed by GitHub
parent 9c27549946
commit faf6e27f46
3 changed files with 48 additions and 23 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. 62
      system/ubloxd/ublox_msg.cc
  2. 6
      system/ubloxd/ublox_msg.h
  3. 3
      system/ubloxd/ubloxd.cc

62
system/ubloxd/ublox_msg.cc
Unescape View File

@ -81,7 +81,8 @@ inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) {
return year;
}
bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
last_log_time = log_time;
int needed = needed_bytes();
if(needed > 0) {
bytes_consumed = std::min((uint32_t)needed, incoming_data_len );
@ -266,12 +267,8 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
}
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>();
}
// This parser assumes that no 2 satellites of the same frequency
// can be in view at the same time
auto body = *msg->body();
assert(body.size() == 4);
{
@ -284,23 +281,48 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
kaitai::kstream stream(string_data);
glonass_t gl_string(&stream);
int string_number = gl_string.string_number();
if (string_number < 1 || 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();
// Check if new string either has same superframe_id or log transmission times make sense
bool superframe_unknown = false;
bool needs_clear = false;
for (int i = 1; i <= 5; i++) {
if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end())
continue;
if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) {
superframe_unknown = true;
}
else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
needs_clear = true;
}
glonass_strings[msg->sv_id()][string_number] = string_data;
// Check if string times add up to being from the same frame
// If superframe is known this is redundant
// Strings are sent 2s apart and frames are 30s apart
if (superframe_unknown &&
std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10)
needs_clear = true;
}
if (needs_clear) {
glonass_strings[msg->freq_id()].clear();
glonass_string_superframes[msg->freq_id()].clear();
glonass_string_times[msg->freq_id()].clear();
}
glonass_strings[msg->freq_id()][string_number] = string_data;
glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number();
glonass_string_times[msg->freq_id()][string_number] = last_log_time;
}
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>();
}
// publish if strings 1-5 have been collected
if (glonass_strings[msg->sv_id()].size() != 5) {
if (glonass_strings[msg->freq_id()].size() != 5) {
return kj::Array<capnp::word>();
}
@ -311,7 +333,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
// string number 1
{
kaitai::kstream stream(glonass_strings[msg->sv_id()][1]);
kaitai::kstream stream(glonass_strings[msg->freq_id()][1]);
glonass_t gl_stream(&stream);
glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
@ -324,7 +346,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
// string number 2
{
kaitai::kstream stream(glonass_strings[msg->sv_id()][2]);
kaitai::kstream stream(glonass_strings[msg->freq_id()][2]);
glonass_t gl_stream(&stream);
glonass_t::string_2_t* data = static_cast<glonass_t::string_2_t*>(gl_stream.data());
@ -338,7 +360,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
// string number 3
{
kaitai::kstream stream(glonass_strings[msg->sv_id()][3]);
kaitai::kstream stream(glonass_strings[msg->freq_id()][3]);
glonass_t gl_stream(&stream);
glonass_t::string_3_t* data = static_cast<glonass_t::string_3_t*>(gl_stream.data());
@ -352,7 +374,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
// string number 4
{
kaitai::kstream stream(glonass_strings[msg->sv_id()][4]);
kaitai::kstream stream(glonass_strings[msg->freq_id()][4]);
glonass_t gl_stream(&stream);
glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
@ -370,7 +392,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
// string number 5
{
kaitai::kstream stream(glonass_strings[msg->sv_id()][5]);
kaitai::kstream stream(glonass_strings[msg->freq_id()][5]);
glonass_t gl_stream(&stream);
glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
@ -399,7 +421,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
eph.setSecond((eph.getTk() & 0x1) * 30);
}
glonass_strings[msg->sv_id()].clear();
glonass_strings[msg->freq_id()].clear();
return capnp::messageToFlatArray(msg_builder);
}

6
system/ubloxd/ublox_msg.h
Unescape View File

@ -86,7 +86,7 @@ namespace ublox {
class UbloxMsgParser {
public:
bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
bool add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
inline void reset() {bytes_in_parse_buf = 0;}
inline int needed_bytes();
inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);}
@ -109,6 +109,7 @@ class UbloxMsgParser {
std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
float last_log_time = 0.0;
size_t bytes_in_parse_buf = 0;
uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
@ -119,5 +120,6 @@ class UbloxMsgParser {
{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;
std::unordered_map<int, std::unordered_map<int, long>> glonass_string_times;
std::unordered_map<int, std::unordered_map<int, int>> glonass_string_superframes;
};

3
system/ubloxd/ubloxd.cc
Unescape View File

@ -35,6 +35,7 @@ int main() {
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto ubloxRaw = event.getUbloxRaw();
float log_time = 1e-9 * event.getLogMonoTime();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
@ -42,7 +43,7 @@ int main() {
while(bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
if(parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
try {
auto ublox_msg = parser.gen_msg();

Write Preview
Loading…
Cancel
Save