sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
14925 Commits
123 Branches
82 Tags
5.0 GiB
 
 
 
 
 
 
Tag: Branch: Tree: agnos11.12
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'agnos11.12'
${ noResults }
openpilot_comma/system/ubloxd/tests/test_glonass_kaitai.cc

360 lines
13 KiB
Raw Permalink Blame History

#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);
}