mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-18 05:23:57 +08:00
ubloxd: move to system/ (#27536)
* ubloxd: move to system/ * more fixes * add missing sconscript * fix valgrind tests * uncomment old-commit-hash: c97911dbad15c3cc7dc577b305ab9aed775e021f
This commit is contained in:
Adeeb Shihadeh
@@ -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("Sats: %d Accuracy: %.2f m cnos" % (ug.measurementReport.numMeas, gle.accuracy), sorted(cnos))
|
||||
except Exception:
|
||||
pass
|
||||
sm.update()
|
||||
time.sleep(0.1)
|
||||
@@ -1,360 +0,0 @@
|
||||
#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);
|
||||
}
|
||||
@@ -1,2 +0,0 @@
|
||||
#define CATCH_CONFIG_MAIN
|
||||
#include "catch2/catch.hpp"
|
||||
@@ -1,115 +0,0 @@
|
||||
import unittest
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
from laika import AstroDog
|
||||
from laika.helpers import ConstellationId
|
||||
from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ublox
|
||||
from laika.opt import calc_pos_fix
|
||||
from selfdrive.test.openpilotci import get_url
|
||||
from tools.lib.logreader import LogReader
|
||||
from selfdrive.test.helpers import with_processes
|
||||
import cereal.messaging as messaging
|
||||
|
||||
def get_gnss_measurements(log_reader):
|
||||
gnss_measurements = []
|
||||
for msg in log_reader:
|
||||
if msg.which() == "ubloxGnss":
|
||||
ublox_msg = msg.ubloxGnss
|
||||
if ublox_msg.which == 'measurementReport':
|
||||
report = ublox_msg.measurementReport
|
||||
if len(report.measurements) > 0:
|
||||
gnss_measurements.append(read_raw_ublox(report))
|
||||
return gnss_measurements
|
||||
|
||||
def get_ublox_raw(log_reader):
|
||||
ublox_raw = []
|
||||
for msg in log_reader:
|
||||
if msg.which() == "ubloxRaw":
|
||||
ublox_raw.append(msg)
|
||||
return ublox_raw
|
||||
|
||||
class TestUbloxProcessing(unittest.TestCase):
|
||||
NUM_TEST_PROCESS_MEAS = 10
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0))
|
||||
cls.gnss_measurements = get_gnss_measurements(lr)
|
||||
|
||||
# test gps ephemeris continuity check (drive has ephemeris issues with cutover data)
|
||||
lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14))
|
||||
cls.ublox_raw = get_ublox_raw(lr)
|
||||
|
||||
def test_read_ublox_raw(self):
|
||||
count_gps = 0
|
||||
count_glonass = 0
|
||||
for measurements in self.gnss_measurements:
|
||||
for m in measurements:
|
||||
if m.constellation_id == ConstellationId.GPS:
|
||||
count_gps += 1
|
||||
elif m.constellation_id == ConstellationId.GLONASS:
|
||||
count_glonass += 1
|
||||
|
||||
self.assertEqual(count_gps, 5036)
|
||||
self.assertEqual(count_glonass, 3651)
|
||||
|
||||
def test_get_fix(self):
|
||||
dog = AstroDog()
|
||||
position_fix_found = 0
|
||||
count_processed_measurements = 0
|
||||
count_corrected_measurements = 0
|
||||
position_fix_found_after_correcting = 0
|
||||
|
||||
pos_ests = []
|
||||
for measurements in self.gnss_measurements[:self.NUM_TEST_PROCESS_MEAS]:
|
||||
processed_meas = process_measurements(measurements, dog)
|
||||
count_processed_measurements += len(processed_meas)
|
||||
pos_fix = calc_pos_fix(processed_meas)
|
||||
if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
|
||||
position_fix_found += 1
|
||||
|
||||
corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog)
|
||||
count_corrected_measurements += len(corrected_meas)
|
||||
|
||||
pos_fix = calc_pos_fix(corrected_meas)
|
||||
if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
|
||||
pos_ests.append(pos_fix[0])
|
||||
position_fix_found_after_correcting += 1
|
||||
|
||||
mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0)
|
||||
np.testing.assert_allclose(mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557], rtol=0, atol=1)
|
||||
|
||||
# Note that can happen that there are less corrected measurements compared to processed when they are invalid.
|
||||
# However, not for the current segment
|
||||
self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS)
|
||||
self.assertEqual(position_fix_found_after_correcting, self.NUM_TEST_PROCESS_MEAS)
|
||||
self.assertEqual(count_processed_measurements, 69)
|
||||
self.assertEqual(count_corrected_measurements, 69)
|
||||
|
||||
@with_processes(['ubloxd'])
|
||||
def test_ublox_gps_cutover(self):
|
||||
time.sleep(2)
|
||||
ugs = messaging.sub_sock("ubloxGnss", timeout=0.1)
|
||||
ur_pm = messaging.PubMaster(['ubloxRaw'])
|
||||
|
||||
def replay_segment():
|
||||
rcv_msgs = []
|
||||
for msg in self.ublox_raw:
|
||||
ur_pm.send(msg.which(), msg.as_builder())
|
||||
time.sleep(0.01)
|
||||
rcv_msgs += messaging.drain_sock(ugs)
|
||||
|
||||
time.sleep(0.1)
|
||||
rcv_msgs += messaging.drain_sock(ugs)
|
||||
return rcv_msgs
|
||||
|
||||
# replay twice to enforce cutover data on rewind
|
||||
rcv_msgs = replay_segment()
|
||||
rcv_msgs += replay_segment()
|
||||
|
||||
ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs)
|
||||
self.assertEqual(ephems_cnt, 15)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1,938 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# pylint: skip-file
|
||||
'''
|
||||
UBlox binary protocol handling
|
||||
|
||||
Copyright Andrew Tridgell, October 2012
|
||||
Released under GNU GPL version 3 or later
|
||||
|
||||
WARNING: This code has originally intended for
|
||||
ublox version 7, it has been adapted to work
|
||||
for ublox version 8, not all functions may work.
|
||||
'''
|
||||
|
||||
|
||||
import struct
|
||||
import time
|
||||
|
||||
# protocol constants
|
||||
PREAMBLE1 = 0xb5
|
||||
PREAMBLE2 = 0x62
|
||||
|
||||
# message classes
|
||||
CLASS_NAV = 0x01
|
||||
CLASS_RXM = 0x02
|
||||
CLASS_INF = 0x04
|
||||
CLASS_ACK = 0x05
|
||||
CLASS_CFG = 0x06
|
||||
CLASS_MON = 0x0A
|
||||
CLASS_AID = 0x0B
|
||||
CLASS_TIM = 0x0D
|
||||
CLASS_ESF = 0x10
|
||||
|
||||
# ACK messages
|
||||
MSG_ACK_NACK = 0x00
|
||||
MSG_ACK_ACK = 0x01
|
||||
|
||||
# NAV messages
|
||||
MSG_NAV_POSECEF = 0x1
|
||||
MSG_NAV_POSLLH = 0x2
|
||||
MSG_NAV_STATUS = 0x3
|
||||
MSG_NAV_DOP = 0x4
|
||||
MSG_NAV_SOL = 0x6
|
||||
MSG_NAV_PVT = 0x7
|
||||
MSG_NAV_POSUTM = 0x8
|
||||
MSG_NAV_VELNED = 0x12
|
||||
MSG_NAV_VELECEF = 0x11
|
||||
MSG_NAV_TIMEGPS = 0x20
|
||||
MSG_NAV_TIMEUTC = 0x21
|
||||
MSG_NAV_CLOCK = 0x22
|
||||
MSG_NAV_SVINFO = 0x30
|
||||
MSG_NAV_AOPSTATUS = 0x60
|
||||
MSG_NAV_DGPS = 0x31
|
||||
MSG_NAV_DOP = 0x04
|
||||
MSG_NAV_EKFSTATUS = 0x40
|
||||
MSG_NAV_SBAS = 0x32
|
||||
MSG_NAV_SOL = 0x06
|
||||
MSG_NAV_SAT = 0x35
|
||||
|
||||
# RXM messages
|
||||
MSG_RXM_RAW = 0x15
|
||||
MSG_RXM_SFRB = 0x11
|
||||
MSG_RXM_SFRBX = 0x13
|
||||
MSG_RXM_SVSI = 0x20
|
||||
MSG_RXM_EPH = 0x31
|
||||
MSG_RXM_ALM = 0x30
|
||||
MSG_RXM_PMREQ = 0x41
|
||||
|
||||
# AID messages
|
||||
MSG_AID_ALM = 0x30
|
||||
MSG_AID_EPH = 0x31
|
||||
MSG_AID_ALPSRV = 0x32
|
||||
MSG_AID_AOP = 0x33
|
||||
MSG_AID_DATA = 0x10
|
||||
MSG_AID_ALP = 0x50
|
||||
MSG_AID_DATA = 0x10
|
||||
MSG_AID_HUI = 0x02
|
||||
MSG_AID_INI = 0x01
|
||||
MSG_AID_REQ = 0x00
|
||||
|
||||
# CFG messages
|
||||
MSG_CFG_PRT = 0x00
|
||||
MSG_CFG_ANT = 0x13
|
||||
MSG_CFG_DAT = 0x06
|
||||
MSG_CFG_EKF = 0x12
|
||||
MSG_CFG_ESFGWT = 0x29
|
||||
MSG_CFG_CFG = 0x09
|
||||
MSG_CFG_USB = 0x1b
|
||||
MSG_CFG_RATE = 0x08
|
||||
MSG_CFG_SET_RATE = 0x01
|
||||
MSG_CFG_NAV5 = 0x24
|
||||
MSG_CFG_FXN = 0x0E
|
||||
MSG_CFG_INF = 0x02
|
||||
MSG_CFG_ITFM = 0x39
|
||||
MSG_CFG_MSG = 0x01
|
||||
MSG_CFG_NAVX5 = 0x23
|
||||
MSG_CFG_NMEA = 0x17
|
||||
MSG_CFG_NVS = 0x22
|
||||
MSG_CFG_PM2 = 0x3B
|
||||
MSG_CFG_PM = 0x32
|
||||
MSG_CFG_ITMF = 0x39
|
||||
MSG_CFG_RINV = 0x34
|
||||
MSG_CFG_RST = 0x04
|
||||
MSG_CFG_RXM = 0x11
|
||||
MSG_CFG_SBAS = 0x16
|
||||
MSG_CFG_TMODE2 = 0x3D
|
||||
MSG_CFG_TMODE = 0x1D
|
||||
MSG_CFG_TPS = 0x31
|
||||
MSG_CFG_TP = 0x07
|
||||
MSG_CFG_GNSS = 0x3E
|
||||
MSG_CFG_ODO = 0x1E
|
||||
|
||||
# ESF messages
|
||||
MSG_ESF_MEAS = 0x02
|
||||
MSG_ESF_STATUS = 0x10
|
||||
|
||||
# INF messages
|
||||
MSG_INF_DEBUG = 0x04
|
||||
MSG_INF_ERROR = 0x00
|
||||
MSG_INF_NOTICE = 0x02
|
||||
MSG_INF_TEST = 0x03
|
||||
MSG_INF_WARNING = 0x01
|
||||
|
||||
# MON messages
|
||||
MSG_MON_SCHD = 0x01
|
||||
MSG_MON_HW = 0x09
|
||||
MSG_MON_HW2 = 0x0B
|
||||
MSG_MON_IO = 0x02
|
||||
MSG_MON_MSGPP = 0x06
|
||||
MSG_MON_RXBUF = 0x07
|
||||
MSG_MON_RXR = 0x21
|
||||
MSG_MON_TXBUF = 0x08
|
||||
MSG_MON_VER = 0x04
|
||||
|
||||
# TIM messages
|
||||
MSG_TIM_TP = 0x01
|
||||
MSG_TIM_TM2 = 0x03
|
||||
MSG_TIM_SVIN = 0x04
|
||||
MSG_TIM_VRFY = 0x06
|
||||
|
||||
# port IDs
|
||||
PORT_DDC = 0
|
||||
PORT_SERIAL1 = 1
|
||||
PORT_SERIAL2 = 2
|
||||
PORT_USB = 3
|
||||
PORT_SPI = 4
|
||||
|
||||
# dynamic models
|
||||
DYNAMIC_MODEL_PORTABLE = 0
|
||||
DYNAMIC_MODEL_STATIONARY = 2
|
||||
DYNAMIC_MODEL_PEDESTRIAN = 3
|
||||
DYNAMIC_MODEL_AUTOMOTIVE = 4
|
||||
DYNAMIC_MODEL_SEA = 5
|
||||
DYNAMIC_MODEL_AIRBORNE1G = 6
|
||||
DYNAMIC_MODEL_AIRBORNE2G = 7
|
||||
DYNAMIC_MODEL_AIRBORNE4G = 8
|
||||
|
||||
#reset items
|
||||
RESET_HOT = 0
|
||||
RESET_WARM = 1
|
||||
RESET_COLD = 0xFFFF
|
||||
|
||||
RESET_HW = 0
|
||||
RESET_SW = 1
|
||||
RESET_SW_GPS = 2
|
||||
RESET_HW_GRACEFUL = 4
|
||||
RESET_GPS_STOP = 8
|
||||
RESET_GPS_START = 9
|
||||
|
||||
|
||||
class UBloxError(Exception):
|
||||
'''Ublox error class'''
|
||||
|
||||
def __init__(self, msg):
|
||||
Exception.__init__(self, msg)
|
||||
self.message = msg
|
||||
|
||||
|
||||
class UBloxAttrDict(dict):
|
||||
'''allow dictionary members as attributes'''
|
||||
|
||||
def __init__(self):
|
||||
dict.__init__(self)
|
||||
|
||||
def __getattr__(self, name):
|
||||
try:
|
||||
return self.__getitem__(name)
|
||||
except KeyError:
|
||||
raise AttributeError(name)
|
||||
|
||||
def __setattr__(self, name, value):
|
||||
if name in self.__dict__:
|
||||
# allow set on normal attributes
|
||||
dict.__setattr__(self, name, value)
|
||||
else:
|
||||
self.__setitem__(name, value)
|
||||
|
||||
|
||||
def ArrayParse(field):
|
||||
'''parse an array descriptor'''
|
||||
arridx = field.find('[')
|
||||
if arridx == -1:
|
||||
return (field, -1)
|
||||
alen = int(field[arridx + 1:-1])
|
||||
fieldname = field[:arridx]
|
||||
return (fieldname, alen)
|
||||
|
||||
|
||||
class UBloxDescriptor:
|
||||
'''class used to describe the layout of a UBlox message'''
|
||||
|
||||
def __init__(self,
|
||||
name,
|
||||
msg_format,
|
||||
fields=None,
|
||||
count_field=None,
|
||||
format2=None,
|
||||
fields2=None):
|
||||
if fields is None:
|
||||
fields = []
|
||||
|
||||
self.name = name
|
||||
self.msg_format = msg_format
|
||||
self.fields = fields
|
||||
self.count_field = count_field
|
||||
self.format2 = format2
|
||||
self.fields2 = fields2
|
||||
|
||||
def unpack(self, msg):
|
||||
'''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg'''
|
||||
msg._fields = {}
|
||||
|
||||
# unpack main message blocks. A comm
|
||||
formats = self.msg_format.split(',')
|
||||
buf = msg._buf[6:-2]
|
||||
count = 0
|
||||
msg._recs = []
|
||||
fields = self.fields[:]
|
||||
|
||||
for fmt in formats:
|
||||
size1 = struct.calcsize(fmt)
|
||||
if size1 > len(buf):
|
||||
raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf)))
|
||||
f1 = list(struct.unpack(fmt, buf[:size1]))
|
||||
i = 0
|
||||
while i < len(f1):
|
||||
field = fields.pop(0)
|
||||
(fieldname, alen) = ArrayParse(field)
|
||||
if alen == -1:
|
||||
msg._fields[fieldname] = f1[i]
|
||||
if self.count_field == fieldname:
|
||||
count = int(f1[i])
|
||||
i += 1
|
||||
else:
|
||||
msg._fields[fieldname] = [0] * alen
|
||||
for a in range(alen):
|
||||
msg._fields[fieldname][a] = f1[i]
|
||||
i += 1
|
||||
buf = buf[size1:]
|
||||
if len(buf) == 0:
|
||||
break
|
||||
|
||||
if self.count_field == '_remaining':
|
||||
count = len(buf) // struct.calcsize(self.format2)
|
||||
|
||||
if count == 0:
|
||||
msg._unpacked = True
|
||||
if len(buf) != 0:
|
||||
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
|
||||
return
|
||||
|
||||
size2 = struct.calcsize(self.format2)
|
||||
for c in range(count):
|
||||
r = UBloxAttrDict()
|
||||
if size2 > len(buf):
|
||||
raise UBloxError("INVALID_SIZE=%u, " % len(buf))
|
||||
f2 = list(struct.unpack(self.format2, buf[:size2]))
|
||||
for i in range(len(self.fields2)):
|
||||
r[self.fields2[i]] = f2[i]
|
||||
buf = buf[size2:]
|
||||
msg._recs.append(r)
|
||||
if len(buf) != 0:
|
||||
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
|
||||
msg._unpacked = True
|
||||
|
||||
def pack(self, msg, msg_class=None, msg_id=None):
|
||||
'''pack a UBloxMessage from the .fields and ._recs attributes in msg'''
|
||||
f1 = []
|
||||
if msg_class is None:
|
||||
msg_class = msg.msg_class()
|
||||
if msg_id is None:
|
||||
msg_id = msg.msg_id()
|
||||
msg._buf = ''
|
||||
|
||||
fields = self.fields[:]
|
||||
for f in fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if fieldname not in msg._fields:
|
||||
break
|
||||
if alen == -1:
|
||||
f1.append(msg._fields[fieldname])
|
||||
else:
|
||||
for a in range(alen):
|
||||
f1.append(msg._fields[fieldname][a])
|
||||
try:
|
||||
# try full length message
|
||||
fmt = self.msg_format.replace(',', '')
|
||||
msg._buf = struct.pack(fmt, *tuple(f1))
|
||||
except Exception:
|
||||
# try without optional part
|
||||
fmt = self.msg_format.split(',')[0]
|
||||
msg._buf = struct.pack(fmt, *tuple(f1))
|
||||
|
||||
length = len(msg._buf)
|
||||
if msg._recs:
|
||||
length += len(msg._recs) * struct.calcsize(self.format2)
|
||||
header = struct.pack('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
|
||||
msg._buf = header + msg._buf
|
||||
|
||||
for r in msg._recs:
|
||||
f2 = []
|
||||
for f in self.fields2:
|
||||
f2.append(r[f])
|
||||
msg._buf += struct.pack(self.format2, *tuple(f2))
|
||||
msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))
|
||||
|
||||
def format(self, msg):
|
||||
'''return a formatted string for a message'''
|
||||
if not msg._unpacked:
|
||||
self.unpack(msg)
|
||||
ret = self.name + ': '
|
||||
for f in self.fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if fieldname not in msg._fields:
|
||||
continue
|
||||
v = msg._fields[fieldname]
|
||||
if isinstance(v, list):
|
||||
ret += '%s=[' % fieldname
|
||||
for a in range(alen):
|
||||
ret += '%s, ' % v[a]
|
||||
ret = ret[:-2] + '], '
|
||||
elif isinstance(v, str):
|
||||
ret += '{}="{}", '.format(f, v.rstrip(' \0'))
|
||||
else:
|
||||
ret += '%s=%s, ' % (f, v)
|
||||
for r in msg._recs:
|
||||
ret += '[ '
|
||||
for f in self.fields2:
|
||||
v = r[f]
|
||||
ret += '%s=%s, ' % (f, v)
|
||||
ret = ret[:-2] + ' ], '
|
||||
return ret[:-2]
|
||||
|
||||
|
||||
# list of supported message types.
|
||||
msg_types = {
|
||||
(CLASS_ACK, MSG_ACK_ACK):
|
||||
UBloxDescriptor('ACK_ACK', '<BB', ['clsID', 'msgID']),
|
||||
(CLASS_ACK, MSG_ACK_NACK):
|
||||
UBloxDescriptor('ACK_NACK', '<BB', ['clsID', 'msgID']),
|
||||
(CLASS_CFG, MSG_CFG_USB):
|
||||
UBloxDescriptor('CFG_USB', '<HHHHHH32s32s32s', [
|
||||
'vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption', 'flags',
|
||||
'vendorString', 'productString', 'serialNumber'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_PRT):
|
||||
UBloxDescriptor('CFG_PRT', '<BBHIIHHHH', [
|
||||
'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
|
||||
'reserved4', 'reserved5'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_ITMF):
|
||||
UBloxDescriptor('CFG_ITMF', '<II', [
|
||||
'config', 'config2'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_CFG):
|
||||
UBloxDescriptor('CFG_CFG', '<III,B',
|
||||
['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
|
||||
(CLASS_CFG, MSG_CFG_RXM):
|
||||
UBloxDescriptor('CFG_RXM', '<BB',
|
||||
['reserved1', 'lpMode']),
|
||||
(CLASS_CFG, MSG_CFG_RST):
|
||||
UBloxDescriptor('CFG_RST', '<HBB', ['navBbrMask ', 'resetMode', 'reserved1']),
|
||||
(CLASS_CFG, MSG_CFG_SBAS):
|
||||
UBloxDescriptor('CFG_SBAS', '<BBBBI',
|
||||
['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
|
||||
(CLASS_CFG, MSG_CFG_GNSS):
|
||||
UBloxDescriptor('CFG_GNSS', '<BBBB',
|
||||
['msgVer', 'numTrkChHw', 'numTrkChUse',
|
||||
'numConfigBlocks'], 'numConfigBlocks', '<BBBBI',
|
||||
['gnssId', 'resTrkCh', 'maxTrkCh', 'reserved1', 'flags']),
|
||||
(CLASS_CFG, MSG_CFG_RATE):
|
||||
UBloxDescriptor('CFG_RATE', '<HHH', ['measRate', 'navRate', 'timeRef']),
|
||||
(CLASS_CFG, MSG_CFG_MSG):
|
||||
UBloxDescriptor('CFG_MSG', '<BB6B', ['msgClass', 'msgId', 'rates[6]']),
|
||||
(CLASS_NAV, MSG_NAV_POSLLH):
|
||||
UBloxDescriptor('NAV_POSLLH', '<IiiiiII',
|
||||
['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
|
||||
(CLASS_NAV, MSG_NAV_VELNED):
|
||||
UBloxDescriptor('NAV_VELNED', '<IiiiIIiII', [
|
||||
'iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 'sAcc', 'cAcc'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_DOP):
|
||||
UBloxDescriptor('NAV_DOP', '<IHHHHHHH',
|
||||
['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
|
||||
(CLASS_NAV, MSG_NAV_STATUS):
|
||||
UBloxDescriptor('NAV_STATUS', '<IBBBBII',
|
||||
['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
|
||||
(CLASS_NAV, MSG_NAV_SOL):
|
||||
UBloxDescriptor('NAV_SOL', '<IihBBiiiIiiiIHBBI', [
|
||||
'iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ', 'pAcc',
|
||||
'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 'numSV', 'reserved2'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_PVT):
|
||||
UBloxDescriptor('NAV_PVT', '<IHBBBBBBIiBBBBiiiiIIiiiiiIIH6BihH', [
|
||||
'iTOW', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid', 'tAcc', 'nano',
|
||||
'fixType', 'flags', 'flags2', 'numSV', 'lon', 'lat', 'height', 'hMSL', 'hAcc', 'vAcc',
|
||||
'velN', 'velE', 'velD', 'gSpeed', 'headMot', 'sAcc', 'headAcc', 'pDOP',
|
||||
'reserverd1[6]', 'headVeh', 'magDec', 'magAcc'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_POSUTM):
|
||||
UBloxDescriptor('NAV_POSUTM', '<Iiiibb',
|
||||
['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
|
||||
(CLASS_NAV, MSG_NAV_SBAS):
|
||||
UBloxDescriptor('NAV_SBAS', '<IBBbBBBBB', [
|
||||
'iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02',
|
||||
'reserved03'
|
||||
], 'cnt', 'BBBBBBhHh', [
|
||||
'svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1', 'prc', 'reserved2', 'ic'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_POSECEF):
|
||||
UBloxDescriptor('NAV_POSECEF', '<IiiiI', ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
|
||||
(CLASS_NAV, MSG_NAV_VELECEF):
|
||||
UBloxDescriptor('NAV_VELECEF', '<IiiiI', ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ',
|
||||
'sAcc']),
|
||||
(CLASS_NAV, MSG_NAV_TIMEGPS):
|
||||
UBloxDescriptor('NAV_TIMEGPS', '<IihbBI',
|
||||
['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
|
||||
(CLASS_NAV, MSG_NAV_TIMEUTC):
|
||||
UBloxDescriptor('NAV_TIMEUTC', '<IIiHBBBBBB', [
|
||||
'iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_CLOCK):
|
||||
UBloxDescriptor('NAV_CLOCK', '<IiiII', ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
|
||||
(CLASS_NAV, MSG_NAV_DGPS):
|
||||
UBloxDescriptor('NAV_DGPS', '<IihhBBH',
|
||||
['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
|
||||
'numCh', '<BBHff', ['svid', 'flags', 'ageC', 'prc', 'prrc']),
|
||||
(CLASS_NAV, MSG_NAV_SVINFO):
|
||||
UBloxDescriptor('NAV_SVINFO', '<IBBH', ['iTOW', 'numCh', 'globalFlags',
|
||||
'reserved2'], 'numCh', '<BBBBBbhi',
|
||||
['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
|
||||
(CLASS_RXM, MSG_RXM_SVSI):
|
||||
UBloxDescriptor('RXM_SVSI', '<IhBB', ['iTOW', 'week', 'numVis', 'numSV'], 'numSV',
|
||||
'<BBhbB', ['svid', 'svFlag', 'azim', 'elev', 'age']),
|
||||
(CLASS_RXM, MSG_RXM_EPH):
|
||||
UBloxDescriptor('RXM_EPH', '<II , 8I 8I 8I',
|
||||
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
|
||||
(CLASS_AID, MSG_AID_EPH):
|
||||
UBloxDescriptor('AID_EPH', '<II , 8I 8I 8I',
|
||||
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
|
||||
(CLASS_AID, MSG_AID_HUI):
|
||||
UBloxDescriptor('AID_HUI', '<Iddi 6h 8f I',
|
||||
['health', 'utcA0', 'utcA1', 'utcTOW', 'utcWNT', 'utcLS', 'utcWNF',
|
||||
'utcDN', 'utcLSF', 'utcSpare', 'klobA0', 'klobA1', 'klobA2', 'klobA3',
|
||||
'klobB0', 'klobB1', 'klobB2', 'klobB3', 'flags']),
|
||||
(CLASS_AID, MSG_AID_AOP):
|
||||
UBloxDescriptor('AID_AOP', '<B47B , 48B 48B 48B',
|
||||
['svid', 'data[47]', 'optional0[48]', 'optional1[48]',
|
||||
'optional1[48]']),
|
||||
(CLASS_RXM, MSG_RXM_RAW):
|
||||
UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
|
||||
'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
|
||||
], 'numMeas', '<ddfBBBBHBBBBBB', [
|
||||
'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'sigId', 'freqId', 'locktime', 'cno',
|
||||
'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
|
||||
]),
|
||||
(CLASS_RXM, MSG_RXM_SFRB):
|
||||
UBloxDescriptor('RXM_SFRB', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
|
||||
(CLASS_RXM, MSG_RXM_SFRBX):
|
||||
UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
|
||||
'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
|
||||
(CLASS_AID, MSG_AID_ALM):
|
||||
UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
|
||||
(CLASS_RXM, MSG_RXM_ALM):
|
||||
UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
|
||||
(CLASS_CFG, MSG_CFG_ANT):
|
||||
UBloxDescriptor('CFG_ANT', '<HH', ['flags', 'pins']),
|
||||
(CLASS_CFG, MSG_CFG_ODO):
|
||||
UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
|
||||
'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
|
||||
'cogMaxPosAcc', 'reserved3[2]', 'velLpGain', 'cogLpGain', 'reserved[2]'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_NAV5):
|
||||
UBloxDescriptor('CFG_NAV5', '<HBBiIbBHHHHBBIII', [
|
||||
'mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 'drLimit',
|
||||
'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 'dgpsTimeOut', 'reserved2',
|
||||
'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_NAVX5):
|
||||
UBloxDescriptor('CFG_NAVX5', '<HHIBBBBBBBBBBHIBBBBBBHII', [
|
||||
'version', 'mask1', 'reserved0', 'reserved1', 'reserved2', 'minSVs', 'maxSVs',
|
||||
'minCNO', 'reserved5', 'iniFix3D', 'reserved6', 'reserved7', 'reserved8',
|
||||
'wknRollover', 'reserved9', 'reserved10', 'reserved11', 'usePPP', 'useAOP',
|
||||
'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW):
|
||||
UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB17BHIII', [
|
||||
'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
|
||||
'flags', 'reserved1', 'usedMask', 'VP[17]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
|
||||
'pullL'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW2):
|
||||
UBloxDescriptor('MON_HW2', '<bBbBB3BI8BI4B', [
|
||||
'ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]', 'lowLevCfg',
|
||||
'reserved2[8]', 'postStatus', 'reserved3[4]'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_SCHD):
|
||||
UBloxDescriptor('MON_SCHD', '<IIIIHHHBB', [
|
||||
'tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack', 'stackSize', 'CPUIdle', 'flySly',
|
||||
'ptlSly'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_VER):
|
||||
UBloxDescriptor('MON_VER', '<30s10s,30s', ['swVersion', 'hwVersion', 'romVersion'],
|
||||
'_remaining', '30s', ['extension']),
|
||||
(CLASS_TIM, MSG_TIM_TP):
|
||||
UBloxDescriptor('TIM_TP', '<IIiHBB',
|
||||
['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
|
||||
(CLASS_TIM, MSG_TIM_TM2):
|
||||
UBloxDescriptor('TIM_TM2', '<BBHHHIIIII', [
|
||||
'ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 'towMsF', 'towSubMsF',
|
||||
'accEst'
|
||||
]),
|
||||
(CLASS_TIM, MSG_TIM_SVIN):
|
||||
UBloxDescriptor('TIM_SVIN', '<IiiiIIBBH', [
|
||||
'dur', 'meanX', 'meanY', 'meanZ', 'meanV', 'obs', 'valid', 'active', 'reserved1'
|
||||
]),
|
||||
(CLASS_INF, MSG_INF_ERROR):
|
||||
UBloxDescriptor('INF_ERR', '<18s', ['str']),
|
||||
(CLASS_INF, MSG_INF_DEBUG):
|
||||
UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
|
||||
}
|
||||
|
||||
|
||||
class UBloxMessage:
|
||||
'''UBlox message class - holds a UBX binary message'''
|
||||
|
||||
def __init__(self):
|
||||
self._buf = b""
|
||||
self._fields = {}
|
||||
self._recs = []
|
||||
self._unpacked = False
|
||||
self.debug_level = 1
|
||||
|
||||
def __str__(self):
|
||||
'''format a message as a string'''
|
||||
if not self.valid():
|
||||
return 'UBloxMessage(INVALID)'
|
||||
type = self.msg_type()
|
||||
if type in msg_types:
|
||||
return msg_types[type].format(self)
|
||||
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
|
||||
|
||||
def as_dict(self):
|
||||
'''format a message as a string'''
|
||||
if not self.valid():
|
||||
return 'UBloxMessage(INVALID)'
|
||||
type = self.msg_type()
|
||||
if type in msg_types:
|
||||
return msg_types[type].format(self)
|
||||
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
|
||||
|
||||
def __getattr__(self, name):
|
||||
'''allow access to message fields'''
|
||||
try:
|
||||
return self._fields[name]
|
||||
except KeyError:
|
||||
if name == 'recs':
|
||||
return self._recs
|
||||
raise AttributeError(name)
|
||||
|
||||
def __setattr__(self, name, value):
|
||||
'''allow access to message fields'''
|
||||
if name.startswith('_'):
|
||||
self.__dict__[name] = value
|
||||
else:
|
||||
self._fields[name] = value
|
||||
|
||||
def have_field(self, name):
|
||||
'''return True if a message contains the given field'''
|
||||
return name in self._fields
|
||||
|
||||
def debug(self, level, msg):
|
||||
'''write a debug message'''
|
||||
if self.debug_level >= level:
|
||||
print(msg)
|
||||
|
||||
def unpack(self):
|
||||
'''unpack a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
msg_types[type].unpack(self)
|
||||
return self._fields, self._recs
|
||||
|
||||
def pack(self):
|
||||
'''pack a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s' % str(type))
|
||||
msg_types[type].pack(self)
|
||||
|
||||
def name(self):
|
||||
'''return the short string name for a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
return msg_types[type].name
|
||||
|
||||
def msg_class(self):
|
||||
'''return the message class'''
|
||||
return self._buf[2]
|
||||
|
||||
def msg_id(self):
|
||||
'''return the message id within the class'''
|
||||
return self._buf[3]
|
||||
|
||||
def msg_type(self):
|
||||
'''return the message type tuple (class, id)'''
|
||||
return (self.msg_class(), self.msg_id())
|
||||
|
||||
def msg_length(self):
|
||||
'''return the payload length'''
|
||||
(payload_length, ) = struct.unpack('<H', self._buf[4:6])
|
||||
return payload_length
|
||||
|
||||
def valid_so_far(self):
|
||||
'''check if the message is valid so far'''
|
||||
if len(self._buf) > 0 and self._buf[0] != PREAMBLE1:
|
||||
return False
|
||||
if len(self._buf) > 1 and self._buf[1] != PREAMBLE2:
|
||||
self.debug(1, "bad pre2")
|
||||
return False
|
||||
if self.needed_bytes() == 0 and not self.valid():
|
||||
if len(self._buf) > 8:
|
||||
self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf),
|
||||
self.needed_bytes()))
|
||||
else:
|
||||
self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes()))
|
||||
return False
|
||||
return True
|
||||
|
||||
def add(self, bytes):
|
||||
'''add some bytes to a message'''
|
||||
self._buf += bytes
|
||||
while not self.valid_so_far() and len(self._buf) > 0:
|
||||
'''handle corrupted streams'''
|
||||
self._buf = self._buf[1:]
|
||||
if self.needed_bytes() < 0:
|
||||
self._buf = ""
|
||||
|
||||
def checksum(self, data=None):
|
||||
'''return a checksum tuple for a message'''
|
||||
if data is None:
|
||||
data = self._buf[2:-2]
|
||||
#cs = 0
|
||||
ck_a = 0
|
||||
ck_b = 0
|
||||
for i in data:
|
||||
ck_a = (ck_a + i) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
return (ck_a, ck_b)
|
||||
|
||||
def valid_checksum(self):
|
||||
'''check if the checksum is OK'''
|
||||
(ck_a, ck_b) = self.checksum()
|
||||
#d = self._buf[2:-2]
|
||||
(ck_a2, ck_b2) = struct.unpack('<BB', self._buf[-2:])
|
||||
return ck_a == ck_a2 and ck_b == ck_b2
|
||||
|
||||
def needed_bytes(self):
|
||||
'''return number of bytes still needed'''
|
||||
if len(self._buf) < 6:
|
||||
return 8 - len(self._buf)
|
||||
return self.msg_length() + 8 - len(self._buf)
|
||||
|
||||
def valid(self):
|
||||
'''check if a message is valid'''
|
||||
return len(self._buf) >= 8 and self.needed_bytes() == 0 and self.valid_checksum()
|
||||
|
||||
|
||||
class UBlox:
|
||||
def __init__(self, dev, baudrate):
|
||||
self.dev = dev
|
||||
self.baudrate = baudrate
|
||||
|
||||
self.use_sendrecv = False
|
||||
self.read_only = False
|
||||
self.debug_level = 0
|
||||
|
||||
self.logfile = None
|
||||
self.log = None
|
||||
self.preferred_dynamic_model = None
|
||||
self.preferred_usePPP = None
|
||||
self.preferred_dgps_timeout = None
|
||||
|
||||
def close(self):
|
||||
'''close the device'''
|
||||
self.dev.close()
|
||||
self.dev = None
|
||||
|
||||
def set_debug(self, debug_level):
|
||||
'''set debug level'''
|
||||
self.debug_level = debug_level
|
||||
|
||||
def debug(self, level, msg):
|
||||
'''write a debug message'''
|
||||
if self.debug_level >= level:
|
||||
print(msg)
|
||||
|
||||
def set_logfile(self, logfile, append=False):
|
||||
'''setup logging to a file'''
|
||||
if self.log is not None:
|
||||
self.log.close()
|
||||
self.log = None
|
||||
self.logfile = logfile
|
||||
if self.logfile is not None:
|
||||
if append:
|
||||
mode = 'ab'
|
||||
else:
|
||||
mode = 'wb'
|
||||
self.log = open(self.logfile, mode=mode)
|
||||
|
||||
def set_preferred_dynamic_model(self, model):
|
||||
'''set the preferred dynamic model for receiver'''
|
||||
self.preferred_dynamic_model = model
|
||||
if model is not None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
|
||||
def set_preferred_dgps_timeout(self, timeout):
|
||||
'''set the preferred DGPS timeout for receiver'''
|
||||
self.preferred_dgps_timeout = timeout
|
||||
if timeout is not None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
|
||||
def set_preferred_usePPP(self, usePPP):
|
||||
'''set the preferred usePPP setting for the receiver'''
|
||||
if usePPP is None:
|
||||
self.preferred_usePPP = None
|
||||
return
|
||||
self.preferred_usePPP = int(usePPP)
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
|
||||
|
||||
def nmea_checksum(self, msg):
|
||||
d = msg[1:]
|
||||
cs = 0
|
||||
for i in d:
|
||||
cs ^= ord(i)
|
||||
return cs
|
||||
|
||||
def write(self, buf):
|
||||
'''write some bytes'''
|
||||
if not self.read_only:
|
||||
if self.use_sendrecv:
|
||||
return self.dev.send(buf)
|
||||
if type(buf) == str:
|
||||
return self.dev.write(str.encode(buf))
|
||||
else:
|
||||
return self.dev.write(buf)
|
||||
|
||||
def read(self, n):
|
||||
'''read some bytes'''
|
||||
if self.use_sendrecv:
|
||||
try:
|
||||
return self.dev.recv(n)
|
||||
except OSError:
|
||||
return ''
|
||||
return self.dev.read(n)
|
||||
|
||||
def send_nmea(self, msg):
|
||||
if not self.read_only:
|
||||
s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n"
|
||||
self.write(s)
|
||||
|
||||
def set_binary(self):
|
||||
'''put a UBlox into binary mode using a NMEA string'''
|
||||
if not self.read_only:
|
||||
print("try set binary at %u" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate)
|
||||
|
||||
def disable_nmea(self):
|
||||
''' stop sending all types of nmea messages '''
|
||||
self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0")
|
||||
self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0")
|
||||
|
||||
def seek_percent(self, pct):
|
||||
'''seek to the given percentage of a file'''
|
||||
self.dev.seek(0, 2)
|
||||
filesize = self.dev.tell()
|
||||
self.dev.seek(pct * 0.01 * filesize)
|
||||
|
||||
def special_handling(self, msg):
|
||||
'''handle automatic configuration changes'''
|
||||
if msg.name() == 'CFG_NAV5':
|
||||
msg.unpack()
|
||||
sendit = False
|
||||
pollit = False
|
||||
if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model:
|
||||
msg.dynModel = self.preferred_dynamic_model
|
||||
sendit = True
|
||||
pollit = True
|
||||
if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout:
|
||||
msg.dgpsTimeOut = self.preferred_dgps_timeout
|
||||
self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut)
|
||||
sendit = True
|
||||
# we don't re-poll for this one, as some receivers refuse to set it
|
||||
if sendit:
|
||||
msg.pack()
|
||||
self.send(msg)
|
||||
if pollit:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None:
|
||||
msg.unpack()
|
||||
if msg.usePPP != self.preferred_usePPP:
|
||||
msg.usePPP = self.preferred_usePPP
|
||||
msg.mask = 1 << 13
|
||||
msg.pack()
|
||||
self.send(msg)
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
|
||||
|
||||
def receive_message(self, ignore_eof=False):
|
||||
'''blocking receive of one ublox message'''
|
||||
msg = UBloxMessage()
|
||||
while True:
|
||||
n = msg.needed_bytes()
|
||||
b = self.read(n)
|
||||
if not b:
|
||||
if ignore_eof:
|
||||
time.sleep(0.01)
|
||||
continue
|
||||
if len(msg._buf) > 0:
|
||||
self.debug(1, "dropping %d bytes" % len(msg._buf))
|
||||
return None
|
||||
msg.add(b)
|
||||
if self.log is not None:
|
||||
self.log.write(b)
|
||||
self.log.flush()
|
||||
if msg.valid():
|
||||
self.special_handling(msg)
|
||||
return msg
|
||||
|
||||
def receive_message_noerror(self, ignore_eof=False):
|
||||
'''blocking receive of one ublox message, ignoring errors'''
|
||||
try:
|
||||
return self.receive_message(ignore_eof=ignore_eof)
|
||||
except UBloxError as e:
|
||||
print(e)
|
||||
return None
|
||||
except OSError as e:
|
||||
# Occasionally we get hit with 'resource temporarily unavailable'
|
||||
# messages here on the serial device, catch them too.
|
||||
print(e)
|
||||
return None
|
||||
|
||||
def send(self, msg):
|
||||
'''send a preformatted ublox message'''
|
||||
if not msg.valid():
|
||||
self.debug(1, "invalid send")
|
||||
return
|
||||
if not self.read_only:
|
||||
self.write(msg._buf)
|
||||
|
||||
def send_message(self, msg_class, msg_id, payload):
|
||||
'''send a ublox message with class, id and payload'''
|
||||
msg = UBloxMessage()
|
||||
msg._buf = struct.pack('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))
|
||||
msg._buf += payload
|
||||
(ck_a, ck_b) = msg.checksum(msg._buf[2:])
|
||||
msg._buf += struct.pack('<BB', ck_a, ck_b)
|
||||
self.send(msg)
|
||||
|
||||
def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
|
||||
'''configure the solution rate in milliseconds'''
|
||||
payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)
|
||||
|
||||
def configure_message_rate(self, msg_class, msg_id, rate):
|
||||
'''configure the message rate for a given message'''
|
||||
payload = struct.pack('<BBB', msg_class, msg_id, rate)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)
|
||||
|
||||
def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
|
||||
'''configure a IO port'''
|
||||
if baudrate is None:
|
||||
baudrate = self.baudrate
|
||||
payload = struct.pack('<BBH8BHHBBBB', port, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, inMask,
|
||||
outMask, 0, 0, 0, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)
|
||||
|
||||
def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
|
||||
'''configure configuration load/save'''
|
||||
payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
|
||||
|
||||
def configure_poll(self, msg_class, msg_id, payload=b''):
|
||||
'''poll a configuration message'''
|
||||
self.send_message(msg_class, msg_id, payload)
|
||||
|
||||
def configure_poll_port(self, portID=None):
|
||||
'''poll a port configuration'''
|
||||
if portID is None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
|
||||
else:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))
|
||||
|
||||
def configure_min_max_sats(self, min_sats=4, max_sats=32):
|
||||
'''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
|
||||
payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)
|
||||
|
||||
def module_reset(self, set, mode):
|
||||
''' Reset the module for hot/warm/cold start'''
|
||||
payload = struct.pack('<HBB', set, mode, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_RST, payload)
|
||||
@@ -1,90 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# type: ignore
|
||||
# pylint: skip-file
|
||||
|
||||
from selfdrive.locationd.test import ublox
|
||||
import struct
|
||||
|
||||
baudrate = 460800
|
||||
rate = 100 # send new data every 100ms
|
||||
|
||||
|
||||
def configure_ublox(dev):
|
||||
# configure ports and solution parameters and rate
|
||||
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
|
||||
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
|
||||
|
||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
|
||||
|
||||
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
|
||||
dev.configure_poll_port()
|
||||
dev.configure_poll_port(ublox.PORT_SERIAL1)
|
||||
dev.configure_poll_port(ublox.PORT_USB)
|
||||
dev.configure_solution_rate(rate_ms=rate)
|
||||
|
||||
# Configure solution
|
||||
payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
|
||||
payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
|
||||
3, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
|
||||
#bits_ITMF_config1 = '10101101011000101010110111111111'
|
||||
#bits_ITMF_config2 = '00000000000000000110001100011110'
|
||||
ITMF_config1 = 2908925439
|
||||
ITMF_config2 = 25374
|
||||
payload = struct.pack('<II', ITMF_config1, ITMF_config2)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF, payload)
|
||||
payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, (1 << 10), 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
|
||||
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF)
|
||||
|
||||
# Configure RAW, PVT and HW messages to be sent every solution cycle
|
||||
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_SAT, 1)
|
||||
|
||||
# Query the backup restore status
|
||||
print("backup restore polling message (implement custom response handler!):")
|
||||
dev.configure_poll(0x09, 0x14)
|
||||
|
||||
print("if successful, send this to clear the flash:")
|
||||
dev.send_message(0x09, 0x14, b"\x01\x00\x00\x00")
|
||||
|
||||
print("send on stop:")
|
||||
|
||||
# Save on shutdown
|
||||
# Controlled GNSS stop and hot start
|
||||
payload = struct.pack('<HBB', 0x0000, 0x08, 0x00)
|
||||
dev.send_message(ublox.CLASS_CFG, ublox.MSG_CFG_RST, payload)
|
||||
|
||||
# UBX-UPD-SOS backup
|
||||
dev.send_message(0x09, 0x14, b"\x00\x00\x00\x00")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
class Device:
|
||||
def write(self, s):
|
||||
d = '"{}"s'.format(''.join('\\x{:02X}'.format(b) for b in s))
|
||||
print(f" if (!send_with_ack({d})) continue;")
|
||||
|
||||
dev = ublox.UBlox(Device(), baudrate=baudrate)
|
||||
configure_ublox(dev)
|
||||
Reference in New Issue
Block a user