mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-06 00:36:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rewrite ubloxd in Python (#36112)

Browse Source 
* Rewrite ubloxd in Python

* lil more

* rm from third_party/

* cleanup

* ubx replay

* try this

* back to kaitai

* Revert "ubx replay"

This reverts commit 570bd3d25fbabc590379ce0a9f98d30de55cf1b3.
This commit is contained in:
Adeeb Shihadeh
2025-09-07 15:40:48 -07:00
committed by GitHub
parent a885111c0c
commit 8dca43881a
25 changed files with 1241 additions and 4628 deletions
Download Patch File Download Diff File Expand all files Collapse all files
system/manager/process_config.py
+1 -1
View File
@@ -97,7 +97,7 @@ procs = [
PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
PythonProcess("ubloxd", "system.ubloxd.ubloxd", ublox, enabled=TICI),
PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),
PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver),
system/ubloxd/.gitignore
-2
View File
@@ -1,2 +0,0 @@
ubloxd
tests/test_glonass_runner
system/ubloxd/SConscript
+8 -17
View File
@@ -1,20 +1,11 @@
Import('env', 'common', 'messaging')
loc_libs = [messaging, common, 'kaitai', 'pthread']
Import('env')
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)
current_dir = Dir('./generated/').srcnode().abspath
python_cmd = f"kaitai-struct-compiler --target python --outdir {current_dir} $SOURCES"
env.Command(File('./generated/ubx.py'), 'ubx.ksy', python_cmd)
env.Command(File('./generated/gps.py'), 'gps.ksy', python_cmd)
env.Command(File('./generated/glonass.py'), 'glonass.ksy', python_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('extras'):
env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs])
py_glonass_fix = env.Command(None, File('./generated/glonass.py'), "sed -i 's/self._io.align_to_byte()/# self._io.align_to_byte()/' $SOURCES")
env.Depends(py_glonass_fix, File('./generated/glonass.py'))
system/ubloxd/generated/glonass.cpp
-353
View File
@@ -1,353 +0,0 @@
// 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;
}
system/ubloxd/generated/glonass.h
-375
View File
@@ -1,375 +0,0 @@
#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_
system/ubloxd/generated/glonass.py
+247
View File
@@ -0,0 +1,247 @@
# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
import kaitaistruct
from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO
if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9):
raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__))
class Glonass(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.idle_chip = self._io.read_bits_int_be(1) != 0
self.string_number = self._io.read_bits_int_be(4)
# workaround for kaitai bit alignment issue (see glonass_fix.patch for C++)
# self._io.align_to_byte()
_on = self.string_number
if _on == 4:
self.data = Glonass.String4(self._io, self, self._root)
elif _on == 1:
self.data = Glonass.String1(self._io, self, self._root)
elif _on == 3:
self.data = Glonass.String3(self._io, self, self._root)
elif _on == 5:
self.data = Glonass.String5(self._io, self, self._root)
elif _on == 2:
self.data = Glonass.String2(self._io, self, self._root)
else:
self.data = Glonass.StringNonImmediate(self._io, self, self._root)
self.hamming_code = self._io.read_bits_int_be(8)
self.pad_1 = self._io.read_bits_int_be(11)
self.superframe_number = self._io.read_bits_int_be(16)
self.pad_2 = self._io.read_bits_int_be(8)
self.frame_number = self._io.read_bits_int_be(8)
class String4(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.tau_n_sign = self._io.read_bits_int_be(1) != 0
self.tau_n_value = self._io.read_bits_int_be(21)
self.delta_tau_n_sign = self._io.read_bits_int_be(1) != 0
self.delta_tau_n_value = self._io.read_bits_int_be(4)
self.e_n = self._io.read_bits_int_be(5)
self.not_used_1 = self._io.read_bits_int_be(14)
self.p4 = self._io.read_bits_int_be(1) != 0
self.f_t = self._io.read_bits_int_be(4)
self.not_used_2 = self._io.read_bits_int_be(3)
self.n_t = self._io.read_bits_int_be(11)
self.n = self._io.read_bits_int_be(5)
self.m = self._io.read_bits_int_be(2)
@property
def tau_n(self):
if hasattr(self, '_m_tau_n'):
return self._m_tau_n
self._m_tau_n = ((self.tau_n_value * -1) if self.tau_n_sign else self.tau_n_value)
return getattr(self, '_m_tau_n', None)
@property
def delta_tau_n(self):
if hasattr(self, '_m_delta_tau_n'):
return self._m_delta_tau_n
self._m_delta_tau_n = ((self.delta_tau_n_value * -1) if self.delta_tau_n_sign else self.delta_tau_n_value)
return getattr(self, '_m_delta_tau_n', None)
class StringNonImmediate(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.data_1 = self._io.read_bits_int_be(64)
self.data_2 = self._io.read_bits_int_be(8)
class String5(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.n_a = self._io.read_bits_int_be(11)
self.tau_c = self._io.read_bits_int_be(32)
self.not_used = self._io.read_bits_int_be(1) != 0
self.n_4 = self._io.read_bits_int_be(5)
self.tau_gps = self._io.read_bits_int_be(22)
self.l_n = self._io.read_bits_int_be(1) != 0
class String1(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.not_used = self._io.read_bits_int_be(2)
self.p1 = self._io.read_bits_int_be(2)
self.t_k = self._io.read_bits_int_be(12)
self.x_vel_sign = self._io.read_bits_int_be(1) != 0
self.x_vel_value = self._io.read_bits_int_be(23)
self.x_accel_sign = self._io.read_bits_int_be(1) != 0
self.x_accel_value = self._io.read_bits_int_be(4)
self.x_sign = self._io.read_bits_int_be(1) != 0
self.x_value = self._io.read_bits_int_be(26)
@property
def x_vel(self):
if hasattr(self, '_m_x_vel'):
return self._m_x_vel
self._m_x_vel = ((self.x_vel_value * -1) if self.x_vel_sign else self.x_vel_value)
return getattr(self, '_m_x_vel', None)
@property
def x_accel(self):
if hasattr(self, '_m_x_accel'):
return self._m_x_accel
self._m_x_accel = ((self.x_accel_value * -1) if self.x_accel_sign else self.x_accel_value)
return getattr(self, '_m_x_accel', None)
@property
def x(self):
if hasattr(self, '_m_x'):
return self._m_x
self._m_x = ((self.x_value * -1) if self.x_sign else self.x_value)
return getattr(self, '_m_x', None)
class String2(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.b_n = self._io.read_bits_int_be(3)
self.p2 = self._io.read_bits_int_be(1) != 0
self.t_b = self._io.read_bits_int_be(7)
self.not_used = self._io.read_bits_int_be(5)
self.y_vel_sign = self._io.read_bits_int_be(1) != 0
self.y_vel_value = self._io.read_bits_int_be(23)
self.y_accel_sign = self._io.read_bits_int_be(1) != 0
self.y_accel_value = self._io.read_bits_int_be(4)
self.y_sign = self._io.read_bits_int_be(1) != 0
self.y_value = self._io.read_bits_int_be(26)
@property
def y_vel(self):
if hasattr(self, '_m_y_vel'):
return self._m_y_vel
self._m_y_vel = ((self.y_vel_value * -1) if self.y_vel_sign else self.y_vel_value)
return getattr(self, '_m_y_vel', None)
@property
def y_accel(self):
if hasattr(self, '_m_y_accel'):
return self._m_y_accel
self._m_y_accel = ((self.y_accel_value * -1) if self.y_accel_sign else self.y_accel_value)
return getattr(self, '_m_y_accel', None)
@property
def y(self):
if hasattr(self, '_m_y'):
return self._m_y
self._m_y = ((self.y_value * -1) if self.y_sign else self.y_value)
return getattr(self, '_m_y', None)
class String3(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.p3 = self._io.read_bits_int_be(1) != 0
self.gamma_n_sign = self._io.read_bits_int_be(1) != 0
self.gamma_n_value = self._io.read_bits_int_be(10)
self.not_used = self._io.read_bits_int_be(1) != 0
self.p = self._io.read_bits_int_be(2)
self.l_n = self._io.read_bits_int_be(1) != 0
self.z_vel_sign = self._io.read_bits_int_be(1) != 0
self.z_vel_value = self._io.read_bits_int_be(23)
self.z_accel_sign = self._io.read_bits_int_be(1) != 0
self.z_accel_value = self._io.read_bits_int_be(4)
self.z_sign = self._io.read_bits_int_be(1) != 0
self.z_value = self._io.read_bits_int_be(26)
@property
def gamma_n(self):
if hasattr(self, '_m_gamma_n'):
return self._m_gamma_n
self._m_gamma_n = ((self.gamma_n_value * -1) if self.gamma_n_sign else self.gamma_n_value)
return getattr(self, '_m_gamma_n', None)
@property
def z_vel(self):
if hasattr(self, '_m_z_vel'):
return self._m_z_vel
self._m_z_vel = ((self.z_vel_value * -1) if self.z_vel_sign else self.z_vel_value)
return getattr(self, '_m_z_vel', None)
@property
def z_accel(self):
if hasattr(self, '_m_z_accel'):
return self._m_z_accel
self._m_z_accel = ((self.z_accel_value * -1) if self.z_accel_sign else self.z_accel_value)
return getattr(self, '_m_z_accel', None)
@property
def z(self):
if hasattr(self, '_m_z'):
return self._m_z
self._m_z = ((self.z_value * -1) if self.z_sign else self.z_value)
return getattr(self, '_m_z', None)
system/ubloxd/generated/gps.cpp
-325
View File
@@ -1,325 +0,0 @@
// 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() {
}
system/ubloxd/generated/gps.h
-359
View File
@@ -1,359 +0,0 @@
#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_
system/ubloxd/generated/gps.py
+193
View File
@@ -0,0 +1,193 @@
# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
import kaitaistruct
from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO
if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9):
raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__))
class Gps(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.tlm = Gps.Tlm(self._io, self, self._root)
self.how = Gps.How(self._io, self, self._root)
_on = self.how.subframe_id
if _on == 1:
self.body = Gps.Subframe1(self._io, self, self._root)
elif _on == 2:
self.body = Gps.Subframe2(self._io, self, self._root)
elif _on == 3:
self.body = Gps.Subframe3(self._io, self, self._root)
elif _on == 4:
self.body = Gps.Subframe4(self._io, self, self._root)
class Subframe1(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.week_no = self._io.read_bits_int_be(10)
self.code = self._io.read_bits_int_be(2)
self.sv_accuracy = self._io.read_bits_int_be(4)
self.sv_health = self._io.read_bits_int_be(6)
self.iodc_msb = self._io.read_bits_int_be(2)
self.l2_p_data_flag = self._io.read_bits_int_be(1) != 0
self.reserved1 = self._io.read_bits_int_be(23)
self.reserved2 = self._io.read_bits_int_be(24)
self.reserved3 = self._io.read_bits_int_be(24)
self.reserved4 = self._io.read_bits_int_be(16)
self._io.align_to_byte()
self.t_gd = self._io.read_s1()
self.iodc_lsb = self._io.read_u1()
self.t_oc = self._io.read_u2be()
self.af_2 = self._io.read_s1()
self.af_1 = self._io.read_s2be()
self.af_0_sign = self._io.read_bits_int_be(1) != 0
self.af_0_value = self._io.read_bits_int_be(21)
self.reserved5 = self._io.read_bits_int_be(2)
@property
def af_0(self):
if hasattr(self, '_m_af_0'):
return self._m_af_0
self._m_af_0 = ((self.af_0_value - (1 << 21)) if self.af_0_sign else self.af_0_value)
return getattr(self, '_m_af_0', None)
class Subframe3(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.c_ic = self._io.read_s2be()
self.omega_0 = self._io.read_s4be()
self.c_is = self._io.read_s2be()
self.i_0 = self._io.read_s4be()
self.c_rc = self._io.read_s2be()
self.omega = self._io.read_s4be()
self.omega_dot_sign = self._io.read_bits_int_be(1) != 0
self.omega_dot_value = self._io.read_bits_int_be(23)
self._io.align_to_byte()
self.iode = self._io.read_u1()
self.idot_sign = self._io.read_bits_int_be(1) != 0
self.idot_value = self._io.read_bits_int_be(13)
self.reserved = self._io.read_bits_int_be(2)
@property
def omega_dot(self):
if hasattr(self, '_m_omega_dot'):
return self._m_omega_dot
self._m_omega_dot = ((self.omega_dot_value - (1 << 23)) if self.omega_dot_sign else self.omega_dot_value)
return getattr(self, '_m_omega_dot', None)
@property
def idot(self):
if hasattr(self, '_m_idot'):
return self._m_idot
self._m_idot = ((self.idot_value - (1 << 13)) if self.idot_sign else self.idot_value)
return getattr(self, '_m_idot', None)
class Subframe4(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.data_id = self._io.read_bits_int_be(2)
self.page_id = self._io.read_bits_int_be(6)
self._io.align_to_byte()
_on = self.page_id
if _on == 56:
self.body = Gps.Subframe4.IonosphereData(self._io, self, self._root)
class IonosphereData(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.a0 = self._io.read_s1()
self.a1 = self._io.read_s1()
self.a2 = self._io.read_s1()
self.a3 = self._io.read_s1()
self.b0 = self._io.read_s1()
self.b1 = self._io.read_s1()
self.b2 = self._io.read_s1()
self.b3 = self._io.read_s1()
class How(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.tow_count = self._io.read_bits_int_be(17)
self.alert = self._io.read_bits_int_be(1) != 0
self.anti_spoof = self._io.read_bits_int_be(1) != 0
self.subframe_id = self._io.read_bits_int_be(3)
self.reserved = self._io.read_bits_int_be(2)
class Tlm(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.preamble = self._io.read_bytes(1)
if not self.preamble == b"\x8B":
raise kaitaistruct.ValidationNotEqualError(b"\x8B", self.preamble, self._io, u"/types/tlm/seq/0")
self.tlm = self._io.read_bits_int_be(14)
self.integrity_status = self._io.read_bits_int_be(1) != 0
self.reserved = self._io.read_bits_int_be(1) != 0
class Subframe2(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.iode = self._io.read_u1()
self.c_rs = self._io.read_s2be()
self.delta_n = self._io.read_s2be()
self.m_0 = self._io.read_s4be()
self.c_uc = self._io.read_s2be()
self.e = self._io.read_s4be()
self.c_us = self._io.read_s2be()
self.sqrt_a = self._io.read_u4be()
self.t_oe = self._io.read_u2be()
self.fit_interval_flag = self._io.read_bits_int_be(1) != 0
self.aoda = self._io.read_bits_int_be(5)
self.reserved = self._io.read_bits_int_be(2)
system/ubloxd/generated/ubx.cpp
-424
View File
@@ -1,424 +0,0 @@
// 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 309: {
n_body = false;
m_body = new nav_sat_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_meas = 0;
m__raw_meas = 0;
m__io__raw_meas = 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_meas = new std::vector<std::string>();
m__io__raw_meas = new std::vector<kaitai::kstream*>();
m_meas = new std::vector<measurement_t*>();
const int l_meas = num_meas();
for (int i = 0; i < l_meas; i++) {
m__raw_meas->push_back(m__io->read_bytes(32));
kaitai::kstream* io__raw_meas = new kaitai::kstream(m__raw_meas->at(m__raw_meas->size() - 1));
m__io__raw_meas->push_back(io__raw_meas);
m_meas->push_back(new measurement_t(io__raw_meas, this, m__root));
}
}
ubx_t::rxm_rawx_t::~rxm_rawx_t() {
_clean_up();
}
void ubx_t::rxm_rawx_t::_clean_up() {
if (m__raw_meas) {
delete m__raw_meas; m__raw_meas = 0;
}
if (m__io__raw_meas) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_meas->begin(); it != m__io__raw_meas->end(); ++it) {
delete *it;
}
delete m__io__raw_meas; m__io__raw_meas = 0;
}
if (m_meas) {
for (std::vector<measurement_t*>::iterator it = m_meas->begin(); it != m_meas->end(); ++it) {
delete *it;
}
delete m_meas; m_meas = 0;
}
}
ubx_t::rxm_rawx_t::measurement_t::measurement_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::measurement_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::measurement_t::~measurement_t() {
_clean_up();
}
void ubx_t::rxm_rawx_t::measurement_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_sat_t::nav_sat_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_svs = 0;
m__raw_svs = 0;
m__io__raw_svs = 0;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::nav_sat_t::_read() {
m_itow = m__io->read_u4le();
m_version = m__io->read_u1();
m_num_svs = m__io->read_u1();
m_reserved = m__io->read_bytes(2);
m__raw_svs = new std::vector<std::string>();
m__io__raw_svs = new std::vector<kaitai::kstream*>();
m_svs = new std::vector<nav_t*>();
const int l_svs = num_svs();
for (int i = 0; i < l_svs; i++) {
m__raw_svs->push_back(m__io->read_bytes(12));
kaitai::kstream* io__raw_svs = new kaitai::kstream(m__raw_svs->at(m__raw_svs->size() - 1));
m__io__raw_svs->push_back(io__raw_svs);
m_svs->push_back(new nav_t(io__raw_svs, this, m__root));
}
}
ubx_t::nav_sat_t::~nav_sat_t() {
_clean_up();
}
void ubx_t::nav_sat_t::_clean_up() {
if (m__raw_svs) {
delete m__raw_svs; m__raw_svs = 0;
}
if (m__io__raw_svs) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_svs->begin(); it != m__io__raw_svs->end(); ++it) {
delete *it;
}
delete m__io__raw_svs; m__io__raw_svs = 0;
}
if (m_svs) {
for (std::vector<nav_t*>::iterator it = m_svs->begin(); it != m_svs->end(); ++it) {
delete *it;
}
delete m_svs; m_svs = 0;
}
}
ubx_t::nav_sat_t::nav_t::nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_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_sat_t::nav_t::_read() {
m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
m_sv_id = m__io->read_u1();
m_cno = m__io->read_u1();
m_elev = m__io->read_s1();
m_azim = m__io->read_s2le();
m_pr_res = m__io->read_s2le();
m_flags = m__io->read_u4le();
}
ubx_t::nav_sat_t::nav_t::~nav_t() {
_clean_up();
}
void ubx_t::nav_sat_t::nav_t::_clean_up() {
}
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;
}
system/ubloxd/generated/ubx.h
-484
View File
@@ -1,484 +0,0 @@
#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_sat_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 measurement_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 measurement_t : public kaitai::kstruct {
public:
measurement_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:
~measurement_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<measurement_t*>* m_meas;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_meas;
std::vector<kaitai::kstream*>* m__io__raw_meas;
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<measurement_t*>* meas() const { return m_meas; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_meas() const { return m__raw_meas; }
std::vector<kaitai::kstream*>* _io__raw_meas() const { return m__io__raw_meas; }
};
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_sat_t : public kaitai::kstruct {
public:
class nav_t;
nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_sat_t();
class nav_t : public kaitai::kstruct {
public:
nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_t();
private:
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
uint8_t m_cno;
int8_t m_elev;
int16_t m_azim;
int16_t m_pr_res;
uint32_t m_flags;
ubx_t* m__root;
ubx_t::nav_sat_t* m__parent;
public:
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
uint8_t cno() const { return m_cno; }
int8_t elev() const { return m_elev; }
int16_t azim() const { return m_azim; }
int16_t pr_res() const { return m_pr_res; }
uint32_t flags() const { return m_flags; }
ubx_t* _root() const { return m__root; }
ubx_t::nav_sat_t* _parent() const { return m__parent; }
};
private:
uint32_t m_itow;
uint8_t m_version;
uint8_t m_num_svs;
std::string m_reserved;
std::vector<nav_t*>* m_svs;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_svs;
std::vector<kaitai::kstream*>* m__io__raw_svs;
public:
uint32_t itow() const { return m_itow; }
uint8_t version() const { return m_version; }
uint8_t num_svs() const { return m_num_svs; }
std::string reserved() const { return m_reserved; }
std::vector<nav_t*>* svs() const { return m_svs; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_svs() const { return m__raw_svs; }
std::vector<kaitai::kstream*>* _io__raw_svs() const { return m__io__raw_svs; }
};
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_
system/ubloxd/generated/ubx.py
+273
View File
@@ -0,0 +1,273 @@
# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
import kaitaistruct
from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO
from enum import Enum
if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9):
raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__))
class Ubx(KaitaiStruct):
class GnssType(Enum):
gps = 0
sbas = 1
galileo = 2
beidou = 3
imes = 4
qzss = 5
glonass = 6
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.magic = self._io.read_bytes(2)
if not self.magic == b"\xB5\x62":
raise kaitaistruct.ValidationNotEqualError(b"\xB5\x62", self.magic, self._io, u"/seq/0")
self.msg_type = self._io.read_u2be()
self.length = self._io.read_u2le()
_on = self.msg_type
if _on == 2569:
self.body = Ubx.MonHw(self._io, self, self._root)
elif _on == 533:
self.body = Ubx.RxmRawx(self._io, self, self._root)
elif _on == 531:
self.body = Ubx.RxmSfrbx(self._io, self, self._root)
elif _on == 309:
self.body = Ubx.NavSat(self._io, self, self._root)
elif _on == 2571:
self.body = Ubx.MonHw2(self._io, self, self._root)
elif _on == 263:
self.body = Ubx.NavPvt(self._io, self, self._root)
class RxmRawx(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.rcv_tow = self._io.read_f8le()
self.week = self._io.read_u2le()
self.leap_s = self._io.read_s1()
self.num_meas = self._io.read_u1()
self.rec_stat = self._io.read_u1()
self.reserved1 = self._io.read_bytes(3)
self._raw_meas = []
self.meas = []
for i in range(self.num_meas):
self._raw_meas.append(self._io.read_bytes(32))
_io__raw_meas = KaitaiStream(BytesIO(self._raw_meas[i]))
self.meas.append(Ubx.RxmRawx.Measurement(_io__raw_meas, self, self._root))
class Measurement(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.pr_mes = self._io.read_f8le()
self.cp_mes = self._io.read_f8le()
self.do_mes = self._io.read_f4le()
self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1())
self.sv_id = self._io.read_u1()
self.reserved2 = self._io.read_bytes(1)
self.freq_id = self._io.read_u1()
self.lock_time = self._io.read_u2le()
self.cno = self._io.read_u1()
self.pr_stdev = self._io.read_u1()
self.cp_stdev = self._io.read_u1()
self.do_stdev = self._io.read_u1()
self.trk_stat = self._io.read_u1()
self.reserved3 = self._io.read_bytes(1)
class RxmSfrbx(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1())
self.sv_id = self._io.read_u1()
self.reserved1 = self._io.read_bytes(1)
self.freq_id = self._io.read_u1()
self.num_words = self._io.read_u1()
self.reserved2 = self._io.read_bytes(1)
self.version = self._io.read_u1()
self.reserved3 = self._io.read_bytes(1)
self.body = []
for i in range(self.num_words):
self.body.append(self._io.read_u4le())
class NavSat(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.itow = self._io.read_u4le()
self.version = self._io.read_u1()
self.num_svs = self._io.read_u1()
self.reserved = self._io.read_bytes(2)
self._raw_svs = []
self.svs = []
for i in range(self.num_svs):
self._raw_svs.append(self._io.read_bytes(12))
_io__raw_svs = KaitaiStream(BytesIO(self._raw_svs[i]))
self.svs.append(Ubx.NavSat.Nav(_io__raw_svs, self, self._root))
class Nav(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1())
self.sv_id = self._io.read_u1()
self.cno = self._io.read_u1()
self.elev = self._io.read_s1()
self.azim = self._io.read_s2le()
self.pr_res = self._io.read_s2le()
self.flags = self._io.read_u4le()
class NavPvt(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.i_tow = self._io.read_u4le()
self.year = self._io.read_u2le()
self.month = self._io.read_u1()
self.day = self._io.read_u1()
self.hour = self._io.read_u1()
self.min = self._io.read_u1()
self.sec = self._io.read_u1()
self.valid = self._io.read_u1()
self.t_acc = self._io.read_u4le()
self.nano = self._io.read_s4le()
self.fix_type = self._io.read_u1()
self.flags = self._io.read_u1()
self.flags2 = self._io.read_u1()
self.num_sv = self._io.read_u1()
self.lon = self._io.read_s4le()
self.lat = self._io.read_s4le()
self.height = self._io.read_s4le()
self.h_msl = self._io.read_s4le()
self.h_acc = self._io.read_u4le()
self.v_acc = self._io.read_u4le()
self.vel_n = self._io.read_s4le()
self.vel_e = self._io.read_s4le()
self.vel_d = self._io.read_s4le()
self.g_speed = self._io.read_s4le()
self.head_mot = self._io.read_s4le()
self.s_acc = self._io.read_s4le()
self.head_acc = self._io.read_u4le()
self.p_dop = self._io.read_u2le()
self.flags3 = self._io.read_u1()
self.reserved1 = self._io.read_bytes(5)
self.head_veh = self._io.read_s4le()
self.mag_dec = self._io.read_s2le()
self.mag_acc = self._io.read_u2le()
class MonHw2(KaitaiStruct):
class ConfigSource(Enum):
flash = 102
otp = 111
config_pins = 112
rom = 113
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.ofs_i = self._io.read_s1()
self.mag_i = self._io.read_u1()
self.ofs_q = self._io.read_s1()
self.mag_q = self._io.read_u1()
self.cfg_source = KaitaiStream.resolve_enum(Ubx.MonHw2.ConfigSource, self._io.read_u1())
self.reserved1 = self._io.read_bytes(3)
self.low_lev_cfg = self._io.read_u4le()
self.reserved2 = self._io.read_bytes(8)
self.post_status = self._io.read_u4le()
self.reserved3 = self._io.read_bytes(4)
class MonHw(KaitaiStruct):
class AntennaStatus(Enum):
init = 0
dontknow = 1
ok = 2
short = 3
open = 4
class AntennaPower(Enum):
false = 0
true = 1
dontknow = 2
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.pin_sel = self._io.read_u4le()
self.pin_bank = self._io.read_u4le()
self.pin_dir = self._io.read_u4le()
self.pin_val = self._io.read_u4le()
self.noise_per_ms = self._io.read_u2le()
self.agc_cnt = self._io.read_u2le()
self.a_status = KaitaiStream.resolve_enum(Ubx.MonHw.AntennaStatus, self._io.read_u1())
self.a_power = KaitaiStream.resolve_enum(Ubx.MonHw.AntennaPower, self._io.read_u1())
self.flags = self._io.read_u1()
self.reserved1 = self._io.read_bytes(1)
self.used_mask = self._io.read_u4le()
self.vp = self._io.read_bytes(17)
self.jam_ind = self._io.read_u1()
self.reserved2 = self._io.read_bytes(2)
self.pin_irq = self._io.read_u4le()
self.pull_h = self._io.read_u4le()
self.pull_l = self._io.read_u4le()
@property
def checksum(self):
if hasattr(self, '_m_checksum'):
return self._m_checksum
_pos = self._io.pos()
self._io.seek((self.length + 6))
self._m_checksum = self._io.read_u2le()
self._io.seek(_pos)
return getattr(self, '_m_checksum', None)
system/ubloxd/tests/print_gps_stats.py
-20
View File
@@ -1,20 +0,0 @@
#!/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(f"Sats: {ug.measurementReport.numMeas} Accuracy: {gle.horizontalAccuracy:.2f} m cnos", sorted(cnos))
except Exception:
pass
sm.update()
time.sleep(0.1)
system/ubloxd/tests/test_glonass_kaitai.cc
-360
View File
@@ -1,360 +0,0 @@
#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);
}
system/ubloxd/tests/test_glonass_runner.cc
-2
View File
@@ -1,2 +0,0 @@
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
system/ubloxd/ublox_msg.cc
-530
View File
@@ -1,530 +0,0 @@
#include "system/ubloxd/ublox_msg.h"
#include <unistd.h>
#include <algorithm>
#include <cassert>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <unordered_map>
#include <utility>
#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;
}
bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
last_log_time = log_time;
int needed = needed_bytes();
if (needed > 0) {
bytes_consumed = std::min((uint32_t)needed, incoming_data_len);
// 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:
return {"ubloxGnss", gen_nav_sat(static_cast<ubx_t::nav_sat_t*>(body))};
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.setHasFix((msg->flags() % 2) == 1);
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.setHorizontalAccuracy(msg->h_acc() * 1e-03);
gpsLoc.setSatelliteCount(msg->num_sv());
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) {
// don't 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;
int week;
// 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());
// Each message is incremented to be greater or equal than week 1877 (2015-12-27).
// To skip this use the current_time argument
week = subframe_1->week_no();
week += 1024;
if (week < 1877) {
week += 1024;
}
//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());
// GPS week refers to current week, the ephemeris can be valid for the next
// if toe equals 0, this can be verified by the TOW count if it is within the
// last 2 hours of the week (gps ephemeris valid for 4hours)
if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){
week += 1;
}
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();
}
eph.setToeWeek(week);
eph.setTocWeek(week);
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) {
// This parser assumes that no 2 satellites of the same frequency
// can be in view at the same time
auto body = *msg->body();
assert(body.size() == 4);
{
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()) {
// don't parse non immediate data, idle_chip == 0
return kj::Array<capnp::word>();
}
// Check if new string either has same superframe_id or log transmission times make sense
bool superframe_unknown = false;
bool needs_clear = false;
for (int i = 1; i <= 5; i++) {
if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end())
continue;
if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) {
superframe_unknown = true;
} else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
needs_clear = true;
}
// Check if string times add up to being from the same frame
// If superframe is known this is redundant
// Strings are sent 2s apart and frames are 30s apart
if (superframe_unknown &&
std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10)
needs_clear = true;
}
if (needs_clear) {
glonass_strings[msg->freq_id()].clear();
glonass_string_superframes[msg->freq_id()].clear();
glonass_string_times[msg->freq_id()].clear();
}
glonass_strings[msg->freq_id()][string_number] = string_data;
glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number();
glonass_string_times[msg->freq_id()][string_number] = last_log_time;
}
if (msg->sv_id() == 255) {
// data can be decoded before identifying the SV number, in this case 255
// is returned, which means "unknown" (ublox p32)
return kj::Array<capnp::word>();
}
// publish if strings 1-5 have been collected
if (glonass_strings[msg->freq_id()].size() != 5) {
return kj::Array<capnp::word>();
}
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
eph.setSvId(msg->sv_id());
eph.setFreqNum(msg->freq_id() - 7);
uint16_t current_day = 0;
uint16_t tk = 0;
// string number 1
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][1]);
glonass_t gl_stream(&stream);
glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
eph.setP1(data->p1());
tk = data->t_k();
eph.setTkDEPRECATED(tk);
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->freq_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->freq_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->freq_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.setNt(current_day);
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 %" PRIu64, msg->sv_id(), data->n());
}
eph.setSvType(data->m());
}
// string number 5
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][5]);
glonass_t gl_stream(&stream);
glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
// 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)
eph.setN4(data->n_4());
int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30;
eph.setTkSeconds(tk_seconds);
}
glonass_strings[msg->freq_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->meas();
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_nav_sat(ubx_t::nav_sat_t *msg) {
MessageBuilder msg_builder;
auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport();
sr.setITow(msg->itow());
auto svs = sr.initSvs(msg->num_svs());
auto svs_data = *msg->svs();
for (int8_t i = 0; i < msg->num_svs(); i++) {
svs[i].setSvId(svs_data[i]->sv_id());
svs[i].setGnssId(svs_data[i]->gnss_id());
svs[i].setFlagsBitfield(svs_data[i]->flags());
svs[i].setCno(svs_data[i]->cno());
svs[i].setElevationDeg(svs_data[i]->elev());
svs[i].setAzimuthDeg(svs_data[i]->azim());
svs[i].setPseudorangeResidual(svs_data[i]->pr_res() * 0.1);
}
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);
}
system/ubloxd/ublox_msg.h
-131
View File
@@ -1,131 +0,0 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <ctime>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#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;
const int SECS_IN_MIN = 60;
const int SECS_IN_HR = 60 * SECS_IN_MIN;
const int SECS_IN_DAY = 24 * SECS_IN_HR;
const int SECS_IN_WEEK = 7 * SECS_IN_DAY;
// 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(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
inline void reset() {bytes_in_parse_buf = 0;}
inline int needed_bytes();
inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);}
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);
kj::Array<capnp::word> gen_nav_sat(ubx_t::nav_sat_t *msg);
private:
inline bool valid_cheksum();
inline bool valid();
inline bool valid_so_far();
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;
float last_log_time = 0.0;
size_t bytes_in_parse_buf = 0;
uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
// 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, std::unordered_map<int, long>> glonass_string_times;
std::unordered_map<int, std::unordered_map<int, int>> glonass_string_superframes;
};
system/ubloxd/ubloxd.cc
-62
View File
@@ -1,62 +0,0 @@
#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) {
continue;
}
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto ubloxRaw = event.getUbloxRaw();
float log_time = 1e-9 * event.getLogMonoTime();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
size_t bytes_consumed = 0;
while (bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if (parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
try {
auto ublox_msg = parser.gen_msg();
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;
}
system/ubloxd/ubloxd.py
Executable
+519
View File
@@ -0,0 +1,519 @@
#!/usr/bin/env python3
import math
import capnp
import calendar
import numpy as np
from collections import defaultdict
from dataclasses import dataclass
from cereal import log
from cereal import messaging
from openpilot.system.ubloxd.generated.ubx import Ubx
from openpilot.system.ubloxd.generated.gps import Gps
from openpilot.system.ubloxd.generated.glonass import Glonass
SECS_IN_MIN = 60
SECS_IN_HR = 60 * SECS_IN_MIN
SECS_IN_DAY = 24 * SECS_IN_HR
SECS_IN_WEEK = 7 * SECS_IN_DAY
class UbxFramer:
PREAMBLE1 = 0xB5
PREAMBLE2 = 0x62
HEADER_SIZE = 6
CHECKSUM_SIZE = 2
def __init__(self) -> None:
self.buf = bytearray()
self.last_log_time = 0.0
def reset(self) -> None:
self.buf.clear()
@staticmethod
def _checksum_ok(frame: bytes) -> bool:
ck_a = 0
ck_b = 0
for b in frame[2:-2]:
ck_a = (ck_a + b) & 0xFF
ck_b = (ck_b + ck_a) & 0xFF
return ck_a == frame[-2] and ck_b == frame[-1]
def add_data(self, log_time: float, incoming: bytes) -> list[bytes]:
self.last_log_time = log_time
out: list[bytes] = []
if not incoming:
return out
self.buf += incoming
while True:
# find preamble
if len(self.buf) < 2:
break
start = self.buf.find(b"\xB5\x62")
if start < 0:
# no preamble in buffer
self.buf.clear()
break
if start > 0:
# drop garbage before preamble
self.buf = self.buf[start:]
if len(self.buf) < self.HEADER_SIZE:
break
length_le = int.from_bytes(self.buf[4:6], 'little', signed=False)
total_len = self.HEADER_SIZE + length_le + self.CHECKSUM_SIZE
if len(self.buf) < total_len:
break
candidate = bytes(self.buf[:total_len])
if self._checksum_ok(candidate):
out.append(candidate)
# consume this frame
self.buf = self.buf[total_len:]
else:
# drop first byte and retry
self.buf = self.buf[1:]
return out
def _bit(b: int, shift: int) -> bool:
return (b & (1 << shift)) != 0
@dataclass
class EphemerisCaches:
gps_subframes: defaultdict[int, dict[int, bytes]]
glonass_strings: defaultdict[int, dict[int, bytes]]
glonass_string_times: defaultdict[int, dict[int, float]]
glonass_string_superframes: defaultdict[int, dict[int, int]]
class UbloxMsgParser:
gpsPi = 3.1415926535898
# user range accuracy in meters
glonass_URA_lookup: dict[int, float] = {
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,
}
def __init__(self) -> None:
self.framer = UbxFramer()
self.caches = EphemerisCaches(
gps_subframes=defaultdict(dict),
glonass_strings=defaultdict(dict),
glonass_string_times=defaultdict(dict),
glonass_string_superframes=defaultdict(dict),
)
# Message generation entry point
def parse_frame(self, frame: bytes) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
# Quick header parse
msg_type = int.from_bytes(frame[2:4], 'big')
payload = frame[6:-2]
if msg_type == 0x0107:
body = Ubx.NavPvt.from_bytes(payload)
return self._gen_nav_pvt(body)
if msg_type == 0x0213:
# Manually parse RXM-SFRBX to avoid Kaitai EOF on some frames
if len(payload) < 8:
return None
gnss_id = payload[0]
sv_id = payload[1]
freq_id = payload[3]
num_words = payload[4]
exp = 8 + 4 * num_words
if exp != len(payload):
return None
words: list[int] = []
off = 8
for _ in range(num_words):
words.append(int.from_bytes(payload[off:off+4], 'little'))
off += 4
class _SfrbxView:
def __init__(self, gid: int, sid: int, fid: int, body: list[int]):
self.gnss_id = Ubx.GnssType(gid)
self.sv_id = sid
self.freq_id = fid
self.body = body
view = _SfrbxView(gnss_id, sv_id, freq_id, words)
return self._gen_rxm_sfrbx(view)
if msg_type == 0x0215:
body = Ubx.RxmRawx.from_bytes(payload)
return self._gen_rxm_rawx(body)
if msg_type == 0x0A09:
body = Ubx.MonHw.from_bytes(payload)
return self._gen_mon_hw(body)
if msg_type == 0x0A0B:
body = Ubx.MonHw2.from_bytes(payload)
return self._gen_mon_hw2(body)
if msg_type == 0x0135:
body = Ubx.NavSat.from_bytes(payload)
return self._gen_nav_sat(body)
return None
# NAV-PVT -> gpsLocationExternal
def _gen_nav_pvt(self, msg: Ubx.NavPvt) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('gpsLocationExternal', valid=True)
gps = dat.gpsLocationExternal
gps.source = log.GpsLocationData.SensorSource.ublox
gps.flags = msg.flags
gps.hasFix = (msg.flags % 2) == 1
gps.latitude = msg.lat * 1e-07
gps.longitude = msg.lon * 1e-07
gps.altitude = msg.height * 1e-03
gps.speed = msg.g_speed * 1e-03
gps.bearingDeg = msg.head_mot * 1e-5
gps.horizontalAccuracy = msg.h_acc * 1e-03
gps.satelliteCount = msg.num_sv
# build UTC timestamp millis (NAV-PVT is in UTC)
# tolerate invalid or unset date values like C++ timegm
try:
utc_tt = calendar.timegm((msg.year, msg.month, msg.day, msg.hour, msg.min, msg.sec, 0, 0, 0))
except Exception:
utc_tt = 0
gps.unixTimestampMillis = int(utc_tt * 1e3 + (msg.nano * 1e-6))
# match C++ float32 rounding semantics exactly
gps.vNED = [
float(np.float32(msg.vel_n) * np.float32(1e-03)),
float(np.float32(msg.vel_e) * np.float32(1e-03)),
float(np.float32(msg.vel_d) * np.float32(1e-03)),
]
gps.verticalAccuracy = msg.v_acc * 1e-03
gps.speedAccuracy = msg.s_acc * 1e-03
gps.bearingAccuracyDeg = msg.head_acc * 1e-05
return ('gpsLocationExternal', dat)
# RXM-SFRBX dispatch to GPS or GLONASS ephemeris
def _gen_rxm_sfrbx(self, msg) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
if msg.gnss_id == Ubx.GnssType.gps:
return self._parse_gps_ephemeris(msg)
if msg.gnss_id == Ubx.GnssType.glonass:
return self._parse_glonass_ephemeris(msg)
return None
def _parse_gps_ephemeris(self, msg: Ubx.RxmSfrbx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
# body is list of 10 words; convert to 30-byte subframe (strip parity/padding)
body = msg.body
if len(body) != 10:
return None
subframe_data = bytearray()
for word in body:
word >>= 6
subframe_data.append((word >> 16) & 0xFF)
subframe_data.append((word >> 8) & 0xFF)
subframe_data.append(word & 0xFF)
sf = Gps.from_bytes(bytes(subframe_data))
subframe_id = sf.how.subframe_id
if subframe_id < 1 or subframe_id > 3:
return None
self.caches.gps_subframes[msg.sv_id][subframe_id] = bytes(subframe_data)
if len(self.caches.gps_subframes[msg.sv_id]) != 3:
return None
dat = messaging.new_message('ubloxGnss', valid=True)
eph = dat.ubloxGnss.init('ephemeris')
eph.svId = msg.sv_id
iode_s2 = 0
iode_s3 = 0
iodc_lsb = 0
week = 0
# Subframe 1
sf1 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][1])
s1 = sf1.body
assert isinstance(s1, Gps.Subframe1)
week = s1.week_no
week += 1024
if week < 1877:
week += 1024
eph.tgd = s1.t_gd * math.pow(2, -31)
eph.toc = s1.t_oc * math.pow(2, 4)
eph.af2 = s1.af_2 * math.pow(2, -55)
eph.af1 = s1.af_1 * math.pow(2, -43)
eph.af0 = s1.af_0 * math.pow(2, -31)
eph.svHealth = s1.sv_health
eph.towCount = sf1.how.tow_count
iodc_lsb = s1.iodc_lsb
# Subframe 2
sf2 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][2])
s2 = sf2.body
assert isinstance(s2, Gps.Subframe2)
if s2.t_oe == 0 and sf2.how.tow_count * 6 >= (SECS_IN_WEEK - 2 * SECS_IN_HR):
week += 1
eph.crs = s2.c_rs * math.pow(2, -5)
eph.deltaN = s2.delta_n * math.pow(2, -43) * self.gpsPi
eph.m0 = s2.m_0 * math.pow(2, -31) * self.gpsPi
eph.cuc = s2.c_uc * math.pow(2, -29)
eph.ecc = s2.e * math.pow(2, -33)
eph.cus = s2.c_us * math.pow(2, -29)
eph.a = math.pow(s2.sqrt_a * math.pow(2, -19), 2.0)
eph.toe = s2.t_oe * math.pow(2, 4)
iode_s2 = s2.iode
# Subframe 3
sf3 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][3])
s3 = sf3.body
assert isinstance(s3, Gps.Subframe3)
eph.cic = s3.c_ic * math.pow(2, -29)
eph.omega0 = s3.omega_0 * math.pow(2, -31) * self.gpsPi
eph.cis = s3.c_is * math.pow(2, -29)
eph.i0 = s3.i_0 * math.pow(2, -31) * self.gpsPi
eph.crc = s3.c_rc * math.pow(2, -5)
eph.omega = s3.omega * math.pow(2, -31) * self.gpsPi
eph.omegaDot = s3.omega_dot * math.pow(2, -43) * self.gpsPi
eph.iode = s3.iode
eph.iDot = s3.idot * math.pow(2, -43) * self.gpsPi
iode_s3 = s3.iode
eph.toeWeek = week
eph.tocWeek = week
# clear cache for this SV
self.caches.gps_subframes[msg.sv_id].clear()
if not (iodc_lsb == iode_s2 == iode_s3):
return None
return ('ubloxGnss', dat)
def _parse_glonass_ephemeris(self, msg: Ubx.RxmSfrbx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
# words are 4 bytes each; Glonass parser expects 16 bytes (string)
body = msg.body
if len(body) != 4:
return None
string_bytes = bytearray()
for word in body:
for i in (3, 2, 1, 0):
string_bytes.append((word >> (8 * i)) & 0xFF)
gl = Glonass.from_bytes(bytes(string_bytes))
string_number = gl.string_number
if string_number < 1 or string_number > 5 or gl.idle_chip:
return None
# correlate by superframe and timing, similar to C++ logic
freq_id = msg.freq_id
superframe_unknown = False
needs_clear = False
for i in range(1, 6):
if i not in self.caches.glonass_strings[freq_id]:
continue
sf_prev = self.caches.glonass_string_superframes[freq_id].get(i, 0)
if sf_prev == 0 or gl.superframe_number == 0:
superframe_unknown = True
elif sf_prev != gl.superframe_number:
needs_clear = True
if superframe_unknown:
prev_time = self.caches.glonass_string_times[freq_id].get(i, 0.0)
if abs((prev_time - 2.0 * i) - (self.framer.last_log_time - 2.0 * string_number)) > 10:
needs_clear = True
if needs_clear:
self.caches.glonass_strings[freq_id].clear()
self.caches.glonass_string_superframes[freq_id].clear()
self.caches.glonass_string_times[freq_id].clear()
self.caches.glonass_strings[freq_id][string_number] = bytes(string_bytes)
self.caches.glonass_string_superframes[freq_id][string_number] = gl.superframe_number
self.caches.glonass_string_times[freq_id][string_number] = self.framer.last_log_time
if msg.sv_id == 255:
# unknown SV id
return None
if len(self.caches.glonass_strings[freq_id]) != 5:
return None
dat = messaging.new_message('ubloxGnss', valid=True)
eph = dat.ubloxGnss.init('glonassEphemeris')
eph.svId = msg.sv_id
eph.freqNum = msg.freq_id - 7
current_day = 0
tk = 0
# string 1
try:
s1 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][1]).data
except Exception:
return None
assert isinstance(s1, Glonass.String1)
eph.p1 = int(s1.p1)
tk = int(s1.t_k)
eph.tkDEPRECATED = tk
eph.xVel = float(s1.x_vel) * math.pow(2, -20)
eph.xAccel = float(s1.x_accel) * math.pow(2, -30)
eph.x = float(s1.x) * math.pow(2, -11)
# string 2
try:
s2 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][2]).data
except Exception:
return None
assert isinstance(s2, Glonass.String2)
eph.svHealth = int(s2.b_n >> 2)
eph.p2 = int(s2.p2)
eph.tb = int(s2.t_b)
eph.yVel = float(s2.y_vel) * math.pow(2, -20)
eph.yAccel = float(s2.y_accel) * math.pow(2, -30)
eph.y = float(s2.y) * math.pow(2, -11)
# string 3
try:
s3 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][3]).data
except Exception:
return None
assert isinstance(s3, Glonass.String3)
eph.p3 = int(s3.p3)
eph.gammaN = float(s3.gamma_n) * math.pow(2, -40)
eph.svHealth = int(eph.svHealth | (1 if s3.l_n else 0))
eph.zVel = float(s3.z_vel) * math.pow(2, -20)
eph.zAccel = float(s3.z_accel) * math.pow(2, -30)
eph.z = float(s3.z) * math.pow(2, -11)
# string 4
try:
s4 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][4]).data
except Exception:
return None
assert isinstance(s4, Glonass.String4)
current_day = int(s4.n_t)
eph.nt = current_day
eph.tauN = float(s4.tau_n) * math.pow(2, -30)
eph.deltaTauN = float(s4.delta_tau_n) * math.pow(2, -30)
eph.age = int(s4.e_n)
eph.p4 = int(s4.p4)
eph.svURA = float(self.glonass_URA_lookup.get(int(s4.f_t), 0.0))
# consistency check: SV slot number
# if it doesn't match, keep going but note mismatch (no logging here)
eph.svType = int(s4.m)
# string 5
try:
s5 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][5]).data
except Exception:
return None
assert isinstance(s5, Glonass.String5)
eph.n4 = int(s5.n_4)
tk_seconds = int(SECS_IN_HR * ((tk >> 7) & 0x1F) + SECS_IN_MIN * ((tk >> 1) & 0x3F) + (tk & 0x1) * 30)
eph.tkSeconds = tk_seconds
self.caches.glonass_strings[freq_id].clear()
return ('ubloxGnss', dat)
def _gen_rxm_rawx(self, msg: Ubx.RxmRawx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
mr = dat.ubloxGnss.init('measurementReport')
mr.rcvTow = msg.rcv_tow
mr.gpsWeek = msg.week
mr.leapSeconds = msg.leap_s
mb = mr.init('measurements', msg.num_meas)
for i, m in enumerate(msg.meas):
mb[i].svId = m.sv_id
mb[i].pseudorange = m.pr_mes
mb[i].carrierCycles = m.cp_mes
mb[i].doppler = m.do_mes
mb[i].gnssId = int(m.gnss_id.value)
mb[i].glonassFrequencyIndex = m.freq_id
mb[i].locktime = m.lock_time
mb[i].cno = m.cno
mb[i].pseudorangeStdev = 0.01 * (math.pow(2, (m.pr_stdev & 15)))
mb[i].carrierPhaseStdev = 0.004 * (m.cp_stdev & 15)
mb[i].dopplerStdev = 0.002 * (math.pow(2, (m.do_stdev & 15)))
ts = mb[i].init('trackingStatus')
trk = m.trk_stat
ts.pseudorangeValid = _bit(trk, 0)
ts.carrierPhaseValid = _bit(trk, 1)
ts.halfCycleValid = _bit(trk, 2)
ts.halfCycleSubtracted = _bit(trk, 3)
mr.numMeas = msg.num_meas
rs = mr.init('receiverStatus')
rs.leapSecValid = _bit(msg.rec_stat, 0)
rs.clkReset = _bit(msg.rec_stat, 2)
return ('ubloxGnss', dat)
def _gen_nav_sat(self, msg: Ubx.NavSat) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
sr = dat.ubloxGnss.init('satReport')
sr.iTow = msg.itow
svs = sr.init('svs', msg.num_svs)
for i, s in enumerate(msg.svs):
svs[i].svId = s.sv_id
svs[i].gnssId = int(s.gnss_id.value)
svs[i].flagsBitfield = s.flags
svs[i].cno = s.cno
svs[i].elevationDeg = s.elev
svs[i].azimuthDeg = s.azim
svs[i].pseudorangeResidual = s.pr_res * 0.1
return ('ubloxGnss', dat)
def _gen_mon_hw(self, msg: Ubx.MonHw) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
hw = dat.ubloxGnss.init('hwStatus')
hw.noisePerMS = msg.noise_per_ms
hw.flags = msg.flags
hw.agcCnt = msg.agc_cnt
hw.aStatus = int(msg.a_status.value)
hw.aPower = int(msg.a_power.value)
hw.jamInd = msg.jam_ind
return ('ubloxGnss', dat)
def _gen_mon_hw2(self, msg: Ubx.MonHw2) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
hw = dat.ubloxGnss.init('hwStatus2')
hw.ofsI = msg.ofs_i
hw.magI = msg.mag_i
hw.ofsQ = msg.ofs_q
hw.magQ = msg.mag_q
# Map Ubx enum to cereal enum {undefined=0, rom=1, otp=2, configpins=3, flash=4}
cfg_map = {
Ubx.MonHw2.ConfigSource.rom: 1,
Ubx.MonHw2.ConfigSource.otp: 2,
Ubx.MonHw2.ConfigSource.config_pins: 3,
Ubx.MonHw2.ConfigSource.flash: 4,
}
hw.cfgSource = cfg_map.get(msg.cfg_source, 0)
hw.lowLevCfg = msg.low_lev_cfg
hw.postStatus = msg.post_status
return ('ubloxGnss', dat)
def main():
parser = UbloxMsgParser()
pm = messaging.PubMaster(['ubloxGnss', 'gpsLocationExternal'])
sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False)
while True:
msg = messaging.recv_one_or_none(sock)
if msg is None:
continue
data = bytes(msg.ubloxRaw)
log_time = msg.logMonoTime * 1e-9
frames = parser.framer.add_data(log_time, data)
for frame in frames:
try:
res = parser.parse_frame(frame)
except Exception:
continue
if not res:
continue
service, dat = res
pm.send(service, dat)
if __name__ == '__main__':
main()
third_party/SConscript
Vendored
-1
View File
@@ -1,4 +1,3 @@
Import('env')
env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call'])
env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE'])
third_party/kaitai/custom_decoder.h
Vendored
-16
View File
@@ -1,16 +0,0 @@
#ifndef KAITAI_CUSTOM_DECODER_H
#define KAITAI_CUSTOM_DECODER_H
#include <string>
namespace kaitai {
class custom_decoder {
public:
virtual ~custom_decoder() {};
virtual std::string decode(std::string src) = 0;
};
}
#endif
third_party/kaitai/exceptions.h
Vendored
-189
View File
@@ -1,189 +0,0 @@
#ifndef KAITAI_EXCEPTIONS_H
#define KAITAI_EXCEPTIONS_H
#include <kaitai/kaitaistream.h>
#include <string>
#include <stdexcept>
// We need to use "noexcept" in virtual destructor of our exceptions
// subclasses. Different compilers have different ideas on how to
// achieve that: C++98 compilers prefer `throw()`, C++11 and later
// use `noexcept`. We define KS_NOEXCEPT macro for that.
#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900)
#define KS_NOEXCEPT noexcept
#else
#define KS_NOEXCEPT throw()
#endif
namespace kaitai {
/**
* Common ancestor for all error originating from Kaitai Struct usage.
* Stores KSY source path, pointing to an element supposedly guilty of
* an error.
*/
class kstruct_error: public std::runtime_error {
public:
kstruct_error(const std::string what, const std::string src_path):
std::runtime_error(src_path + ": " + what),
m_src_path(src_path)
{
}
virtual ~kstruct_error() KS_NOEXCEPT {};
protected:
const std::string m_src_path;
};
/**
* Error that occurs when default endianness should be decided with
* a switch, but nothing matches (although using endianness expression
* implies that there should be some positive result).
*/
class undecided_endianness_error: public kstruct_error {
public:
undecided_endianness_error(const std::string src_path):
kstruct_error("unable to decide on endianness for a type", src_path)
{
}
virtual ~undecided_endianness_error() KS_NOEXCEPT {};
};
/**
* Common ancestor for all validation failures. Stores pointer to
* KaitaiStream IO object which was involved in an error.
*/
class validation_failed_error: public kstruct_error {
public:
validation_failed_error(const std::string what, kstream* io, const std::string src_path):
kstruct_error("at pos " + kstream::to_string(static_cast<int>(io->pos())) + ": validation failed: " + what, src_path),
m_io(io)
{
}
// "at pos #{io.pos}: validation failed: #{msg}"
virtual ~validation_failed_error() KS_NOEXCEPT {};
protected:
kstream* m_io;
};
/**
* Signals validation failure: we required "actual" value to be equal to
* "expected", but it turned out that it's not.
*/
template<typename T>
class validation_not_equal_error: public validation_failed_error {
public:
validation_not_equal_error<T>(const T& expected, const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not equal", io, src_path),
m_expected(expected),
m_actual(actual)
{
}
// "not equal, expected #{expected.inspect}, but got #{actual.inspect}"
virtual ~validation_not_equal_error<T>() KS_NOEXCEPT {};
protected:
const T& m_expected;
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to be greater
* than or equal to "min", but it turned out that it's not.
*/
template<typename T>
class validation_less_than_error: public validation_failed_error {
public:
validation_less_than_error<T>(const T& min, const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not in range", io, src_path),
m_min(min),
m_actual(actual)
{
}
// "not in range, min #{min.inspect}, but got #{actual.inspect}"
virtual ~validation_less_than_error<T>() KS_NOEXCEPT {};
protected:
const T& m_min;
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to be less
* than or equal to "max", but it turned out that it's not.
*/
template<typename T>
class validation_greater_than_error: public validation_failed_error {
public:
validation_greater_than_error<T>(const T& max, const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not in range", io, src_path),
m_max(max),
m_actual(actual)
{
}
// "not in range, max #{max.inspect}, but got #{actual.inspect}"
virtual ~validation_greater_than_error<T>() KS_NOEXCEPT {};
protected:
const T& m_max;
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to be from
* the list, but it turned out that it's not.
*/
template<typename T>
class validation_not_any_of_error: public validation_failed_error {
public:
validation_not_any_of_error<T>(const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not any of the list", io, src_path),
m_actual(actual)
{
}
// "not any of the list, got #{actual.inspect}"
virtual ~validation_not_any_of_error<T>() KS_NOEXCEPT {};
protected:
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to match
* the expression, but it turned out that it doesn't.
*/
template<typename T>
class validation_expr_error: public validation_failed_error {
public:
validation_expr_error<T>(const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not matching the expression", io, src_path),
m_actual(actual)
{
}
// "not matching the expression, got #{actual.inspect}"
virtual ~validation_expr_error<T>() KS_NOEXCEPT {};
protected:
const T& m_actual;
};
}
#endif
third_party/kaitai/kaitaistream.cpp
Vendored
-689
View File
@@ -1,689 +0,0 @@
#include <kaitai/kaitaistream.h>
#if defined(__APPLE__)
#include <machine/endian.h>
#include <libkern/OSByteOrder.h>
#define bswap_16(x) OSSwapInt16(x)
#define bswap_32(x) OSSwapInt32(x)
#define bswap_64(x) OSSwapInt64(x)
#define __BYTE_ORDER BYTE_ORDER
#define __BIG_ENDIAN BIG_ENDIAN
#define __LITTLE_ENDIAN LITTLE_ENDIAN
#elif defined(_MSC_VER) // !__APPLE__
#include <stdlib.h>
#define __LITTLE_ENDIAN 1234
#define __BIG_ENDIAN 4321
#define __BYTE_ORDER __LITTLE_ENDIAN
#define bswap_16(x) _byteswap_ushort(x)
#define bswap_32(x) _byteswap_ulong(x)
#define bswap_64(x) _byteswap_uint64(x)
#else // !__APPLE__ or !_MSC_VER
#include <endian.h>
#include <byteswap.h>
#endif
#include <iostream>
#include <vector>
#include <stdexcept>
kaitai::kstream::kstream(std::istream* io) {
m_io = io;
init();
}
kaitai::kstream::kstream(std::string& data): m_io_str(data) {
m_io = &m_io_str;
init();
}
void kaitai::kstream::init() {
exceptions_enable();
align_to_byte();
}
void kaitai::kstream::close() {
// m_io->close();
}
void kaitai::kstream::exceptions_enable() const {
m_io->exceptions(
std::istream::eofbit |
std::istream::failbit |
std::istream::badbit
);
}
// ========================================================================
// Stream positioning
// ========================================================================
bool kaitai::kstream::is_eof() const {
if (m_bits_left > 0) {
return false;
}
char t;
m_io->exceptions(
std::istream::badbit
);
m_io->get(t);
if (m_io->eof()) {
m_io->clear();
exceptions_enable();
return true;
} else {
m_io->unget();
exceptions_enable();
return false;
}
}
void kaitai::kstream::seek(uint64_t pos) {
m_io->seekg(pos);
}
uint64_t kaitai::kstream::pos() {
return m_io->tellg();
}
uint64_t kaitai::kstream::size() {
std::iostream::pos_type cur_pos = m_io->tellg();
m_io->seekg(0, std::ios::end);
std::iostream::pos_type len = m_io->tellg();
m_io->seekg(cur_pos);
return len;
}
// ========================================================================
// Integer numbers
// ========================================================================
// ------------------------------------------------------------------------
// Signed
// ------------------------------------------------------------------------
int8_t kaitai::kstream::read_s1() {
char t;
m_io->get(t);
return t;
}
// ........................................................................
// Big-endian
// ........................................................................
int16_t kaitai::kstream::read_s2be() {
int16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_16(t);
#endif
return t;
}
int32_t kaitai::kstream::read_s4be() {
int32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_32(t);
#endif
return t;
}
int64_t kaitai::kstream::read_s8be() {
int64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ........................................................................
// Little-endian
// ........................................................................
int16_t kaitai::kstream::read_s2le() {
int16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_16(t);
#endif
return t;
}
int32_t kaitai::kstream::read_s4le() {
int32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_32(t);
#endif
return t;
}
int64_t kaitai::kstream::read_s8le() {
int64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ------------------------------------------------------------------------
// Unsigned
// ------------------------------------------------------------------------
uint8_t kaitai::kstream::read_u1() {
char t;
m_io->get(t);
return t;
}
// ........................................................................
// Big-endian
// ........................................................................
uint16_t kaitai::kstream::read_u2be() {
uint16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_16(t);
#endif
return t;
}
uint32_t kaitai::kstream::read_u4be() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_32(t);
#endif
return t;
}
uint64_t kaitai::kstream::read_u8be() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ........................................................................
// Little-endian
// ........................................................................
uint16_t kaitai::kstream::read_u2le() {
uint16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_16(t);
#endif
return t;
}
uint32_t kaitai::kstream::read_u4le() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_32(t);
#endif
return t;
}
uint64_t kaitai::kstream::read_u8le() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ========================================================================
// Floating point numbers
// ========================================================================
// ........................................................................
// Big-endian
// ........................................................................
float kaitai::kstream::read_f4be() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_32(t);
#endif
return reinterpret_cast<float&>(t);
}
double kaitai::kstream::read_f8be() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_64(t);
#endif
return reinterpret_cast<double&>(t);
}
// ........................................................................
// Little-endian
// ........................................................................
float kaitai::kstream::read_f4le() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_32(t);
#endif
return reinterpret_cast<float&>(t);
}
double kaitai::kstream::read_f8le() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_64(t);
#endif
return reinterpret_cast<double&>(t);
}
// ========================================================================
// Unaligned bit values
// ========================================================================
void kaitai::kstream::align_to_byte() {
m_bits_left = 0;
m_bits = 0;
}
uint64_t kaitai::kstream::read_bits_int_be(int n) {
int bits_needed = n - m_bits_left;
if (bits_needed > 0) {
// 1 bit => 1 byte
// 8 bits => 1 byte
// 9 bits => 2 bytes
int bytes_needed = ((bits_needed - 1) / 8) + 1;
if (bytes_needed > 8)
throw std::runtime_error("read_bits_int: more than 8 bytes requested");
char buf[8];
m_io->read(buf, bytes_needed);
for (int i = 0; i < bytes_needed; i++) {
uint8_t b = buf[i];
m_bits <<= 8;
m_bits |= b;
m_bits_left += 8;
}
}
// raw mask with required number of 1s, starting from lowest bit
uint64_t mask = get_mask_ones(n);
// shift mask to align with highest bits available in @bits
int shift_bits = m_bits_left - n;
mask <<= shift_bits;
// derive reading result
uint64_t res = (m_bits & mask) >> shift_bits;
// clear top bits that we've just read => AND with 1s
m_bits_left -= n;
mask = get_mask_ones(m_bits_left);
m_bits &= mask;
return res;
}
// Deprecated, use read_bits_int_be() instead.
uint64_t kaitai::kstream::read_bits_int(int n) {
return read_bits_int_be(n);
}
uint64_t kaitai::kstream::read_bits_int_le(int n) {
int bits_needed = n - m_bits_left;
if (bits_needed > 0) {
// 1 bit => 1 byte
// 8 bits => 1 byte
// 9 bits => 2 bytes
int bytes_needed = ((bits_needed - 1) / 8) + 1;
if (bytes_needed > 8)
throw std::runtime_error("read_bits_int_le: more than 8 bytes requested");
char buf[8];
m_io->read(buf, bytes_needed);
for (int i = 0; i < bytes_needed; i++) {
uint8_t b = buf[i];
m_bits |= (static_cast<uint64_t>(b) << m_bits_left);
m_bits_left += 8;
}
}
// raw mask with required number of 1s, starting from lowest bit
uint64_t mask = get_mask_ones(n);
// derive reading result
uint64_t res = m_bits & mask;
// remove bottom bits that we've just read by shifting
m_bits >>= n;
m_bits_left -= n;
return res;
}
uint64_t kaitai::kstream::get_mask_ones(int n) {
if (n == 64) {
return 0xFFFFFFFFFFFFFFFF;
} else {
return ((uint64_t) 1 << n) - 1;
}
}
// ========================================================================
// Byte arrays
// ========================================================================
std::string kaitai::kstream::read_bytes(std::streamsize len) {
std::vector<char> result(len);
// NOTE: streamsize type is signed, negative values are only *supposed* to not be used.
// http://en.cppreference.com/w/cpp/io/streamsize
if (len < 0) {
throw std::runtime_error("read_bytes: requested a negative amount");
}
if (len > 0) {
m_io->read(&result[0], len);
}
return std::string(result.begin(), result.end());
}
std::string kaitai::kstream::read_bytes_full() {
std::iostream::pos_type p1 = m_io->tellg();
m_io->seekg(0, std::ios::end);
std::iostream::pos_type p2 = m_io->tellg();
size_t len = p2 - p1;
// Note: this requires a std::string to be backed with a
// contiguous buffer. Officially, it's a only requirement since
// C++11 (C++98 and C++03 didn't have this requirement), but all
// major implementations had contiguous buffers anyway.
std::string result(len, ' ');
m_io->seekg(p1);
m_io->read(&result[0], len);
return result;
}
std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) {
std::string result;
std::getline(*m_io, result, term);
if (m_io->eof()) {
// encountered EOF
if (eos_error) {
throw std::runtime_error("read_bytes_term: encountered EOF");
}
} else {
// encountered terminator
if (include)
result.push_back(term);
if (!consume)
m_io->unget();
}
return result;
}
std::string kaitai::kstream::ensure_fixed_contents(std::string expected) {
std::string actual = read_bytes(expected.length());
if (actual != expected) {
// NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr.
throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data");
}
return actual;
}
std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) {
std::size_t new_len = src.length();
while (new_len > 0 && src[new_len - 1] == pad_byte)
new_len--;
return src.substr(0, new_len);
}
std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) {
std::size_t new_len = 0;
std::size_t max_len = src.length();
while (new_len < max_len && src[new_len] != term)
new_len++;
if (include && new_len < max_len)
new_len++;
return src.substr(0, new_len);
}
// ========================================================================
// Byte array processing
// ========================================================================
std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) {
size_t len = data.length();
std::string result(len, ' ');
for (size_t i = 0; i < len; i++)
result[i] = data[i] ^ key;
return result;
}
std::string kaitai::kstream::process_xor_many(std::string data, std::string key) {
size_t len = data.length();
size_t kl = key.length();
std::string result(len, ' ');
size_t ki = 0;
for (size_t i = 0; i < len; i++) {
result[i] = data[i] ^ key[ki];
ki++;
if (ki >= kl)
ki = 0;
}
return result;
}
std::string kaitai::kstream::process_rotate_left(std::string data, int amount) {
size_t len = data.length();
std::string result(len, ' ');
for (size_t i = 0; i < len; i++) {
uint8_t bits = data[i];
result[i] = (bits << amount) | (bits >> (8 - amount));
}
return result;
}
#ifdef KS_ZLIB
#include <zlib.h>
std::string kaitai::kstream::process_zlib(std::string data) {
int ret;
unsigned char *src_ptr = reinterpret_cast<unsigned char*>(&data[0]);
std::stringstream dst_strm;
z_stream strm;
strm.zalloc = Z_NULL;
strm.zfree = Z_NULL;
strm.opaque = Z_NULL;
ret = inflateInit(&strm);
if (ret != Z_OK)
throw std::runtime_error("process_zlib: inflateInit error");
strm.next_in = src_ptr;
strm.avail_in = data.length();
unsigned char outbuffer[ZLIB_BUF_SIZE];
std::string outstring;
// get the decompressed bytes blockwise using repeated calls to inflate
do {
strm.next_out = reinterpret_cast<Bytef*>(outbuffer);
strm.avail_out = sizeof(outbuffer);
ret = inflate(&strm, 0);
if (outstring.size() < strm.total_out)
outstring.append(reinterpret_cast<char*>(outbuffer), strm.total_out - outstring.size());
} while (ret == Z_OK);
if (ret != Z_STREAM_END) { // an error occurred that was not EOF
std::ostringstream exc_msg;
exc_msg << "process_zlib: error #" << ret << "): " << strm.msg;
throw std::runtime_error(exc_msg.str());
}
if (inflateEnd(&strm) != Z_OK)
throw std::runtime_error("process_zlib: inflateEnd error");
return outstring;
}
#endif
// ========================================================================
// Misc utility methods
// ========================================================================
int kaitai::kstream::mod(int a, int b) {
if (b <= 0)
throw std::invalid_argument("mod: divisor b <= 0");
int r = a % b;
if (r < 0)
r += b;
return r;
}
#include <stdio.h>
std::string kaitai::kstream::to_string(int val) {
// if int is 32 bits, "-2147483648" is the longest string representation
// => 11 chars + zero => 12 chars
// if int is 64 bits, "-9223372036854775808" is the longest
// => 20 chars + zero => 21 chars
char buf[25];
int got_len = snprintf(buf, sizeof(buf), "%d", val);
// should never happen, but check nonetheless
if (got_len > sizeof(buf))
throw std::invalid_argument("to_string: integer is longer than string buffer");
return std::string(buf);
}
#include <algorithm>
std::string kaitai::kstream::reverse(std::string val) {
std::reverse(val.begin(), val.end());
return val;
}
uint8_t kaitai::kstream::byte_array_min(const std::string val) {
uint8_t min = 0xff; // UINT8_MAX
std::string::const_iterator end = val.end();
for (std::string::const_iterator it = val.begin(); it != end; ++it) {
uint8_t cur = static_cast<uint8_t>(*it);
if (cur < min) {
min = cur;
}
}
return min;
}
uint8_t kaitai::kstream::byte_array_max(const std::string val) {
uint8_t max = 0; // UINT8_MIN
std::string::const_iterator end = val.end();
for (std::string::const_iterator it = val.begin(); it != end; ++it) {
uint8_t cur = static_cast<uint8_t>(*it);
if (cur > max) {
max = cur;
}
}
return max;
}
// ========================================================================
// Other internal methods
// ========================================================================
#ifndef KS_STR_DEFAULT_ENCODING
#define KS_STR_DEFAULT_ENCODING "UTF-8"
#endif
#ifdef KS_STR_ENCODING_ICONV
#include <iconv.h>
#include <cerrno>
#include <stdexcept>
std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str());
if (cd == (iconv_t) -1) {
if (errno == EINVAL) {
throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested");
} else {
throw std::runtime_error("bytes_to_str: error opening iconv");
}
}
size_t src_len = src.length();
size_t src_left = src_len;
// Start with a buffer length of double the source length.
size_t dst_len = src_len * 2;
std::string dst(dst_len, ' ');
size_t dst_left = dst_len;
char *src_ptr = &src[0];
char *dst_ptr = &dst[0];
while (true) {
size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left);
if (res == (size_t) -1) {
if (errno == E2BIG) {
// dst buffer is not enough to accomodate whole string
// enlarge the buffer and try again
size_t dst_used = dst_len - dst_left;
dst_left += dst_len;
dst_len += dst_len;
dst.resize(dst_len);
// dst.resize might have allocated destination buffer in another area
// of memory, thus our previous pointer "dst" will be invalid; re-point
// it using "dst_used".
dst_ptr = &dst[dst_used];
} else {
throw std::runtime_error("bytes_to_str: iconv error");
}
} else {
// conversion successful
dst.resize(dst_len - dst_left);
break;
}
}
if (iconv_close(cd) != 0) {
throw std::runtime_error("bytes_to_str: iconv close error");
}
return dst;
}
#elif defined(KS_STR_ENCODING_NONE)
std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
return src;
}
#else
#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE
#endif
third_party/kaitai/kaitaistream.h
Vendored
-268
View File
@@ -1,268 +0,0 @@
#ifndef KAITAI_STREAM_H
#define KAITAI_STREAM_H
// Kaitai Struct runtime API version: x.y.z = 'xxxyyyzzz' decimal
#define KAITAI_STRUCT_VERSION 9000L
#include <istream>
#include <sstream>
#include <stdint.h>
#include <sys/types.h>
namespace kaitai {
/**
* Kaitai Stream class (kaitai::kstream) is an implementation of
* <a href="https://doc.kaitai.io/stream_api.html">Kaitai Struct stream API</a>
* for C++/STL. It's implemented as a wrapper over generic STL std::istream.
*
* It provides a wide variety of simple methods to read (parse) binary
* representations of primitive types, such as integer and floating
* point numbers, byte arrays and strings, and also provides stream
* positioning / navigation methods with unified cross-language and
* cross-toolkit semantics.
*
* Typically, end users won't access Kaitai Stream class manually, but would
* describe a binary structure format using .ksy language and then would use
* Kaitai Struct compiler to generate source code in desired target language.
* That code, in turn, would use this class and API to do the actual parsing
* job.
*/
class kstream {
public:
/**
* Constructs new Kaitai Stream object, wrapping a given std::istream.
* \param io istream object to use for this Kaitai Stream
*/
kstream(std::istream* io);
/**
* Constructs new Kaitai Stream object, wrapping a given in-memory data
* buffer.
* \param data data buffer to use for this Kaitai Stream
*/
kstream(std::string& data);
void close();
/** @name Stream positioning */
//@{
/**
* Check if stream pointer is at the end of stream. Note that the semantics
* are different from traditional STL semantics: one does *not* need to do a
* read (which will fail) after the actual end of the stream to trigger EOF
* flag, which can be accessed after that read. It is sufficient to just be
* at the end of the stream for this method to return true.
* \return "true" if we are located at the end of the stream.
*/
bool is_eof() const;
/**
* Set stream pointer to designated position.
* \param pos new position (offset in bytes from the beginning of the stream)
*/
void seek(uint64_t pos);
/**
* Get current position of a stream pointer.
* \return pointer position, number of bytes from the beginning of the stream
*/
uint64_t pos();
/**
* Get total size of the stream in bytes.
* \return size of the stream in bytes
*/
uint64_t size();
//@}
/** @name Integer numbers */
//@{
// ------------------------------------------------------------------------
// Signed
// ------------------------------------------------------------------------
int8_t read_s1();
// ........................................................................
// Big-endian
// ........................................................................
int16_t read_s2be();
int32_t read_s4be();
int64_t read_s8be();
// ........................................................................
// Little-endian
// ........................................................................
int16_t read_s2le();
int32_t read_s4le();
int64_t read_s8le();
// ------------------------------------------------------------------------
// Unsigned
// ------------------------------------------------------------------------
uint8_t read_u1();
// ........................................................................
// Big-endian
// ........................................................................
uint16_t read_u2be();
uint32_t read_u4be();
uint64_t read_u8be();
// ........................................................................
// Little-endian
// ........................................................................
uint16_t read_u2le();
uint32_t read_u4le();
uint64_t read_u8le();
//@}
/** @name Floating point numbers */
//@{
// ........................................................................
// Big-endian
// ........................................................................
float read_f4be();
double read_f8be();
// ........................................................................
// Little-endian
// ........................................................................
float read_f4le();
double read_f8le();
//@}
/** @name Unaligned bit values */
//@{
void align_to_byte();
uint64_t read_bits_int_be(int n);
uint64_t read_bits_int(int n);
uint64_t read_bits_int_le(int n);
//@}
/** @name Byte arrays */
//@{
std::string read_bytes(std::streamsize len);
std::string read_bytes_full();
std::string read_bytes_term(char term, bool include, bool consume, bool eos_error);
std::string ensure_fixed_contents(std::string expected);
static std::string bytes_strip_right(std::string src, char pad_byte);
static std::string bytes_terminate(std::string src, char term, bool include);
static std::string bytes_to_str(std::string src, std::string src_enc);
//@}
/** @name Byte array processing */
//@{
/**
* Performs a XOR processing with given data, XORing every byte of input with a single
* given value.
* @param data data to process
* @param key value to XOR with
* @return processed data
*/
static std::string process_xor_one(std::string data, uint8_t key);
/**
* Performs a XOR processing with given data, XORing every byte of input with a key
* array, repeating key array many times, if necessary (i.e. if data array is longer
* than key array).
* @param data data to process
* @param key array of bytes to XOR with
* @return processed data
*/
static std::string process_xor_many(std::string data, std::string key);
/**
* Performs a circular left rotation shift for a given buffer by a given amount of bits,
* using groups of 1 bytes each time. Right circular rotation should be performed
* using this procedure with corrected amount.
* @param data source data to process
* @param amount number of bits to shift by
* @return copy of source array with requested shift applied
*/
static std::string process_rotate_left(std::string data, int amount);
#ifdef KS_ZLIB
/**
* Performs an unpacking ("inflation") of zlib-compressed data with usual zlib headers.
* @param data data to unpack
* @return unpacked data
* @throws IOException
*/
static std::string process_zlib(std::string data);
#endif
//@}
/**
* Performs modulo operation between two integers: dividend `a`
* and divisor `b`. Divisor `b` is expected to be positive. The
* result is always 0 <= x <= b - 1.
*/
static int mod(int a, int b);
/**
* Converts given integer `val` to a decimal string representation.
* Should be used in place of std::to_string() (which is available only
* since C++11) in older C++ implementations.
*/
static std::string to_string(int val);
/**
* Reverses given string `val`, so that the first character becomes the
* last and the last one becomes the first. This should be used to avoid
* the need of local variables at the caller.
*/
static std::string reverse(std::string val);
/**
* Finds the minimal byte in a byte array, treating bytes as
* unsigned values.
* @param val byte array to scan
* @return minimal byte in byte array as integer
*/
static uint8_t byte_array_min(const std::string val);
/**
* Finds the maximal byte in a byte array, treating bytes as
* unsigned values.
* @param val byte array to scan
* @return maximal byte in byte array as integer
*/
static uint8_t byte_array_max(const std::string val);
private:
std::istream* m_io;
std::istringstream m_io_str;
int m_bits_left;
uint64_t m_bits;
void init();
void exceptions_enable() const;
static uint64_t get_mask_ones(int n);
static const int ZLIB_BUF_SIZE = 128 * 1024;
};
}
#endif
third_party/kaitai/kaitaistruct.h
Vendored
-20
View File
@@ -1,20 +0,0 @@
#ifndef KAITAI_STRUCT_H
#define KAITAI_STRUCT_H
#include <kaitai/kaitaistream.h>
namespace kaitai {
class kstruct {
public:
kstruct(kstream *_io) { m__io = _io; }
virtual ~kstruct() {}
protected:
kstream *m__io;
public:
kstream *_io() { return m__io; }
};
}
#endif