Ubloxd: glonass parsing tests (#27125)
* add glonass kaitai parsing * add kaita generated files * remove glonass from build * add string non immediate type * fix kaitai bug * cleanUp * add patch file * fix scons order * make lookup const * remove comments * rename * add to release files * remove printf * add signs * add glonass parsing test * finish up glonass kaitai tests * move cereal back to master * ignore test_runner * sign is not two complement * address comments --------- Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>pull/27161/head
Kurt Nistelberger
2 years ago
committed by
GitHub
parent
fba4827a7b
commit
e2f5b164bd
10 changed files with 796 additions and 67 deletions
@ -0,0 +1,360 @@ |
||||
#include <iostream> |
||||
#include <vector> |
||||
#include <bitset> |
||||
#include <cassert> |
||||
#include <cstdlib> |
||||
#include <ctime> |
||||
|
||||
#include "catch2/catch.hpp" |
||||
#include "selfdrive/locationd/generated/glonass.h" |
||||
|
||||
typedef std::vector<std::pair<int, int64_t>> string_data; |
||||
|
||||
#define IDLE_CHIP_IDX 0 |
||||
#define STRING_NUMBER_IDX 1 |
||||
// string data 1-5
|
||||
#define HC_IDX 0 |
||||
#define PAD1_IDX 1 |
||||
#define SUPERFRAME_IDX 2 |
||||
#define PAD2_IDX 3 |
||||
#define FRAME_IDX 4 |
||||
|
||||
// Indexes for string number 1
|
||||
#define ST1_NU_IDX 2 |
||||
#define ST1_P1_IDX 3 |
||||
#define ST1_T_K_IDX 4 |
||||
#define ST1_X_VEL_S_IDX 5 |
||||
#define ST1_X_VEL_V_IDX 6 |
||||
#define ST1_X_ACCEL_S_IDX 7 |
||||
#define ST1_X_ACCEL_V_IDX 8 |
||||
#define ST1_X_S_IDX 9 |
||||
#define ST1_X_V_IDX 10 |
||||
#define ST1_HC_OFF 11 |
||||
|
||||
// Indexes for string number 2
|
||||
#define ST2_BN_IDX 2 |
||||
#define ST2_P2_IDX 3 |
||||
#define ST2_TB_IDX 4 |
||||
#define ST2_NU_IDX 5 |
||||
#define ST2_Y_VEL_S_IDX 6 |
||||
#define ST2_Y_VEL_V_IDX 7 |
||||
#define ST2_Y_ACCEL_S_IDX 8 |
||||
#define ST2_Y_ACCEL_V_IDX 9 |
||||
#define ST2_Y_S_IDX 10 |
||||
#define ST2_Y_V_IDX 11 |
||||
#define ST2_HC_OFF 12 |
||||
|
||||
// Indexes for string number 3
|
||||
#define ST3_P3_IDX 2 |
||||
#define ST3_GAMMA_N_S_IDX 3 |
||||
#define ST3_GAMMA_N_V_IDX 4 |
||||
#define ST3_NU_1_IDX 5 |
||||
#define ST3_P_IDX 6 |
||||
#define ST3_L_N_IDX 7 |
||||
#define ST3_Z_VEL_S_IDX 8 |
||||
#define ST3_Z_VEL_V_IDX 9 |
||||
#define ST3_Z_ACCEL_S_IDX 10 |
||||
#define ST3_Z_ACCEL_V_IDX 11 |
||||
#define ST3_Z_S_IDX 12 |
||||
#define ST3_Z_V_IDX 13 |
||||
#define ST3_HC_OFF 14 |
||||
|
||||
// Indexes for string number 4
|
||||
#define ST4_TAU_N_S_IDX 2 |
||||
#define ST4_TAU_N_V_IDX 3 |
||||
#define ST4_DELTA_TAU_N_S_IDX 4 |
||||
#define ST4_DELTA_TAU_N_V_IDX 5 |
||||
#define ST4_E_N_IDX 6 |
||||
#define ST4_NU_1_IDX 7 |
||||
#define ST4_P4_IDX 8 |
||||
#define ST4_F_T_IDX 9 |
||||
#define ST4_NU_2_IDX 10 |
||||
#define ST4_N_T_IDX 11 |
||||
#define ST4_N_IDX 12 |
||||
#define ST4_M_IDX 13 |
||||
#define ST4_HC_OFF 14 |
||||
|
||||
// Indexes for string number 5
|
||||
#define ST5_N_A_IDX 2 |
||||
#define ST5_TAU_C_IDX 3 |
||||
#define ST5_NU_IDX 4 |
||||
#define ST5_N_4_IDX 5 |
||||
#define ST5_TAU_GPS_IDX 6 |
||||
#define ST5_L_N_IDX 7 |
||||
#define ST5_HC_OFF 8 |
||||
|
||||
// Indexes for non immediate
|
||||
#define ST6_DATA_1_IDX 2 |
||||
#define ST6_DATA_2_IDX 3 |
||||
#define ST6_HC_OFF 4 |
||||
|
||||
|
||||
std::string generate_inp_data(string_data& data) { |
||||
std::string inp_data = ""; |
||||
for (auto& [b, v] : data) { |
||||
std::string tmp = std::bitset<64>(v).to_string(); |
||||
inp_data += tmp.substr(64-b, b); |
||||
} |
||||
assert(inp_data.size() == 128); |
||||
|
||||
std::string string_data; |
||||
string_data.reserve(16); |
||||
for (int i = 0; i < 128; i+=8) { |
||||
std::string substr = inp_data.substr(i, 8); |
||||
string_data.push_back( (uint8_t)std::stoi(substr.c_str(), 0, 2)); |
||||
} |
||||
|
||||
return string_data; |
||||
} |
||||
|
||||
string_data generate_string_data(uint8_t string_number) { |
||||
|
||||
srand((unsigned)time(0)); |
||||
string_data data; //<bit length, value>
|
||||
data.push_back({1, 0}); // idle chip
|
||||
data.push_back({4, string_number}); // string number
|
||||
|
||||
if (string_number == 1) { |
||||
data.push_back({2, 3}); // not_used
|
||||
data.push_back({2, 1}); // p1
|
||||
data.push_back({12, 113}); // t_k
|
||||
data.push_back({1, rand() & 1}); // x_vel_sign
|
||||
data.push_back({23, 7122}); // x_vel_value
|
||||
data.push_back({1, rand() & 1}); // x_accel_sign
|
||||
data.push_back({4, 3}); // x_accel_value
|
||||
data.push_back({1, rand() & 1}); // x_sign
|
||||
data.push_back({26, 33554431}); // x_value
|
||||
} else if (string_number == 2) { |
||||
data.push_back({3, 3}); // b_n
|
||||
data.push_back({1, 1}); // p2
|
||||
data.push_back({7, 123}); // t_b
|
||||
data.push_back({5, 31}); // not_used
|
||||
data.push_back({1, rand() & 1}); // y_vel_sign
|
||||
data.push_back({23, 7422}); // y_vel_value
|
||||
data.push_back({1, rand() & 1}); // y_accel_sign
|
||||
data.push_back({4, 3}); // y_accel_value
|
||||
data.push_back({1, rand() & 1}); // y_sign
|
||||
data.push_back({26, 67108863}); // y_value
|
||||
} else if (string_number == 3) { |
||||
data.push_back({1, 0}); // p3
|
||||
data.push_back({1, 1}); // gamma_n_sign
|
||||
data.push_back({10, 123}); // gamma_n_value
|
||||
data.push_back({1, 0}); // not_used
|
||||
data.push_back({2, 2}); // p
|
||||
data.push_back({1, 1}); // l_n
|
||||
data.push_back({1, rand() & 1}); // z_vel_sign
|
||||
data.push_back({23, 1337}); // z_vel_value
|
||||
data.push_back({1, rand() & 1}); // z_accel_sign
|
||||
data.push_back({4, 9}); // z_accel_value
|
||||
data.push_back({1, rand() & 1}); // z_sign
|
||||
data.push_back({26, 100023}); // z_value
|
||||
} else if (string_number == 4) { |
||||
data.push_back({1, rand() & 1}); // tau_n_sign
|
||||
data.push_back({21, 197152}); // tau_n_value
|
||||
data.push_back({1, rand() & 1}); // delta_tau_n_sign
|
||||
data.push_back({4, 4}); // delta_tau_n_value
|
||||
data.push_back({5, 0}); // e_n
|
||||
data.push_back({14, 2}); // not_used_1
|
||||
data.push_back({1, 1}); // p4
|
||||
data.push_back({4, 9}); // f_t
|
||||
data.push_back({3, 3}); // not_used_2
|
||||
data.push_back({11, 2047}); // n_t
|
||||
data.push_back({5, 2}); // n
|
||||
data.push_back({2, 1}); // m
|
||||
} else if (string_number == 5) { |
||||
data.push_back({11, 2047}); // n_a
|
||||
data.push_back({32, 4294767295}); // tau_c
|
||||
data.push_back({1, 0}); // not_used_1
|
||||
data.push_back({5, 2}); // n_4
|
||||
data.push_back({22, 4114304}); // tau_gps
|
||||
data.push_back({1, 0}); // l_n
|
||||
} else { // non-immediate data is not parsed
|
||||
data.push_back({64, rand()}); // data_1
|
||||
data.push_back({8, 6}); // data_2
|
||||
} |
||||
|
||||
data.push_back({8, rand() & 0xFF}); // hamming code
|
||||
data.push_back({11, rand() & 0x7FF}); // pad
|
||||
data.push_back({16, rand() & 0xFFFF}); // superframe
|
||||
data.push_back({8, rand() & 0xFF}); // pad
|
||||
data.push_back({8, rand() & 0xFF}); // frame
|
||||
return data; |
||||
} |
||||
|
||||
TEST_CASE("parse_string_number_1"){ |
||||
string_data data = generate_string_data(1); |
||||
std::string inp_data = generate_inp_data(data); |
||||
|
||||
kaitai::kstream stream(inp_data); |
||||
glonass_t gl_string(&stream); |
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); |
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); |
||||
REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second); |
||||
REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second); |
||||
REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second); |
||||
REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second); |
||||
REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second); |
||||
|
||||
kaitai::kstream str1(inp_data); |
||||
glonass_t str1_data(&str1); |
||||
glonass_t::string_1_t* s1 = static_cast<glonass_t::string_1_t*>(str1_data.data()); |
||||
|
||||
REQUIRE(s1->not_used() == data[ST1_NU_IDX].second); |
||||
REQUIRE(s1->p1() == data[ST1_P1_IDX].second); |
||||
REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second); |
||||
|
||||
int mul = s1->x_vel_sign() ? (-1) : 1; |
||||
REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul)); |
||||
mul = s1->x_accel_sign() ? (-1) : 1; |
||||
REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul)); |
||||
mul = s1->x_sign() ? (-1) : 1; |
||||
REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul)); |
||||
} |
||||
|
||||
TEST_CASE("parse_string_number_2"){ |
||||
string_data data = generate_string_data(2); |
||||
std::string inp_data = generate_inp_data(data); |
||||
|
||||
kaitai::kstream stream(inp_data); |
||||
glonass_t gl_string(&stream); |
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); |
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); |
||||
REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second); |
||||
REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second); |
||||
REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second); |
||||
REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second); |
||||
REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second); |
||||
|
||||
kaitai::kstream str2(inp_data); |
||||
glonass_t str2_data(&str2); |
||||
glonass_t::string_2_t* s2 = static_cast<glonass_t::string_2_t*>(str2_data.data()); |
||||
|
||||
REQUIRE(s2->b_n() == data[ST2_BN_IDX].second); |
||||
REQUIRE(s2->not_used() == data[ST2_NU_IDX].second); |
||||
REQUIRE(s2->p2() == data[ST2_P2_IDX].second); |
||||
REQUIRE(s2->t_b() == data[ST2_TB_IDX].second); |
||||
int mul = s2->y_vel_sign() ? (-1) : 1; |
||||
REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul)); |
||||
mul = s2->y_accel_sign() ? (-1) : 1; |
||||
REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul)); |
||||
mul = s2->y_sign() ? (-1) : 1; |
||||
REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul)); |
||||
} |
||||
|
||||
TEST_CASE("parse_string_number_3"){ |
||||
string_data data = generate_string_data(3); |
||||
std::string inp_data = generate_inp_data(data); |
||||
|
||||
kaitai::kstream stream(inp_data); |
||||
glonass_t gl_string(&stream); |
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); |
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); |
||||
REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second); |
||||
REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second); |
||||
REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second); |
||||
REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second); |
||||
REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second); |
||||
|
||||
kaitai::kstream str3(inp_data); |
||||
glonass_t str3_data(&str3); |
||||
glonass_t::string_3_t* s3 = static_cast<glonass_t::string_3_t*>(str3_data.data()); |
||||
|
||||
REQUIRE(s3->p3() == data[ST3_P3_IDX].second); |
||||
int mul = s3->gamma_n_sign() ? (-1) : 1; |
||||
REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul)); |
||||
REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second); |
||||
REQUIRE(s3->p() == data[ST3_P_IDX].second); |
||||
REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second); |
||||
mul = s3->z_vel_sign() ? (-1) : 1; |
||||
REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul)); |
||||
mul = s3->z_accel_sign() ? (-1) : 1; |
||||
REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul)); |
||||
mul = s3->z_sign() ? (-1) : 1; |
||||
REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul)); |
||||
} |
||||
|
||||
TEST_CASE("parse_string_number_4"){ |
||||
string_data data = generate_string_data(4); |
||||
std::string inp_data = generate_inp_data(data); |
||||
|
||||
kaitai::kstream stream(inp_data); |
||||
glonass_t gl_string(&stream); |
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); |
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); |
||||
REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second); |
||||
REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second); |
||||
REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second); |
||||
REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second); |
||||
REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second); |
||||
|
||||
kaitai::kstream str4(inp_data); |
||||
glonass_t str4_data(&str4); |
||||
glonass_t::string_4_t* s4 = static_cast<glonass_t::string_4_t*>(str4_data.data()); |
||||
|
||||
int mul = s4->tau_n_sign() ? (-1) : 1; |
||||
REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul)); |
||||
mul = s4->delta_tau_n_sign() ? (-1) : 1; |
||||
REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul)); |
||||
REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second); |
||||
REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second); |
||||
REQUIRE(s4->p4() == data[ST4_P4_IDX].second); |
||||
REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second); |
||||
REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second); |
||||
REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second); |
||||
REQUIRE(s4->n() == data[ST4_N_IDX].second); |
||||
REQUIRE(s4->m() == data[ST4_M_IDX].second); |
||||
} |
||||
|
||||
TEST_CASE("parse_string_number_5"){ |
||||
string_data data = generate_string_data(5); |
||||
std::string inp_data = generate_inp_data(data); |
||||
|
||||
kaitai::kstream stream(inp_data); |
||||
glonass_t gl_string(&stream); |
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); |
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); |
||||
REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second); |
||||
REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second); |
||||
REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second); |
||||
REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second); |
||||
REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second); |
||||
|
||||
kaitai::kstream str5(inp_data); |
||||
glonass_t str5_data(&str5); |
||||
glonass_t::string_5_t* s5 = static_cast<glonass_t::string_5_t*>(str5_data.data()); |
||||
|
||||
REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second); |
||||
REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second); |
||||
REQUIRE(s5->not_used() == data[ST5_NU_IDX].second); |
||||
REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second); |
||||
REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second); |
||||
REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second); |
||||
} |
||||
|
||||
TEST_CASE("parse_string_number_NI"){ |
||||
string_data data = generate_string_data((rand() % 10) + 6); |
||||
std::string inp_data = generate_inp_data(data); |
||||
|
||||
kaitai::kstream stream(inp_data); |
||||
glonass_t gl_string(&stream); |
||||
|
||||
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); |
||||
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); |
||||
REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second); |
||||
REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second); |
||||
REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second); |
||||
REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second); |
||||
REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second); |
||||
|
||||
kaitai::kstream strni(inp_data); |
||||
glonass_t strni_data(&strni); |
||||
glonass_t::string_non_immediate_t* sni = static_cast<glonass_t::string_non_immediate_t*>(strni_data.data()); |
||||
|
||||
REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); |
||||
REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); |
||||
} |
||||
@ -0,0 +1,2 @@ |
||||
#define CATCH_CONFIG_MAIN |
||||
#include "catch2/catch.hpp" |
||||
Loading…
Reference in new issue