From eacacb8fba8a58f32132c06e16012f99c2ee5f26 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 14 Apr 2021 16:17:30 +0200 Subject: [PATCH] Cleanup ubloxd message parsing (#20664) * use kaitai to generate parsers * add scons flag to generate parsers * add outer layer ephemeris parsing * create ephemeris packet at the right time * parse subframe 1 and 2 * add more fields * add generated files to release * fix signs * no cast * add unordered map include * add exception handling * small cleanup * parse iono data * cleanup --- SConstruct | 5 + phonelibs/SConscript | 2 + phonelibs/kaitai/custom_decoder.h | 16 + phonelibs/kaitai/exceptions.h | 189 +++++++ phonelibs/kaitai/kaitaistream.cpp | 689 ++++++++++++++++++++++++++ phonelibs/kaitai/kaitaistream.h | 268 ++++++++++ phonelibs/kaitai/kaitaistruct.h | 20 + release/files_common | 7 + selfdrive/locationd/SConscript | 11 +- selfdrive/locationd/generated/gps.cpp | 325 ++++++++++++ selfdrive/locationd/generated/gps.h | 359 ++++++++++++++ selfdrive/locationd/generated/ubx.cpp | 340 +++++++++++++ selfdrive/locationd/generated/ubx.h | 410 +++++++++++++++ selfdrive/locationd/gps.ksy | 189 +++++++ selfdrive/locationd/ublox_msg.cc | 588 ++++++++++------------ selfdrive/locationd/ublox_msg.h | 215 ++------ selfdrive/locationd/ubloxd.cc | 59 +-- selfdrive/locationd/ubx.ksy | 259 ++++++++++ 18 files changed, 3404 insertions(+), 547 deletions(-) create mode 100644 phonelibs/kaitai/custom_decoder.h create mode 100644 phonelibs/kaitai/exceptions.h create mode 100644 phonelibs/kaitai/kaitaistream.cpp create mode 100644 phonelibs/kaitai/kaitaistream.h create mode 100644 phonelibs/kaitai/kaitaistruct.h create mode 100644 selfdrive/locationd/generated/gps.cpp create mode 100644 selfdrive/locationd/generated/gps.h create mode 100644 selfdrive/locationd/generated/ubx.cpp create mode 100644 selfdrive/locationd/generated/ubx.h create mode 100644 selfdrive/locationd/gps.ksy create mode 100644 selfdrive/locationd/ubx.ksy diff --git a/SConstruct b/SConstruct index 91c7186d..3a5cd5f2 100644 --- a/SConstruct +++ b/SConstruct @@ -13,6 +13,10 @@ AddOption('--test', action='store_true', help='build test files') +AddOption('--kaitai', + action='store_true', + help='Regenerate kaitai struct parsers') + AddOption('--asan', action='store_true', help='turn on ASAN') @@ -181,6 +185,7 @@ env = Environment( "#phonelibs/snpe/include", "#phonelibs/nanovg", "#phonelibs/qrcode", + "#phonelibs", "#selfdrive/boardd", "#selfdrive/common", "#selfdrive/camerad", diff --git a/phonelibs/SConscript b/phonelibs/SConscript index a23b02a8..5a7a62ae 100644 --- a/phonelibs/SConscript +++ b/phonelibs/SConscript @@ -2,3 +2,5 @@ Import('env') env.Library('json11', ['json11/json11.cpp']) env.Append(CPPPATH=[Dir('json11')]) + +env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) diff --git a/phonelibs/kaitai/custom_decoder.h b/phonelibs/kaitai/custom_decoder.h new file mode 100644 index 00000000..6da7f5fd --- /dev/null +++ b/phonelibs/kaitai/custom_decoder.h @@ -0,0 +1,16 @@ +#ifndef KAITAI_CUSTOM_DECODER_H +#define KAITAI_CUSTOM_DECODER_H + +#include + +namespace kaitai { + +class custom_decoder { +public: + virtual ~custom_decoder() {}; + virtual std::string decode(std::string src) = 0; +}; + +} + +#endif diff --git a/phonelibs/kaitai/exceptions.h b/phonelibs/kaitai/exceptions.h new file mode 100644 index 00000000..5c09c467 --- /dev/null +++ b/phonelibs/kaitai/exceptions.h @@ -0,0 +1,189 @@ +#ifndef KAITAI_EXCEPTIONS_H +#define KAITAI_EXCEPTIONS_H + +#include + +#include +#include + +// We need to use "noexcept" in virtual destructor of our exceptions +// subclasses. Different compilers have different ideas on how to +// achieve that: C++98 compilers prefer `throw()`, C++11 and later +// use `noexcept`. We define KS_NOEXCEPT macro for that. + +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) +#define KS_NOEXCEPT noexcept +#else +#define KS_NOEXCEPT throw() +#endif + +namespace kaitai { + +/** + * Common ancestor for all error originating from Kaitai Struct usage. + * Stores KSY source path, pointing to an element supposedly guilty of + * an error. + */ +class kstruct_error: public std::runtime_error { +public: + kstruct_error(const std::string what, const std::string src_path): + std::runtime_error(src_path + ": " + what), + m_src_path(src_path) + { + } + + virtual ~kstruct_error() KS_NOEXCEPT {}; + +protected: + const std::string m_src_path; +}; + +/** + * Error that occurs when default endianness should be decided with + * a switch, but nothing matches (although using endianness expression + * implies that there should be some positive result). + */ +class undecided_endianness_error: public kstruct_error { +public: + undecided_endianness_error(const std::string src_path): + kstruct_error("unable to decide on endianness for a type", src_path) + { + } + + virtual ~undecided_endianness_error() KS_NOEXCEPT {}; +}; + +/** + * Common ancestor for all validation failures. Stores pointer to + * KaitaiStream IO object which was involved in an error. + */ +class validation_failed_error: public kstruct_error { +public: + validation_failed_error(const std::string what, kstream* io, const std::string src_path): + kstruct_error("at pos " + kstream::to_string(static_cast(io->pos())) + ": validation failed: " + what, src_path), + m_io(io) + { + } + +// "at pos #{io.pos}: validation failed: #{msg}" + + virtual ~validation_failed_error() KS_NOEXCEPT {}; + +protected: + kstream* m_io; +}; + +/** + * Signals validation failure: we required "actual" value to be equal to + * "expected", but it turned out that it's not. + */ +template +class validation_not_equal_error: public validation_failed_error { +public: + validation_not_equal_error(const T& expected, const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not equal", io, src_path), + m_expected(expected), + m_actual(actual) + { + } + + // "not equal, expected #{expected.inspect}, but got #{actual.inspect}" + + virtual ~validation_not_equal_error() KS_NOEXCEPT {}; + +protected: + const T& m_expected; + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to be greater + * than or equal to "min", but it turned out that it's not. + */ +template +class validation_less_than_error: public validation_failed_error { +public: + validation_less_than_error(const T& min, const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not in range", io, src_path), + m_min(min), + m_actual(actual) + { + } + + // "not in range, min #{min.inspect}, but got #{actual.inspect}" + + virtual ~validation_less_than_error() KS_NOEXCEPT {}; + +protected: + const T& m_min; + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to be less + * than or equal to "max", but it turned out that it's not. + */ +template +class validation_greater_than_error: public validation_failed_error { +public: + validation_greater_than_error(const T& max, const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not in range", io, src_path), + m_max(max), + m_actual(actual) + { + } + + // "not in range, max #{max.inspect}, but got #{actual.inspect}" + + virtual ~validation_greater_than_error() KS_NOEXCEPT {}; + +protected: + const T& m_max; + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to be from + * the list, but it turned out that it's not. + */ +template +class validation_not_any_of_error: public validation_failed_error { +public: + validation_not_any_of_error(const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not any of the list", io, src_path), + m_actual(actual) + { + } + + // "not any of the list, got #{actual.inspect}" + + virtual ~validation_not_any_of_error() KS_NOEXCEPT {}; + +protected: + const T& m_actual; +}; + +/** + * Signals validation failure: we required "actual" value to match + * the expression, but it turned out that it doesn't. + */ +template +class validation_expr_error: public validation_failed_error { +public: + validation_expr_error(const T& actual, kstream* io, const std::string src_path): + validation_failed_error("not matching the expression", io, src_path), + m_actual(actual) + { + } + + // "not matching the expression, got #{actual.inspect}" + + virtual ~validation_expr_error() KS_NOEXCEPT {}; + +protected: + const T& m_actual; +}; + +} + +#endif diff --git a/phonelibs/kaitai/kaitaistream.cpp b/phonelibs/kaitai/kaitaistream.cpp new file mode 100644 index 00000000..d82ddb7e --- /dev/null +++ b/phonelibs/kaitai/kaitaistream.cpp @@ -0,0 +1,689 @@ +#include + +#if defined(__APPLE__) +#include +#include +#define bswap_16(x) OSSwapInt16(x) +#define bswap_32(x) OSSwapInt32(x) +#define bswap_64(x) OSSwapInt64(x) +#define __BYTE_ORDER BYTE_ORDER +#define __BIG_ENDIAN BIG_ENDIAN +#define __LITTLE_ENDIAN LITTLE_ENDIAN +#elif defined(_MSC_VER) // !__APPLE__ +#include +#define __LITTLE_ENDIAN 1234 +#define __BIG_ENDIAN 4321 +#define __BYTE_ORDER __LITTLE_ENDIAN +#define bswap_16(x) _byteswap_ushort(x) +#define bswap_32(x) _byteswap_ulong(x) +#define bswap_64(x) _byteswap_uint64(x) +#else // !__APPLE__ or !_MSC_VER +#include +#include +#endif + +#include +#include +#include + +kaitai::kstream::kstream(std::istream* io) { + m_io = io; + init(); +} + +kaitai::kstream::kstream(std::string& data): m_io_str(data) { + m_io = &m_io_str; + init(); +} + +void kaitai::kstream::init() { + exceptions_enable(); + align_to_byte(); +} + +void kaitai::kstream::close() { + // m_io->close(); +} + +void kaitai::kstream::exceptions_enable() const { + m_io->exceptions( + std::istream::eofbit | + std::istream::failbit | + std::istream::badbit + ); +} + +// ======================================================================== +// Stream positioning +// ======================================================================== + +bool kaitai::kstream::is_eof() const { + if (m_bits_left > 0) { + return false; + } + char t; + m_io->exceptions( + std::istream::badbit + ); + m_io->get(t); + if (m_io->eof()) { + m_io->clear(); + exceptions_enable(); + return true; + } else { + m_io->unget(); + exceptions_enable(); + return false; + } +} + +void kaitai::kstream::seek(uint64_t pos) { + m_io->seekg(pos); +} + +uint64_t kaitai::kstream::pos() { + return m_io->tellg(); +} + +uint64_t kaitai::kstream::size() { + std::iostream::pos_type cur_pos = m_io->tellg(); + m_io->seekg(0, std::ios::end); + std::iostream::pos_type len = m_io->tellg(); + m_io->seekg(cur_pos); + return len; +} + +// ======================================================================== +// Integer numbers +// ======================================================================== + +// ------------------------------------------------------------------------ +// Signed +// ------------------------------------------------------------------------ + +int8_t kaitai::kstream::read_s1() { + char t; + m_io->get(t); + return t; +} + +// ........................................................................ +// Big-endian +// ........................................................................ + +int16_t kaitai::kstream::read_s2be() { + int16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +int32_t kaitai::kstream::read_s4be() { + int32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +int64_t kaitai::kstream::read_s8be() { + int64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ........................................................................ +// Little-endian +// ........................................................................ + +int16_t kaitai::kstream::read_s2le() { + int16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +int32_t kaitai::kstream::read_s4le() { + int32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +int64_t kaitai::kstream::read_s8le() { + int64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ------------------------------------------------------------------------ +// Unsigned +// ------------------------------------------------------------------------ + +uint8_t kaitai::kstream::read_u1() { + char t; + m_io->get(t); + return t; +} + +// ........................................................................ +// Big-endian +// ........................................................................ + +uint16_t kaitai::kstream::read_u2be() { + uint16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +uint32_t kaitai::kstream::read_u4be() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +uint64_t kaitai::kstream::read_u8be() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ........................................................................ +// Little-endian +// ........................................................................ + +uint16_t kaitai::kstream::read_u2le() { + uint16_t t; + m_io->read(reinterpret_cast(&t), 2); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_16(t); +#endif + return t; +} + +uint32_t kaitai::kstream::read_u4le() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_32(t); +#endif + return t; +} + +uint64_t kaitai::kstream::read_u8le() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_64(t); +#endif + return t; +} + +// ======================================================================== +// Floating point numbers +// ======================================================================== + +// ........................................................................ +// Big-endian +// ........................................................................ + +float kaitai::kstream::read_f4be() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_32(t); +#endif + return reinterpret_cast(t); +} + +double kaitai::kstream::read_f8be() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __LITTLE_ENDIAN + t = bswap_64(t); +#endif + return reinterpret_cast(t); +} + +// ........................................................................ +// Little-endian +// ........................................................................ + +float kaitai::kstream::read_f4le() { + uint32_t t; + m_io->read(reinterpret_cast(&t), 4); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_32(t); +#endif + return reinterpret_cast(t); +} + +double kaitai::kstream::read_f8le() { + uint64_t t; + m_io->read(reinterpret_cast(&t), 8); +#if __BYTE_ORDER == __BIG_ENDIAN + t = bswap_64(t); +#endif + return reinterpret_cast(t); +} + +// ======================================================================== +// Unaligned bit values +// ======================================================================== + +void kaitai::kstream::align_to_byte() { + m_bits_left = 0; + m_bits = 0; +} + +uint64_t kaitai::kstream::read_bits_int_be(int n) { + int bits_needed = n - m_bits_left; + if (bits_needed > 0) { + // 1 bit => 1 byte + // 8 bits => 1 byte + // 9 bits => 2 bytes + int bytes_needed = ((bits_needed - 1) / 8) + 1; + if (bytes_needed > 8) + throw std::runtime_error("read_bits_int: more than 8 bytes requested"); + char buf[8]; + m_io->read(buf, bytes_needed); + for (int i = 0; i < bytes_needed; i++) { + uint8_t b = buf[i]; + m_bits <<= 8; + m_bits |= b; + m_bits_left += 8; + } + } + + // raw mask with required number of 1s, starting from lowest bit + uint64_t mask = get_mask_ones(n); + // shift mask to align with highest bits available in @bits + int shift_bits = m_bits_left - n; + mask <<= shift_bits; + // derive reading result + uint64_t res = (m_bits & mask) >> shift_bits; + // clear top bits that we've just read => AND with 1s + m_bits_left -= n; + mask = get_mask_ones(m_bits_left); + m_bits &= mask; + + return res; +} + +// Deprecated, use read_bits_int_be() instead. +uint64_t kaitai::kstream::read_bits_int(int n) { + return read_bits_int_be(n); +} + +uint64_t kaitai::kstream::read_bits_int_le(int n) { + int bits_needed = n - m_bits_left; + if (bits_needed > 0) { + // 1 bit => 1 byte + // 8 bits => 1 byte + // 9 bits => 2 bytes + int bytes_needed = ((bits_needed - 1) / 8) + 1; + if (bytes_needed > 8) + throw std::runtime_error("read_bits_int_le: more than 8 bytes requested"); + char buf[8]; + m_io->read(buf, bytes_needed); + for (int i = 0; i < bytes_needed; i++) { + uint8_t b = buf[i]; + m_bits |= (static_cast(b) << m_bits_left); + m_bits_left += 8; + } + } + + // raw mask with required number of 1s, starting from lowest bit + uint64_t mask = get_mask_ones(n); + // derive reading result + uint64_t res = m_bits & mask; + // remove bottom bits that we've just read by shifting + m_bits >>= n; + m_bits_left -= n; + + return res; +} + +uint64_t kaitai::kstream::get_mask_ones(int n) { + if (n == 64) { + return 0xFFFFFFFFFFFFFFFF; + } else { + return ((uint64_t) 1 << n) - 1; + } +} + +// ======================================================================== +// Byte arrays +// ======================================================================== + +std::string kaitai::kstream::read_bytes(std::streamsize len) { + std::vector result(len); + + // NOTE: streamsize type is signed, negative values are only *supposed* to not be used. + // http://en.cppreference.com/w/cpp/io/streamsize + if (len < 0) { + throw std::runtime_error("read_bytes: requested a negative amount"); + } + + if (len > 0) { + m_io->read(&result[0], len); + } + + return std::string(result.begin(), result.end()); +} + +std::string kaitai::kstream::read_bytes_full() { + std::iostream::pos_type p1 = m_io->tellg(); + m_io->seekg(0, std::ios::end); + std::iostream::pos_type p2 = m_io->tellg(); + size_t len = p2 - p1; + + // Note: this requires a std::string to be backed with a + // contiguous buffer. Officially, it's a only requirement since + // C++11 (C++98 and C++03 didn't have this requirement), but all + // major implementations had contiguous buffers anyway. + std::string result(len, ' '); + m_io->seekg(p1); + m_io->read(&result[0], len); + + return result; +} + +std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) { + std::string result; + std::getline(*m_io, result, term); + if (m_io->eof()) { + // encountered EOF + if (eos_error) { + throw std::runtime_error("read_bytes_term: encountered EOF"); + } + } else { + // encountered terminator + if (include) + result.push_back(term); + if (!consume) + m_io->unget(); + } + return result; +} + +std::string kaitai::kstream::ensure_fixed_contents(std::string expected) { + std::string actual = read_bytes(expected.length()); + + if (actual != expected) { + // NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr. + throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data"); + } + + return actual; +} + +std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) { + std::size_t new_len = src.length(); + + while (new_len > 0 && src[new_len - 1] == pad_byte) + new_len--; + + return src.substr(0, new_len); +} + +std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) { + std::size_t new_len = 0; + std::size_t max_len = src.length(); + + while (new_len < max_len && src[new_len] != term) + new_len++; + + if (include && new_len < max_len) + new_len++; + + return src.substr(0, new_len); +} + +// ======================================================================== +// Byte array processing +// ======================================================================== + +std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) { + size_t len = data.length(); + std::string result(len, ' '); + + for (size_t i = 0; i < len; i++) + result[i] = data[i] ^ key; + + return result; +} + +std::string kaitai::kstream::process_xor_many(std::string data, std::string key) { + size_t len = data.length(); + size_t kl = key.length(); + std::string result(len, ' '); + + size_t ki = 0; + for (size_t i = 0; i < len; i++) { + result[i] = data[i] ^ key[ki]; + ki++; + if (ki >= kl) + ki = 0; + } + + return result; +} + +std::string kaitai::kstream::process_rotate_left(std::string data, int amount) { + size_t len = data.length(); + std::string result(len, ' '); + + for (size_t i = 0; i < len; i++) { + uint8_t bits = data[i]; + result[i] = (bits << amount) | (bits >> (8 - amount)); + } + + return result; +} + +#ifdef KS_ZLIB +#include + +std::string kaitai::kstream::process_zlib(std::string data) { + int ret; + + unsigned char *src_ptr = reinterpret_cast(&data[0]); + std::stringstream dst_strm; + + z_stream strm; + strm.zalloc = Z_NULL; + strm.zfree = Z_NULL; + strm.opaque = Z_NULL; + + ret = inflateInit(&strm); + if (ret != Z_OK) + throw std::runtime_error("process_zlib: inflateInit error"); + + strm.next_in = src_ptr; + strm.avail_in = data.length(); + + unsigned char outbuffer[ZLIB_BUF_SIZE]; + std::string outstring; + + // get the decompressed bytes blockwise using repeated calls to inflate + do { + strm.next_out = reinterpret_cast(outbuffer); + strm.avail_out = sizeof(outbuffer); + + ret = inflate(&strm, 0); + + if (outstring.size() < strm.total_out) + outstring.append(reinterpret_cast(outbuffer), strm.total_out - outstring.size()); + } while (ret == Z_OK); + + if (ret != Z_STREAM_END) { // an error occurred that was not EOF + std::ostringstream exc_msg; + exc_msg << "process_zlib: error #" << ret << "): " << strm.msg; + throw std::runtime_error(exc_msg.str()); + } + + if (inflateEnd(&strm) != Z_OK) + throw std::runtime_error("process_zlib: inflateEnd error"); + + return outstring; +} +#endif + +// ======================================================================== +// Misc utility methods +// ======================================================================== + +int kaitai::kstream::mod(int a, int b) { + if (b <= 0) + throw std::invalid_argument("mod: divisor b <= 0"); + int r = a % b; + if (r < 0) + r += b; + return r; +} + +#include +std::string kaitai::kstream::to_string(int val) { + // if int is 32 bits, "-2147483648" is the longest string representation + // => 11 chars + zero => 12 chars + // if int is 64 bits, "-9223372036854775808" is the longest + // => 20 chars + zero => 21 chars + char buf[25]; + int got_len = snprintf(buf, sizeof(buf), "%d", val); + + // should never happen, but check nonetheless + if (got_len > sizeof(buf)) + throw std::invalid_argument("to_string: integer is longer than string buffer"); + + return std::string(buf); +} + +#include +std::string kaitai::kstream::reverse(std::string val) { + std::reverse(val.begin(), val.end()); + + return val; +} + +uint8_t kaitai::kstream::byte_array_min(const std::string val) { + uint8_t min = 0xff; // UINT8_MAX + std::string::const_iterator end = val.end(); + for (std::string::const_iterator it = val.begin(); it != end; ++it) { + uint8_t cur = static_cast(*it); + if (cur < min) { + min = cur; + } + } + return min; +} + +uint8_t kaitai::kstream::byte_array_max(const std::string val) { + uint8_t max = 0; // UINT8_MIN + std::string::const_iterator end = val.end(); + for (std::string::const_iterator it = val.begin(); it != end; ++it) { + uint8_t cur = static_cast(*it); + if (cur > max) { + max = cur; + } + } + return max; +} + +// ======================================================================== +// Other internal methods +// ======================================================================== + +#ifndef KS_STR_DEFAULT_ENCODING +#define KS_STR_DEFAULT_ENCODING "UTF-8" +#endif + +#ifdef KS_STR_ENCODING_ICONV + +#include +#include +#include + +std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) { + iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str()); + + if (cd == (iconv_t) -1) { + if (errno == EINVAL) { + throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested"); + } else { + throw std::runtime_error("bytes_to_str: error opening iconv"); + } + } + + size_t src_len = src.length(); + size_t src_left = src_len; + + // Start with a buffer length of double the source length. + size_t dst_len = src_len * 2; + std::string dst(dst_len, ' '); + size_t dst_left = dst_len; + + char *src_ptr = &src[0]; + char *dst_ptr = &dst[0]; + + while (true) { + size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left); + + if (res == (size_t) -1) { + if (errno == E2BIG) { + // dst buffer is not enough to accomodate whole string + // enlarge the buffer and try again + size_t dst_used = dst_len - dst_left; + dst_left += dst_len; + dst_len += dst_len; + dst.resize(dst_len); + + // dst.resize might have allocated destination buffer in another area + // of memory, thus our previous pointer "dst" will be invalid; re-point + // it using "dst_used". + dst_ptr = &dst[dst_used]; + } else { + throw std::runtime_error("bytes_to_str: iconv error"); + } + } else { + // conversion successful + dst.resize(dst_len - dst_left); + break; + } + } + + if (iconv_close(cd) != 0) { + throw std::runtime_error("bytes_to_str: iconv close error"); + } + + return dst; +} +#elif defined(KS_STR_ENCODING_NONE) +std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) { + return src; +} +#else +#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE +#endif diff --git a/phonelibs/kaitai/kaitaistream.h b/phonelibs/kaitai/kaitaistream.h new file mode 100644 index 00000000..e7f4c6ce --- /dev/null +++ b/phonelibs/kaitai/kaitaistream.h @@ -0,0 +1,268 @@ +#ifndef KAITAI_STREAM_H +#define KAITAI_STREAM_H + +// Kaitai Struct runtime API version: x.y.z = 'xxxyyyzzz' decimal +#define KAITAI_STRUCT_VERSION 9000L + +#include +#include +#include +#include + +namespace kaitai { + +/** + * Kaitai Stream class (kaitai::kstream) is an implementation of + * Kaitai Struct stream API + * for C++/STL. It's implemented as a wrapper over generic STL std::istream. + * + * It provides a wide variety of simple methods to read (parse) binary + * representations of primitive types, such as integer and floating + * point numbers, byte arrays and strings, and also provides stream + * positioning / navigation methods with unified cross-language and + * cross-toolkit semantics. + * + * Typically, end users won't access Kaitai Stream class manually, but would + * describe a binary structure format using .ksy language and then would use + * Kaitai Struct compiler to generate source code in desired target language. + * That code, in turn, would use this class and API to do the actual parsing + * job. + */ +class kstream { +public: + /** + * Constructs new Kaitai Stream object, wrapping a given std::istream. + * \param io istream object to use for this Kaitai Stream + */ + kstream(std::istream* io); + + /** + * Constructs new Kaitai Stream object, wrapping a given in-memory data + * buffer. + * \param data data buffer to use for this Kaitai Stream + */ + kstream(std::string& data); + + void close(); + + /** @name Stream positioning */ + //@{ + /** + * Check if stream pointer is at the end of stream. Note that the semantics + * are different from traditional STL semantics: one does *not* need to do a + * read (which will fail) after the actual end of the stream to trigger EOF + * flag, which can be accessed after that read. It is sufficient to just be + * at the end of the stream for this method to return true. + * \return "true" if we are located at the end of the stream. + */ + bool is_eof() const; + + /** + * Set stream pointer to designated position. + * \param pos new position (offset in bytes from the beginning of the stream) + */ + void seek(uint64_t pos); + + /** + * Get current position of a stream pointer. + * \return pointer position, number of bytes from the beginning of the stream + */ + uint64_t pos(); + + /** + * Get total size of the stream in bytes. + * \return size of the stream in bytes + */ + uint64_t size(); + //@} + + /** @name Integer numbers */ + //@{ + + // ------------------------------------------------------------------------ + // Signed + // ------------------------------------------------------------------------ + + int8_t read_s1(); + + // ........................................................................ + // Big-endian + // ........................................................................ + + int16_t read_s2be(); + int32_t read_s4be(); + int64_t read_s8be(); + + // ........................................................................ + // Little-endian + // ........................................................................ + + int16_t read_s2le(); + int32_t read_s4le(); + int64_t read_s8le(); + + // ------------------------------------------------------------------------ + // Unsigned + // ------------------------------------------------------------------------ + + uint8_t read_u1(); + + // ........................................................................ + // Big-endian + // ........................................................................ + + uint16_t read_u2be(); + uint32_t read_u4be(); + uint64_t read_u8be(); + + // ........................................................................ + // Little-endian + // ........................................................................ + + uint16_t read_u2le(); + uint32_t read_u4le(); + uint64_t read_u8le(); + + //@} + + /** @name Floating point numbers */ + //@{ + + // ........................................................................ + // Big-endian + // ........................................................................ + + float read_f4be(); + double read_f8be(); + + // ........................................................................ + // Little-endian + // ........................................................................ + + float read_f4le(); + double read_f8le(); + + //@} + + /** @name Unaligned bit values */ + //@{ + + void align_to_byte(); + uint64_t read_bits_int_be(int n); + uint64_t read_bits_int(int n); + uint64_t read_bits_int_le(int n); + + //@} + + /** @name Byte arrays */ + //@{ + + std::string read_bytes(std::streamsize len); + std::string read_bytes_full(); + std::string read_bytes_term(char term, bool include, bool consume, bool eos_error); + std::string ensure_fixed_contents(std::string expected); + + static std::string bytes_strip_right(std::string src, char pad_byte); + static std::string bytes_terminate(std::string src, char term, bool include); + static std::string bytes_to_str(std::string src, std::string src_enc); + + //@} + + /** @name Byte array processing */ + //@{ + + /** + * Performs a XOR processing with given data, XORing every byte of input with a single + * given value. + * @param data data to process + * @param key value to XOR with + * @return processed data + */ + static std::string process_xor_one(std::string data, uint8_t key); + + /** + * Performs a XOR processing with given data, XORing every byte of input with a key + * array, repeating key array many times, if necessary (i.e. if data array is longer + * than key array). + * @param data data to process + * @param key array of bytes to XOR with + * @return processed data + */ + static std::string process_xor_many(std::string data, std::string key); + + /** + * Performs a circular left rotation shift for a given buffer by a given amount of bits, + * using groups of 1 bytes each time. Right circular rotation should be performed + * using this procedure with corrected amount. + * @param data source data to process + * @param amount number of bits to shift by + * @return copy of source array with requested shift applied + */ + static std::string process_rotate_left(std::string data, int amount); + +#ifdef KS_ZLIB + /** + * Performs an unpacking ("inflation") of zlib-compressed data with usual zlib headers. + * @param data data to unpack + * @return unpacked data + * @throws IOException + */ + static std::string process_zlib(std::string data); +#endif + + //@} + + /** + * Performs modulo operation between two integers: dividend `a` + * and divisor `b`. Divisor `b` is expected to be positive. The + * result is always 0 <= x <= b - 1. + */ + static int mod(int a, int b); + + /** + * Converts given integer `val` to a decimal string representation. + * Should be used in place of std::to_string() (which is available only + * since C++11) in older C++ implementations. + */ + static std::string to_string(int val); + + /** + * Reverses given string `val`, so that the first character becomes the + * last and the last one becomes the first. This should be used to avoid + * the need of local variables at the caller. + */ + static std::string reverse(std::string val); + + /** + * Finds the minimal byte in a byte array, treating bytes as + * unsigned values. + * @param val byte array to scan + * @return minimal byte in byte array as integer + */ + static uint8_t byte_array_min(const std::string val); + + /** + * Finds the maximal byte in a byte array, treating bytes as + * unsigned values. + * @param val byte array to scan + * @return maximal byte in byte array as integer + */ + static uint8_t byte_array_max(const std::string val); + +private: + std::istream* m_io; + std::istringstream m_io_str; + int m_bits_left; + uint64_t m_bits; + + void init(); + void exceptions_enable() const; + + static uint64_t get_mask_ones(int n); + + static const int ZLIB_BUF_SIZE = 128 * 1024; +}; + +} + +#endif diff --git a/phonelibs/kaitai/kaitaistruct.h b/phonelibs/kaitai/kaitaistruct.h new file mode 100644 index 00000000..8172ede6 --- /dev/null +++ b/phonelibs/kaitai/kaitaistruct.h @@ -0,0 +1,20 @@ +#ifndef KAITAI_STRUCT_H +#define KAITAI_STRUCT_H + +#include + +namespace kaitai { + +class kstruct { +public: + kstruct(kstream *_io) { m__io = _io; } + virtual ~kstruct() {} +protected: + kstream *m__io; +public: + kstream *_io() { return m__io; } +}; + +} + +#endif diff --git a/release/files_common b/release/files_common index f76daaa3..a9bf88bd 100644 --- a/release/files_common +++ b/release/files_common @@ -283,6 +283,10 @@ selfdrive/locationd/SConscript selfdrive/locationd/ubloxd.cc selfdrive/locationd/ublox_msg.cc selfdrive/locationd/ublox_msg.h +selfdrive/locationd/generated/ubx.cpp +selfdrive/locationd/generated/ubx.h +selfdrive/locationd/generated/gps.cpp +selfdrive/locationd/generated/gps.h selfdrive/locationd/locationd.py selfdrive/locationd/paramsd.py @@ -448,6 +452,9 @@ phonelibs/qpoases/** phonelibs/qrcode/*.cc phonelibs/qrcode/*.hpp +phonelibs/kaitai/*.h +phonelibs/kaitai/*.cpp + phonelibs/libyuv/include/** phonelibs/libyuv/lib/** phonelibs/libyuv/larch64/** diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 38f0d026..d170cd04 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,5 +1,12 @@ Import('env', 'common', 'cereal', 'messaging') -loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread'] +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] -env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc"], LIBS=loc_libs) + +if GetOption('kaitai'): + generated = Dir('generated').srcnode().abspath + cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" + env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) + env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) + +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs) diff --git a/selfdrive/locationd/generated/gps.cpp b/selfdrive/locationd/generated/gps.cpp new file mode 100644 index 00000000..9b020735 --- /dev/null +++ b/selfdrive/locationd/generated/gps.cpp @@ -0,0 +1,325 @@ +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "gps.h" +#include "kaitai/exceptions.h" + +gps_t::gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = this; + m_tlm = 0; + m_how = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::_read() { + m_tlm = new tlm_t(m__io, this, m__root); + m_how = new how_t(m__io, this, m__root); + n_body = true; + switch (how()->subframe_id()) { + case 1: { + n_body = false; + m_body = new subframe_1_t(m__io, this, m__root); + break; + } + case 2: { + n_body = false; + m_body = new subframe_2_t(m__io, this, m__root); + break; + } + case 3: { + n_body = false; + m_body = new subframe_3_t(m__io, this, m__root); + break; + } + case 4: { + n_body = false; + m_body = new subframe_4_t(m__io, this, m__root); + break; + } + } +} + +gps_t::~gps_t() { + _clean_up(); +} + +void gps_t::_clean_up() { + if (m_tlm) { + delete m_tlm; m_tlm = 0; + } + if (m_how) { + delete m_how; m_how = 0; + } + if (!n_body) { + if (m_body) { + delete m_body; m_body = 0; + } + } +} + +gps_t::subframe_1_t::subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_af_0 = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_1_t::_read() { + m_week_no = m__io->read_bits_int_be(10); + m_code = m__io->read_bits_int_be(2); + m_sv_accuracy = m__io->read_bits_int_be(4); + m_sv_health = m__io->read_bits_int_be(6); + m_iodc_msb = m__io->read_bits_int_be(2); + m_l2_p_data_flag = m__io->read_bits_int_be(1); + m_reserved1 = m__io->read_bits_int_be(23); + m_reserved2 = m__io->read_bits_int_be(24); + m_reserved3 = m__io->read_bits_int_be(24); + m_reserved4 = m__io->read_bits_int_be(16); + m__io->align_to_byte(); + m_t_gd = m__io->read_s1(); + m_iodc_lsb = m__io->read_u1(); + m_t_oc = m__io->read_u2be(); + m_af_2 = m__io->read_s1(); + m_af_1 = m__io->read_s2be(); + m_af_0_sign = m__io->read_bits_int_be(1); + m_af_0_value = m__io->read_bits_int_be(21); + m_reserved5 = m__io->read_bits_int_be(2); +} + +gps_t::subframe_1_t::~subframe_1_t() { + _clean_up(); +} + +void gps_t::subframe_1_t::_clean_up() { +} + +int32_t gps_t::subframe_1_t::af_0() { + if (f_af_0) + return m_af_0; + m_af_0 = ((af_0_sign()) ? ((af_0_value() - (1 << 21))) : (af_0_value())); + f_af_0 = true; + return m_af_0; +} + +gps_t::subframe_3_t::subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + f_omega_dot = false; + f_idot = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_3_t::_read() { + m_c_ic = m__io->read_s2be(); + m_omega_0 = m__io->read_s4be(); + m_c_is = m__io->read_s2be(); + m_i_0 = m__io->read_s4be(); + m_c_rc = m__io->read_s2be(); + m_omega = m__io->read_s4be(); + m_omega_dot_sign = m__io->read_bits_int_be(1); + m_omega_dot_value = m__io->read_bits_int_be(23); + m__io->align_to_byte(); + m_iode = m__io->read_u1(); + m_idot_sign = m__io->read_bits_int_be(1); + m_idot_value = m__io->read_bits_int_be(13); + m_reserved = m__io->read_bits_int_be(2); +} + +gps_t::subframe_3_t::~subframe_3_t() { + _clean_up(); +} + +void gps_t::subframe_3_t::_clean_up() { +} + +int32_t gps_t::subframe_3_t::omega_dot() { + if (f_omega_dot) + return m_omega_dot; + m_omega_dot = ((omega_dot_sign()) ? ((omega_dot_value() - (1 << 23))) : (omega_dot_value())); + f_omega_dot = true; + return m_omega_dot; +} + +int32_t gps_t::subframe_3_t::idot() { + if (f_idot) + return m_idot; + m_idot = ((idot_sign()) ? ((idot_value() - (1 << 13))) : (idot_value())); + f_idot = true; + return m_idot; +} + +gps_t::subframe_4_t::subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_4_t::_read() { + m_data_id = m__io->read_bits_int_be(2); + m_page_id = m__io->read_bits_int_be(6); + m__io->align_to_byte(); + n_body = true; + switch (page_id()) { + case 56: { + n_body = false; + m_body = new ionosphere_data_t(m__io, this, m__root); + break; + } + } +} + +gps_t::subframe_4_t::~subframe_4_t() { + _clean_up(); +} + +void gps_t::subframe_4_t::_clean_up() { + if (!n_body) { + if (m_body) { + delete m_body; m_body = 0; + } + } +} + +gps_t::subframe_4_t::ionosphere_data_t::ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_4_t::ionosphere_data_t::_read() { + m_a0 = m__io->read_s1(); + m_a1 = m__io->read_s1(); + m_a2 = m__io->read_s1(); + m_a3 = m__io->read_s1(); + m_b0 = m__io->read_s1(); + m_b1 = m__io->read_s1(); + m_b2 = m__io->read_s1(); + m_b3 = m__io->read_s1(); +} + +gps_t::subframe_4_t::ionosphere_data_t::~ionosphere_data_t() { + _clean_up(); +} + +void gps_t::subframe_4_t::ionosphere_data_t::_clean_up() { +} + +gps_t::how_t::how_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::how_t::_read() { + m_tow_count = m__io->read_bits_int_be(17); + m_alert = m__io->read_bits_int_be(1); + m_anti_spoof = m__io->read_bits_int_be(1); + m_subframe_id = m__io->read_bits_int_be(3); + m_reserved = m__io->read_bits_int_be(2); +} + +gps_t::how_t::~how_t() { + _clean_up(); +} + +void gps_t::how_t::_clean_up() { +} + +gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::tlm_t::_read() { + m_magic = m__io->read_bytes(1); + if (!(magic() == std::string("\x8B", 1))) { + throw kaitai::validation_not_equal_error(std::string("\x8B", 1), magic(), _io(), std::string("/types/tlm/seq/0")); + } + m_tlm = m__io->read_bits_int_be(14); + m_integrity_status = m__io->read_bits_int_be(1); + m_reserved = m__io->read_bits_int_be(1); +} + +gps_t::tlm_t::~tlm_t() { + _clean_up(); +} + +void gps_t::tlm_t::_clean_up() { +} + +gps_t::subframe_2_t::subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void gps_t::subframe_2_t::_read() { + m_iode = m__io->read_u1(); + m_c_rs = m__io->read_s2be(); + m_delta_n = m__io->read_s2be(); + m_m_0 = m__io->read_s4be(); + m_c_uc = m__io->read_s2be(); + m_e = m__io->read_s4be(); + m_c_us = m__io->read_s2be(); + m_sqrt_a = m__io->read_u4be(); + m_t_oe = m__io->read_u2be(); + m_fit_interval_flag = m__io->read_bits_int_be(1); + m_aoda = m__io->read_bits_int_be(5); + m_reserved = m__io->read_bits_int_be(2); +} + +gps_t::subframe_2_t::~subframe_2_t() { + _clean_up(); +} + +void gps_t::subframe_2_t::_clean_up() { +} diff --git a/selfdrive/locationd/generated/gps.h b/selfdrive/locationd/generated/gps.h new file mode 100644 index 00000000..293e2e4a --- /dev/null +++ b/selfdrive/locationd/generated/gps.h @@ -0,0 +1,359 @@ +#ifndef GPS_H_ +#define GPS_H_ + +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "kaitai/kaitaistruct.h" +#include + +#if KAITAI_STRUCT_VERSION < 9000L +#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" +#endif + +class gps_t : public kaitai::kstruct { + +public: + class subframe_1_t; + class subframe_3_t; + class subframe_4_t; + class how_t; + class tlm_t; + class subframe_2_t; + + gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, gps_t* p__root = 0); + +private: + void _read(); + void _clean_up(); + +public: + ~gps_t(); + + class subframe_1_t : public kaitai::kstruct { + + public: + + subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_1_t(); + + private: + bool f_af_0; + int32_t m_af_0; + + public: + int32_t af_0(); + + private: + uint64_t m_week_no; + uint64_t m_code; + uint64_t m_sv_accuracy; + uint64_t m_sv_health; + uint64_t m_iodc_msb; + bool m_l2_p_data_flag; + uint64_t m_reserved1; + uint64_t m_reserved2; + uint64_t m_reserved3; + uint64_t m_reserved4; + int8_t m_t_gd; + uint8_t m_iodc_lsb; + uint16_t m_t_oc; + int8_t m_af_2; + int16_t m_af_1; + bool m_af_0_sign; + uint64_t m_af_0_value; + uint64_t m_reserved5; + gps_t* m__root; + gps_t* m__parent; + + public: + uint64_t week_no() const { return m_week_no; } + uint64_t code() const { return m_code; } + uint64_t sv_accuracy() const { return m_sv_accuracy; } + uint64_t sv_health() const { return m_sv_health; } + uint64_t iodc_msb() const { return m_iodc_msb; } + bool l2_p_data_flag() const { return m_l2_p_data_flag; } + uint64_t reserved1() const { return m_reserved1; } + uint64_t reserved2() const { return m_reserved2; } + uint64_t reserved3() const { return m_reserved3; } + uint64_t reserved4() const { return m_reserved4; } + int8_t t_gd() const { return m_t_gd; } + uint8_t iodc_lsb() const { return m_iodc_lsb; } + uint16_t t_oc() const { return m_t_oc; } + int8_t af_2() const { return m_af_2; } + int16_t af_1() const { return m_af_1; } + bool af_0_sign() const { return m_af_0_sign; } + uint64_t af_0_value() const { return m_af_0_value; } + uint64_t reserved5() const { return m_reserved5; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class subframe_3_t : public kaitai::kstruct { + + public: + + subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_3_t(); + + private: + bool f_omega_dot; + int32_t m_omega_dot; + + public: + int32_t omega_dot(); + + private: + bool f_idot; + int32_t m_idot; + + public: + int32_t idot(); + + private: + int16_t m_c_ic; + int32_t m_omega_0; + int16_t m_c_is; + int32_t m_i_0; + int16_t m_c_rc; + int32_t m_omega; + bool m_omega_dot_sign; + uint64_t m_omega_dot_value; + uint8_t m_iode; + bool m_idot_sign; + uint64_t m_idot_value; + uint64_t m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + int16_t c_ic() const { return m_c_ic; } + int32_t omega_0() const { return m_omega_0; } + int16_t c_is() const { return m_c_is; } + int32_t i_0() const { return m_i_0; } + int16_t c_rc() const { return m_c_rc; } + int32_t omega() const { return m_omega; } + bool omega_dot_sign() const { return m_omega_dot_sign; } + uint64_t omega_dot_value() const { return m_omega_dot_value; } + uint8_t iode() const { return m_iode; } + bool idot_sign() const { return m_idot_sign; } + uint64_t idot_value() const { return m_idot_value; } + uint64_t reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class subframe_4_t : public kaitai::kstruct { + + public: + class ionosphere_data_t; + + subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_4_t(); + + class ionosphere_data_t : public kaitai::kstruct { + + public: + + ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~ionosphere_data_t(); + + private: + int8_t m_a0; + int8_t m_a1; + int8_t m_a2; + int8_t m_a3; + int8_t m_b0; + int8_t m_b1; + int8_t m_b2; + int8_t m_b3; + gps_t* m__root; + gps_t::subframe_4_t* m__parent; + + public: + int8_t a0() const { return m_a0; } + int8_t a1() const { return m_a1; } + int8_t a2() const { return m_a2; } + int8_t a3() const { return m_a3; } + int8_t b0() const { return m_b0; } + int8_t b1() const { return m_b1; } + int8_t b2() const { return m_b2; } + int8_t b3() const { return m_b3; } + gps_t* _root() const { return m__root; } + gps_t::subframe_4_t* _parent() const { return m__parent; } + }; + + private: + uint64_t m_data_id; + uint64_t m_page_id; + ionosphere_data_t* m_body; + bool n_body; + + public: + bool _is_null_body() { body(); return n_body; }; + + private: + gps_t* m__root; + gps_t* m__parent; + + public: + uint64_t data_id() const { return m_data_id; } + uint64_t page_id() const { return m_page_id; } + ionosphere_data_t* body() const { return m_body; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class how_t : public kaitai::kstruct { + + public: + + how_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~how_t(); + + private: + uint64_t m_tow_count; + bool m_alert; + bool m_anti_spoof; + uint64_t m_subframe_id; + uint64_t m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + uint64_t tow_count() const { return m_tow_count; } + bool alert() const { return m_alert; } + bool anti_spoof() const { return m_anti_spoof; } + uint64_t subframe_id() const { return m_subframe_id; } + uint64_t reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class tlm_t : public kaitai::kstruct { + + public: + + tlm_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~tlm_t(); + + private: + std::string m_magic; + uint64_t m_tlm; + bool m_integrity_status; + bool m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + std::string magic() const { return m_magic; } + uint64_t tlm() const { return m_tlm; } + bool integrity_status() const { return m_integrity_status; } + bool reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + + class subframe_2_t : public kaitai::kstruct { + + public: + + subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~subframe_2_t(); + + private: + uint8_t m_iode; + int16_t m_c_rs; + int16_t m_delta_n; + int32_t m_m_0; + int16_t m_c_uc; + int32_t m_e; + int16_t m_c_us; + uint32_t m_sqrt_a; + uint16_t m_t_oe; + bool m_fit_interval_flag; + uint64_t m_aoda; + uint64_t m_reserved; + gps_t* m__root; + gps_t* m__parent; + + public: + uint8_t iode() const { return m_iode; } + int16_t c_rs() const { return m_c_rs; } + int16_t delta_n() const { return m_delta_n; } + int32_t m_0() const { return m_m_0; } + int16_t c_uc() const { return m_c_uc; } + int32_t e() const { return m_e; } + int16_t c_us() const { return m_c_us; } + uint32_t sqrt_a() const { return m_sqrt_a; } + uint16_t t_oe() const { return m_t_oe; } + bool fit_interval_flag() const { return m_fit_interval_flag; } + uint64_t aoda() const { return m_aoda; } + uint64_t reserved() const { return m_reserved; } + gps_t* _root() const { return m__root; } + gps_t* _parent() const { return m__parent; } + }; + +private: + tlm_t* m_tlm; + how_t* m_how; + kaitai::kstruct* m_body; + bool n_body; + +public: + bool _is_null_body() { body(); return n_body; }; + +private: + gps_t* m__root; + kaitai::kstruct* m__parent; + +public: + tlm_t* tlm() const { return m_tlm; } + how_t* how() const { return m_how; } + kaitai::kstruct* body() const { return m_body; } + gps_t* _root() const { return m__root; } + kaitai::kstruct* _parent() const { return m__parent; } +}; + +#endif // GPS_H_ diff --git a/selfdrive/locationd/generated/ubx.cpp b/selfdrive/locationd/generated/ubx.cpp new file mode 100644 index 00000000..5e743e1e --- /dev/null +++ b/selfdrive/locationd/generated/ubx.cpp @@ -0,0 +1,340 @@ +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "ubx.h" +#include "kaitai/exceptions.h" + +ubx_t::ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = this; + f_checksum = false; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::_read() { + m_magic = m__io->read_bytes(2); + if (!(magic() == std::string("\xB5\x62", 2))) { + throw kaitai::validation_not_equal_error(std::string("\xB5\x62", 2), magic(), _io(), std::string("/seq/0")); + } + m_msg_type = m__io->read_u2be(); + m_length = m__io->read_u2le(); + n_body = true; + switch (msg_type()) { + case 2569: { + n_body = false; + m_body = new mon_hw_t(m__io, this, m__root); + break; + } + case 533: { + n_body = false; + m_body = new rxm_rawx_t(m__io, this, m__root); + break; + } + case 531: { + n_body = false; + m_body = new rxm_sfrbx_t(m__io, this, m__root); + break; + } + case 2571: { + n_body = false; + m_body = new mon_hw2_t(m__io, this, m__root); + break; + } + case 263: { + n_body = false; + m_body = new nav_pvt_t(m__io, this, m__root); + break; + } + } +} + +ubx_t::~ubx_t() { + _clean_up(); +} + +void ubx_t::_clean_up() { + if (!n_body) { + if (m_body) { + delete m_body; m_body = 0; + } + } + if (f_checksum) { + } +} + +ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + m_measurements = 0; + m__raw_measurements = 0; + m__io__raw_measurements = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::rxm_rawx_t::_read() { + m_rcv_tow = m__io->read_f8le(); + m_week = m__io->read_u2le(); + m_leap_s = m__io->read_s1(); + m_num_meas = m__io->read_u1(); + m_rec_stat = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(3); + int l_measurements = num_meas(); + m__raw_measurements = new std::vector(); + m__raw_measurements->reserve(l_measurements); + m__io__raw_measurements = new std::vector(); + m__io__raw_measurements->reserve(l_measurements); + m_measurements = new std::vector(); + m_measurements->reserve(l_measurements); + for (int i = 0; i < l_measurements; i++) { + m__raw_measurements->push_back(m__io->read_bytes(32)); + kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); + m__io__raw_measurements->push_back(io__raw_measurements); + m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root)); + } +} + +ubx_t::rxm_rawx_t::~rxm_rawx_t() { + _clean_up(); +} + +void ubx_t::rxm_rawx_t::_clean_up() { + if (m__raw_measurements) { + delete m__raw_measurements; m__raw_measurements = 0; + } + if (m__io__raw_measurements) { + for (std::vector::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) { + delete *it; + } + delete m__io__raw_measurements; m__io__raw_measurements = 0; + } + if (m_measurements) { + for (std::vector::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) { + delete *it; + } + delete m_measurements; m_measurements = 0; + } +} + +ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::rxm_rawx_t::meas_t::_read() { + m_pr_mes = m__io->read_f8le(); + m_cp_mes = m__io->read_f8le(); + m_do_mes = m__io->read_f4le(); + m_gnss_id = static_cast(m__io->read_u1()); + m_sv_id = m__io->read_u1(); + m_reserved2 = m__io->read_bytes(1); + m_freq_id = m__io->read_u1(); + m_lock_time = m__io->read_u2le(); + m_cno = m__io->read_u1(); + m_pr_stdev = m__io->read_u1(); + m_cp_stdev = m__io->read_u1(); + m_do_stdev = m__io->read_u1(); + m_trk_stat = m__io->read_u1(); + m_reserved3 = m__io->read_bytes(1); +} + +ubx_t::rxm_rawx_t::meas_t::~meas_t() { + _clean_up(); +} + +void ubx_t::rxm_rawx_t::meas_t::_clean_up() { +} + +ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + m_body = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::rxm_sfrbx_t::_read() { + m_gnss_id = static_cast(m__io->read_u1()); + m_sv_id = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(1); + m_freq_id = m__io->read_u1(); + m_num_words = m__io->read_u1(); + m_reserved2 = m__io->read_bytes(1); + m_version = m__io->read_u1(); + m_reserved3 = m__io->read_bytes(1); + int l_body = num_words(); + m_body = new std::vector(); + m_body->reserve(l_body); + for (int i = 0; i < l_body; i++) { + m_body->push_back(m__io->read_u4le()); + } +} + +ubx_t::rxm_sfrbx_t::~rxm_sfrbx_t() { + _clean_up(); +} + +void ubx_t::rxm_sfrbx_t::_clean_up() { + if (m_body) { + delete m_body; m_body = 0; + } +} + +ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::nav_pvt_t::_read() { + m_i_tow = m__io->read_u4le(); + m_year = m__io->read_u2le(); + m_month = m__io->read_u1(); + m_day = m__io->read_u1(); + m_hour = m__io->read_u1(); + m_min = m__io->read_u1(); + m_sec = m__io->read_u1(); + m_valid = m__io->read_u1(); + m_t_acc = m__io->read_u4le(); + m_nano = m__io->read_s4le(); + m_fix_type = m__io->read_u1(); + m_flags = m__io->read_u1(); + m_flags2 = m__io->read_u1(); + m_num_sv = m__io->read_u1(); + m_lon = m__io->read_s4le(); + m_lat = m__io->read_s4le(); + m_height = m__io->read_s4le(); + m_h_msl = m__io->read_s4le(); + m_h_acc = m__io->read_u4le(); + m_v_acc = m__io->read_u4le(); + m_vel_n = m__io->read_s4le(); + m_vel_e = m__io->read_s4le(); + m_vel_d = m__io->read_s4le(); + m_g_speed = m__io->read_s4le(); + m_head_mot = m__io->read_s4le(); + m_s_acc = m__io->read_s4le(); + m_head_acc = m__io->read_u4le(); + m_p_dop = m__io->read_u2le(); + m_flags3 = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(5); + m_head_veh = m__io->read_s4le(); + m_mag_dec = m__io->read_s2le(); + m_mag_acc = m__io->read_u2le(); +} + +ubx_t::nav_pvt_t::~nav_pvt_t() { + _clean_up(); +} + +void ubx_t::nav_pvt_t::_clean_up() { +} + +ubx_t::mon_hw2_t::mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::mon_hw2_t::_read() { + m_ofs_i = m__io->read_s1(); + m_mag_i = m__io->read_u1(); + m_ofs_q = m__io->read_s1(); + m_mag_q = m__io->read_u1(); + m_cfg_source = static_cast(m__io->read_u1()); + m_reserved1 = m__io->read_bytes(3); + m_low_lev_cfg = m__io->read_u4le(); + m_reserved2 = m__io->read_bytes(8); + m_post_status = m__io->read_u4le(); + m_reserved3 = m__io->read_bytes(4); +} + +ubx_t::mon_hw2_t::~mon_hw2_t() { + _clean_up(); +} + +void ubx_t::mon_hw2_t::_clean_up() { +} + +ubx_t::mon_hw_t::mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::mon_hw_t::_read() { + m_pin_sel = m__io->read_u4le(); + m_pin_bank = m__io->read_u4le(); + m_pin_dir = m__io->read_u4le(); + m_pin_val = m__io->read_u4le(); + m_noise_per_ms = m__io->read_u2le(); + m_agc_cnt = m__io->read_u2le(); + m_a_status = static_cast(m__io->read_u1()); + m_a_power = static_cast(m__io->read_u1()); + m_flags = m__io->read_u1(); + m_reserved1 = m__io->read_bytes(1); + m_used_mask = m__io->read_u4le(); + m_vp = m__io->read_bytes(17); + m_jam_ind = m__io->read_u1(); + m_reserved2 = m__io->read_bytes(2); + m_pin_irq = m__io->read_u4le(); + m_pull_h = m__io->read_u4le(); + m_pull_l = m__io->read_u4le(); +} + +ubx_t::mon_hw_t::~mon_hw_t() { + _clean_up(); +} + +void ubx_t::mon_hw_t::_clean_up() { +} + +uint16_t ubx_t::checksum() { + if (f_checksum) + return m_checksum; + std::streampos _pos = m__io->pos(); + m__io->seek((length() + 6)); + m_checksum = m__io->read_u2le(); + m__io->seek(_pos); + f_checksum = true; + return m_checksum; +} diff --git a/selfdrive/locationd/generated/ubx.h b/selfdrive/locationd/generated/ubx.h new file mode 100644 index 00000000..6be4ce8c --- /dev/null +++ b/selfdrive/locationd/generated/ubx.h @@ -0,0 +1,410 @@ +#ifndef UBX_H_ +#define UBX_H_ + +// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +#include "kaitai/kaitaistruct.h" +#include +#include + +#if KAITAI_STRUCT_VERSION < 9000L +#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" +#endif + +class ubx_t : public kaitai::kstruct { + +public: + class rxm_rawx_t; + class rxm_sfrbx_t; + class nav_pvt_t; + class mon_hw2_t; + class mon_hw_t; + + enum gnss_type_t { + GNSS_TYPE_GPS = 0, + GNSS_TYPE_SBAS = 1, + GNSS_TYPE_GALILEO = 2, + GNSS_TYPE_BEIDOU = 3, + GNSS_TYPE_IMES = 4, + GNSS_TYPE_QZSS = 5, + GNSS_TYPE_GLONASS = 6 + }; + + ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, ubx_t* p__root = 0); + +private: + void _read(); + void _clean_up(); + +public: + ~ubx_t(); + + class rxm_rawx_t : public kaitai::kstruct { + + public: + class meas_t; + + rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~rxm_rawx_t(); + + class meas_t : public kaitai::kstruct { + + public: + + meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~meas_t(); + + private: + double m_pr_mes; + double m_cp_mes; + float m_do_mes; + gnss_type_t m_gnss_id; + uint8_t m_sv_id; + std::string m_reserved2; + uint8_t m_freq_id; + uint16_t m_lock_time; + uint8_t m_cno; + uint8_t m_pr_stdev; + uint8_t m_cp_stdev; + uint8_t m_do_stdev; + uint8_t m_trk_stat; + std::string m_reserved3; + ubx_t* m__root; + ubx_t::rxm_rawx_t* m__parent; + + public: + double pr_mes() const { return m_pr_mes; } + double cp_mes() const { return m_cp_mes; } + float do_mes() const { return m_do_mes; } + gnss_type_t gnss_id() const { return m_gnss_id; } + uint8_t sv_id() const { return m_sv_id; } + std::string reserved2() const { return m_reserved2; } + uint8_t freq_id() const { return m_freq_id; } + uint16_t lock_time() const { return m_lock_time; } + uint8_t cno() const { return m_cno; } + uint8_t pr_stdev() const { return m_pr_stdev; } + uint8_t cp_stdev() const { return m_cp_stdev; } + uint8_t do_stdev() const { return m_do_stdev; } + uint8_t trk_stat() const { return m_trk_stat; } + std::string reserved3() const { return m_reserved3; } + ubx_t* _root() const { return m__root; } + ubx_t::rxm_rawx_t* _parent() const { return m__parent; } + }; + + private: + double m_rcv_tow; + uint16_t m_week; + int8_t m_leap_s; + uint8_t m_num_meas; + uint8_t m_rec_stat; + std::string m_reserved1; + std::vector* m_measurements; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_measurements; + std::vector* m__io__raw_measurements; + + public: + double rcv_tow() const { return m_rcv_tow; } + uint16_t week() const { return m_week; } + int8_t leap_s() const { return m_leap_s; } + uint8_t num_meas() const { return m_num_meas; } + uint8_t rec_stat() const { return m_rec_stat; } + std::string reserved1() const { return m_reserved1; } + std::vector* measurements() const { return m_measurements; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_measurements() const { return m__raw_measurements; } + std::vector* _io__raw_measurements() const { return m__io__raw_measurements; } + }; + + class rxm_sfrbx_t : public kaitai::kstruct { + + public: + + rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~rxm_sfrbx_t(); + + private: + gnss_type_t m_gnss_id; + uint8_t m_sv_id; + std::string m_reserved1; + uint8_t m_freq_id; + uint8_t m_num_words; + std::string m_reserved2; + uint8_t m_version; + std::string m_reserved3; + std::vector* m_body; + ubx_t* m__root; + ubx_t* m__parent; + + public: + gnss_type_t gnss_id() const { return m_gnss_id; } + uint8_t sv_id() const { return m_sv_id; } + std::string reserved1() const { return m_reserved1; } + uint8_t freq_id() const { return m_freq_id; } + uint8_t num_words() const { return m_num_words; } + std::string reserved2() const { return m_reserved2; } + uint8_t version() const { return m_version; } + std::string reserved3() const { return m_reserved3; } + std::vector* body() const { return m_body; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + + class nav_pvt_t : public kaitai::kstruct { + + public: + + nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~nav_pvt_t(); + + private: + uint32_t m_i_tow; + uint16_t m_year; + uint8_t m_month; + uint8_t m_day; + uint8_t m_hour; + uint8_t m_min; + uint8_t m_sec; + uint8_t m_valid; + uint32_t m_t_acc; + int32_t m_nano; + uint8_t m_fix_type; + uint8_t m_flags; + uint8_t m_flags2; + uint8_t m_num_sv; + int32_t m_lon; + int32_t m_lat; + int32_t m_height; + int32_t m_h_msl; + uint32_t m_h_acc; + uint32_t m_v_acc; + int32_t m_vel_n; + int32_t m_vel_e; + int32_t m_vel_d; + int32_t m_g_speed; + int32_t m_head_mot; + int32_t m_s_acc; + uint32_t m_head_acc; + uint16_t m_p_dop; + uint8_t m_flags3; + std::string m_reserved1; + int32_t m_head_veh; + int16_t m_mag_dec; + uint16_t m_mag_acc; + ubx_t* m__root; + ubx_t* m__parent; + + public: + uint32_t i_tow() const { return m_i_tow; } + uint16_t year() const { return m_year; } + uint8_t month() const { return m_month; } + uint8_t day() const { return m_day; } + uint8_t hour() const { return m_hour; } + uint8_t min() const { return m_min; } + uint8_t sec() const { return m_sec; } + uint8_t valid() const { return m_valid; } + uint32_t t_acc() const { return m_t_acc; } + int32_t nano() const { return m_nano; } + uint8_t fix_type() const { return m_fix_type; } + uint8_t flags() const { return m_flags; } + uint8_t flags2() const { return m_flags2; } + uint8_t num_sv() const { return m_num_sv; } + int32_t lon() const { return m_lon; } + int32_t lat() const { return m_lat; } + int32_t height() const { return m_height; } + int32_t h_msl() const { return m_h_msl; } + uint32_t h_acc() const { return m_h_acc; } + uint32_t v_acc() const { return m_v_acc; } + int32_t vel_n() const { return m_vel_n; } + int32_t vel_e() const { return m_vel_e; } + int32_t vel_d() const { return m_vel_d; } + int32_t g_speed() const { return m_g_speed; } + int32_t head_mot() const { return m_head_mot; } + int32_t s_acc() const { return m_s_acc; } + uint32_t head_acc() const { return m_head_acc; } + uint16_t p_dop() const { return m_p_dop; } + uint8_t flags3() const { return m_flags3; } + std::string reserved1() const { return m_reserved1; } + int32_t head_veh() const { return m_head_veh; } + int16_t mag_dec() const { return m_mag_dec; } + uint16_t mag_acc() const { return m_mag_acc; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + + class mon_hw2_t : public kaitai::kstruct { + + public: + + enum config_source_t { + CONFIG_SOURCE_FLASH = 102, + CONFIG_SOURCE_OTP = 111, + CONFIG_SOURCE_CONFIG_PINS = 112, + CONFIG_SOURCE_ROM = 113 + }; + + mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~mon_hw2_t(); + + private: + int8_t m_ofs_i; + uint8_t m_mag_i; + int8_t m_ofs_q; + uint8_t m_mag_q; + config_source_t m_cfg_source; + std::string m_reserved1; + uint32_t m_low_lev_cfg; + std::string m_reserved2; + uint32_t m_post_status; + std::string m_reserved3; + ubx_t* m__root; + ubx_t* m__parent; + + public: + int8_t ofs_i() const { return m_ofs_i; } + uint8_t mag_i() const { return m_mag_i; } + int8_t ofs_q() const { return m_ofs_q; } + uint8_t mag_q() const { return m_mag_q; } + config_source_t cfg_source() const { return m_cfg_source; } + std::string reserved1() const { return m_reserved1; } + uint32_t low_lev_cfg() const { return m_low_lev_cfg; } + std::string reserved2() const { return m_reserved2; } + uint32_t post_status() const { return m_post_status; } + std::string reserved3() const { return m_reserved3; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + + class mon_hw_t : public kaitai::kstruct { + + public: + + enum antenna_status_t { + ANTENNA_STATUS_INIT = 0, + ANTENNA_STATUS_DONTKNOW = 1, + ANTENNA_STATUS_OK = 2, + ANTENNA_STATUS_SHORT = 3, + ANTENNA_STATUS_OPEN = 4 + }; + + enum antenna_power_t { + ANTENNA_POWER_FALSE = 0, + ANTENNA_POWER_TRUE = 1, + ANTENNA_POWER_DONTKNOW = 2 + }; + + mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); + + private: + void _read(); + void _clean_up(); + + public: + ~mon_hw_t(); + + private: + uint32_t m_pin_sel; + uint32_t m_pin_bank; + uint32_t m_pin_dir; + uint32_t m_pin_val; + uint16_t m_noise_per_ms; + uint16_t m_agc_cnt; + antenna_status_t m_a_status; + antenna_power_t m_a_power; + uint8_t m_flags; + std::string m_reserved1; + uint32_t m_used_mask; + std::string m_vp; + uint8_t m_jam_ind; + std::string m_reserved2; + uint32_t m_pin_irq; + uint32_t m_pull_h; + uint32_t m_pull_l; + ubx_t* m__root; + ubx_t* m__parent; + + public: + uint32_t pin_sel() const { return m_pin_sel; } + uint32_t pin_bank() const { return m_pin_bank; } + uint32_t pin_dir() const { return m_pin_dir; } + uint32_t pin_val() const { return m_pin_val; } + uint16_t noise_per_ms() const { return m_noise_per_ms; } + uint16_t agc_cnt() const { return m_agc_cnt; } + antenna_status_t a_status() const { return m_a_status; } + antenna_power_t a_power() const { return m_a_power; } + uint8_t flags() const { return m_flags; } + std::string reserved1() const { return m_reserved1; } + uint32_t used_mask() const { return m_used_mask; } + std::string vp() const { return m_vp; } + uint8_t jam_ind() const { return m_jam_ind; } + std::string reserved2() const { return m_reserved2; } + uint32_t pin_irq() const { return m_pin_irq; } + uint32_t pull_h() const { return m_pull_h; } + uint32_t pull_l() const { return m_pull_l; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + }; + +private: + bool f_checksum; + uint16_t m_checksum; + +public: + uint16_t checksum(); + +private: + std::string m_magic; + uint16_t m_msg_type; + uint16_t m_length; + kaitai::kstruct* m_body; + bool n_body; + +public: + bool _is_null_body() { body(); return n_body; }; + +private: + ubx_t* m__root; + kaitai::kstruct* m__parent; + +public: + std::string magic() const { return m_magic; } + uint16_t msg_type() const { return m_msg_type; } + uint16_t length() const { return m_length; } + kaitai::kstruct* body() const { return m_body; } + ubx_t* _root() const { return m__root; } + kaitai::kstruct* _parent() const { return m__parent; } +}; + +#endif // UBX_H_ diff --git a/selfdrive/locationd/gps.ksy b/selfdrive/locationd/gps.ksy new file mode 100644 index 00000000..6f5cde31 --- /dev/null +++ b/selfdrive/locationd/gps.ksy @@ -0,0 +1,189 @@ +# https://www.gps.gov/technical/icwg/IS-GPS-200E.pdf +meta: + id: gps + endian: be + bit-endian: be +seq: + - id: tlm + type: tlm + - id: how + type: how + - id: body + type: + switch-on: how.subframe_id + cases: + 1: subframe_1 + 2: subframe_2 + 3: subframe_3 + 4: subframe_4 +types: + tlm: + seq: + - id: magic + contents: [0x8b] + - id: tlm + type: b14 + - id: integrity_status + type: b1 + - id: reserved + type: b1 + how: + seq: + - id: tow_count + type: b17 + - id: alert + type: b1 + - id: anti_spoof + type: b1 + - id: subframe_id + type: b3 + - id: reserved + type: b2 + subframe_1: + seq: + # Word 3 + - id: week_no + type: b10 + - id: code + type: b2 + - id: sv_accuracy + type: b4 + - id: sv_health + type: b6 + - id: iodc_msb + type: b2 + # Word 4 + - id: l2_p_data_flag + type: b1 + - id: reserved1 + type: b23 + # Word 5 + - id: reserved2 + type: b24 + # Word 6 + - id: reserved3 + type: b24 + # Word 7 + - id: reserved4 + type: b16 + - id: t_gd + type: s1 + # Word 8 + - id: iodc_lsb + type: u1 + - id: t_oc + type: u2 + # Word 9 + - id: af_2 + type: s1 + - id: af_1 + type: s2 + # Word 10 + - id: af_0_sign + type: b1 + - id: af_0_value + type: b21 + - id: reserved5 + type: b2 + instances: + af_0: + value: 'af_0_sign ? (af_0_value - (1 << 21)) : af_0_value' + subframe_2: + seq: + # Word 3 + - id: iode + type: u1 + - id: c_rs + type: s2 + # Word 4 & 5 + - id: delta_n + type: s2 + - id: m_0 + type: s4 + # Word 6 & 7 + - id: c_uc + type: s2 + - id: e + type: s4 + # Word 8 & 9 + - id: c_us + type: s2 + - id: sqrt_a + type: u4 + # Word 10 + - id: t_oe + type: u2 + - id: fit_interval_flag + type: b1 + - id: aoda + type: b5 + - id: reserved + type: b2 + subframe_3: + seq: + # Word 3 & 4 + - id: c_ic + type: s2 + - id: omega_0 + type: s4 + # Word 5 & 6 + - id: c_is + type: s2 + - id: i_0 + type: s4 + # Word 7 & 8 + - id: c_rc + type: s2 + - id: omega + type: s4 + # Word 9 + - id: omega_dot_sign + type: b1 + - id: omega_dot_value + type: b23 + # Word 10 + - id: iode + type: u1 + - id: idot_sign + type: b1 + - id: idot_value + type: b13 + - id: reserved + type: b2 + instances: + omega_dot: + value: 'omega_dot_sign ? (omega_dot_value - (1 << 23)) : omega_dot_value' + idot: + value: 'idot_sign ? (idot_value - (1 << 13)) : idot_value' + subframe_4: + seq: + # Word 3 + - id: data_id + type: b2 + - id: page_id + type: b6 + - id: body + type: + switch-on: page_id + cases: + 56: ionosphere_data + types: + ionosphere_data: + seq: + - id: a0 + type: s1 + - id: a1 + type: s1 + - id: a2 + type: s1 + - id: a3 + type: s1 + - id: b0 + type: s1 + - id: b1 + type: s1 + - id: b2 + type: s1 + - id: b3 + type: s1 + diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index e0463d8e..0b91f080 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -1,144 +1,29 @@ -#include -#include +#include +#include #include -#include +#include #include #include +#include +#include +#include #include "common/swaglog.h" #include "ublox_msg.h" +const double gpsPi = 3.1415926535898; #define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4]) -#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) -namespace ublox { - -inline int twos_complement(uint32_t v, uint32_t nb) { - int sign = v >> (nb - 1); - int value = v; - if(sign != 0) - value = value - (1 << nb); - return value; -} - -inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) { - int v = GET_FIELD_U(w, nb, pos); - return twos_complement(v, nb); -} - -class EphemerisData { - public: - EphemerisData(uint8_t svId, subframes_map &subframes) { - this->svId = svId; - int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20); - int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6); - int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( - subframes[1][2+5], 8, 22); - - int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6); - int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22); - int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6); - int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8); - - int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6); - int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14); - int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( - subframes[2][2+2], 24, 6); - int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14); - int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6); - int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14); - uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( - subframes[2][2+6], 24, 6); - int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14); - - int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14); - int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( - subframes[3][2+1], 24, 6); - int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14); - int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( - subframes[3][2+3], 24, 6); - int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14); - int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][2+5], 24, 6); - int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6); - int idot = GET_FIELD_S(subframes[3][2+7], 14, 8); - - this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6); - this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6); - this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6); - this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14); - this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8); - - double gpsPi = 3.1415926535898; - - // now form variables in radians, meters and seconds etc - this->Tgd = t_gd * pow(2, -31); - this->A = pow(a_powhalf * pow(2, -19), 2.0); - this->cic = c_ic * pow(2, -29); - this->cis = c_is * pow(2, -29); - this->crc = c_rc * pow(2, -5); - this->crs = c_rs * pow(2, -5); - this->cuc = c_uc * pow(2, -29); - this->cus = c_us * pow(2, -29); - this->deltaN = delta_n * pow(2, -43) * gpsPi; - this->ecc = e * pow(2, -33); - this->i0 = i_0 * pow(2, -31) * gpsPi; - this->idot = idot * pow(2, -43) * gpsPi; - this->M0 = m_0 * pow(2, -31) * gpsPi; - this->omega = w * pow(2, -31) * gpsPi; - this->omega_dot = omega_dot * pow(2, -43) * gpsPi; - this->omega0 = omega_0 * pow(2, -31) * gpsPi; - this->toe = t_oe * pow(2, 4); - - this->toc = t_oc * pow(2, 4); - this->gpsWeek = week_no; - this->af0 = a_f0 * pow(2, -31); - this->af1 = a_f1 * pow(2, -43); - this->af2 = a_f2 * pow(2, -55); - - uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22); - uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22); - this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff)); - this->iode = iode1; - - if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 && - GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 && - GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) { - double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30); - double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27); - double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24); - double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24); - double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11); - double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14); - double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16); - double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16); - this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3; - this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3; - this->ionoCoeffsValid = true; - } else { - this->ionoCoeffsValid = false; - } - } - uint16_t svId; - double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc; - uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo; - double af0, af1, af2; - bool valid; - double ionoAlpha[4], ionoBeta[4]; - bool ionoCoeffsValid; -}; - -UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) { - nav_frame_buffer[0U] = std::map(); - for(int i = 1;i < 33;i++) - nav_frame_buffer[0U][i] = subframes_map(); +inline static bool bit_to_bool(uint8_t val, int shifts) { + return (bool)(val & (1 << shifts)); } inline int UbloxMsgParser::needed_bytes() { // Msg header incomplete? - if(bytes_in_parse_buf < UBLOX_HEADER_SIZE) - return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; - uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE; + if(bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE) + return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; + uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE; // too much data if(needed < (uint16_t)bytes_in_parse_buf) return -1; @@ -147,7 +32,7 @@ inline int UbloxMsgParser::needed_bytes() { inline bool UbloxMsgParser::valid_cheksum() { uint8_t ck_a = 0, ck_b = 0; - for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) { + for(int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE;i++) { ck_a = (ck_a + msg_parse_buf[i]) & 0xFF; ck_b = (ck_b + ck_a) & 0xFF; } @@ -163,17 +48,15 @@ inline bool UbloxMsgParser::valid_cheksum() { } inline bool UbloxMsgParser::valid() { - return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE && + return bytes_in_parse_buf >= ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE && needed_bytes() == 0 && valid_cheksum(); } inline bool UbloxMsgParser::valid_so_far() { - if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) { - //LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]); + if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) { return false; } - if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) { - //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]); + if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) { return false; } if(needed_bytes() == 0 && !valid()) { @@ -182,192 +65,6 @@ inline bool UbloxMsgParser::valid_so_far() { return true; } -kj::Array UbloxMsgParser::gen_solution() { - nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - MessageBuilder msg_builder; - auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal(); - gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); - gpsLoc.setFlags(msg->flags); - gpsLoc.setLatitude(msg->lat * 1e-07); - gpsLoc.setLongitude(msg->lon * 1e-07); - gpsLoc.setAltitude(msg->height * 1e-03); - gpsLoc.setSpeed(msg->gSpeed * 1e-03); - gpsLoc.setBearingDeg(msg->headMot * 1e-5); - gpsLoc.setAccuracy(msg->hAcc * 1e-03); - std::tm timeinfo = std::tm(); - timeinfo.tm_year = msg->year - 1900; - timeinfo.tm_mon = msg->month - 1; - timeinfo.tm_mday = msg->day; - timeinfo.tm_hour = msg->hour; - timeinfo.tm_min = msg->min; - timeinfo.tm_sec = msg->sec; - std::time_t utc_tt = timegm(&timeinfo); - gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); - float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; - gpsLoc.setVNED(f); - gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); - gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); - gpsLoc.setBearingAccuracyDeg(msg->headAcc * 1e-05); - return capnp::messageToFlatArray(msg_builder); -} - -inline bool bit_to_bool(uint8_t val, int shifts) { - return (bool)(val & (1 << shifts)); -} - -kj::Array UbloxMsgParser::gen_raw() { - rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - if(bytes_in_parse_buf != ( - UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE - )) { - LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); - return kj::Array(); - } - rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)]; - MessageBuilder msg_builder; - auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); - mr.setRcvTow(msg->rcvTow); - mr.setGpsWeek(msg->week); - mr.setLeapSeconds(msg->leapS); - mr.setGpsWeek(msg->week); - auto mb = mr.initMeasurements(msg->numMeas); - for(int8_t i = 0; i < msg->numMeas; i++) { - mb[i].setSvId(measurements[i].svId); - mb[i].setSigId(measurements[i].sigId); - mb[i].setPseudorange(measurements[i].prMes); - mb[i].setCarrierCycles(measurements[i].cpMes); - mb[i].setDoppler(measurements[i].doMes); - mb[i].setGnssId(measurements[i].gnssId); - mb[i].setGlonassFrequencyIndex(measurements[i].freqId); - mb[i].setLocktime(measurements[i].locktime); - mb[i].setCno(measurements[i].cno); - mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong - mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15)); - mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong - auto ts = mb[i].initTrackingStatus(); - ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0)); - ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1)); - ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2)); - ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3)); - } - - mr.setNumMeas(msg->numMeas); - auto rs = mr.initReceiverStatus(); - rs.setLeapSecValid(bit_to_bool(msg->recStat, 0)); - rs.setClkReset(bit_to_bool(msg->recStat, 2)); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_nav_data() { - rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - if(bytes_in_parse_buf != ( - UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE - )) { - LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); - return kj::Array(); - } - rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)]; - if(msg->gnssId == 0) { - uint8_t subframeId = GET_FIELD_U(measurements[1].dwrd, 3, 8); - std::vector words; - for(int i = 0; i < msg->numWords;i++) - words.push_back(measurements[i].dwrd); - - subframes_map &map = nav_frame_buffer[msg->gnssId][msg->svid]; - if (subframeId == 1) { - map = subframes_map(); - map[subframeId] = words; - } else if (map.find(subframeId-1) != map.end()) { - map[subframeId] = words; - } - if(map.size() == 5) { - EphemerisData ephem_data(msg->svid, map); - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); - eph.setSvId(ephem_data.svId); - eph.setToc(ephem_data.toc); - eph.setGpsWeek(ephem_data.gpsWeek); - eph.setAf0(ephem_data.af0); - eph.setAf1(ephem_data.af1); - eph.setAf2(ephem_data.af2); - eph.setIode(ephem_data.iode); - eph.setCrs(ephem_data.crs); - eph.setDeltaN(ephem_data.deltaN); - eph.setM0(ephem_data.M0); - eph.setCuc(ephem_data.cuc); - eph.setEcc(ephem_data.ecc); - eph.setCus(ephem_data.cus); - eph.setA(ephem_data.A); - eph.setToe(ephem_data.toe); - eph.setCic(ephem_data.cic); - eph.setOmega0(ephem_data.omega0); - eph.setCis(ephem_data.cis); - eph.setI0(ephem_data.i0); - eph.setCrc(ephem_data.crc); - eph.setOmega(ephem_data.omega); - eph.setOmegaDot(ephem_data.omega_dot); - eph.setIDot(ephem_data.idot); - eph.setTgd(ephem_data.Tgd); - eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); - if(ephem_data.ionoCoeffsValid) { - eph.setIonoAlpha(ephem_data.ionoAlpha); - eph.setIonoBeta(ephem_data.ionoBeta); - } else { - eph.setIonoAlpha(kj::ArrayPtr()); - eph.setIonoBeta(kj::ArrayPtr()); - } - return capnp::messageToFlatArray(msg_builder); - } - } - return kj::Array(); -} - -kj::Array UbloxMsgParser::gen_mon_hw() { - mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - - MessageBuilder msg_builder; - auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); - hwStatus.setNoisePerMS(msg->noisePerMS); - hwStatus.setAgcCnt(msg->agcCnt); - hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus); - hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower); - hwStatus.setJamInd(msg->jamInd); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_mon_hw2() { - mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; - - MessageBuilder msg_builder; - auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); - hwStatus.setOfsI(msg->ofsI); - hwStatus.setMagI(msg->magI); - hwStatus.setOfsQ(msg->ofsQ); - hwStatus.setMagQ(msg->magQ); - - switch (msg->cfgSource) { - case 114: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); - break; - case 111: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); - break; - case 112: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); - break; - case 102: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); - break; - default: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); - break; - } - - hwStatus.setLowLevCfg(msg->lowLevCfg); - hwStatus.setPostStatus(msg->postStatus); - - return capnp::messageToFlatArray(msg_builder); -} bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); @@ -379,18 +76,267 @@ bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_da } else { bytes_consumed = incoming_data_len; } + // Validate msg format, detect invalid header and invalid checksum. while(!valid_so_far() && bytes_in_parse_buf != 0) { - //LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf); // Corrupted msg, drop a byte. bytes_in_parse_buf -= 1; if(bytes_in_parse_buf > 0) memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf); } + // There is redundant data at the end of buffer, reset the buffer. - if(needed_bytes() == -1) + if(needed_bytes() == -1) { bytes_in_parse_buf = 0; + } return valid(); } + +std::pair> UbloxMsgParser::gen_msg() { + std::string dat = data(); + kaitai::kstream stream(dat); + + ubx_t ubx_message(&stream); + auto body = ubx_message.body(); + + switch (ubx_message.msg_type()) { + case 0x0107: + return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; + break; + case 0x0213: + return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; + break; + case 0x0215: + return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; + break; + case 0x0a09: + return {"ubloxGnss", gen_mon_hw(static_cast(body))}; + break; + case 0x0a0b: + return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; + break; + default: + LOGE("Unkown message type %x", ubx_message.msg_type()); + return {"ubloxGnss", kj::Array()}; + break; + } +} + + +kj::Array UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { + MessageBuilder msg_builder; + auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal(); + gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); + gpsLoc.setFlags(msg->flags()); + gpsLoc.setLatitude(msg->lat() * 1e-07); + gpsLoc.setLongitude(msg->lon() * 1e-07); + gpsLoc.setAltitude(msg->height() * 1e-03); + gpsLoc.setSpeed(msg->g_speed() * 1e-03); + gpsLoc.setBearingDeg(msg->head_mot() * 1e-5); + gpsLoc.setAccuracy(msg->h_acc() * 1e-03); + std::tm timeinfo = std::tm(); + timeinfo.tm_year = msg->year() - 1900; + timeinfo.tm_mon = msg->month() - 1; + timeinfo.tm_mday = msg->day(); + timeinfo.tm_hour = msg->hour(); + timeinfo.tm_min = msg->min(); + timeinfo.tm_sec = msg->sec(); + + std::time_t utc_tt = timegm(&timeinfo); + gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano() * 1e-06); + float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f }; + gpsLoc.setVNED(f); + gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03); + gpsLoc.setSpeedAccuracy(msg->s_acc() * 1e-03); + gpsLoc.setBearingAccuracyDeg(msg->head_acc() * 1e-05); + return capnp::messageToFlatArray(msg_builder); +} + + +kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { + auto body = *msg->body(); + + 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(); + + if (subframe_id == 1) gps_subframes[msg->sv_id()].clear(); + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; + + 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(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)); + } + + // Subframe 2 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); + gps_t subframe(&stream); + gps_t::subframe_2_t* subframe_2 = static_cast(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 3 + { + kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); + gps_t subframe(&stream); + gps_t::subframe_3_t* subframe_3 = static_cast(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(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(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); + } + } + return kj::Array(); +} + +kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { + MessageBuilder msg_builder; + auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); + mr.setRcvTow(msg->rcv_tow()); + mr.setGpsWeek(msg->week()); + mr.setLeapSeconds(msg->leap_s()); + mr.setGpsWeek(msg->week()); + + auto mb = mr.initMeasurements(msg->num_meas()); + auto measurements = *msg->measurements(); + for(int8_t i = 0; i < msg->num_meas(); i++) { + mb[i].setSvId(measurements[i]->sv_id()); + mb[i].setPseudorange(measurements[i]->pr_mes()); + mb[i].setCarrierCycles(measurements[i]->cp_mes()); + mb[i].setDoppler(measurements[i]->do_mes()); + mb[i].setGnssId(measurements[i]->gnss_id()); + mb[i].setGlonassFrequencyIndex(measurements[i]->freq_id()); + mb[i].setLocktime(measurements[i]->lock_time()); + mb[i].setCno(measurements[i]->cno()); + mb[i].setPseudorangeStdev(0.01 * (pow(2, (measurements[i]->pr_stdev() & 15)))); // weird scaling, might be wrong + mb[i].setCarrierPhaseStdev(0.004 * (measurements[i]->cp_stdev() & 15)); + mb[i].setDopplerStdev(0.002 * (pow(2, (measurements[i]->do_stdev() & 15)))); // weird scaling, might be wrong + + auto ts = mb[i].initTrackingStatus(); + auto trk_stat = measurements[i]->trk_stat(); + ts.setPseudorangeValid(bit_to_bool(trk_stat, 0)); + ts.setCarrierPhaseValid(bit_to_bool(trk_stat, 1)); + ts.setHalfCycleValid(bit_to_bool(trk_stat, 2)); + ts.setHalfCycleSubtracted(bit_to_bool(trk_stat, 3)); + } + + mr.setNumMeas(msg->num_meas()); + auto rs = mr.initReceiverStatus(); + rs.setLeapSecValid(bit_to_bool(msg->rec_stat(), 0)); + rs.setClkReset(bit_to_bool(msg->rec_stat(), 2)); + return capnp::messageToFlatArray(msg_builder); +} + +kj::Array UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) { + MessageBuilder msg_builder; + auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); + hwStatus.setNoisePerMS(msg->noise_per_ms()); + hwStatus.setAgcCnt(msg->agc_cnt()); + hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status()); + hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power()); + hwStatus.setJamInd(msg->jam_ind()); + return capnp::messageToFlatArray(msg_builder); +} + +kj::Array UbloxMsgParser::gen_mon_hw2(ubx_t::mon_hw2_t *msg) { + MessageBuilder msg_builder; + auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); + hwStatus.setOfsI(msg->ofs_i()); + hwStatus.setMagI(msg->mag_i()); + hwStatus.setOfsQ(msg->ofs_q()); + hwStatus.setMagQ(msg->mag_q()); + + switch (msg->cfg_source()) { + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_ROM: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); + break; + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); + break; + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); + break; + case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); + break; + default: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); + break; + } + + hwStatus.setLowLevCfg(msg->low_lev_cfg()); + hwStatus.setPostStatus(msg->post_status()); + + return capnp::messageToFlatArray(msg_builder); } diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 2e99e94e..57a713e0 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -1,191 +1,52 @@ #pragma once -#include +#include +#include +#include +#include + #include "messaging.hpp" +#include "generated/ubx.h" +#include "generated/gps.h" -// NAV_PVT -typedef struct __attribute__((packed)) { - uint32_t iTOW; - uint16_t year; - int8_t month; - int8_t day; - int8_t hour; - int8_t min; - int8_t sec; - int8_t valid; - uint32_t tAcc; - int32_t nano; - int8_t fixType; - int8_t flags; - int8_t flags2; - int8_t numSV; - int32_t lon; - int32_t lat; - int32_t height; - int32_t hMSL; - uint32_t hAcc; - uint32_t vAcc; - int32_t velN; - int32_t velE; - int32_t velD; - int32_t gSpeed; - int32_t headMot; - uint32_t sAcc; - uint32_t headAcc; - uint16_t pDOP; - int8_t reserverd1[6]; - int32_t headVeh; - int16_t magDec; - uint16_t magAcc; -} nav_pvt_msg; - -// RXM_RAW -typedef struct __attribute__((packed)) { - double rcvTow; - uint16_t week; - int8_t leapS; - int8_t numMeas; - int8_t recStat; - int8_t reserved1[3]; -} rxm_raw_msg; - -// Extra data count is in numMeas -typedef struct __attribute__((packed)) { - double prMes; - double cpMes; - float doMes; - int8_t gnssId; - int8_t svId; - int8_t sigId; - int8_t freqId; - uint16_t locktime; - int8_t cno; - int8_t prStdev; - int8_t cpStdev; - int8_t doStdev; - int8_t trkStat; - int8_t reserved3; -} rxm_raw_msg_extra; - -// RXM_SFRBX -typedef struct __attribute__((packed)) { - int8_t gnssId; - int8_t svid; - int8_t reserved1; - int8_t freqId; - int8_t numWords; - int8_t reserved2; - int8_t version; - int8_t reserved3; -} rxm_sfrbx_msg; - -// Extra data count is in numWords -typedef struct __attribute__((packed)) { - uint32_t dwrd; -} rxm_sfrbx_msg_extra; - -// MON_HW -typedef struct __attribute__((packed)) { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t reserved1; - uint32_t usedMask; - uint8_t VP[17]; - uint8_t jamInd; - uint8_t reserved2[2]; - uint32_t pinIrq; - uint32_t pullH; - uint32_t pullL; -} mon_hw_msg; - -// MON_HW2 -typedef struct __attribute__((packed)) { - int8_t ofsI; - uint8_t magI; - int8_t ofsQ; - uint8_t magQ; - uint8_t cfgSource; - uint8_t reserved1[3]; - uint32_t lowLevCfg; - uint8_t reserved2[8]; - uint32_t postStatus; - uint8_t reserved3[4]; -} mon_hw2_msg; - +// protocol constants namespace ublox { - // protocol constants const uint8_t PREAMBLE1 = 0xb5; const uint8_t PREAMBLE2 = 0x62; - // message classes - const uint8_t CLASS_NAV = 0x01; - const uint8_t CLASS_RXM = 0x02; - const uint8_t CLASS_MON = 0x0A; - - // NAV messages - const uint8_t MSG_NAV_PVT = 0x7; - - // RXM messages - const uint8_t MSG_RXM_RAW = 0x15; - const uint8_t MSG_RXM_SFRBX = 0x13; - - // MON messages - const uint8_t MSG_MON_HW = 0x09; - const uint8_t MSG_MON_HW2 = 0x0B; - const int UBLOX_HEADER_SIZE = 6; const int UBLOX_CHECKSUM_SIZE = 2; const int UBLOX_MAX_MSG_SIZE = 65536; - typedef std::map> subframes_map; - - class UbloxMsgParser { - public: - - UbloxMsgParser(); - kj::Array gen_solution(); - kj::Array gen_raw(); - kj::Array gen_mon_hw(); - kj::Array gen_mon_hw2(); - - kj::Array gen_nav_data(); - bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); - inline void reset() {bytes_in_parse_buf = 0;} - inline uint8_t msg_class() { - return msg_parse_buf[2]; - } - - inline uint8_t msg_id() { - return msg_parse_buf[3]; - } - inline int needed_bytes(); - - void hexdump(uint8_t *d, int l) { - for (int i = 0; i < l; i++) { - if (i%0x10 == 0 && i != 0) printf("\n"); - printf("%02X ", d[i]); - } - printf("\n"); - } - private: - inline bool valid_cheksum(); - inline bool valid(); - inline bool valid_so_far(); - - uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE]; - int bytes_in_parse_buf; - std::map> nav_frame_buffer; - }; - + // Boardd still uses these: + const uint8_t CLASS_NAV = 0x01; + const uint8_t CLASS_RXM = 0x02; + const uint8_t CLASS_MON = 0x0A; } -typedef Message * (*poll_ubloxraw_msg_func)(Poller *poller); -typedef int (*send_gps_event_func)(PubSocket *s, const void *buf, size_t len); -int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func); +class UbloxMsgParser { + public: + bool add_data(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);} + + std::pair> gen_msg(); + kj::Array gen_nav_pvt(ubx_t::nav_pvt_t *msg); + kj::Array gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg); + kj::Array gen_rxm_rawx(ubx_t::rxm_rawx_t *msg); + kj::Array gen_mon_hw(ubx_t::mon_hw_t *msg); + kj::Array gen_mon_hw2(ubx_t::mon_hw2_t *msg); + + private: + inline bool valid_cheksum(); + inline bool valid(); + inline bool valid_so_far(); + + 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]; + +}; + diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc index 566b8202..d62eb214 100644 --- a/selfdrive/locationd/ubloxd.cc +++ b/selfdrive/locationd/ubloxd.cc @@ -3,6 +3,7 @@ #include "common/swaglog.h" #include "ublox_msg.h" +#include "kaitai/kaitaistream.h" ExitHandler do_exit; using namespace ublox; @@ -35,57 +36,21 @@ int main() { const uint8_t *data = ubloxRaw.begin(); size_t len = ubloxRaw.size(); size_t bytes_consumed = 0; + 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)) { - // New message available - if(parser.msg_class() == CLASS_NAV) { - if(parser.msg_id() == MSG_NAV_PVT) { - //LOGD("MSG_NAV_PVT"); - auto words = parser.gen_solution(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("gpsLocationExternal", bytes.begin(), bytes.size()); - } - } else - LOGW("Unknown nav msg id: 0x%02X", parser.msg_id()); - } else if(parser.msg_class() == CLASS_RXM) { - if(parser.msg_id() == MSG_RXM_RAW) { - //LOGD("MSG_RXM_RAW"); - auto words = parser.gen_raw(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else if(parser.msg_id() == MSG_RXM_SFRBX) { - //LOGD("MSG_RXM_SFRBX"); - auto words = parser.gen_nav_data(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else - LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); - } else if(parser.msg_class() == CLASS_MON) { - if(parser.msg_id() == MSG_MON_HW) { - //LOGD("MSG_MON_HW"); - auto words = parser.gen_mon_hw(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else if(parser.msg_id() == MSG_MON_HW2) { - //LOGD("MSG_MON_HW2"); - auto words = parser.gen_mon_hw2(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - pm.send("ubloxGnss", bytes.begin(), bytes.size()); - } - } else { - LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); + + try { + auto msg = parser.gen_msg(); + if (msg.second.size() > 0) { + auto bytes = msg.second.asBytes(); + pm.send(msg.first.c_str(), bytes.begin(), bytes.size()); } - } else - LOGW("Unknown msg class: 0x%02X", parser.msg_class()); + } catch (const std::exception& e) { + LOGE("Error parsing ublox message %s", e.what()); + } + parser.reset(); } bytes_consumed += bytes_consumed_this_time; diff --git a/selfdrive/locationd/ubx.ksy b/selfdrive/locationd/ubx.ksy new file mode 100644 index 00000000..6945680d --- /dev/null +++ b/selfdrive/locationd/ubx.ksy @@ -0,0 +1,259 @@ +meta: + id: ubx + endian: le +seq: + - id: magic + contents: [0xb5, 0x62] + - id: msg_type + type: u2be + - id: length + type: u2 + - id: body + type: + switch-on: msg_type + cases: + 0x0107: nav_pvt + 0x0213: rxm_sfrbx + 0x0215: rxm_rawx + 0x0a09: mon_hw + 0x0a0b: mon_hw2 +instances: + checksum: + pos: length + 6 + type: u2 + +types: + mon_hw: + seq: + - id: pin_sel + type: u4 + - id: pin_bank + type: u4 + - id: pin_dir + type: u4 + - id: pin_val + type: u4 + - id: noise_per_ms + type: u2 + - id: agc_cnt + type: u2 + - id: a_status + type: u1 + enum: antenna_status + - id: a_power + type: u1 + enum: antenna_power + - id: flags + type: u1 + - id: reserved1 + size: 1 + - id: used_mask + type: u4 + - id: vp + size: 17 + - id: jam_ind + type: u1 + - id: reserved2 + size: 2 + - id: pin_irq + type: u4 + - id: pull_h + type: u4 + - id: pull_l + type: u4 + enums: + antenna_status: + 0: init + 1: dontknow + 2: ok + 3: short + 4: open + antenna_power: + 0: off + 1: on + 2: dontknow + + mon_hw2: + seq: + - id: ofs_i + type: s1 + - id: mag_i + type: u1 + - id: ofs_q + type: s1 + - id: mag_q + type: u1 + - id: cfg_source + type: u1 + enum: config_source + - id: reserved1 + size: 3 + - id: low_lev_cfg + type: u4 + - id: reserved2 + size: 8 + - id: post_status + type: u4 + - id: reserved3 + size: 4 + + enums: + config_source: + 113: rom + 111: otp + 112: config_pins + 102: flash + + rxm_sfrbx: + seq: + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: reserved1 + size: 1 + - id: freq_id + type: u1 + - id: num_words + type: u1 + - id: reserved2 + size: 1 + - id: version + type: u1 + - id: reserved3 + size: 1 + - id: body + type: u4 + repeat: expr + repeat-expr: num_words + + rxm_rawx: + seq: + - id: rcv_tow + type: f8 + - id: week + type: u2 + - id: leap_s + type: s1 + - id: num_meas + type: u1 + - id: rec_stat + type: u1 + - id: reserved1 + size: 3 + - id: measurements + type: meas + size: 32 + repeat: expr + repeat-expr: num_meas + types: + meas: + seq: + - id: pr_mes + type: f8 + - id: cp_mes + type: f8 + - id: do_mes + type: f4 + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: reserved2 + size: 1 + - id: freq_id + type: u1 + - id: lock_time + type: u2 + - id: cno + type: u1 + - id: pr_stdev + type: u1 + - id: cp_stdev + type: u1 + - id: do_stdev + type: u1 + - id: trk_stat + type: u1 + - id: reserved3 + size: 1 + + nav_pvt: + seq: + - id: i_tow + type: u4 + - id: year + type: u2 + - id: month + type: u1 + - id: day + type: u1 + - id: hour + type: u1 + - id: min + type: u1 + - id: sec + type: u1 + - id: valid + type: u1 + - id: t_acc + type: u4 + - id: nano + type: s4 + - id: fix_type + type: u1 + - id: flags + type: u1 + - id: flags2 + type: u1 + - id: num_sv + type: u1 + - id: lon + type: s4 + - id: lat + type: s4 + - id: height + type: s4 + - id: h_msl + type: s4 + - id: h_acc + type: u4 + - id: v_acc + type: u4 + - id: vel_n + type: s4 + - id: vel_e + type: s4 + - id: vel_d + type: s4 + - id: g_speed + type: s4 + - id: head_mot + type: s4 + - id: s_acc + type: s4 + - id: head_acc + type: u4 + - id: p_dop + type: u2 + - id: flags3 + type: u1 + - id: reserved1 + size: 5 + - id: head_veh + type: s4 + - id: mag_dec + type: s2 + - id: mag_acc + type: u2 +enums: + gnss_type: + 0: gps + 1: sbas + 2: galileo + 3: beidou + 4: imes + 5: qzss + 6: glonass