mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 00:03:45 +08:00
ubloxd: move to system/ (#27536)
* ubloxd: move to system/ * more fixes * add missing sconscript * fix valgrind tests * uncomment old-commit-hash: c97911dbad15c3cc7dc577b305ab9aed775e021f
This commit is contained in:
Adeeb Shihadeh
@@ -0,0 +1,2 @@
|
||||
ubloxd
|
||||
tests/test_glonass_runner
|
||||
@@ -0,0 +1,20 @@
|
||||
Import('env', 'common', 'cereal', 'messaging')
|
||||
|
||||
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread']
|
||||
|
||||
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)
|
||||
glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd)
|
||||
|
||||
# kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910
|
||||
patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES')
|
||||
env.Depends(patch, glonass)
|
||||
|
||||
glonass_obj = env.Object('generated/glonass.cpp')
|
||||
env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs)
|
||||
|
||||
if GetOption('test'):
|
||||
env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs])
|
||||
@@ -0,0 +1,353 @@
|
||||
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
|
||||
|
||||
#include "glonass.h"
|
||||
|
||||
glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = this;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::_read() {
|
||||
m_idle_chip = m__io->read_bits_int_be(1);
|
||||
m_string_number = m__io->read_bits_int_be(4);
|
||||
//m__io->align_to_byte();
|
||||
switch (string_number()) {
|
||||
case 4: {
|
||||
m_data = new string_4_t(m__io, this, m__root);
|
||||
break;
|
||||
}
|
||||
case 1: {
|
||||
m_data = new string_1_t(m__io, this, m__root);
|
||||
break;
|
||||
}
|
||||
case 3: {
|
||||
m_data = new string_3_t(m__io, this, m__root);
|
||||
break;
|
||||
}
|
||||
case 5: {
|
||||
m_data = new string_5_t(m__io, this, m__root);
|
||||
break;
|
||||
}
|
||||
case 2: {
|
||||
m_data = new string_2_t(m__io, this, m__root);
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
m_data = new string_non_immediate_t(m__io, this, m__root);
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_hamming_code = m__io->read_bits_int_be(8);
|
||||
m_pad_1 = m__io->read_bits_int_be(11);
|
||||
m_superframe_number = m__io->read_bits_int_be(16);
|
||||
m_pad_2 = m__io->read_bits_int_be(8);
|
||||
m_frame_number = m__io->read_bits_int_be(8);
|
||||
}
|
||||
|
||||
glonass_t::~glonass_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::_clean_up() {
|
||||
if (m_data) {
|
||||
delete m_data; m_data = 0;
|
||||
}
|
||||
}
|
||||
|
||||
glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = p__root;
|
||||
f_tau_n = false;
|
||||
f_delta_tau_n = false;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::string_4_t::_read() {
|
||||
m_tau_n_sign = m__io->read_bits_int_be(1);
|
||||
m_tau_n_value = m__io->read_bits_int_be(21);
|
||||
m_delta_tau_n_sign = m__io->read_bits_int_be(1);
|
||||
m_delta_tau_n_value = m__io->read_bits_int_be(4);
|
||||
m_e_n = m__io->read_bits_int_be(5);
|
||||
m_not_used_1 = m__io->read_bits_int_be(14);
|
||||
m_p4 = m__io->read_bits_int_be(1);
|
||||
m_f_t = m__io->read_bits_int_be(4);
|
||||
m_not_used_2 = m__io->read_bits_int_be(3);
|
||||
m_n_t = m__io->read_bits_int_be(11);
|
||||
m_n = m__io->read_bits_int_be(5);
|
||||
m_m = m__io->read_bits_int_be(2);
|
||||
}
|
||||
|
||||
glonass_t::string_4_t::~string_4_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::string_4_t::_clean_up() {
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_4_t::tau_n() {
|
||||
if (f_tau_n)
|
||||
return m_tau_n;
|
||||
m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value()));
|
||||
f_tau_n = true;
|
||||
return m_tau_n;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_4_t::delta_tau_n() {
|
||||
if (f_delta_tau_n)
|
||||
return m_delta_tau_n;
|
||||
m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value()));
|
||||
f_delta_tau_n = true;
|
||||
return m_delta_tau_n;
|
||||
}
|
||||
|
||||
glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = p__root;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::string_non_immediate_t::_read() {
|
||||
m_data_1 = m__io->read_bits_int_be(64);
|
||||
m_data_2 = m__io->read_bits_int_be(8);
|
||||
}
|
||||
|
||||
glonass_t::string_non_immediate_t::~string_non_immediate_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::string_non_immediate_t::_clean_up() {
|
||||
}
|
||||
|
||||
glonass_t::string_5_t::string_5_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = p__root;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::string_5_t::_read() {
|
||||
m_n_a = m__io->read_bits_int_be(11);
|
||||
m_tau_c = m__io->read_bits_int_be(32);
|
||||
m_not_used = m__io->read_bits_int_be(1);
|
||||
m_n_4 = m__io->read_bits_int_be(5);
|
||||
m_tau_gps = m__io->read_bits_int_be(22);
|
||||
m_l_n = m__io->read_bits_int_be(1);
|
||||
}
|
||||
|
||||
glonass_t::string_5_t::~string_5_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::string_5_t::_clean_up() {
|
||||
}
|
||||
|
||||
glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = p__root;
|
||||
f_x_vel = false;
|
||||
f_x_accel = false;
|
||||
f_x = false;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::string_1_t::_read() {
|
||||
m_not_used = m__io->read_bits_int_be(2);
|
||||
m_p1 = m__io->read_bits_int_be(2);
|
||||
m_t_k = m__io->read_bits_int_be(12);
|
||||
m_x_vel_sign = m__io->read_bits_int_be(1);
|
||||
m_x_vel_value = m__io->read_bits_int_be(23);
|
||||
m_x_accel_sign = m__io->read_bits_int_be(1);
|
||||
m_x_accel_value = m__io->read_bits_int_be(4);
|
||||
m_x_sign = m__io->read_bits_int_be(1);
|
||||
m_x_value = m__io->read_bits_int_be(26);
|
||||
}
|
||||
|
||||
glonass_t::string_1_t::~string_1_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::string_1_t::_clean_up() {
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_1_t::x_vel() {
|
||||
if (f_x_vel)
|
||||
return m_x_vel;
|
||||
m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value()));
|
||||
f_x_vel = true;
|
||||
return m_x_vel;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_1_t::x_accel() {
|
||||
if (f_x_accel)
|
||||
return m_x_accel;
|
||||
m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value()));
|
||||
f_x_accel = true;
|
||||
return m_x_accel;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_1_t::x() {
|
||||
if (f_x)
|
||||
return m_x;
|
||||
m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value()));
|
||||
f_x = true;
|
||||
return m_x;
|
||||
}
|
||||
|
||||
glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = p__root;
|
||||
f_y_vel = false;
|
||||
f_y_accel = false;
|
||||
f_y = false;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::string_2_t::_read() {
|
||||
m_b_n = m__io->read_bits_int_be(3);
|
||||
m_p2 = m__io->read_bits_int_be(1);
|
||||
m_t_b = m__io->read_bits_int_be(7);
|
||||
m_not_used = m__io->read_bits_int_be(5);
|
||||
m_y_vel_sign = m__io->read_bits_int_be(1);
|
||||
m_y_vel_value = m__io->read_bits_int_be(23);
|
||||
m_y_accel_sign = m__io->read_bits_int_be(1);
|
||||
m_y_accel_value = m__io->read_bits_int_be(4);
|
||||
m_y_sign = m__io->read_bits_int_be(1);
|
||||
m_y_value = m__io->read_bits_int_be(26);
|
||||
}
|
||||
|
||||
glonass_t::string_2_t::~string_2_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::string_2_t::_clean_up() {
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_2_t::y_vel() {
|
||||
if (f_y_vel)
|
||||
return m_y_vel;
|
||||
m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value()));
|
||||
f_y_vel = true;
|
||||
return m_y_vel;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_2_t::y_accel() {
|
||||
if (f_y_accel)
|
||||
return m_y_accel;
|
||||
m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value()));
|
||||
f_y_accel = true;
|
||||
return m_y_accel;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_2_t::y() {
|
||||
if (f_y)
|
||||
return m_y;
|
||||
m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value()));
|
||||
f_y = true;
|
||||
return m_y;
|
||||
}
|
||||
|
||||
glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
|
||||
m__parent = p__parent;
|
||||
m__root = p__root;
|
||||
f_gamma_n = false;
|
||||
f_z_vel = false;
|
||||
f_z_accel = false;
|
||||
f_z = false;
|
||||
|
||||
try {
|
||||
_read();
|
||||
} catch(...) {
|
||||
_clean_up();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void glonass_t::string_3_t::_read() {
|
||||
m_p3 = m__io->read_bits_int_be(1);
|
||||
m_gamma_n_sign = m__io->read_bits_int_be(1);
|
||||
m_gamma_n_value = m__io->read_bits_int_be(10);
|
||||
m_not_used = m__io->read_bits_int_be(1);
|
||||
m_p = m__io->read_bits_int_be(2);
|
||||
m_l_n = m__io->read_bits_int_be(1);
|
||||
m_z_vel_sign = m__io->read_bits_int_be(1);
|
||||
m_z_vel_value = m__io->read_bits_int_be(23);
|
||||
m_z_accel_sign = m__io->read_bits_int_be(1);
|
||||
m_z_accel_value = m__io->read_bits_int_be(4);
|
||||
m_z_sign = m__io->read_bits_int_be(1);
|
||||
m_z_value = m__io->read_bits_int_be(26);
|
||||
}
|
||||
|
||||
glonass_t::string_3_t::~string_3_t() {
|
||||
_clean_up();
|
||||
}
|
||||
|
||||
void glonass_t::string_3_t::_clean_up() {
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_3_t::gamma_n() {
|
||||
if (f_gamma_n)
|
||||
return m_gamma_n;
|
||||
m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value()));
|
||||
f_gamma_n = true;
|
||||
return m_gamma_n;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_3_t::z_vel() {
|
||||
if (f_z_vel)
|
||||
return m_z_vel;
|
||||
m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value()));
|
||||
f_z_vel = true;
|
||||
return m_z_vel;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_3_t::z_accel() {
|
||||
if (f_z_accel)
|
||||
return m_z_accel;
|
||||
m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value()));
|
||||
f_z_accel = true;
|
||||
return m_z_accel;
|
||||
}
|
||||
|
||||
int32_t glonass_t::string_3_t::z() {
|
||||
if (f_z)
|
||||
return m_z;
|
||||
m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value()));
|
||||
f_z = true;
|
||||
return m_z;
|
||||
}
|
||||
@@ -0,0 +1,375 @@
|
||||
#ifndef GLONASS_H_
|
||||
#define GLONASS_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 glonass_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
class string_4_t;
|
||||
class string_non_immediate_t;
|
||||
class string_5_t;
|
||||
class string_1_t;
|
||||
class string_2_t;
|
||||
class string_3_t;
|
||||
|
||||
glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~glonass_t();
|
||||
|
||||
class string_4_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
|
||||
string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~string_4_t();
|
||||
|
||||
private:
|
||||
bool f_tau_n;
|
||||
int32_t m_tau_n;
|
||||
|
||||
public:
|
||||
int32_t tau_n();
|
||||
|
||||
private:
|
||||
bool f_delta_tau_n;
|
||||
int32_t m_delta_tau_n;
|
||||
|
||||
public:
|
||||
int32_t delta_tau_n();
|
||||
|
||||
private:
|
||||
bool m_tau_n_sign;
|
||||
uint64_t m_tau_n_value;
|
||||
bool m_delta_tau_n_sign;
|
||||
uint64_t m_delta_tau_n_value;
|
||||
uint64_t m_e_n;
|
||||
uint64_t m_not_used_1;
|
||||
bool m_p4;
|
||||
uint64_t m_f_t;
|
||||
uint64_t m_not_used_2;
|
||||
uint64_t m_n_t;
|
||||
uint64_t m_n;
|
||||
uint64_t m_m;
|
||||
glonass_t* m__root;
|
||||
glonass_t* m__parent;
|
||||
|
||||
public:
|
||||
bool tau_n_sign() const { return m_tau_n_sign; }
|
||||
uint64_t tau_n_value() const { return m_tau_n_value; }
|
||||
bool delta_tau_n_sign() const { return m_delta_tau_n_sign; }
|
||||
uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; }
|
||||
uint64_t e_n() const { return m_e_n; }
|
||||
uint64_t not_used_1() const { return m_not_used_1; }
|
||||
bool p4() const { return m_p4; }
|
||||
uint64_t f_t() const { return m_f_t; }
|
||||
uint64_t not_used_2() const { return m_not_used_2; }
|
||||
uint64_t n_t() const { return m_n_t; }
|
||||
uint64_t n() const { return m_n; }
|
||||
uint64_t m() const { return m_m; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
glonass_t* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
class string_non_immediate_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
|
||||
string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~string_non_immediate_t();
|
||||
|
||||
private:
|
||||
uint64_t m_data_1;
|
||||
uint64_t m_data_2;
|
||||
glonass_t* m__root;
|
||||
glonass_t* m__parent;
|
||||
|
||||
public:
|
||||
uint64_t data_1() const { return m_data_1; }
|
||||
uint64_t data_2() const { return m_data_2; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
glonass_t* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
class string_5_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
|
||||
string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~string_5_t();
|
||||
|
||||
private:
|
||||
uint64_t m_n_a;
|
||||
uint64_t m_tau_c;
|
||||
bool m_not_used;
|
||||
uint64_t m_n_4;
|
||||
uint64_t m_tau_gps;
|
||||
bool m_l_n;
|
||||
glonass_t* m__root;
|
||||
glonass_t* m__parent;
|
||||
|
||||
public:
|
||||
uint64_t n_a() const { return m_n_a; }
|
||||
uint64_t tau_c() const { return m_tau_c; }
|
||||
bool not_used() const { return m_not_used; }
|
||||
uint64_t n_4() const { return m_n_4; }
|
||||
uint64_t tau_gps() const { return m_tau_gps; }
|
||||
bool l_n() const { return m_l_n; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
glonass_t* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
class string_1_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
|
||||
string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~string_1_t();
|
||||
|
||||
private:
|
||||
bool f_x_vel;
|
||||
int32_t m_x_vel;
|
||||
|
||||
public:
|
||||
int32_t x_vel();
|
||||
|
||||
private:
|
||||
bool f_x_accel;
|
||||
int32_t m_x_accel;
|
||||
|
||||
public:
|
||||
int32_t x_accel();
|
||||
|
||||
private:
|
||||
bool f_x;
|
||||
int32_t m_x;
|
||||
|
||||
public:
|
||||
int32_t x();
|
||||
|
||||
private:
|
||||
uint64_t m_not_used;
|
||||
uint64_t m_p1;
|
||||
uint64_t m_t_k;
|
||||
bool m_x_vel_sign;
|
||||
uint64_t m_x_vel_value;
|
||||
bool m_x_accel_sign;
|
||||
uint64_t m_x_accel_value;
|
||||
bool m_x_sign;
|
||||
uint64_t m_x_value;
|
||||
glonass_t* m__root;
|
||||
glonass_t* m__parent;
|
||||
|
||||
public:
|
||||
uint64_t not_used() const { return m_not_used; }
|
||||
uint64_t p1() const { return m_p1; }
|
||||
uint64_t t_k() const { return m_t_k; }
|
||||
bool x_vel_sign() const { return m_x_vel_sign; }
|
||||
uint64_t x_vel_value() const { return m_x_vel_value; }
|
||||
bool x_accel_sign() const { return m_x_accel_sign; }
|
||||
uint64_t x_accel_value() const { return m_x_accel_value; }
|
||||
bool x_sign() const { return m_x_sign; }
|
||||
uint64_t x_value() const { return m_x_value; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
glonass_t* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
class string_2_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
|
||||
string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~string_2_t();
|
||||
|
||||
private:
|
||||
bool f_y_vel;
|
||||
int32_t m_y_vel;
|
||||
|
||||
public:
|
||||
int32_t y_vel();
|
||||
|
||||
private:
|
||||
bool f_y_accel;
|
||||
int32_t m_y_accel;
|
||||
|
||||
public:
|
||||
int32_t y_accel();
|
||||
|
||||
private:
|
||||
bool f_y;
|
||||
int32_t m_y;
|
||||
|
||||
public:
|
||||
int32_t y();
|
||||
|
||||
private:
|
||||
uint64_t m_b_n;
|
||||
bool m_p2;
|
||||
uint64_t m_t_b;
|
||||
uint64_t m_not_used;
|
||||
bool m_y_vel_sign;
|
||||
uint64_t m_y_vel_value;
|
||||
bool m_y_accel_sign;
|
||||
uint64_t m_y_accel_value;
|
||||
bool m_y_sign;
|
||||
uint64_t m_y_value;
|
||||
glonass_t* m__root;
|
||||
glonass_t* m__parent;
|
||||
|
||||
public:
|
||||
uint64_t b_n() const { return m_b_n; }
|
||||
bool p2() const { return m_p2; }
|
||||
uint64_t t_b() const { return m_t_b; }
|
||||
uint64_t not_used() const { return m_not_used; }
|
||||
bool y_vel_sign() const { return m_y_vel_sign; }
|
||||
uint64_t y_vel_value() const { return m_y_vel_value; }
|
||||
bool y_accel_sign() const { return m_y_accel_sign; }
|
||||
uint64_t y_accel_value() const { return m_y_accel_value; }
|
||||
bool y_sign() const { return m_y_sign; }
|
||||
uint64_t y_value() const { return m_y_value; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
glonass_t* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
class string_3_t : public kaitai::kstruct {
|
||||
|
||||
public:
|
||||
|
||||
string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _clean_up();
|
||||
|
||||
public:
|
||||
~string_3_t();
|
||||
|
||||
private:
|
||||
bool f_gamma_n;
|
||||
int32_t m_gamma_n;
|
||||
|
||||
public:
|
||||
int32_t gamma_n();
|
||||
|
||||
private:
|
||||
bool f_z_vel;
|
||||
int32_t m_z_vel;
|
||||
|
||||
public:
|
||||
int32_t z_vel();
|
||||
|
||||
private:
|
||||
bool f_z_accel;
|
||||
int32_t m_z_accel;
|
||||
|
||||
public:
|
||||
int32_t z_accel();
|
||||
|
||||
private:
|
||||
bool f_z;
|
||||
int32_t m_z;
|
||||
|
||||
public:
|
||||
int32_t z();
|
||||
|
||||
private:
|
||||
bool m_p3;
|
||||
bool m_gamma_n_sign;
|
||||
uint64_t m_gamma_n_value;
|
||||
bool m_not_used;
|
||||
uint64_t m_p;
|
||||
bool m_l_n;
|
||||
bool m_z_vel_sign;
|
||||
uint64_t m_z_vel_value;
|
||||
bool m_z_accel_sign;
|
||||
uint64_t m_z_accel_value;
|
||||
bool m_z_sign;
|
||||
uint64_t m_z_value;
|
||||
glonass_t* m__root;
|
||||
glonass_t* m__parent;
|
||||
|
||||
public:
|
||||
bool p3() const { return m_p3; }
|
||||
bool gamma_n_sign() const { return m_gamma_n_sign; }
|
||||
uint64_t gamma_n_value() const { return m_gamma_n_value; }
|
||||
bool not_used() const { return m_not_used; }
|
||||
uint64_t p() const { return m_p; }
|
||||
bool l_n() const { return m_l_n; }
|
||||
bool z_vel_sign() const { return m_z_vel_sign; }
|
||||
uint64_t z_vel_value() const { return m_z_vel_value; }
|
||||
bool z_accel_sign() const { return m_z_accel_sign; }
|
||||
uint64_t z_accel_value() const { return m_z_accel_value; }
|
||||
bool z_sign() const { return m_z_sign; }
|
||||
uint64_t z_value() const { return m_z_value; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
glonass_t* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
private:
|
||||
bool m_idle_chip;
|
||||
uint64_t m_string_number;
|
||||
kaitai::kstruct* m_data;
|
||||
uint64_t m_hamming_code;
|
||||
uint64_t m_pad_1;
|
||||
uint64_t m_superframe_number;
|
||||
uint64_t m_pad_2;
|
||||
uint64_t m_frame_number;
|
||||
glonass_t* m__root;
|
||||
kaitai::kstruct* m__parent;
|
||||
|
||||
public:
|
||||
bool idle_chip() const { return m_idle_chip; }
|
||||
uint64_t string_number() const { return m_string_number; }
|
||||
kaitai::kstruct* data() const { return m_data; }
|
||||
uint64_t hamming_code() const { return m_hamming_code; }
|
||||
uint64_t pad_1() const { return m_pad_1; }
|
||||
uint64_t superframe_number() const { return m_superframe_number; }
|
||||
uint64_t pad_2() const { return m_pad_2; }
|
||||
uint64_t frame_number() const { return m_frame_number; }
|
||||
glonass_t* _root() const { return m__root; }
|
||||
kaitai::kstruct* _parent() const { return m__parent; }
|
||||
};
|
||||
|
||||
#endif // GLONASS_H_
|
||||
@@ -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_preamble = m__io->read_bytes(1);
|
||||
if (!(preamble() == std::string("\x8B", 1))) {
|
||||
throw kaitai::validation_not_equal_error<std::string>(std::string("\x8B", 1), preamble(), _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_preamble;
|
||||
uint64_t m_tlm;
|
||||
bool m_integrity_status;
|
||||
bool m_reserved;
|
||||
gps_t* m__root;
|
||||
gps_t* m__parent;
|
||||
|
||||
public:
|
||||
std::string preamble() const { return m_preamble; }
|
||||
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,336 @@
|
||||
// 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);
|
||||
m__raw_measurements = new std::vector<std::string>();
|
||||
m__io__raw_measurements = new std::vector<kaitai::kstream*>();
|
||||
m_measurements = new std::vector<meas_t*>();
|
||||
const int l_measurements = num_meas();
|
||||
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);
|
||||
m_body = new std::vector<uint32_t>();
|
||||
const int l_body = num_words();
|
||||
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,176 @@
|
||||
# http://gauss.gge.unb.ca/GLONASS.ICD.pdf
|
||||
# some variables are misprinted but good in the old doc
|
||||
# https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf
|
||||
meta:
|
||||
id: glonass
|
||||
endian: be
|
||||
bit-endian: be
|
||||
seq:
|
||||
- id: idle_chip
|
||||
type: b1
|
||||
- id: string_number
|
||||
type: b4
|
||||
- id: data
|
||||
type:
|
||||
switch-on: string_number
|
||||
cases:
|
||||
1: string_1
|
||||
2: string_2
|
||||
3: string_3
|
||||
4: string_4
|
||||
5: string_5
|
||||
_: string_non_immediate
|
||||
- id: hamming_code
|
||||
type: b8
|
||||
- id: pad_1
|
||||
type: b11
|
||||
- id: superframe_number
|
||||
type: b16
|
||||
- id: pad_2
|
||||
type: b8
|
||||
- id: frame_number
|
||||
type: b8
|
||||
|
||||
types:
|
||||
string_1:
|
||||
seq:
|
||||
- id: not_used
|
||||
type: b2
|
||||
- id: p1
|
||||
type: b2
|
||||
- id: t_k
|
||||
type: b12
|
||||
- id: x_vel_sign
|
||||
type: b1
|
||||
- id: x_vel_value
|
||||
type: b23
|
||||
- id: x_accel_sign
|
||||
type: b1
|
||||
- id: x_accel_value
|
||||
type: b4
|
||||
- id: x_sign
|
||||
type: b1
|
||||
- id: x_value
|
||||
type: b26
|
||||
instances:
|
||||
x_vel:
|
||||
value: 'x_vel_sign ? (x_vel_value * (-1)) : x_vel_value'
|
||||
x_accel:
|
||||
value: 'x_accel_sign ? (x_accel_value * (-1)) : x_accel_value'
|
||||
x:
|
||||
value: 'x_sign ? (x_value * (-1)) : x_value'
|
||||
string_2:
|
||||
seq:
|
||||
- id: b_n
|
||||
type: b3
|
||||
- id: p2
|
||||
type: b1
|
||||
- id: t_b
|
||||
type: b7
|
||||
- id: not_used
|
||||
type: b5
|
||||
- id: y_vel_sign
|
||||
type: b1
|
||||
- id: y_vel_value
|
||||
type: b23
|
||||
- id: y_accel_sign
|
||||
type: b1
|
||||
- id: y_accel_value
|
||||
type: b4
|
||||
- id: y_sign
|
||||
type: b1
|
||||
- id: y_value
|
||||
type: b26
|
||||
instances:
|
||||
y_vel:
|
||||
value: 'y_vel_sign ? (y_vel_value * (-1)) : y_vel_value'
|
||||
y_accel:
|
||||
value: 'y_accel_sign ? (y_accel_value * (-1)) : y_accel_value'
|
||||
y:
|
||||
value: 'y_sign ? (y_value * (-1)) : y_value'
|
||||
string_3:
|
||||
seq:
|
||||
- id: p3
|
||||
type: b1
|
||||
- id: gamma_n_sign
|
||||
type: b1
|
||||
- id: gamma_n_value
|
||||
type: b10
|
||||
- id: not_used
|
||||
type: b1
|
||||
- id: p
|
||||
type: b2
|
||||
- id: l_n
|
||||
type: b1
|
||||
- id: z_vel_sign
|
||||
type: b1
|
||||
- id: z_vel_value
|
||||
type: b23
|
||||
- id: z_accel_sign
|
||||
type: b1
|
||||
- id: z_accel_value
|
||||
type: b4
|
||||
- id: z_sign
|
||||
type: b1
|
||||
- id: z_value
|
||||
type: b26
|
||||
instances:
|
||||
gamma_n:
|
||||
value: 'gamma_n_sign ? (gamma_n_value * (-1)) : gamma_n_value'
|
||||
z_vel:
|
||||
value: 'z_vel_sign ? (z_vel_value * (-1)) : z_vel_value'
|
||||
z_accel:
|
||||
value: 'z_accel_sign ? (z_accel_value * (-1)) : z_accel_value'
|
||||
z:
|
||||
value: 'z_sign ? (z_value * (-1)) : z_value'
|
||||
string_4:
|
||||
seq:
|
||||
- id: tau_n_sign
|
||||
type: b1
|
||||
- id: tau_n_value
|
||||
type: b21
|
||||
- id: delta_tau_n_sign
|
||||
type: b1
|
||||
- id: delta_tau_n_value
|
||||
type: b4
|
||||
- id: e_n
|
||||
type: b5
|
||||
- id: not_used_1
|
||||
type: b14
|
||||
- id: p4
|
||||
type: b1
|
||||
- id: f_t
|
||||
type: b4
|
||||
- id: not_used_2
|
||||
type: b3
|
||||
- id: n_t
|
||||
type: b11
|
||||
- id: n
|
||||
type: b5
|
||||
- id: m
|
||||
type: b2
|
||||
instances:
|
||||
tau_n:
|
||||
value: 'tau_n_sign ? (tau_n_value * (-1)) : tau_n_value'
|
||||
delta_tau_n:
|
||||
value: 'delta_tau_n_sign ? (delta_tau_n_value * (-1)) : delta_tau_n_value'
|
||||
string_5:
|
||||
seq:
|
||||
- id: n_a
|
||||
type: b11
|
||||
- id: tau_c
|
||||
type: b32
|
||||
- id: not_used
|
||||
type: b1
|
||||
- id: n_4
|
||||
type: b5
|
||||
- id: tau_gps
|
||||
type: b22
|
||||
- id: l_n
|
||||
type: b1
|
||||
string_non_immediate:
|
||||
seq:
|
||||
- id: data_1
|
||||
type: b64
|
||||
- id: data_2
|
||||
type: b8
|
||||
@@ -0,0 +1,13 @@
|
||||
diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp
|
||||
index 5b17bc327..b5c6aa610 100644
|
||||
--- a/system/ubloxd/generated/glonass.cpp
|
||||
+++ b/system/ubloxd/generated/glonass.cpp
|
||||
@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass
|
||||
void glonass_t::_read() {
|
||||
m_idle_chip = m__io->read_bits_int_be(1);
|
||||
m_string_number = m__io->read_bits_int_be(4);
|
||||
- m__io->align_to_byte();
|
||||
+ //m__io->align_to_byte();
|
||||
switch (string_number()) {
|
||||
case 4: {
|
||||
m_data = new string_4_t(m__io, this, m__root);
|
||||
@@ -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: preamble
|
||||
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
|
||||
|
||||
Executable
+20
@@ -0,0 +1,20 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
import cereal.messaging as messaging
|
||||
|
||||
if __name__ == "__main__":
|
||||
sm = messaging.SubMaster(['ubloxGnss', 'gpsLocationExternal'])
|
||||
|
||||
while 1:
|
||||
ug = sm['ubloxGnss']
|
||||
gle = sm['gpsLocationExternal']
|
||||
|
||||
try:
|
||||
cnos = []
|
||||
for m in ug.measurementReport.measurements:
|
||||
cnos.append(m.cno)
|
||||
print("Sats: %d Accuracy: %.2f m cnos" % (ug.measurementReport.numMeas, gle.accuracy), sorted(cnos))
|
||||
except Exception:
|
||||
pass
|
||||
sm.update()
|
||||
time.sleep(0.1)
|
||||
@@ -0,0 +1,360 @@
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <bitset>
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
|
||||
#include "catch2/catch.hpp"
|
||||
#include "system/ubloxd/generated/glonass.h"
|
||||
|
||||
typedef std::vector<std::pair<int, int64_t>> string_data;
|
||||
|
||||
#define IDLE_CHIP_IDX 0
|
||||
#define STRING_NUMBER_IDX 1
|
||||
// string data 1-5
|
||||
#define HC_IDX 0
|
||||
#define PAD1_IDX 1
|
||||
#define SUPERFRAME_IDX 2
|
||||
#define PAD2_IDX 3
|
||||
#define FRAME_IDX 4
|
||||
|
||||
// Indexes for string number 1
|
||||
#define ST1_NU_IDX 2
|
||||
#define ST1_P1_IDX 3
|
||||
#define ST1_T_K_IDX 4
|
||||
#define ST1_X_VEL_S_IDX 5
|
||||
#define ST1_X_VEL_V_IDX 6
|
||||
#define ST1_X_ACCEL_S_IDX 7
|
||||
#define ST1_X_ACCEL_V_IDX 8
|
||||
#define ST1_X_S_IDX 9
|
||||
#define ST1_X_V_IDX 10
|
||||
#define ST1_HC_OFF 11
|
||||
|
||||
// Indexes for string number 2
|
||||
#define ST2_BN_IDX 2
|
||||
#define ST2_P2_IDX 3
|
||||
#define ST2_TB_IDX 4
|
||||
#define ST2_NU_IDX 5
|
||||
#define ST2_Y_VEL_S_IDX 6
|
||||
#define ST2_Y_VEL_V_IDX 7
|
||||
#define ST2_Y_ACCEL_S_IDX 8
|
||||
#define ST2_Y_ACCEL_V_IDX 9
|
||||
#define ST2_Y_S_IDX 10
|
||||
#define ST2_Y_V_IDX 11
|
||||
#define ST2_HC_OFF 12
|
||||
|
||||
// Indexes for string number 3
|
||||
#define ST3_P3_IDX 2
|
||||
#define ST3_GAMMA_N_S_IDX 3
|
||||
#define ST3_GAMMA_N_V_IDX 4
|
||||
#define ST3_NU_1_IDX 5
|
||||
#define ST3_P_IDX 6
|
||||
#define ST3_L_N_IDX 7
|
||||
#define ST3_Z_VEL_S_IDX 8
|
||||
#define ST3_Z_VEL_V_IDX 9
|
||||
#define ST3_Z_ACCEL_S_IDX 10
|
||||
#define ST3_Z_ACCEL_V_IDX 11
|
||||
#define ST3_Z_S_IDX 12
|
||||
#define ST3_Z_V_IDX 13
|
||||
#define ST3_HC_OFF 14
|
||||
|
||||
// Indexes for string number 4
|
||||
#define ST4_TAU_N_S_IDX 2
|
||||
#define ST4_TAU_N_V_IDX 3
|
||||
#define ST4_DELTA_TAU_N_S_IDX 4
|
||||
#define ST4_DELTA_TAU_N_V_IDX 5
|
||||
#define ST4_E_N_IDX 6
|
||||
#define ST4_NU_1_IDX 7
|
||||
#define ST4_P4_IDX 8
|
||||
#define ST4_F_T_IDX 9
|
||||
#define ST4_NU_2_IDX 10
|
||||
#define ST4_N_T_IDX 11
|
||||
#define ST4_N_IDX 12
|
||||
#define ST4_M_IDX 13
|
||||
#define ST4_HC_OFF 14
|
||||
|
||||
// Indexes for string number 5
|
||||
#define ST5_N_A_IDX 2
|
||||
#define ST5_TAU_C_IDX 3
|
||||
#define ST5_NU_IDX 4
|
||||
#define ST5_N_4_IDX 5
|
||||
#define ST5_TAU_GPS_IDX 6
|
||||
#define ST5_L_N_IDX 7
|
||||
#define ST5_HC_OFF 8
|
||||
|
||||
// Indexes for non immediate
|
||||
#define ST6_DATA_1_IDX 2
|
||||
#define ST6_DATA_2_IDX 3
|
||||
#define ST6_HC_OFF 4
|
||||
|
||||
|
||||
std::string generate_inp_data(string_data& data) {
|
||||
std::string inp_data = "";
|
||||
for (auto& [b, v] : data) {
|
||||
std::string tmp = std::bitset<64>(v).to_string();
|
||||
inp_data += tmp.substr(64-b, b);
|
||||
}
|
||||
assert(inp_data.size() == 128);
|
||||
|
||||
std::string string_data;
|
||||
string_data.reserve(16);
|
||||
for (int i = 0; i < 128; i+=8) {
|
||||
std::string substr = inp_data.substr(i, 8);
|
||||
string_data.push_back( (uint8_t)std::stoi(substr.c_str(), 0, 2));
|
||||
}
|
||||
|
||||
return string_data;
|
||||
}
|
||||
|
||||
string_data generate_string_data(uint8_t string_number) {
|
||||
|
||||
srand((unsigned)time(0));
|
||||
string_data data; //<bit length, value>
|
||||
data.push_back({1, 0}); // idle chip
|
||||
data.push_back({4, string_number}); // string number
|
||||
|
||||
if (string_number == 1) {
|
||||
data.push_back({2, 3}); // not_used
|
||||
data.push_back({2, 1}); // p1
|
||||
data.push_back({12, 113}); // t_k
|
||||
data.push_back({1, rand() & 1}); // x_vel_sign
|
||||
data.push_back({23, 7122}); // x_vel_value
|
||||
data.push_back({1, rand() & 1}); // x_accel_sign
|
||||
data.push_back({4, 3}); // x_accel_value
|
||||
data.push_back({1, rand() & 1}); // x_sign
|
||||
data.push_back({26, 33554431}); // x_value
|
||||
} else if (string_number == 2) {
|
||||
data.push_back({3, 3}); // b_n
|
||||
data.push_back({1, 1}); // p2
|
||||
data.push_back({7, 123}); // t_b
|
||||
data.push_back({5, 31}); // not_used
|
||||
data.push_back({1, rand() & 1}); // y_vel_sign
|
||||
data.push_back({23, 7422}); // y_vel_value
|
||||
data.push_back({1, rand() & 1}); // y_accel_sign
|
||||
data.push_back({4, 3}); // y_accel_value
|
||||
data.push_back({1, rand() & 1}); // y_sign
|
||||
data.push_back({26, 67108863}); // y_value
|
||||
} else if (string_number == 3) {
|
||||
data.push_back({1, 0}); // p3
|
||||
data.push_back({1, 1}); // gamma_n_sign
|
||||
data.push_back({10, 123}); // gamma_n_value
|
||||
data.push_back({1, 0}); // not_used
|
||||
data.push_back({2, 2}); // p
|
||||
data.push_back({1, 1}); // l_n
|
||||
data.push_back({1, rand() & 1}); // z_vel_sign
|
||||
data.push_back({23, 1337}); // z_vel_value
|
||||
data.push_back({1, rand() & 1}); // z_accel_sign
|
||||
data.push_back({4, 9}); // z_accel_value
|
||||
data.push_back({1, rand() & 1}); // z_sign
|
||||
data.push_back({26, 100023}); // z_value
|
||||
} else if (string_number == 4) {
|
||||
data.push_back({1, rand() & 1}); // tau_n_sign
|
||||
data.push_back({21, 197152}); // tau_n_value
|
||||
data.push_back({1, rand() & 1}); // delta_tau_n_sign
|
||||
data.push_back({4, 4}); // delta_tau_n_value
|
||||
data.push_back({5, 0}); // e_n
|
||||
data.push_back({14, 2}); // not_used_1
|
||||
data.push_back({1, 1}); // p4
|
||||
data.push_back({4, 9}); // f_t
|
||||
data.push_back({3, 3}); // not_used_2
|
||||
data.push_back({11, 2047}); // n_t
|
||||
data.push_back({5, 2}); // n
|
||||
data.push_back({2, 1}); // m
|
||||
} else if (string_number == 5) {
|
||||
data.push_back({11, 2047}); // n_a
|
||||
data.push_back({32, 4294767295}); // tau_c
|
||||
data.push_back({1, 0}); // not_used_1
|
||||
data.push_back({5, 2}); // n_4
|
||||
data.push_back({22, 4114304}); // tau_gps
|
||||
data.push_back({1, 0}); // l_n
|
||||
} else { // non-immediate data is not parsed
|
||||
data.push_back({64, rand()}); // data_1
|
||||
data.push_back({8, 6}); // data_2
|
||||
}
|
||||
|
||||
data.push_back({8, rand() & 0xFF}); // hamming code
|
||||
data.push_back({11, rand() & 0x7FF}); // pad
|
||||
data.push_back({16, rand() & 0xFFFF}); // superframe
|
||||
data.push_back({8, rand() & 0xFF}); // pad
|
||||
data.push_back({8, rand() & 0xFF}); // frame
|
||||
return data;
|
||||
}
|
||||
|
||||
TEST_CASE("parse_string_number_1"){
|
||||
string_data data = generate_string_data(1);
|
||||
std::string inp_data = generate_inp_data(data);
|
||||
|
||||
kaitai::kstream stream(inp_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
|
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
|
||||
REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second);
|
||||
REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second);
|
||||
REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second);
|
||||
REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second);
|
||||
REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second);
|
||||
|
||||
kaitai::kstream str1(inp_data);
|
||||
glonass_t str1_data(&str1);
|
||||
glonass_t::string_1_t* s1 = static_cast<glonass_t::string_1_t*>(str1_data.data());
|
||||
|
||||
REQUIRE(s1->not_used() == data[ST1_NU_IDX].second);
|
||||
REQUIRE(s1->p1() == data[ST1_P1_IDX].second);
|
||||
REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second);
|
||||
|
||||
int mul = s1->x_vel_sign() ? (-1) : 1;
|
||||
REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul));
|
||||
mul = s1->x_accel_sign() ? (-1) : 1;
|
||||
REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul));
|
||||
mul = s1->x_sign() ? (-1) : 1;
|
||||
REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul));
|
||||
}
|
||||
|
||||
TEST_CASE("parse_string_number_2"){
|
||||
string_data data = generate_string_data(2);
|
||||
std::string inp_data = generate_inp_data(data);
|
||||
|
||||
kaitai::kstream stream(inp_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
|
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
|
||||
REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second);
|
||||
REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second);
|
||||
REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second);
|
||||
REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second);
|
||||
REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second);
|
||||
|
||||
kaitai::kstream str2(inp_data);
|
||||
glonass_t str2_data(&str2);
|
||||
glonass_t::string_2_t* s2 = static_cast<glonass_t::string_2_t*>(str2_data.data());
|
||||
|
||||
REQUIRE(s2->b_n() == data[ST2_BN_IDX].second);
|
||||
REQUIRE(s2->not_used() == data[ST2_NU_IDX].second);
|
||||
REQUIRE(s2->p2() == data[ST2_P2_IDX].second);
|
||||
REQUIRE(s2->t_b() == data[ST2_TB_IDX].second);
|
||||
int mul = s2->y_vel_sign() ? (-1) : 1;
|
||||
REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul));
|
||||
mul = s2->y_accel_sign() ? (-1) : 1;
|
||||
REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul));
|
||||
mul = s2->y_sign() ? (-1) : 1;
|
||||
REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul));
|
||||
}
|
||||
|
||||
TEST_CASE("parse_string_number_3"){
|
||||
string_data data = generate_string_data(3);
|
||||
std::string inp_data = generate_inp_data(data);
|
||||
|
||||
kaitai::kstream stream(inp_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
|
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
|
||||
REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second);
|
||||
REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second);
|
||||
REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second);
|
||||
REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second);
|
||||
REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second);
|
||||
|
||||
kaitai::kstream str3(inp_data);
|
||||
glonass_t str3_data(&str3);
|
||||
glonass_t::string_3_t* s3 = static_cast<glonass_t::string_3_t*>(str3_data.data());
|
||||
|
||||
REQUIRE(s3->p3() == data[ST3_P3_IDX].second);
|
||||
int mul = s3->gamma_n_sign() ? (-1) : 1;
|
||||
REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul));
|
||||
REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second);
|
||||
REQUIRE(s3->p() == data[ST3_P_IDX].second);
|
||||
REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second);
|
||||
mul = s3->z_vel_sign() ? (-1) : 1;
|
||||
REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul));
|
||||
mul = s3->z_accel_sign() ? (-1) : 1;
|
||||
REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul));
|
||||
mul = s3->z_sign() ? (-1) : 1;
|
||||
REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul));
|
||||
}
|
||||
|
||||
TEST_CASE("parse_string_number_4"){
|
||||
string_data data = generate_string_data(4);
|
||||
std::string inp_data = generate_inp_data(data);
|
||||
|
||||
kaitai::kstream stream(inp_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
|
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
|
||||
REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second);
|
||||
REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second);
|
||||
REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second);
|
||||
REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second);
|
||||
REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second);
|
||||
|
||||
kaitai::kstream str4(inp_data);
|
||||
glonass_t str4_data(&str4);
|
||||
glonass_t::string_4_t* s4 = static_cast<glonass_t::string_4_t*>(str4_data.data());
|
||||
|
||||
int mul = s4->tau_n_sign() ? (-1) : 1;
|
||||
REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul));
|
||||
mul = s4->delta_tau_n_sign() ? (-1) : 1;
|
||||
REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul));
|
||||
REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second);
|
||||
REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second);
|
||||
REQUIRE(s4->p4() == data[ST4_P4_IDX].second);
|
||||
REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second);
|
||||
REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second);
|
||||
REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second);
|
||||
REQUIRE(s4->n() == data[ST4_N_IDX].second);
|
||||
REQUIRE(s4->m() == data[ST4_M_IDX].second);
|
||||
}
|
||||
|
||||
TEST_CASE("parse_string_number_5"){
|
||||
string_data data = generate_string_data(5);
|
||||
std::string inp_data = generate_inp_data(data);
|
||||
|
||||
kaitai::kstream stream(inp_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
|
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
|
||||
REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second);
|
||||
REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second);
|
||||
REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second);
|
||||
REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second);
|
||||
REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second);
|
||||
|
||||
kaitai::kstream str5(inp_data);
|
||||
glonass_t str5_data(&str5);
|
||||
glonass_t::string_5_t* s5 = static_cast<glonass_t::string_5_t*>(str5_data.data());
|
||||
|
||||
REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second);
|
||||
REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second);
|
||||
REQUIRE(s5->not_used() == data[ST5_NU_IDX].second);
|
||||
REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second);
|
||||
REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second);
|
||||
REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second);
|
||||
}
|
||||
|
||||
TEST_CASE("parse_string_number_NI"){
|
||||
string_data data = generate_string_data((rand() % 10) + 6);
|
||||
std::string inp_data = generate_inp_data(data);
|
||||
|
||||
kaitai::kstream stream(inp_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
|
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
|
||||
REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second);
|
||||
REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second);
|
||||
REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second);
|
||||
REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second);
|
||||
REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second);
|
||||
|
||||
kaitai::kstream strni(inp_data);
|
||||
glonass_t strni_data(&strni);
|
||||
glonass_t::string_non_immediate_t* sni = static_cast<glonass_t::string_non_immediate_t*>(strni_data.data());
|
||||
|
||||
REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second);
|
||||
REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second);
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
#define CATCH_CONFIG_MAIN
|
||||
#include "catch2/catch.hpp"
|
||||
Executable
+115
@@ -0,0 +1,115 @@
|
||||
import unittest
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
from laika import AstroDog
|
||||
from laika.helpers import ConstellationId
|
||||
from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ublox
|
||||
from laika.opt import calc_pos_fix
|
||||
from selfdrive.test.openpilotci import get_url
|
||||
from tools.lib.logreader import LogReader
|
||||
from selfdrive.test.helpers import with_processes
|
||||
import cereal.messaging as messaging
|
||||
|
||||
def get_gnss_measurements(log_reader):
|
||||
gnss_measurements = []
|
||||
for msg in log_reader:
|
||||
if msg.which() == "ubloxGnss":
|
||||
ublox_msg = msg.ubloxGnss
|
||||
if ublox_msg.which == 'measurementReport':
|
||||
report = ublox_msg.measurementReport
|
||||
if len(report.measurements) > 0:
|
||||
gnss_measurements.append(read_raw_ublox(report))
|
||||
return gnss_measurements
|
||||
|
||||
def get_ublox_raw(log_reader):
|
||||
ublox_raw = []
|
||||
for msg in log_reader:
|
||||
if msg.which() == "ubloxRaw":
|
||||
ublox_raw.append(msg)
|
||||
return ublox_raw
|
||||
|
||||
class TestUbloxProcessing(unittest.TestCase):
|
||||
NUM_TEST_PROCESS_MEAS = 10
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0))
|
||||
cls.gnss_measurements = get_gnss_measurements(lr)
|
||||
|
||||
# test gps ephemeris continuity check (drive has ephemeris issues with cutover data)
|
||||
lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14))
|
||||
cls.ublox_raw = get_ublox_raw(lr)
|
||||
|
||||
def test_read_ublox_raw(self):
|
||||
count_gps = 0
|
||||
count_glonass = 0
|
||||
for measurements in self.gnss_measurements:
|
||||
for m in measurements:
|
||||
if m.constellation_id == ConstellationId.GPS:
|
||||
count_gps += 1
|
||||
elif m.constellation_id == ConstellationId.GLONASS:
|
||||
count_glonass += 1
|
||||
|
||||
self.assertEqual(count_gps, 5036)
|
||||
self.assertEqual(count_glonass, 3651)
|
||||
|
||||
def test_get_fix(self):
|
||||
dog = AstroDog()
|
||||
position_fix_found = 0
|
||||
count_processed_measurements = 0
|
||||
count_corrected_measurements = 0
|
||||
position_fix_found_after_correcting = 0
|
||||
|
||||
pos_ests = []
|
||||
for measurements in self.gnss_measurements[:self.NUM_TEST_PROCESS_MEAS]:
|
||||
processed_meas = process_measurements(measurements, dog)
|
||||
count_processed_measurements += len(processed_meas)
|
||||
pos_fix = calc_pos_fix(processed_meas)
|
||||
if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
|
||||
position_fix_found += 1
|
||||
|
||||
corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog)
|
||||
count_corrected_measurements += len(corrected_meas)
|
||||
|
||||
pos_fix = calc_pos_fix(corrected_meas)
|
||||
if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
|
||||
pos_ests.append(pos_fix[0])
|
||||
position_fix_found_after_correcting += 1
|
||||
|
||||
mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0)
|
||||
np.testing.assert_allclose(mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557], rtol=0, atol=1)
|
||||
|
||||
# Note that can happen that there are less corrected measurements compared to processed when they are invalid.
|
||||
# However, not for the current segment
|
||||
self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS)
|
||||
self.assertEqual(position_fix_found_after_correcting, self.NUM_TEST_PROCESS_MEAS)
|
||||
self.assertEqual(count_processed_measurements, 69)
|
||||
self.assertEqual(count_corrected_measurements, 69)
|
||||
|
||||
@with_processes(['ubloxd'])
|
||||
def test_ublox_gps_cutover(self):
|
||||
time.sleep(2)
|
||||
ugs = messaging.sub_sock("ubloxGnss", timeout=0.1)
|
||||
ur_pm = messaging.PubMaster(['ubloxRaw'])
|
||||
|
||||
def replay_segment():
|
||||
rcv_msgs = []
|
||||
for msg in self.ublox_raw:
|
||||
ur_pm.send(msg.which(), msg.as_builder())
|
||||
time.sleep(0.01)
|
||||
rcv_msgs += messaging.drain_sock(ugs)
|
||||
|
||||
time.sleep(0.1)
|
||||
rcv_msgs += messaging.drain_sock(ugs)
|
||||
return rcv_msgs
|
||||
|
||||
# replay twice to enforce cutover data on rewind
|
||||
rcv_msgs = replay_segment()
|
||||
rcv_msgs += replay_segment()
|
||||
|
||||
ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs)
|
||||
self.assertEqual(ephems_cnt, 15)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,938 @@
|
||||
#!/usr/bin/env python3
|
||||
# pylint: skip-file
|
||||
'''
|
||||
UBlox binary protocol handling
|
||||
|
||||
Copyright Andrew Tridgell, October 2012
|
||||
Released under GNU GPL version 3 or later
|
||||
|
||||
WARNING: This code has originally intended for
|
||||
ublox version 7, it has been adapted to work
|
||||
for ublox version 8, not all functions may work.
|
||||
'''
|
||||
|
||||
|
||||
import struct
|
||||
import time
|
||||
|
||||
# protocol constants
|
||||
PREAMBLE1 = 0xb5
|
||||
PREAMBLE2 = 0x62
|
||||
|
||||
# message classes
|
||||
CLASS_NAV = 0x01
|
||||
CLASS_RXM = 0x02
|
||||
CLASS_INF = 0x04
|
||||
CLASS_ACK = 0x05
|
||||
CLASS_CFG = 0x06
|
||||
CLASS_MON = 0x0A
|
||||
CLASS_AID = 0x0B
|
||||
CLASS_TIM = 0x0D
|
||||
CLASS_ESF = 0x10
|
||||
|
||||
# ACK messages
|
||||
MSG_ACK_NACK = 0x00
|
||||
MSG_ACK_ACK = 0x01
|
||||
|
||||
# NAV messages
|
||||
MSG_NAV_POSECEF = 0x1
|
||||
MSG_NAV_POSLLH = 0x2
|
||||
MSG_NAV_STATUS = 0x3
|
||||
MSG_NAV_DOP = 0x4
|
||||
MSG_NAV_SOL = 0x6
|
||||
MSG_NAV_PVT = 0x7
|
||||
MSG_NAV_POSUTM = 0x8
|
||||
MSG_NAV_VELNED = 0x12
|
||||
MSG_NAV_VELECEF = 0x11
|
||||
MSG_NAV_TIMEGPS = 0x20
|
||||
MSG_NAV_TIMEUTC = 0x21
|
||||
MSG_NAV_CLOCK = 0x22
|
||||
MSG_NAV_SVINFO = 0x30
|
||||
MSG_NAV_AOPSTATUS = 0x60
|
||||
MSG_NAV_DGPS = 0x31
|
||||
MSG_NAV_DOP = 0x04
|
||||
MSG_NAV_EKFSTATUS = 0x40
|
||||
MSG_NAV_SBAS = 0x32
|
||||
MSG_NAV_SOL = 0x06
|
||||
MSG_NAV_SAT = 0x35
|
||||
|
||||
# RXM messages
|
||||
MSG_RXM_RAW = 0x15
|
||||
MSG_RXM_SFRB = 0x11
|
||||
MSG_RXM_SFRBX = 0x13
|
||||
MSG_RXM_SVSI = 0x20
|
||||
MSG_RXM_EPH = 0x31
|
||||
MSG_RXM_ALM = 0x30
|
||||
MSG_RXM_PMREQ = 0x41
|
||||
|
||||
# AID messages
|
||||
MSG_AID_ALM = 0x30
|
||||
MSG_AID_EPH = 0x31
|
||||
MSG_AID_ALPSRV = 0x32
|
||||
MSG_AID_AOP = 0x33
|
||||
MSG_AID_DATA = 0x10
|
||||
MSG_AID_ALP = 0x50
|
||||
MSG_AID_DATA = 0x10
|
||||
MSG_AID_HUI = 0x02
|
||||
MSG_AID_INI = 0x01
|
||||
MSG_AID_REQ = 0x00
|
||||
|
||||
# CFG messages
|
||||
MSG_CFG_PRT = 0x00
|
||||
MSG_CFG_ANT = 0x13
|
||||
MSG_CFG_DAT = 0x06
|
||||
MSG_CFG_EKF = 0x12
|
||||
MSG_CFG_ESFGWT = 0x29
|
||||
MSG_CFG_CFG = 0x09
|
||||
MSG_CFG_USB = 0x1b
|
||||
MSG_CFG_RATE = 0x08
|
||||
MSG_CFG_SET_RATE = 0x01
|
||||
MSG_CFG_NAV5 = 0x24
|
||||
MSG_CFG_FXN = 0x0E
|
||||
MSG_CFG_INF = 0x02
|
||||
MSG_CFG_ITFM = 0x39
|
||||
MSG_CFG_MSG = 0x01
|
||||
MSG_CFG_NAVX5 = 0x23
|
||||
MSG_CFG_NMEA = 0x17
|
||||
MSG_CFG_NVS = 0x22
|
||||
MSG_CFG_PM2 = 0x3B
|
||||
MSG_CFG_PM = 0x32
|
||||
MSG_CFG_ITMF = 0x39
|
||||
MSG_CFG_RINV = 0x34
|
||||
MSG_CFG_RST = 0x04
|
||||
MSG_CFG_RXM = 0x11
|
||||
MSG_CFG_SBAS = 0x16
|
||||
MSG_CFG_TMODE2 = 0x3D
|
||||
MSG_CFG_TMODE = 0x1D
|
||||
MSG_CFG_TPS = 0x31
|
||||
MSG_CFG_TP = 0x07
|
||||
MSG_CFG_GNSS = 0x3E
|
||||
MSG_CFG_ODO = 0x1E
|
||||
|
||||
# ESF messages
|
||||
MSG_ESF_MEAS = 0x02
|
||||
MSG_ESF_STATUS = 0x10
|
||||
|
||||
# INF messages
|
||||
MSG_INF_DEBUG = 0x04
|
||||
MSG_INF_ERROR = 0x00
|
||||
MSG_INF_NOTICE = 0x02
|
||||
MSG_INF_TEST = 0x03
|
||||
MSG_INF_WARNING = 0x01
|
||||
|
||||
# MON messages
|
||||
MSG_MON_SCHD = 0x01
|
||||
MSG_MON_HW = 0x09
|
||||
MSG_MON_HW2 = 0x0B
|
||||
MSG_MON_IO = 0x02
|
||||
MSG_MON_MSGPP = 0x06
|
||||
MSG_MON_RXBUF = 0x07
|
||||
MSG_MON_RXR = 0x21
|
||||
MSG_MON_TXBUF = 0x08
|
||||
MSG_MON_VER = 0x04
|
||||
|
||||
# TIM messages
|
||||
MSG_TIM_TP = 0x01
|
||||
MSG_TIM_TM2 = 0x03
|
||||
MSG_TIM_SVIN = 0x04
|
||||
MSG_TIM_VRFY = 0x06
|
||||
|
||||
# port IDs
|
||||
PORT_DDC = 0
|
||||
PORT_SERIAL1 = 1
|
||||
PORT_SERIAL2 = 2
|
||||
PORT_USB = 3
|
||||
PORT_SPI = 4
|
||||
|
||||
# dynamic models
|
||||
DYNAMIC_MODEL_PORTABLE = 0
|
||||
DYNAMIC_MODEL_STATIONARY = 2
|
||||
DYNAMIC_MODEL_PEDESTRIAN = 3
|
||||
DYNAMIC_MODEL_AUTOMOTIVE = 4
|
||||
DYNAMIC_MODEL_SEA = 5
|
||||
DYNAMIC_MODEL_AIRBORNE1G = 6
|
||||
DYNAMIC_MODEL_AIRBORNE2G = 7
|
||||
DYNAMIC_MODEL_AIRBORNE4G = 8
|
||||
|
||||
#reset items
|
||||
RESET_HOT = 0
|
||||
RESET_WARM = 1
|
||||
RESET_COLD = 0xFFFF
|
||||
|
||||
RESET_HW = 0
|
||||
RESET_SW = 1
|
||||
RESET_SW_GPS = 2
|
||||
RESET_HW_GRACEFUL = 4
|
||||
RESET_GPS_STOP = 8
|
||||
RESET_GPS_START = 9
|
||||
|
||||
|
||||
class UBloxError(Exception):
|
||||
'''Ublox error class'''
|
||||
|
||||
def __init__(self, msg):
|
||||
Exception.__init__(self, msg)
|
||||
self.message = msg
|
||||
|
||||
|
||||
class UBloxAttrDict(dict):
|
||||
'''allow dictionary members as attributes'''
|
||||
|
||||
def __init__(self):
|
||||
dict.__init__(self)
|
||||
|
||||
def __getattr__(self, name):
|
||||
try:
|
||||
return self.__getitem__(name)
|
||||
except KeyError:
|
||||
raise AttributeError(name)
|
||||
|
||||
def __setattr__(self, name, value):
|
||||
if name in self.__dict__:
|
||||
# allow set on normal attributes
|
||||
dict.__setattr__(self, name, value)
|
||||
else:
|
||||
self.__setitem__(name, value)
|
||||
|
||||
|
||||
def ArrayParse(field):
|
||||
'''parse an array descriptor'''
|
||||
arridx = field.find('[')
|
||||
if arridx == -1:
|
||||
return (field, -1)
|
||||
alen = int(field[arridx + 1:-1])
|
||||
fieldname = field[:arridx]
|
||||
return (fieldname, alen)
|
||||
|
||||
|
||||
class UBloxDescriptor:
|
||||
'''class used to describe the layout of a UBlox message'''
|
||||
|
||||
def __init__(self,
|
||||
name,
|
||||
msg_format,
|
||||
fields=None,
|
||||
count_field=None,
|
||||
format2=None,
|
||||
fields2=None):
|
||||
if fields is None:
|
||||
fields = []
|
||||
|
||||
self.name = name
|
||||
self.msg_format = msg_format
|
||||
self.fields = fields
|
||||
self.count_field = count_field
|
||||
self.format2 = format2
|
||||
self.fields2 = fields2
|
||||
|
||||
def unpack(self, msg):
|
||||
'''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg'''
|
||||
msg._fields = {}
|
||||
|
||||
# unpack main message blocks. A comm
|
||||
formats = self.msg_format.split(',')
|
||||
buf = msg._buf[6:-2]
|
||||
count = 0
|
||||
msg._recs = []
|
||||
fields = self.fields[:]
|
||||
|
||||
for fmt in formats:
|
||||
size1 = struct.calcsize(fmt)
|
||||
if size1 > len(buf):
|
||||
raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf)))
|
||||
f1 = list(struct.unpack(fmt, buf[:size1]))
|
||||
i = 0
|
||||
while i < len(f1):
|
||||
field = fields.pop(0)
|
||||
(fieldname, alen) = ArrayParse(field)
|
||||
if alen == -1:
|
||||
msg._fields[fieldname] = f1[i]
|
||||
if self.count_field == fieldname:
|
||||
count = int(f1[i])
|
||||
i += 1
|
||||
else:
|
||||
msg._fields[fieldname] = [0] * alen
|
||||
for a in range(alen):
|
||||
msg._fields[fieldname][a] = f1[i]
|
||||
i += 1
|
||||
buf = buf[size1:]
|
||||
if len(buf) == 0:
|
||||
break
|
||||
|
||||
if self.count_field == '_remaining':
|
||||
count = len(buf) // struct.calcsize(self.format2)
|
||||
|
||||
if count == 0:
|
||||
msg._unpacked = True
|
||||
if len(buf) != 0:
|
||||
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
|
||||
return
|
||||
|
||||
size2 = struct.calcsize(self.format2)
|
||||
for c in range(count):
|
||||
r = UBloxAttrDict()
|
||||
if size2 > len(buf):
|
||||
raise UBloxError("INVALID_SIZE=%u, " % len(buf))
|
||||
f2 = list(struct.unpack(self.format2, buf[:size2]))
|
||||
for i in range(len(self.fields2)):
|
||||
r[self.fields2[i]] = f2[i]
|
||||
buf = buf[size2:]
|
||||
msg._recs.append(r)
|
||||
if len(buf) != 0:
|
||||
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
|
||||
msg._unpacked = True
|
||||
|
||||
def pack(self, msg, msg_class=None, msg_id=None):
|
||||
'''pack a UBloxMessage from the .fields and ._recs attributes in msg'''
|
||||
f1 = []
|
||||
if msg_class is None:
|
||||
msg_class = msg.msg_class()
|
||||
if msg_id is None:
|
||||
msg_id = msg.msg_id()
|
||||
msg._buf = ''
|
||||
|
||||
fields = self.fields[:]
|
||||
for f in fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if fieldname not in msg._fields:
|
||||
break
|
||||
if alen == -1:
|
||||
f1.append(msg._fields[fieldname])
|
||||
else:
|
||||
for a in range(alen):
|
||||
f1.append(msg._fields[fieldname][a])
|
||||
try:
|
||||
# try full length message
|
||||
fmt = self.msg_format.replace(',', '')
|
||||
msg._buf = struct.pack(fmt, *tuple(f1))
|
||||
except Exception:
|
||||
# try without optional part
|
||||
fmt = self.msg_format.split(',')[0]
|
||||
msg._buf = struct.pack(fmt, *tuple(f1))
|
||||
|
||||
length = len(msg._buf)
|
||||
if msg._recs:
|
||||
length += len(msg._recs) * struct.calcsize(self.format2)
|
||||
header = struct.pack('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
|
||||
msg._buf = header + msg._buf
|
||||
|
||||
for r in msg._recs:
|
||||
f2 = []
|
||||
for f in self.fields2:
|
||||
f2.append(r[f])
|
||||
msg._buf += struct.pack(self.format2, *tuple(f2))
|
||||
msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))
|
||||
|
||||
def format(self, msg):
|
||||
'''return a formatted string for a message'''
|
||||
if not msg._unpacked:
|
||||
self.unpack(msg)
|
||||
ret = self.name + ': '
|
||||
for f in self.fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if fieldname not in msg._fields:
|
||||
continue
|
||||
v = msg._fields[fieldname]
|
||||
if isinstance(v, list):
|
||||
ret += '%s=[' % fieldname
|
||||
for a in range(alen):
|
||||
ret += '%s, ' % v[a]
|
||||
ret = ret[:-2] + '], '
|
||||
elif isinstance(v, str):
|
||||
ret += '{}="{}", '.format(f, v.rstrip(' \0'))
|
||||
else:
|
||||
ret += '%s=%s, ' % (f, v)
|
||||
for r in msg._recs:
|
||||
ret += '[ '
|
||||
for f in self.fields2:
|
||||
v = r[f]
|
||||
ret += '%s=%s, ' % (f, v)
|
||||
ret = ret[:-2] + ' ], '
|
||||
return ret[:-2]
|
||||
|
||||
|
||||
# list of supported message types.
|
||||
msg_types = {
|
||||
(CLASS_ACK, MSG_ACK_ACK):
|
||||
UBloxDescriptor('ACK_ACK', '<BB', ['clsID', 'msgID']),
|
||||
(CLASS_ACK, MSG_ACK_NACK):
|
||||
UBloxDescriptor('ACK_NACK', '<BB', ['clsID', 'msgID']),
|
||||
(CLASS_CFG, MSG_CFG_USB):
|
||||
UBloxDescriptor('CFG_USB', '<HHHHHH32s32s32s', [
|
||||
'vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption', 'flags',
|
||||
'vendorString', 'productString', 'serialNumber'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_PRT):
|
||||
UBloxDescriptor('CFG_PRT', '<BBHIIHHHH', [
|
||||
'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
|
||||
'reserved4', 'reserved5'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_ITMF):
|
||||
UBloxDescriptor('CFG_ITMF', '<II', [
|
||||
'config', 'config2'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_CFG):
|
||||
UBloxDescriptor('CFG_CFG', '<III,B',
|
||||
['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
|
||||
(CLASS_CFG, MSG_CFG_RXM):
|
||||
UBloxDescriptor('CFG_RXM', '<BB',
|
||||
['reserved1', 'lpMode']),
|
||||
(CLASS_CFG, MSG_CFG_RST):
|
||||
UBloxDescriptor('CFG_RST', '<HBB', ['navBbrMask ', 'resetMode', 'reserved1']),
|
||||
(CLASS_CFG, MSG_CFG_SBAS):
|
||||
UBloxDescriptor('CFG_SBAS', '<BBBBI',
|
||||
['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
|
||||
(CLASS_CFG, MSG_CFG_GNSS):
|
||||
UBloxDescriptor('CFG_GNSS', '<BBBB',
|
||||
['msgVer', 'numTrkChHw', 'numTrkChUse',
|
||||
'numConfigBlocks'], 'numConfigBlocks', '<BBBBI',
|
||||
['gnssId', 'resTrkCh', 'maxTrkCh', 'reserved1', 'flags']),
|
||||
(CLASS_CFG, MSG_CFG_RATE):
|
||||
UBloxDescriptor('CFG_RATE', '<HHH', ['measRate', 'navRate', 'timeRef']),
|
||||
(CLASS_CFG, MSG_CFG_MSG):
|
||||
UBloxDescriptor('CFG_MSG', '<BB6B', ['msgClass', 'msgId', 'rates[6]']),
|
||||
(CLASS_NAV, MSG_NAV_POSLLH):
|
||||
UBloxDescriptor('NAV_POSLLH', '<IiiiiII',
|
||||
['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
|
||||
(CLASS_NAV, MSG_NAV_VELNED):
|
||||
UBloxDescriptor('NAV_VELNED', '<IiiiIIiII', [
|
||||
'iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 'sAcc', 'cAcc'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_DOP):
|
||||
UBloxDescriptor('NAV_DOP', '<IHHHHHHH',
|
||||
['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
|
||||
(CLASS_NAV, MSG_NAV_STATUS):
|
||||
UBloxDescriptor('NAV_STATUS', '<IBBBBII',
|
||||
['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
|
||||
(CLASS_NAV, MSG_NAV_SOL):
|
||||
UBloxDescriptor('NAV_SOL', '<IihBBiiiIiiiIHBBI', [
|
||||
'iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ', 'pAcc',
|
||||
'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 'numSV', 'reserved2'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_PVT):
|
||||
UBloxDescriptor('NAV_PVT', '<IHBBBBBBIiBBBBiiiiIIiiiiiIIH6BihH', [
|
||||
'iTOW', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid', 'tAcc', 'nano',
|
||||
'fixType', 'flags', 'flags2', 'numSV', 'lon', 'lat', 'height', 'hMSL', 'hAcc', 'vAcc',
|
||||
'velN', 'velE', 'velD', 'gSpeed', 'headMot', 'sAcc', 'headAcc', 'pDOP',
|
||||
'reserverd1[6]', 'headVeh', 'magDec', 'magAcc'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_POSUTM):
|
||||
UBloxDescriptor('NAV_POSUTM', '<Iiiibb',
|
||||
['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
|
||||
(CLASS_NAV, MSG_NAV_SBAS):
|
||||
UBloxDescriptor('NAV_SBAS', '<IBBbBBBBB', [
|
||||
'iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02',
|
||||
'reserved03'
|
||||
], 'cnt', 'BBBBBBhHh', [
|
||||
'svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1', 'prc', 'reserved2', 'ic'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_POSECEF):
|
||||
UBloxDescriptor('NAV_POSECEF', '<IiiiI', ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
|
||||
(CLASS_NAV, MSG_NAV_VELECEF):
|
||||
UBloxDescriptor('NAV_VELECEF', '<IiiiI', ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ',
|
||||
'sAcc']),
|
||||
(CLASS_NAV, MSG_NAV_TIMEGPS):
|
||||
UBloxDescriptor('NAV_TIMEGPS', '<IihbBI',
|
||||
['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
|
||||
(CLASS_NAV, MSG_NAV_TIMEUTC):
|
||||
UBloxDescriptor('NAV_TIMEUTC', '<IIiHBBBBBB', [
|
||||
'iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_CLOCK):
|
||||
UBloxDescriptor('NAV_CLOCK', '<IiiII', ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
|
||||
(CLASS_NAV, MSG_NAV_DGPS):
|
||||
UBloxDescriptor('NAV_DGPS', '<IihhBBH',
|
||||
['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
|
||||
'numCh', '<BBHff', ['svid', 'flags', 'ageC', 'prc', 'prrc']),
|
||||
(CLASS_NAV, MSG_NAV_SVINFO):
|
||||
UBloxDescriptor('NAV_SVINFO', '<IBBH', ['iTOW', 'numCh', 'globalFlags',
|
||||
'reserved2'], 'numCh', '<BBBBBbhi',
|
||||
['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
|
||||
(CLASS_RXM, MSG_RXM_SVSI):
|
||||
UBloxDescriptor('RXM_SVSI', '<IhBB', ['iTOW', 'week', 'numVis', 'numSV'], 'numSV',
|
||||
'<BBhbB', ['svid', 'svFlag', 'azim', 'elev', 'age']),
|
||||
(CLASS_RXM, MSG_RXM_EPH):
|
||||
UBloxDescriptor('RXM_EPH', '<II , 8I 8I 8I',
|
||||
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
|
||||
(CLASS_AID, MSG_AID_EPH):
|
||||
UBloxDescriptor('AID_EPH', '<II , 8I 8I 8I',
|
||||
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
|
||||
(CLASS_AID, MSG_AID_HUI):
|
||||
UBloxDescriptor('AID_HUI', '<Iddi 6h 8f I',
|
||||
['health', 'utcA0', 'utcA1', 'utcTOW', 'utcWNT', 'utcLS', 'utcWNF',
|
||||
'utcDN', 'utcLSF', 'utcSpare', 'klobA0', 'klobA1', 'klobA2', 'klobA3',
|
||||
'klobB0', 'klobB1', 'klobB2', 'klobB3', 'flags']),
|
||||
(CLASS_AID, MSG_AID_AOP):
|
||||
UBloxDescriptor('AID_AOP', '<B47B , 48B 48B 48B',
|
||||
['svid', 'data[47]', 'optional0[48]', 'optional1[48]',
|
||||
'optional1[48]']),
|
||||
(CLASS_RXM, MSG_RXM_RAW):
|
||||
UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
|
||||
'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
|
||||
], 'numMeas', '<ddfBBBBHBBBBBB', [
|
||||
'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'sigId', 'freqId', 'locktime', 'cno',
|
||||
'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
|
||||
]),
|
||||
(CLASS_RXM, MSG_RXM_SFRB):
|
||||
UBloxDescriptor('RXM_SFRB', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
|
||||
(CLASS_RXM, MSG_RXM_SFRBX):
|
||||
UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
|
||||
'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
|
||||
(CLASS_AID, MSG_AID_ALM):
|
||||
UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
|
||||
(CLASS_RXM, MSG_RXM_ALM):
|
||||
UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
|
||||
(CLASS_CFG, MSG_CFG_ANT):
|
||||
UBloxDescriptor('CFG_ANT', '<HH', ['flags', 'pins']),
|
||||
(CLASS_CFG, MSG_CFG_ODO):
|
||||
UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
|
||||
'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
|
||||
'cogMaxPosAcc', 'reserved3[2]', 'velLpGain', 'cogLpGain', 'reserved[2]'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_NAV5):
|
||||
UBloxDescriptor('CFG_NAV5', '<HBBiIbBHHHHBBIII', [
|
||||
'mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 'drLimit',
|
||||
'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 'dgpsTimeOut', 'reserved2',
|
||||
'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_NAVX5):
|
||||
UBloxDescriptor('CFG_NAVX5', '<HHIBBBBBBBBBBHIBBBBBBHII', [
|
||||
'version', 'mask1', 'reserved0', 'reserved1', 'reserved2', 'minSVs', 'maxSVs',
|
||||
'minCNO', 'reserved5', 'iniFix3D', 'reserved6', 'reserved7', 'reserved8',
|
||||
'wknRollover', 'reserved9', 'reserved10', 'reserved11', 'usePPP', 'useAOP',
|
||||
'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW):
|
||||
UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB17BHIII', [
|
||||
'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
|
||||
'flags', 'reserved1', 'usedMask', 'VP[17]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
|
||||
'pullL'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW2):
|
||||
UBloxDescriptor('MON_HW2', '<bBbBB3BI8BI4B', [
|
||||
'ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]', 'lowLevCfg',
|
||||
'reserved2[8]', 'postStatus', 'reserved3[4]'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_SCHD):
|
||||
UBloxDescriptor('MON_SCHD', '<IIIIHHHBB', [
|
||||
'tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack', 'stackSize', 'CPUIdle', 'flySly',
|
||||
'ptlSly'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_VER):
|
||||
UBloxDescriptor('MON_VER', '<30s10s,30s', ['swVersion', 'hwVersion', 'romVersion'],
|
||||
'_remaining', '30s', ['extension']),
|
||||
(CLASS_TIM, MSG_TIM_TP):
|
||||
UBloxDescriptor('TIM_TP', '<IIiHBB',
|
||||
['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
|
||||
(CLASS_TIM, MSG_TIM_TM2):
|
||||
UBloxDescriptor('TIM_TM2', '<BBHHHIIIII', [
|
||||
'ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 'towMsF', 'towSubMsF',
|
||||
'accEst'
|
||||
]),
|
||||
(CLASS_TIM, MSG_TIM_SVIN):
|
||||
UBloxDescriptor('TIM_SVIN', '<IiiiIIBBH', [
|
||||
'dur', 'meanX', 'meanY', 'meanZ', 'meanV', 'obs', 'valid', 'active', 'reserved1'
|
||||
]),
|
||||
(CLASS_INF, MSG_INF_ERROR):
|
||||
UBloxDescriptor('INF_ERR', '<18s', ['str']),
|
||||
(CLASS_INF, MSG_INF_DEBUG):
|
||||
UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
|
||||
}
|
||||
|
||||
|
||||
class UBloxMessage:
|
||||
'''UBlox message class - holds a UBX binary message'''
|
||||
|
||||
def __init__(self):
|
||||
self._buf = b""
|
||||
self._fields = {}
|
||||
self._recs = []
|
||||
self._unpacked = False
|
||||
self.debug_level = 1
|
||||
|
||||
def __str__(self):
|
||||
'''format a message as a string'''
|
||||
if not self.valid():
|
||||
return 'UBloxMessage(INVALID)'
|
||||
type = self.msg_type()
|
||||
if type in msg_types:
|
||||
return msg_types[type].format(self)
|
||||
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
|
||||
|
||||
def as_dict(self):
|
||||
'''format a message as a string'''
|
||||
if not self.valid():
|
||||
return 'UBloxMessage(INVALID)'
|
||||
type = self.msg_type()
|
||||
if type in msg_types:
|
||||
return msg_types[type].format(self)
|
||||
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
|
||||
|
||||
def __getattr__(self, name):
|
||||
'''allow access to message fields'''
|
||||
try:
|
||||
return self._fields[name]
|
||||
except KeyError:
|
||||
if name == 'recs':
|
||||
return self._recs
|
||||
raise AttributeError(name)
|
||||
|
||||
def __setattr__(self, name, value):
|
||||
'''allow access to message fields'''
|
||||
if name.startswith('_'):
|
||||
self.__dict__[name] = value
|
||||
else:
|
||||
self._fields[name] = value
|
||||
|
||||
def have_field(self, name):
|
||||
'''return True if a message contains the given field'''
|
||||
return name in self._fields
|
||||
|
||||
def debug(self, level, msg):
|
||||
'''write a debug message'''
|
||||
if self.debug_level >= level:
|
||||
print(msg)
|
||||
|
||||
def unpack(self):
|
||||
'''unpack a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
msg_types[type].unpack(self)
|
||||
return self._fields, self._recs
|
||||
|
||||
def pack(self):
|
||||
'''pack a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s' % str(type))
|
||||
msg_types[type].pack(self)
|
||||
|
||||
def name(self):
|
||||
'''return the short string name for a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
return msg_types[type].name
|
||||
|
||||
def msg_class(self):
|
||||
'''return the message class'''
|
||||
return self._buf[2]
|
||||
|
||||
def msg_id(self):
|
||||
'''return the message id within the class'''
|
||||
return self._buf[3]
|
||||
|
||||
def msg_type(self):
|
||||
'''return the message type tuple (class, id)'''
|
||||
return (self.msg_class(), self.msg_id())
|
||||
|
||||
def msg_length(self):
|
||||
'''return the payload length'''
|
||||
(payload_length, ) = struct.unpack('<H', self._buf[4:6])
|
||||
return payload_length
|
||||
|
||||
def valid_so_far(self):
|
||||
'''check if the message is valid so far'''
|
||||
if len(self._buf) > 0 and self._buf[0] != PREAMBLE1:
|
||||
return False
|
||||
if len(self._buf) > 1 and self._buf[1] != PREAMBLE2:
|
||||
self.debug(1, "bad pre2")
|
||||
return False
|
||||
if self.needed_bytes() == 0 and not self.valid():
|
||||
if len(self._buf) > 8:
|
||||
self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf),
|
||||
self.needed_bytes()))
|
||||
else:
|
||||
self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes()))
|
||||
return False
|
||||
return True
|
||||
|
||||
def add(self, bytes):
|
||||
'''add some bytes to a message'''
|
||||
self._buf += bytes
|
||||
while not self.valid_so_far() and len(self._buf) > 0:
|
||||
'''handle corrupted streams'''
|
||||
self._buf = self._buf[1:]
|
||||
if self.needed_bytes() < 0:
|
||||
self._buf = ""
|
||||
|
||||
def checksum(self, data=None):
|
||||
'''return a checksum tuple for a message'''
|
||||
if data is None:
|
||||
data = self._buf[2:-2]
|
||||
#cs = 0
|
||||
ck_a = 0
|
||||
ck_b = 0
|
||||
for i in data:
|
||||
ck_a = (ck_a + i) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
return (ck_a, ck_b)
|
||||
|
||||
def valid_checksum(self):
|
||||
'''check if the checksum is OK'''
|
||||
(ck_a, ck_b) = self.checksum()
|
||||
#d = self._buf[2:-2]
|
||||
(ck_a2, ck_b2) = struct.unpack('<BB', self._buf[-2:])
|
||||
return ck_a == ck_a2 and ck_b == ck_b2
|
||||
|
||||
def needed_bytes(self):
|
||||
'''return number of bytes still needed'''
|
||||
if len(self._buf) < 6:
|
||||
return 8 - len(self._buf)
|
||||
return self.msg_length() + 8 - len(self._buf)
|
||||
|
||||
def valid(self):
|
||||
'''check if a message is valid'''
|
||||
return len(self._buf) >= 8 and self.needed_bytes() == 0 and self.valid_checksum()
|
||||
|
||||
|
||||
class UBlox:
|
||||
def __init__(self, dev, baudrate):
|
||||
self.dev = dev
|
||||
self.baudrate = baudrate
|
||||
|
||||
self.use_sendrecv = False
|
||||
self.read_only = False
|
||||
self.debug_level = 0
|
||||
|
||||
self.logfile = None
|
||||
self.log = None
|
||||
self.preferred_dynamic_model = None
|
||||
self.preferred_usePPP = None
|
||||
self.preferred_dgps_timeout = None
|
||||
|
||||
def close(self):
|
||||
'''close the device'''
|
||||
self.dev.close()
|
||||
self.dev = None
|
||||
|
||||
def set_debug(self, debug_level):
|
||||
'''set debug level'''
|
||||
self.debug_level = debug_level
|
||||
|
||||
def debug(self, level, msg):
|
||||
'''write a debug message'''
|
||||
if self.debug_level >= level:
|
||||
print(msg)
|
||||
|
||||
def set_logfile(self, logfile, append=False):
|
||||
'''setup logging to a file'''
|
||||
if self.log is not None:
|
||||
self.log.close()
|
||||
self.log = None
|
||||
self.logfile = logfile
|
||||
if self.logfile is not None:
|
||||
if append:
|
||||
mode = 'ab'
|
||||
else:
|
||||
mode = 'wb'
|
||||
self.log = open(self.logfile, mode=mode)
|
||||
|
||||
def set_preferred_dynamic_model(self, model):
|
||||
'''set the preferred dynamic model for receiver'''
|
||||
self.preferred_dynamic_model = model
|
||||
if model is not None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
|
||||
def set_preferred_dgps_timeout(self, timeout):
|
||||
'''set the preferred DGPS timeout for receiver'''
|
||||
self.preferred_dgps_timeout = timeout
|
||||
if timeout is not None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
|
||||
def set_preferred_usePPP(self, usePPP):
|
||||
'''set the preferred usePPP setting for the receiver'''
|
||||
if usePPP is None:
|
||||
self.preferred_usePPP = None
|
||||
return
|
||||
self.preferred_usePPP = int(usePPP)
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
|
||||
|
||||
def nmea_checksum(self, msg):
|
||||
d = msg[1:]
|
||||
cs = 0
|
||||
for i in d:
|
||||
cs ^= ord(i)
|
||||
return cs
|
||||
|
||||
def write(self, buf):
|
||||
'''write some bytes'''
|
||||
if not self.read_only:
|
||||
if self.use_sendrecv:
|
||||
return self.dev.send(buf)
|
||||
if type(buf) == str:
|
||||
return self.dev.write(str.encode(buf))
|
||||
else:
|
||||
return self.dev.write(buf)
|
||||
|
||||
def read(self, n):
|
||||
'''read some bytes'''
|
||||
if self.use_sendrecv:
|
||||
try:
|
||||
return self.dev.recv(n)
|
||||
except OSError:
|
||||
return ''
|
||||
return self.dev.read(n)
|
||||
|
||||
def send_nmea(self, msg):
|
||||
if not self.read_only:
|
||||
s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n"
|
||||
self.write(s)
|
||||
|
||||
def set_binary(self):
|
||||
'''put a UBlox into binary mode using a NMEA string'''
|
||||
if not self.read_only:
|
||||
print("try set binary at %u" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate)
|
||||
|
||||
def disable_nmea(self):
|
||||
''' stop sending all types of nmea messages '''
|
||||
self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0")
|
||||
self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0")
|
||||
|
||||
def seek_percent(self, pct):
|
||||
'''seek to the given percentage of a file'''
|
||||
self.dev.seek(0, 2)
|
||||
filesize = self.dev.tell()
|
||||
self.dev.seek(pct * 0.01 * filesize)
|
||||
|
||||
def special_handling(self, msg):
|
||||
'''handle automatic configuration changes'''
|
||||
if msg.name() == 'CFG_NAV5':
|
||||
msg.unpack()
|
||||
sendit = False
|
||||
pollit = False
|
||||
if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model:
|
||||
msg.dynModel = self.preferred_dynamic_model
|
||||
sendit = True
|
||||
pollit = True
|
||||
if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout:
|
||||
msg.dgpsTimeOut = self.preferred_dgps_timeout
|
||||
self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut)
|
||||
sendit = True
|
||||
# we don't re-poll for this one, as some receivers refuse to set it
|
||||
if sendit:
|
||||
msg.pack()
|
||||
self.send(msg)
|
||||
if pollit:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None:
|
||||
msg.unpack()
|
||||
if msg.usePPP != self.preferred_usePPP:
|
||||
msg.usePPP = self.preferred_usePPP
|
||||
msg.mask = 1 << 13
|
||||
msg.pack()
|
||||
self.send(msg)
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
|
||||
|
||||
def receive_message(self, ignore_eof=False):
|
||||
'''blocking receive of one ublox message'''
|
||||
msg = UBloxMessage()
|
||||
while True:
|
||||
n = msg.needed_bytes()
|
||||
b = self.read(n)
|
||||
if not b:
|
||||
if ignore_eof:
|
||||
time.sleep(0.01)
|
||||
continue
|
||||
if len(msg._buf) > 0:
|
||||
self.debug(1, "dropping %d bytes" % len(msg._buf))
|
||||
return None
|
||||
msg.add(b)
|
||||
if self.log is not None:
|
||||
self.log.write(b)
|
||||
self.log.flush()
|
||||
if msg.valid():
|
||||
self.special_handling(msg)
|
||||
return msg
|
||||
|
||||
def receive_message_noerror(self, ignore_eof=False):
|
||||
'''blocking receive of one ublox message, ignoring errors'''
|
||||
try:
|
||||
return self.receive_message(ignore_eof=ignore_eof)
|
||||
except UBloxError as e:
|
||||
print(e)
|
||||
return None
|
||||
except OSError as e:
|
||||
# Occasionally we get hit with 'resource temporarily unavailable'
|
||||
# messages here on the serial device, catch them too.
|
||||
print(e)
|
||||
return None
|
||||
|
||||
def send(self, msg):
|
||||
'''send a preformatted ublox message'''
|
||||
if not msg.valid():
|
||||
self.debug(1, "invalid send")
|
||||
return
|
||||
if not self.read_only:
|
||||
self.write(msg._buf)
|
||||
|
||||
def send_message(self, msg_class, msg_id, payload):
|
||||
'''send a ublox message with class, id and payload'''
|
||||
msg = UBloxMessage()
|
||||
msg._buf = struct.pack('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))
|
||||
msg._buf += payload
|
||||
(ck_a, ck_b) = msg.checksum(msg._buf[2:])
|
||||
msg._buf += struct.pack('<BB', ck_a, ck_b)
|
||||
self.send(msg)
|
||||
|
||||
def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
|
||||
'''configure the solution rate in milliseconds'''
|
||||
payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)
|
||||
|
||||
def configure_message_rate(self, msg_class, msg_id, rate):
|
||||
'''configure the message rate for a given message'''
|
||||
payload = struct.pack('<BBB', msg_class, msg_id, rate)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)
|
||||
|
||||
def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
|
||||
'''configure a IO port'''
|
||||
if baudrate is None:
|
||||
baudrate = self.baudrate
|
||||
payload = struct.pack('<BBH8BHHBBBB', port, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, inMask,
|
||||
outMask, 0, 0, 0, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)
|
||||
|
||||
def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
|
||||
'''configure configuration load/save'''
|
||||
payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
|
||||
|
||||
def configure_poll(self, msg_class, msg_id, payload=b''):
|
||||
'''poll a configuration message'''
|
||||
self.send_message(msg_class, msg_id, payload)
|
||||
|
||||
def configure_poll_port(self, portID=None):
|
||||
'''poll a port configuration'''
|
||||
if portID is None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
|
||||
else:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))
|
||||
|
||||
def configure_min_max_sats(self, min_sats=4, max_sats=32):
|
||||
'''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
|
||||
payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)
|
||||
|
||||
def module_reset(self, set, mode):
|
||||
''' Reset the module for hot/warm/cold start'''
|
||||
payload = struct.pack('<HBB', set, mode, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_RST, payload)
|
||||
Executable
+90
@@ -0,0 +1,90 @@
|
||||
#!/usr/bin/env python3
|
||||
# type: ignore
|
||||
# pylint: skip-file
|
||||
|
||||
from selfdrive.locationd.test import ublox
|
||||
import struct
|
||||
|
||||
baudrate = 460800
|
||||
rate = 100 # send new data every 100ms
|
||||
|
||||
|
||||
def configure_ublox(dev):
|
||||
# configure ports and solution parameters and rate
|
||||
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
|
||||
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
|
||||
|
||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
|
||||
|
||||
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
|
||||
dev.configure_poll_port()
|
||||
dev.configure_poll_port(ublox.PORT_SERIAL1)
|
||||
dev.configure_poll_port(ublox.PORT_USB)
|
||||
dev.configure_solution_rate(rate_ms=rate)
|
||||
|
||||
# Configure solution
|
||||
payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
|
||||
payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
|
||||
3, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
|
||||
#bits_ITMF_config1 = '10101101011000101010110111111111'
|
||||
#bits_ITMF_config2 = '00000000000000000110001100011110'
|
||||
ITMF_config1 = 2908925439
|
||||
ITMF_config2 = 25374
|
||||
payload = struct.pack('<II', ITMF_config1, ITMF_config2)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF, payload)
|
||||
payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, (1 << 10), 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
|
||||
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF)
|
||||
|
||||
# Configure RAW, PVT and HW messages to be sent every solution cycle
|
||||
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_SAT, 1)
|
||||
|
||||
# Query the backup restore status
|
||||
print("backup restore polling message (implement custom response handler!):")
|
||||
dev.configure_poll(0x09, 0x14)
|
||||
|
||||
print("if successful, send this to clear the flash:")
|
||||
dev.send_message(0x09, 0x14, b"\x01\x00\x00\x00")
|
||||
|
||||
print("send on stop:")
|
||||
|
||||
# Save on shutdown
|
||||
# Controlled GNSS stop and hot start
|
||||
payload = struct.pack('<HBB', 0x0000, 0x08, 0x00)
|
||||
dev.send_message(ublox.CLASS_CFG, ublox.MSG_CFG_RST, payload)
|
||||
|
||||
# UBX-UPD-SOS backup
|
||||
dev.send_message(0x09, 0x14, b"\x00\x00\x00\x00")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
class Device:
|
||||
def write(self, s):
|
||||
d = '"{}"s'.format(''.join('\\x{:02X}'.format(b) for b in s))
|
||||
print(f" if (!send_with_ack({d})) continue;")
|
||||
|
||||
dev = ublox.UBlox(Device(), baudrate=baudrate)
|
||||
configure_ublox(dev)
|
||||
@@ -0,0 +1,498 @@
|
||||
#include "ublox_msg.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
|
||||
const double gpsPi = 3.1415926535898;
|
||||
#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
|
||||
|
||||
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::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;
|
||||
return needed - (uint16_t)bytes_in_parse_buf;
|
||||
}
|
||||
|
||||
inline bool UbloxMsgParser::valid_cheksum() {
|
||||
uint8_t ck_a = 0, ck_b = 0;
|
||||
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;
|
||||
}
|
||||
if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
|
||||
LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
|
||||
return false;
|
||||
}
|
||||
if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
|
||||
LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
inline bool UbloxMsgParser::valid() {
|
||||
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] != ublox::PREAMBLE1) {
|
||||
return false;
|
||||
}
|
||||
if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
|
||||
return false;
|
||||
}
|
||||
if(needed_bytes() == 0 && !valid()) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) {
|
||||
// convert time to year (conversion from A3.1.3)
|
||||
int J = 0;
|
||||
if (1 <= Nt && Nt <= 366) {
|
||||
J = 1;
|
||||
} else if (367 <= Nt && Nt <= 731) {
|
||||
J = 2;
|
||||
} else if (732 <= Nt && Nt <= 1096) {
|
||||
J = 3;
|
||||
} else if (1097 <= Nt && Nt <= 1461) {
|
||||
J = 4;
|
||||
}
|
||||
uint16_t year = 1996 + 4*(N4 -1) + (J - 1);
|
||||
return year;
|
||||
}
|
||||
|
||||
bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
|
||||
int needed = needed_bytes();
|
||||
if(needed > 0) {
|
||||
bytes_consumed = std::min((uint32_t)needed, incoming_data_len );
|
||||
// Add data to buffer
|
||||
memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
|
||||
bytes_in_parse_buf += bytes_consumed;
|
||||
} else {
|
||||
bytes_consumed = incoming_data_len;
|
||||
}
|
||||
|
||||
// Validate msg format, detect invalid header and invalid checksum.
|
||||
while(!valid_so_far() && bytes_in_parse_buf != 0) {
|
||||
// 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) {
|
||||
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))};
|
||||
case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe)
|
||||
return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
|
||||
case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data)
|
||||
return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
|
||||
case 0x0a09:
|
||||
return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
|
||||
case 0x0a0b:
|
||||
return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
|
||||
case 0x0135:
|
||||
// TODO return {"ubloxGnss", gen_nav_sat(static_cast<ubx_t::nav_sat_t*>(body))};
|
||||
return {"ubloxGnss", kj::Array<capnp::word>()};
|
||||
default:
|
||||
LOGE("Unknown message type %x", ubx_message.msg_type());
|
||||
return {"ubloxGnss", kj::Array<capnp::word>()};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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.setUnixTimestampMillis(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::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
|
||||
// GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes
|
||||
// We will first need to separate the data from the padding and parity
|
||||
auto body = *msg->body();
|
||||
assert(body.size() == 10);
|
||||
|
||||
std::string subframe_data;
|
||||
subframe_data.reserve(30);
|
||||
for (uint32_t word : body) {
|
||||
word = word >> 6; // TODO: Verify parity
|
||||
subframe_data.push_back(word >> 16);
|
||||
subframe_data.push_back(word >> 8);
|
||||
subframe_data.push_back(word >> 0);
|
||||
}
|
||||
|
||||
// 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 > 3 || subframe_id < 1) {
|
||||
// dont parse almanac subframes
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
|
||||
}
|
||||
|
||||
// publish if subframes 1-3 have been collected
|
||||
if (gps_subframes[msg->sv_id()].size() == 3) {
|
||||
MessageBuilder msg_builder;
|
||||
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
|
||||
eph.setSvId(msg->sv_id());
|
||||
|
||||
int iode_s2 = 0;
|
||||
int iode_s3 = 0;
|
||||
int iodc_lsb = 0;
|
||||
|
||||
// Subframe 1
|
||||
{
|
||||
kaitai::kstream stream(gps_subframes[msg->sv_id()][1]);
|
||||
gps_t subframe(&stream);
|
||||
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
|
||||
|
||||
eph.setGpsWeek(subframe_1->week_no());
|
||||
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
|
||||
eph.setToc(subframe_1->t_oc() * pow(2, 4));
|
||||
eph.setAf2(subframe_1->af_2() * pow(2, -55));
|
||||
eph.setAf1(subframe_1->af_1() * pow(2, -43));
|
||||
eph.setAf0(subframe_1->af_0() * pow(2, -31));
|
||||
eph.setSvHealth(subframe_1->sv_health());
|
||||
eph.setTowCount(subframe.how()->tow_count());
|
||||
iodc_lsb = subframe_1->iodc_lsb();
|
||||
}
|
||||
|
||||
// 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));
|
||||
iode_s2 = subframe_2->iode();
|
||||
}
|
||||
|
||||
// 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);
|
||||
iode_s3 = subframe_3->iode();
|
||||
}
|
||||
|
||||
gps_subframes[msg->sv_id()].clear();
|
||||
if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
|
||||
// data set cutover, reject ephemeris
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
return capnp::messageToFlatArray(msg_builder);
|
||||
}
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
|
||||
if (msg->sv_id() == 255) {
|
||||
// data can be decoded before identifying the SV number, in this case 255
|
||||
// is returned, which means "unknown" (ublox p32)
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
auto body = *msg->body();
|
||||
assert(body.size() == 4);
|
||||
{
|
||||
std::string string_data;
|
||||
string_data.reserve(16);
|
||||
for (uint32_t word : body) {
|
||||
for (int i = 3; i >= 0; i--)
|
||||
string_data.push_back(word >> 8*i);
|
||||
}
|
||||
|
||||
kaitai::kstream stream(string_data);
|
||||
glonass_t gl_string(&stream);
|
||||
|
||||
int string_number = gl_string.string_number();
|
||||
if (string_number < 1 || string_number > 5 || gl_string.idle_chip()) {
|
||||
// dont parse non immediate data, idle_chip == 0
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
// immediate data is the same within one superframe
|
||||
if (glonass_superframes[msg->sv_id()] != gl_string.superframe_number()) {
|
||||
glonass_strings[msg->sv_id()].clear();
|
||||
glonass_superframes[msg->sv_id()] = gl_string.superframe_number();
|
||||
}
|
||||
glonass_strings[msg->sv_id()][string_number] = string_data;
|
||||
}
|
||||
|
||||
// publish if strings 1-5 have been collected
|
||||
if (glonass_strings[msg->sv_id()].size() != 5) {
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
MessageBuilder msg_builder;
|
||||
auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
|
||||
eph.setSvId(msg->sv_id());
|
||||
uint16_t current_day = 0;
|
||||
|
||||
// string number 1
|
||||
{
|
||||
kaitai::kstream stream(glonass_strings[msg->sv_id()][1]);
|
||||
glonass_t gl_stream(&stream);
|
||||
glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
|
||||
|
||||
eph.setP1(data->p1());
|
||||
eph.setTk(data->t_k());
|
||||
eph.setXVel(data->x_vel() * pow(2, -20));
|
||||
eph.setXAccel(data->x_accel() * pow(2, -30));
|
||||
eph.setX(data->x() * pow(2, -11));
|
||||
}
|
||||
|
||||
// string number 2
|
||||
{
|
||||
kaitai::kstream stream(glonass_strings[msg->sv_id()][2]);
|
||||
glonass_t gl_stream(&stream);
|
||||
glonass_t::string_2_t* data = static_cast<glonass_t::string_2_t*>(gl_stream.data());
|
||||
|
||||
eph.setSvHealth(data->b_n()>>2); // MSB indicates health
|
||||
eph.setP2(data->p2());
|
||||
eph.setTb(data->t_b());
|
||||
eph.setYVel(data->y_vel() * pow(2, -20));
|
||||
eph.setYAccel(data->y_accel() * pow(2, -30));
|
||||
eph.setY(data->y() * pow(2, -11));
|
||||
}
|
||||
|
||||
// string number 3
|
||||
{
|
||||
kaitai::kstream stream(glonass_strings[msg->sv_id()][3]);
|
||||
glonass_t gl_stream(&stream);
|
||||
glonass_t::string_3_t* data = static_cast<glonass_t::string_3_t*>(gl_stream.data());
|
||||
|
||||
eph.setP3(data->p3());
|
||||
eph.setGammaN(data->gamma_n() * pow(2, -40));
|
||||
eph.setSvHealth(eph.getSvHealth() | data->l_n());
|
||||
eph.setZVel(data->z_vel() * pow(2, -20));
|
||||
eph.setZAccel(data->z_accel() * pow(2, -30));
|
||||
eph.setZ(data->z() * pow(2, -11));
|
||||
}
|
||||
|
||||
// string number 4
|
||||
{
|
||||
kaitai::kstream stream(glonass_strings[msg->sv_id()][4]);
|
||||
glonass_t gl_stream(&stream);
|
||||
glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
|
||||
|
||||
current_day = data->n_t();
|
||||
eph.setTauN(data->tau_n() * pow(2, -30));
|
||||
eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
|
||||
eph.setAge(data->e_n());
|
||||
eph.setP4(data->p4());
|
||||
eph.setSvURA(glonass_URA_lookup.at(data->f_t()));
|
||||
if (msg->sv_id() != data->n()) {
|
||||
LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n())
|
||||
}
|
||||
eph.setSvType(data->m());
|
||||
}
|
||||
|
||||
// string number 5
|
||||
{
|
||||
kaitai::kstream stream(glonass_strings[msg->sv_id()][5]);
|
||||
glonass_t gl_stream(&stream);
|
||||
glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
|
||||
|
||||
// string5 parsing is only needed to get the year, this can be removed and
|
||||
// the year can be fetched later in laika (note rollovers and leap year)
|
||||
uint8_t n_4 = data->n_4();
|
||||
uint16_t year = get_glonass_year(n_4, current_day);
|
||||
if (current_day > 1461) {
|
||||
// impossible day within last 4 year, reject ephemeris
|
||||
// TODO: check if this can be detected via hamming code
|
||||
LOGE("INVALID DATA: current day out of range: %d, %d", current_day, n_4);
|
||||
glonass_strings[msg->sv_id()].clear();
|
||||
return kj::Array<capnp::word>();
|
||||
}
|
||||
|
||||
uint16_t last_leap_year = 1996 + 4*(n_4-1);
|
||||
uint16_t days_till_this_year = (year - last_leap_year)*365;
|
||||
if (days_till_this_year != 0) {
|
||||
days_till_this_year++;
|
||||
}
|
||||
|
||||
eph.setYear(year);
|
||||
eph.setDayInYear(current_day - days_till_this_year);
|
||||
eph.setHour((eph.getTk()>>7) & 0x1F);
|
||||
eph.setMinute((eph.getTk()>>1) & 0x3F);
|
||||
eph.setSecond((eph.getTk() & 0x1) * 30);
|
||||
}
|
||||
|
||||
glonass_strings[msg->sv_id()].clear();
|
||||
return capnp::messageToFlatArray(msg_builder);
|
||||
}
|
||||
|
||||
|
||||
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
|
||||
switch (msg->gnss_id()) {
|
||||
case ubx_t::gnss_type_t::GNSS_TYPE_GPS:
|
||||
return parse_gps_ephemeris(msg);
|
||||
case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS:
|
||||
return parse_glonass_ephemeris(msg);
|
||||
default:
|
||||
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.setFlags(msg->flags());
|
||||
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);
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <ctime>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/util.h"
|
||||
#include "system/ubloxd/generated/gps.h"
|
||||
#include "system/ubloxd/generated/glonass.h"
|
||||
#include "system/ubloxd/generated/ubx.h"
|
||||
|
||||
using namespace std::string_literals;
|
||||
|
||||
// protocol constants
|
||||
namespace ublox {
|
||||
const uint8_t PREAMBLE1 = 0xb5;
|
||||
const uint8_t PREAMBLE2 = 0x62;
|
||||
|
||||
const int UBLOX_HEADER_SIZE = 6;
|
||||
const int UBLOX_CHECKSUM_SIZE = 2;
|
||||
const int UBLOX_MAX_MSG_SIZE = 65536;
|
||||
|
||||
struct ubx_mga_ini_time_utc_t {
|
||||
uint8_t type;
|
||||
uint8_t version;
|
||||
uint8_t ref;
|
||||
int8_t leapSecs;
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint8_t day;
|
||||
uint8_t hour;
|
||||
uint8_t minute;
|
||||
uint8_t second;
|
||||
uint8_t reserved1;
|
||||
uint32_t ns;
|
||||
uint16_t tAccS;
|
||||
uint16_t reserved2;
|
||||
uint32_t tAccNs;
|
||||
} __attribute__((packed));
|
||||
|
||||
inline std::string ubx_add_checksum(const std::string &msg) {
|
||||
assert(msg.size() > 2);
|
||||
|
||||
uint8_t ck_a = 0, ck_b = 0;
|
||||
for(int i = 2; i < msg.size(); i++) {
|
||||
ck_a = (ck_a + msg[i]) & 0xFF;
|
||||
ck_b = (ck_b + ck_a) & 0xFF;
|
||||
}
|
||||
|
||||
std::string r = msg;
|
||||
r.push_back(ck_a);
|
||||
r.push_back(ck_b);
|
||||
return r;
|
||||
}
|
||||
|
||||
inline std::string build_ubx_mga_ini_time_utc(struct tm time) {
|
||||
ublox::ubx_mga_ini_time_utc_t payload = {
|
||||
.type = 0x10,
|
||||
.version = 0x0,
|
||||
.ref = 0x0,
|
||||
.leapSecs = -128, // Unknown
|
||||
.year = (uint16_t)(1900 + time.tm_year),
|
||||
.month = (uint8_t)(1 + time.tm_mon),
|
||||
.day = (uint8_t)time.tm_mday,
|
||||
.hour = (uint8_t)time.tm_hour,
|
||||
.minute = (uint8_t)time.tm_min,
|
||||
.second = (uint8_t)time.tm_sec,
|
||||
.reserved1 = 0x0,
|
||||
.ns = 0,
|
||||
.tAccS = 30,
|
||||
.reserved2 = 0x0,
|
||||
.tAccNs = 0,
|
||||
};
|
||||
assert(sizeof(payload) == 24);
|
||||
|
||||
std::string msg = "\xb5\x62\x13\x40\x18\x00"s;
|
||||
msg += std::string((char*)&payload, sizeof(payload));
|
||||
|
||||
return ubx_add_checksum(msg);
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt);
|
||||
|
||||
kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
|
||||
kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);
|
||||
|
||||
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];
|
||||
|
||||
// user range accuracy in meters
|
||||
const std::unordered_map<uint8_t, float> glonass_URA_lookup =
|
||||
{{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7},
|
||||
{ 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32},
|
||||
{11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}};
|
||||
|
||||
std::unordered_map<int, std::unordered_map<int, std::string>> glonass_strings;
|
||||
std::unordered_map<int, int> glonass_superframes;
|
||||
};
|
||||
@@ -0,0 +1,64 @@
|
||||
#include <cassert>
|
||||
|
||||
#include <kaitai/kaitaistream.h>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
#include "system/ubloxd/ublox_msg.h"
|
||||
|
||||
ExitHandler do_exit;
|
||||
using namespace ublox;
|
||||
|
||||
int main() {
|
||||
LOGW("starting ubloxd");
|
||||
AlignedBuffer aligned_buf;
|
||||
UbloxMsgParser parser;
|
||||
|
||||
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
|
||||
|
||||
std::unique_ptr<Context> context(Context::create());
|
||||
std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "ubloxRaw"));
|
||||
assert(subscriber != NULL);
|
||||
subscriber->setTimeout(100);
|
||||
|
||||
|
||||
while (!do_exit) {
|
||||
std::unique_ptr<Message> msg(subscriber->receive());
|
||||
if (!msg) {
|
||||
if (errno == EINTR) {
|
||||
do_exit = true;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
auto ubloxRaw = event.getUbloxRaw();
|
||||
|
||||
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)) {
|
||||
|
||||
try {
|
||||
auto ublox_msg = parser.gen_msg();
|
||||
if (ublox_msg.second.size() > 0) {
|
||||
auto bytes = ublox_msg.second.asBytes();
|
||||
pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size());
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
LOGE("Error parsing ublox message %s", e.what());
|
||||
}
|
||||
|
||||
parser.reset();
|
||||
}
|
||||
bytes_consumed += bytes_consumed_this_time;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -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