mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-28 01:52:06 +08:00
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 old-commit-hash: eacacb8fba8a58f32132c06e16012f99c2ee5f26
This commit is contained in:
Willem Melching
@@ -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",
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:2ac1dffc2e7aab750dc0c581366db0061b28ca25379ff7527c97a3511e92ebe9
|
||||
size 96
|
||||
oid sha256:6f0c49035e54d1c6781f4d167142d87708f646fc9afb618532a4feab095c4686
|
||||
size 185
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:4715786d62496eec7a4099e1ab5c3bfa89a595760b1f04d7f2e09a9f19bff25b
|
||||
size 236
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:678ca5a541cea90fa93d2736ef15c0e2b5b2f8345bfd8cade118ff77f90fd41d
|
||||
size 5243
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:8563da112b3934e3f8a9b3076b2f4e90cebde6ba3dbaf3e46e11f186fa36400e
|
||||
size 18998
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:c986cb51e0e3bfcd16341e73b24e3db5e05f3cdc1a67a0d2d7c9523cc93f6d2b
|
||||
size 8318
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:b9f412d10c04f9cb1e99173d3a62460a1cb3ac26fcd46dee51b860d225b18c40
|
||||
size 286
|
||||
@@ -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/**
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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>(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() {
|
||||
}
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
#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_
|
||||
@@ -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>(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<std::string>();
|
||||
m__raw_measurements->reserve(l_measurements);
|
||||
m__io__raw_measurements = new std::vector<kaitai::kstream*>();
|
||||
m__io__raw_measurements->reserve(l_measurements);
|
||||
m_measurements = new std::vector<meas_t*>();
|
||||
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<kaitai::kstream*>::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<meas_t*>::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<ubx_t::gnss_type_t>(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<ubx_t::gnss_type_t>(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<uint32_t>();
|
||||
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<ubx_t::mon_hw2_t::config_source_t>(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<ubx_t::mon_hw_t::antenna_status_t>(m__io->read_u1());
|
||||
m_a_power = static_cast<ubx_t::mon_hw_t::antenna_power_t>(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;
|
||||
}
|
||||
@@ -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 <stdint.h>
|
||||
#include <vector>
|
||||
|
||||
#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<meas_t*>* m_measurements;
|
||||
ubx_t* m__root;
|
||||
ubx_t* m__parent;
|
||||
std::vector<std::string>* m__raw_measurements;
|
||||
std::vector<kaitai::kstream*>* 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<meas_t*>* measurements() const { return m_measurements; }
|
||||
ubx_t* _root() const { return m__root; }
|
||||
ubx_t* _parent() const { return m__parent; }
|
||||
std::vector<std::string>* _raw_measurements() const { return m__raw_measurements; }
|
||||
std::vector<kaitai::kstream*>* _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<uint32_t>* 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<uint32_t>* 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_
|
||||
@@ -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
|
||||
|
||||
+267
-321
@@ -1,144 +1,29 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <ctime>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <cassert>
|
||||
#include <unordered_map>
|
||||
|
||||
#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<uint8_t, subframes_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<capnp::word> 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<capnp::word> 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<capnp::word>();
|
||||
}
|
||||
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<capnp::word> 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<capnp::word>();
|
||||
}
|
||||
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<uint32_t> 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<const double>());
|
||||
eph.setIonoBeta(kj::ArrayPtr<const double>());
|
||||
}
|
||||
return capnp::messageToFlatArray(msg_builder);
|
||||
}
|
||||
}
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
kj::Array<capnp::word> 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<capnp::word> 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<std::string, kj::Array<capnp::word>> 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<ubx_t::nav_pvt_t*>(body))};
|
||||
break;
|
||||
case 0x0213:
|
||||
return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
|
||||
break;
|
||||
case 0x0215:
|
||||
return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
|
||||
break;
|
||||
case 0x0a09:
|
||||
return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
|
||||
break;
|
||||
case 0x0a0b:
|
||||
return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
|
||||
break;
|
||||
default:
|
||||
LOGE("Unkown message type %x", ubx_message.msg_type());
|
||||
return {"ubloxGnss", kj::Array<capnp::word>()};
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
kj::Array<capnp::word> 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<capnp::word> 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<gps_t::subframe_1_t*>(subframe.body());
|
||||
|
||||
eph.setGpsWeek(subframe_1->week_no());
|
||||
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
|
||||
eph.setToc(subframe_1->t_oc() * pow(2, 4));
|
||||
eph.setAf2(subframe_1->af_2() * pow(2, -55));
|
||||
eph.setAf1(subframe_1->af_1() * pow(2, -43));
|
||||
eph.setAf0(subframe_1->af_0() * pow(2, -31));
|
||||
}
|
||||
|
||||
// Subframe 2
|
||||
{
|
||||
kaitai::kstream stream(gps_subframes[msg->sv_id()][2]);
|
||||
gps_t subframe(&stream);
|
||||
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
|
||||
|
||||
eph.setCrs(subframe_2->c_rs() * pow(2, -5));
|
||||
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
|
||||
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
|
||||
eph.setCuc(subframe_2->c_uc() * pow(2, -29));
|
||||
eph.setEcc(subframe_2->e() * pow(2, -33));
|
||||
eph.setCus(subframe_2->c_us() * pow(2, -29));
|
||||
eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0));
|
||||
eph.setToe(subframe_2->t_oe() * pow(2, 4));
|
||||
}
|
||||
|
||||
// Subframe 3
|
||||
{
|
||||
kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
|
||||
gps_t subframe(&stream);
|
||||
gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
|
||||
|
||||
eph.setCic(subframe_3->c_ic() * pow(2, -29));
|
||||
eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
|
||||
eph.setCis(subframe_3->c_is() * pow(2, -29));
|
||||
eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
|
||||
eph.setCrc(subframe_3->c_rc() * pow(2, -5));
|
||||
eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
|
||||
eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
|
||||
eph.setIode(subframe_3->iode());
|
||||
eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
|
||||
}
|
||||
|
||||
// Subframe 4
|
||||
{
|
||||
kaitai::kstream stream(gps_subframes[msg->sv_id()][4]);
|
||||
gps_t subframe(&stream);
|
||||
gps_t::subframe_4_t* subframe_4 = static_cast<gps_t::subframe_4_t*>(subframe.body());
|
||||
|
||||
// This is page 18, why is the page id 56?
|
||||
if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) {
|
||||
auto iono = static_cast<gps_t::subframe_4_t::ionosphere_data_t*>(subframe_4->body());
|
||||
double a0 = iono->a0() * pow(2, -30);
|
||||
double a1 = iono->a1() * pow(2, -27);
|
||||
double a2 = iono->a2() * pow(2, -24);
|
||||
double a3 = iono->a3() * pow(2, -24);
|
||||
eph.setIonoAlpha({a0, a1, a2, a3});
|
||||
|
||||
double b0 = iono->b0() * pow(2, 11);
|
||||
double b1 = iono->b1() * pow(2, 14);
|
||||
double b2 = iono->b2() * pow(2, 16);
|
||||
double b3 = iono->b3() * pow(2, 16);
|
||||
eph.setIonoBeta({b0, b1, b2, b3});
|
||||
}
|
||||
}
|
||||
|
||||
return capnp::messageToFlatArray(msg_builder);
|
||||
}
|
||||
}
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
kj::Array<capnp::word> 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<capnp::word> 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<capnp::word> 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);
|
||||
}
|
||||
|
||||
+38
-177
@@ -1,191 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
|
||||
#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<uint8_t, std::vector<uint32_t>> subframes_map;
|
||||
|
||||
class UbloxMsgParser {
|
||||
public:
|
||||
|
||||
UbloxMsgParser();
|
||||
kj::Array<capnp::word> gen_solution();
|
||||
kj::Array<capnp::word> gen_raw();
|
||||
kj::Array<capnp::word> gen_mon_hw();
|
||||
kj::Array<capnp::word> gen_mon_hw2();
|
||||
|
||||
kj::Array<capnp::word> 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<uint8_t, std::map<uint8_t, subframes_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<std::string, kj::Array<capnp::word>> gen_msg();
|
||||
kj::Array<capnp::word> gen_nav_pvt(ubx_t::nav_pvt_t *msg);
|
||||
kj::Array<capnp::word> gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg);
|
||||
kj::Array<capnp::word> gen_rxm_rawx(ubx_t::rxm_rawx_t *msg);
|
||||
kj::Array<capnp::word> gen_mon_hw(ubx_t::mon_hw_t *msg);
|
||||
kj::Array<capnp::word> 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<int, std::unordered_map<int, std::string>> gps_subframes;
|
||||
|
||||
size_t bytes_in_parse_buf = 0;
|
||||
uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user