mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-22 08:43:54 +08:00
George Hotz
@@ -0,0 +1 @@
|
||||
out/
|
||||
Executable
+79
@@ -0,0 +1,79 @@
|
||||
#!/usr/bin/env python3
|
||||
import subprocess
|
||||
import os
|
||||
import sys
|
||||
import argparse
|
||||
import tempfile
|
||||
|
||||
from selfdrive.locationd.test.ubloxd_py_test import parser_test
|
||||
from selfdrive.locationd.test.ubloxd_regression_test import compare_results
|
||||
|
||||
|
||||
def mkdirs_exists_ok(path):
|
||||
try:
|
||||
os.makedirs(path)
|
||||
except OSError:
|
||||
if not os.path.isdir(path):
|
||||
raise
|
||||
|
||||
|
||||
def main(args):
|
||||
cur_dir = os.path.dirname(os.path.realpath(__file__))
|
||||
ubloxd_dir = os.path.join(cur_dir, '../')
|
||||
|
||||
cc_output_dir = os.path.join(args.output_dir, 'cc')
|
||||
mkdirs_exists_ok(cc_output_dir)
|
||||
|
||||
py_output_dir = os.path.join(args.output_dir, 'py')
|
||||
mkdirs_exists_ok(py_output_dir)
|
||||
|
||||
archive_file = os.path.join(cur_dir, args.stream_gz_file)
|
||||
|
||||
try:
|
||||
print('Extracting stream file')
|
||||
subprocess.check_call(['tar', 'zxf', archive_file], cwd=tempfile.gettempdir())
|
||||
stream_file_path = os.path.join(tempfile.gettempdir(), 'ubloxRaw.stream')
|
||||
|
||||
if not os.path.isfile(stream_file_path):
|
||||
print('Extract file failed')
|
||||
sys.exit(-3)
|
||||
|
||||
print('Run regression test - CC parser...')
|
||||
if args.valgrind:
|
||||
subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir])
|
||||
else:
|
||||
subprocess.check_call([os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir])
|
||||
|
||||
print('Running regression test - py parser...')
|
||||
parser_test(stream_file_path, py_output_dir)
|
||||
|
||||
print('Running regression test - compare result...')
|
||||
r = compare_results(cc_output_dir, py_output_dir)
|
||||
|
||||
print('All done!')
|
||||
|
||||
subprocess.check_call(["rm", stream_file_path])
|
||||
subprocess.check_call(["rm", '-rf', cc_output_dir])
|
||||
subprocess.check_call(["rm", '-rf', py_output_dir])
|
||||
sys.exit(r)
|
||||
|
||||
except subprocess.CalledProcessError as e:
|
||||
print('CI test failed with {}'.format(e.returncode))
|
||||
sys.exit(e.returncode)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Ubloxd CI test",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("stream_gz_file", nargs='?', default='ubloxRaw.tar.gz',
|
||||
help="UbloxRaw data stream zip file")
|
||||
|
||||
parser.add_argument("output_dir", nargs='?', default='out',
|
||||
help="Output events temp directory")
|
||||
|
||||
parser.add_argument("--valgrind", default=False, action='store_true',
|
||||
help="Run in valgrind")
|
||||
|
||||
args = parser.parse_args()
|
||||
main(args)
|
||||
@@ -0,0 +1,137 @@
|
||||
def GET_FIELD_U(w, nb, pos):
|
||||
return (((w) >> (pos)) & ((1 << (nb)) - 1))
|
||||
|
||||
|
||||
def twos_complement(v, nb):
|
||||
sign = v >> (nb - 1)
|
||||
value = v
|
||||
if sign != 0:
|
||||
value = value - (1 << nb)
|
||||
return value
|
||||
|
||||
|
||||
def GET_FIELD_S(w, nb, pos):
|
||||
v = GET_FIELD_U(w, nb, pos)
|
||||
return twos_complement(v, nb)
|
||||
|
||||
|
||||
def extract_uint8(v, b):
|
||||
return (v >> (8 * (3 - b))) & 255
|
||||
|
||||
def extract_int8(v, b):
|
||||
value = extract_uint8(v, b)
|
||||
if value > 127:
|
||||
value -= 256
|
||||
return value
|
||||
|
||||
class EphemerisData:
|
||||
'''container for parsing a AID_EPH message
|
||||
Thanks to Sylvain Munaut <tnt@246tNt.com>
|
||||
http://cgit.osmocom.org/cgit/osmocom-lcs/tree/gps.c
|
||||
on of this parser
|
||||
|
||||
See IS-GPS-200F.pdf Table 20-III for the field meanings, scaling factors and
|
||||
field widths
|
||||
'''
|
||||
|
||||
def __init__(self, svId, subframes):
|
||||
from math import pow
|
||||
self.svId = svId
|
||||
week_no = GET_FIELD_U(subframes[1][2+0], 10, 20)
|
||||
# code_on_l2 = GET_FIELD_U(subframes[1][0], 2, 12)
|
||||
# sv_ura = GET_FIELD_U(subframes[1][0], 4, 8)
|
||||
# sv_health = GET_FIELD_U(subframes[1][0], 6, 2)
|
||||
# l2_p_flag = GET_FIELD_U(subframes[1][1], 1, 23)
|
||||
t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6)
|
||||
iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U(
|
||||
subframes[1][2+5], 8, 22)
|
||||
|
||||
t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6)
|
||||
a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22)
|
||||
a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6)
|
||||
a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8)
|
||||
|
||||
c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6)
|
||||
delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14)
|
||||
m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[2][2+2], 24, 6)
|
||||
c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14)
|
||||
e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6)
|
||||
c_us = GET_FIELD_S(subframes[2][2+5], 16, 14)
|
||||
a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[2][2+6], 24, 6)
|
||||
t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14)
|
||||
# fit_flag = GET_FIELD_U(subframes[2][7], 1, 7)
|
||||
|
||||
c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14)
|
||||
omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[3][2+1], 24, 6)
|
||||
c_is = GET_FIELD_S(subframes[3][2+2], 16, 14)
|
||||
i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[3][2+3], 24, 6)
|
||||
c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14)
|
||||
w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6)
|
||||
omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6)
|
||||
idot = GET_FIELD_S(subframes[3][2+7], 14, 8)
|
||||
|
||||
self._rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6)
|
||||
self._rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6)
|
||||
self._rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6)
|
||||
self._rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14)
|
||||
self.aodo = GET_FIELD_U(subframes[2][2+7], 5, 8)
|
||||
|
||||
# Definition of Pi used in the GPS coordinate system
|
||||
gpsPi = 3.1415926535898
|
||||
|
||||
# now form variables in radians, meters and seconds etc
|
||||
self.Tgd = t_gd * pow(2, -31)
|
||||
self.A = pow(a_powhalf * pow(2, -19), 2.0)
|
||||
self.cic = c_ic * pow(2, -29)
|
||||
self.cis = c_is * pow(2, -29)
|
||||
self.crc = c_rc * pow(2, -5)
|
||||
self.crs = c_rs * pow(2, -5)
|
||||
self.cuc = c_uc * pow(2, -29)
|
||||
self.cus = c_us * pow(2, -29)
|
||||
self.deltaN = delta_n * pow(2, -43) * gpsPi
|
||||
self.ecc = e * pow(2, -33)
|
||||
self.i0 = i_0 * pow(2, -31) * gpsPi
|
||||
self.idot = idot * pow(2, -43) * gpsPi
|
||||
self.M0 = m_0 * pow(2, -31) * gpsPi
|
||||
self.omega = w * pow(2, -31) * gpsPi
|
||||
self.omega_dot = omega_dot * pow(2, -43) * gpsPi
|
||||
self.omega0 = omega_0 * pow(2, -31) * gpsPi
|
||||
self.toe = t_oe * pow(2, 4)
|
||||
|
||||
# clock correction information
|
||||
self.toc = t_oc * pow(2, 4)
|
||||
self.gpsWeek = week_no
|
||||
self.af0 = a_f0 * pow(2, -31)
|
||||
self.af1 = a_f1 * pow(2, -43)
|
||||
self.af2 = a_f2 * pow(2, -55)
|
||||
|
||||
iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22)
|
||||
iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22)
|
||||
self.valid = (iode1 == iode2) and (iode1 == (iodc & 0xff))
|
||||
self.iode = iode1
|
||||
|
||||
if GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 and \
|
||||
GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 and \
|
||||
GET_FIELD_U(subframes[5][2+0], 2, 28) == 1:
|
||||
a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30)
|
||||
a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27)
|
||||
a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24)
|
||||
a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24)
|
||||
b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11)
|
||||
b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14)
|
||||
b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16)
|
||||
b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16)
|
||||
|
||||
self.ionoAlpha = [a0, a1, a2, a3]
|
||||
self.ionoBeta = [b0, b1, b2, b3]
|
||||
self.ionoCoeffsValid = True
|
||||
else:
|
||||
self.ionoAlpha = []
|
||||
self.ionoBeta = []
|
||||
self.ionoCoeffsValid = False
|
||||
|
||||
|
||||
+53
@@ -0,0 +1,53 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import numpy as np
|
||||
import unittest
|
||||
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
from selfdrive.car.honda.values import CAR
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error
|
||||
|
||||
|
||||
class TestParamsLearner(unittest.TestCase):
|
||||
def setUp(self):
|
||||
|
||||
self.CP = CarInterface.get_params(CAR.CIVIC)
|
||||
bts = self.CP.to_bytes()
|
||||
|
||||
self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0)
|
||||
|
||||
def test_convergence(self):
|
||||
# Setup vehicle model with wrong parameters
|
||||
VM_sim = VehicleModel(self.CP)
|
||||
x_target = 0.75
|
||||
sr_target = self.CP.steerRatio - 0.5
|
||||
ao_target = -1.0
|
||||
VM_sim.update_params(x_target, sr_target)
|
||||
|
||||
# Run simulation
|
||||
times = np.arange(0, 15*3600, 0.01)
|
||||
angle_offset = np.radians(ao_target)
|
||||
steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset
|
||||
speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25
|
||||
|
||||
for i, t in enumerate(times):
|
||||
u = speeds[i]
|
||||
sa = steering_angles[i]
|
||||
psi = VM_sim.yaw_rate(sa - angle_offset, u)
|
||||
liblocationd.params_learner_update(self.params_learner, psi, u, sa)
|
||||
|
||||
# Verify learned parameters
|
||||
sr = liblocationd.params_learner_get_sR(self.params_learner)
|
||||
ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner))
|
||||
x = liblocationd.params_learner_get_x(self.params_learner)
|
||||
self.assertAlmostEqual(x_target, x, places=1)
|
||||
self.assertAlmostEqual(ao_target, ao_slow, places=1)
|
||||
self.assertAlmostEqual(sr_target, sr, places=1)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,996 @@
|
||||
#!/usr/bin/env python3
|
||||
'''
|
||||
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, os
|
||||
|
||||
# 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
|
||||
|
||||
# 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_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 not fieldname 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 not fieldname 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 += '%s="%s", ' % (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_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_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', '<IIIIHHBBBBIB25BHIII', [
|
||||
'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
|
||||
'flags', 'reserved1', 'usedMask', 'VP[25]', '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 not type 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 not type 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 not type 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:
|
||||
'''main UBlox control class.
|
||||
|
||||
port can be a file (for reading only) or a serial device
|
||||
'''
|
||||
|
||||
def __init__(self, port, baudrate=115200, timeout=0, panda=False, grey=False):
|
||||
|
||||
self.serial_device = port
|
||||
self.baudrate = baudrate
|
||||
self.use_sendrecv = False
|
||||
self.read_only = False
|
||||
self.debug_level = 0
|
||||
|
||||
if panda:
|
||||
from panda import Panda, PandaSerial
|
||||
|
||||
self.panda = Panda()
|
||||
|
||||
# resetting U-Blox module
|
||||
self.panda.set_esp_power(0)
|
||||
time.sleep(0.1)
|
||||
self.panda.set_esp_power(1)
|
||||
time.sleep(0.5)
|
||||
|
||||
# can't set above 9600 now...
|
||||
self.baudrate = 9600
|
||||
self.dev = PandaSerial(self.panda, 1, self.baudrate)
|
||||
|
||||
self.baudrate = 460800
|
||||
print("upping baud:",self.baudrate)
|
||||
self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate)
|
||||
time.sleep(0.1)
|
||||
|
||||
self.dev = PandaSerial(self.panda, 1, self.baudrate)
|
||||
elif grey:
|
||||
import cereal.messaging as messaging
|
||||
|
||||
class BoarddSerial():
|
||||
def __init__(self):
|
||||
self.ubloxRaw = messaging.sub_sock('ubloxRaw')
|
||||
self.buf = ""
|
||||
|
||||
def read(self, n):
|
||||
for msg in messaging.drain_sock(self.ubloxRaw, len(self.buf) < n):
|
||||
self.buf += msg.ubloxRaw
|
||||
ret = self.buf[:n]
|
||||
self.buf = self.buf[n:]
|
||||
return ret
|
||||
|
||||
|
||||
def write(self, dat):
|
||||
pass
|
||||
|
||||
self.dev = BoarddSerial()
|
||||
else:
|
||||
if self.serial_device.startswith("tcp:"):
|
||||
import socket
|
||||
a = self.serial_device.split(':')
|
||||
destination_addr = (a[1], int(a[2]))
|
||||
self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.dev.connect(destination_addr)
|
||||
self.dev.setblocking(1)
|
||||
self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
|
||||
self.use_sendrecv = True
|
||||
elif os.path.isfile(self.serial_device):
|
||||
self.read_only = True
|
||||
self.dev = open(self.serial_device, mode='rb')
|
||||
else:
|
||||
import serial
|
||||
self.dev = serial.Serial(
|
||||
self.serial_device,
|
||||
baudrate=self.baudrate,
|
||||
dsrdtr=False,
|
||||
rtscts=False,
|
||||
xonxoff=False,
|
||||
timeout=timeout)
|
||||
|
||||
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)
|
||||
return self.dev.write(buf)
|
||||
|
||||
def read(self, n):
|
||||
'''read some bytes'''
|
||||
if self.use_sendrecv:
|
||||
import socket
|
||||
try:
|
||||
return self.dev.recv(n)
|
||||
except socket.error:
|
||||
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=''):
|
||||
'''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)
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:a15bc80894142f5b959fe83b627edf0c658ce881bac9447126b2979fdf5a2e1a
|
||||
size 2069996
|
||||
Executable
+287
@@ -0,0 +1,287 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import serial
|
||||
from selfdrive.locationd.test import ublox
|
||||
import time
|
||||
import datetime
|
||||
import struct
|
||||
import sys
|
||||
from cereal import log
|
||||
from common import realtime
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U
|
||||
|
||||
panda = os.getenv("PANDA") is not None # panda directly connected
|
||||
grey = not (os.getenv("EVAL") is not None) # panda through boardd
|
||||
debug = os.getenv("DEBUG") is not None # debug prints
|
||||
print_dB = os.getenv("PRINT_DB") is not None # print antenna dB
|
||||
|
||||
timeout = 1
|
||||
dyn_model = 4 # auto model
|
||||
baudrate = 460800
|
||||
ports = ["/dev/ttyACM0","/dev/ttyACM1"]
|
||||
rate = 100 # send new data every 100ms
|
||||
|
||||
# which SV IDs we have seen and when we got iono
|
||||
svid_seen = {}
|
||||
svid_ephemeris = {}
|
||||
iono_seen = 0
|
||||
|
||||
def configure_ublox(dev):
|
||||
# configure ports and solution parameters and rate
|
||||
# TODO: configure constellations and channels to allow for 10Hz and high precision
|
||||
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
|
||||
|
||||
if panda:
|
||||
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
|
||||
else:
|
||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable 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_SERIAL2)
|
||||
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)
|
||||
#payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, 8192, 0, 0, 0,
|
||||
# 0, 0, 0, 0, 0, 0,
|
||||
# 0, 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_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)
|
||||
|
||||
# Configure RAW and PVT 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)
|
||||
|
||||
|
||||
|
||||
def int_to_bool_list(num):
|
||||
# for parsing bool bytes
|
||||
return [bool(num & (1<<n)) for n in range(8)]
|
||||
|
||||
|
||||
def gen_ephemeris(ephem_data):
|
||||
ephem = {'ephemeris':
|
||||
{'svId': ephem_data.svId,
|
||||
|
||||
'toc': ephem_data.toc,
|
||||
'gpsWeek': ephem_data.gpsWeek,
|
||||
|
||||
'af0': ephem_data.af0,
|
||||
'af1': ephem_data.af1,
|
||||
'af2': ephem_data.af2,
|
||||
|
||||
'iode': ephem_data.iode,
|
||||
'crs': ephem_data.crs,
|
||||
'deltaN': ephem_data.deltaN,
|
||||
'm0': ephem_data.M0,
|
||||
|
||||
'cuc': ephem_data.cuc,
|
||||
'ecc': ephem_data.ecc,
|
||||
'cus': ephem_data.cus,
|
||||
'a': ephem_data.A,
|
||||
|
||||
'toe': ephem_data.toe,
|
||||
'cic': ephem_data.cic,
|
||||
'omega0': ephem_data.omega0,
|
||||
'cis': ephem_data.cis,
|
||||
|
||||
'i0': ephem_data.i0,
|
||||
'crc': ephem_data.crc,
|
||||
'omega': ephem_data.omega,
|
||||
'omegaDot': ephem_data.omega_dot,
|
||||
|
||||
'iDot': ephem_data.idot,
|
||||
|
||||
'tgd': ephem_data.Tgd,
|
||||
|
||||
'ionoCoeffsValid': ephem_data.ionoCoeffsValid,
|
||||
'ionoAlpha': ephem_data.ionoAlpha,
|
||||
'ionoBeta': ephem_data.ionoBeta}}
|
||||
return log.Event.new_message(ubloxGnss=ephem)
|
||||
|
||||
|
||||
def gen_solution(msg):
|
||||
msg_data = msg.unpack()[0] # Solutions do not have any data in repeated blocks
|
||||
timestamp = int(((datetime.datetime(msg_data['year'],
|
||||
msg_data['month'],
|
||||
msg_data['day'],
|
||||
msg_data['hour'],
|
||||
msg_data['min'],
|
||||
msg_data['sec'])
|
||||
- datetime.datetime(1970,1,1)).total_seconds())*1e+03
|
||||
+ msg_data['nano']*1e-06)
|
||||
gps_fix = {'bearing': msg_data['headMot']*1e-05, # heading of motion in degrees
|
||||
'altitude': msg_data['height']*1e-03, # altitude above ellipsoid
|
||||
'latitude': msg_data['lat']*1e-07, # latitude in degrees
|
||||
'longitude': msg_data['lon']*1e-07, # longitude in degrees
|
||||
'speed': msg_data['gSpeed']*1e-03, # ground speed in meters
|
||||
'accuracy': msg_data['hAcc']*1e-03, # horizontal accuracy (1 sigma?)
|
||||
'timestamp': timestamp, # UTC time in ms since start of UTC stime
|
||||
'vNED': [msg_data['velN']*1e-03,
|
||||
msg_data['velE']*1e-03,
|
||||
msg_data['velD']*1e-03], # velocity in NED frame in m/s
|
||||
'speedAccuracy': msg_data['sAcc']*1e-03, # speed accuracy in m/s
|
||||
'verticalAccuracy': msg_data['vAcc']*1e-03, # vertical accuracy in meters
|
||||
'bearingAccuracy': msg_data['headAcc']*1e-05, # heading accuracy in degrees
|
||||
'source': 'ublox',
|
||||
'flags': msg_data['flags'],
|
||||
}
|
||||
return log.Event.new_message(gpsLocationExternal=gps_fix)
|
||||
|
||||
def gen_nav_data(msg, nav_frame_buffer):
|
||||
# TODO this stuff needs to be parsed and published.
|
||||
# refer to https://www.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_%28UBX-13003221%29.pdf
|
||||
# section 9.1
|
||||
msg_meta_data, measurements = msg.unpack()
|
||||
|
||||
# parse GPS ephem
|
||||
gnssId = msg_meta_data['gnssId']
|
||||
if gnssId == 0:
|
||||
svId = msg_meta_data['svid']
|
||||
subframeId = GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
|
||||
words = []
|
||||
for m in measurements:
|
||||
words.append(m['dwrd'])
|
||||
|
||||
# parse from
|
||||
if subframeId == 1:
|
||||
nav_frame_buffer[gnssId][svId] = {}
|
||||
nav_frame_buffer[gnssId][svId][subframeId] = words
|
||||
elif subframeId-1 in nav_frame_buffer[gnssId][svId]:
|
||||
nav_frame_buffer[gnssId][svId][subframeId] = words
|
||||
if len(nav_frame_buffer[gnssId][svId]) == 5:
|
||||
ephem_data = EphemerisData(svId, nav_frame_buffer[gnssId][svId])
|
||||
return gen_ephemeris(ephem_data)
|
||||
|
||||
|
||||
|
||||
|
||||
def gen_raw(msg):
|
||||
# meta data is in first part of tuple
|
||||
# list of measurements is in second part
|
||||
msg_meta_data, measurements = msg.unpack()
|
||||
measurements_parsed = []
|
||||
for m in measurements:
|
||||
trackingStatus_bools = int_to_bool_list(m['trkStat'])
|
||||
trackingStatus = {'pseudorangeValid': trackingStatus_bools[0],
|
||||
'carrierPhaseValid': trackingStatus_bools[1],
|
||||
'halfCycleValid': trackingStatus_bools[2],
|
||||
'halfCycleSubtracted': trackingStatus_bools[3]}
|
||||
measurements_parsed.append({
|
||||
'svId': m['svId'],
|
||||
'sigId': m['sigId'],
|
||||
'pseudorange': m['prMes'],
|
||||
'carrierCycles': m['cpMes'],
|
||||
'doppler': m['doMes'],
|
||||
'gnssId': m['gnssId'],
|
||||
'glonassFrequencyIndex': m['freqId'],
|
||||
'locktime': m['locktime'],
|
||||
'cno': m['cno'],
|
||||
'pseudorangeStdev': 0.01*(2**(m['prStdev'] & 15)), # weird scaling, might be wrong
|
||||
'carrierPhaseStdev': 0.004*(m['cpStdev'] & 15),
|
||||
'dopplerStdev': 0.002*(2**(m['doStdev'] & 15)), # weird scaling, might be wrong
|
||||
'trackingStatus': trackingStatus})
|
||||
if print_dB:
|
||||
cnos = {}
|
||||
for meas in measurements_parsed:
|
||||
cnos[meas['svId']] = meas['cno']
|
||||
print('Carrier to noise ratio for each sat: \n', cnos, '\n')
|
||||
receiverStatus_bools = int_to_bool_list(msg_meta_data['recStat'])
|
||||
receiverStatus = {'leapSecValid': receiverStatus_bools[0],
|
||||
'clkReset': receiverStatus_bools[2]}
|
||||
raw_meas = {'measurementReport': {'rcvTow': msg_meta_data['rcvTow'],
|
||||
'gpsWeek': msg_meta_data['week'],
|
||||
'leapSeconds': msg_meta_data['leapS'],
|
||||
'receiverStatus': receiverStatus,
|
||||
'numMeas': msg_meta_data['numMeas'],
|
||||
'measurements': measurements_parsed}}
|
||||
return log.Event.new_message(ubloxGnss=raw_meas)
|
||||
|
||||
def init_reader():
|
||||
port_counter = 0
|
||||
while True:
|
||||
try:
|
||||
dev = ublox.UBlox(ports[port_counter], baudrate=baudrate, timeout=timeout, panda=panda, grey=grey)
|
||||
configure_ublox(dev)
|
||||
return dev
|
||||
except serial.serialutil.SerialException as e:
|
||||
print(e)
|
||||
port_counter = (port_counter + 1)%len(ports)
|
||||
time.sleep(2)
|
||||
|
||||
def handle_msg(dev, msg, nav_frame_buffer):
|
||||
try:
|
||||
if debug:
|
||||
print(str(msg))
|
||||
sys.stdout.flush()
|
||||
if msg.name() == 'NAV_PVT':
|
||||
sol = gen_solution(msg)
|
||||
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
gpsLocationExternal.send(sol.to_bytes())
|
||||
elif msg.name() == 'RXM_RAW':
|
||||
raw = gen_raw(msg)
|
||||
raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(raw.to_bytes())
|
||||
elif msg.name() == 'RXM_SFRBX':
|
||||
nav = gen_nav_data(msg, nav_frame_buffer)
|
||||
if nav is not None:
|
||||
nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(nav.to_bytes())
|
||||
|
||||
else:
|
||||
print("UNKNNOWN MESSAGE:", msg.name())
|
||||
except ublox.UBloxError as e:
|
||||
print(e)
|
||||
|
||||
#if dev is not None and dev.dev is not None:
|
||||
# dev.close()
|
||||
|
||||
def main(gctx=None):
|
||||
global gpsLocationExternal, ubloxGnss
|
||||
nav_frame_buffer = {}
|
||||
nav_frame_buffer[0] = {}
|
||||
for i in range(1,33):
|
||||
nav_frame_buffer[0][i] = {}
|
||||
|
||||
|
||||
gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
|
||||
ubloxGnss = messaging.pub_sock('ubloxGnss')
|
||||
|
||||
dev = init_reader()
|
||||
while True:
|
||||
try:
|
||||
msg = dev.receive_message()
|
||||
except serial.serialutil.SerialException as e:
|
||||
print(e)
|
||||
dev.close()
|
||||
dev = init_reader()
|
||||
if msg is not None:
|
||||
handle_msg(dev, msg, nav_frame_buffer)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+53
@@ -0,0 +1,53 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
from selfdrive.locationd.test import ublox
|
||||
from common import realtime
|
||||
from selfdrive.locationd.test.ubloxd import gen_raw, gen_solution
|
||||
import zmq
|
||||
import cereal.messaging as messaging
|
||||
|
||||
|
||||
unlogger = os.getenv("UNLOGGER") is not None # debug prints
|
||||
|
||||
def main(gctx=None):
|
||||
poller = zmq.Poller()
|
||||
|
||||
gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
|
||||
ubloxGnss = messaging.pub_sock('ubloxGnss')
|
||||
|
||||
# ubloxRaw = messaging.sub_sock('ubloxRaw', poller)
|
||||
|
||||
# buffer with all the messages that still need to be input into the kalman
|
||||
while 1:
|
||||
polld = poller.poll(timeout=1000)
|
||||
for sock, mode in polld:
|
||||
if mode != zmq.POLLIN:
|
||||
continue
|
||||
logs = messaging.drain_sock(sock)
|
||||
for log in logs:
|
||||
buff = log.ubloxRaw
|
||||
time = log.logMonoTime
|
||||
msg = ublox.UBloxMessage()
|
||||
msg.add(buff)
|
||||
if msg.valid():
|
||||
if msg.name() == 'NAV_PVT':
|
||||
sol = gen_solution(msg)
|
||||
if unlogger:
|
||||
sol.logMonoTime = time
|
||||
else:
|
||||
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
gpsLocationExternal.send(sol.to_bytes())
|
||||
elif msg.name() == 'RXM_RAW':
|
||||
raw = gen_raw(msg)
|
||||
if unlogger:
|
||||
raw.logMonoTime = time
|
||||
else:
|
||||
raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(raw.to_bytes())
|
||||
else:
|
||||
print("INVALID MESSAGE")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+77
@@ -0,0 +1,77 @@
|
||||
import sys
|
||||
import os
|
||||
|
||||
from selfdrive.locationd.test.ublox import UBloxMessage
|
||||
from selfdrive.locationd.test.ubloxd import gen_solution, gen_raw, gen_nav_data
|
||||
from common import realtime
|
||||
|
||||
|
||||
def mkdirs_exists_ok(path):
|
||||
try:
|
||||
os.makedirs(path)
|
||||
except OSError:
|
||||
if not os.path.isdir(path):
|
||||
raise
|
||||
|
||||
|
||||
def parser_test(fn, prefix):
|
||||
nav_frame_buffer = {}
|
||||
nav_frame_buffer[0] = {}
|
||||
for i in range(1, 33):
|
||||
nav_frame_buffer[0][i] = {}
|
||||
|
||||
if not os.path.exists(prefix):
|
||||
print('Prefix invalid')
|
||||
sys.exit(-1)
|
||||
|
||||
with open(fn, 'rb') as f:
|
||||
i = 0
|
||||
saved_i = 0
|
||||
msg = UBloxMessage()
|
||||
while True:
|
||||
n = msg.needed_bytes()
|
||||
b = f.read(n)
|
||||
if not b:
|
||||
break
|
||||
msg.add(b)
|
||||
if msg.valid():
|
||||
i += 1
|
||||
if msg.name() == 'NAV_PVT':
|
||||
sol = gen_solution(msg)
|
||||
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
|
||||
f1.write(sol.to_bytes())
|
||||
saved_i += 1
|
||||
elif msg.name() == 'RXM_RAW':
|
||||
raw = gen_raw(msg)
|
||||
raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
|
||||
f1.write(raw.to_bytes())
|
||||
saved_i += 1
|
||||
elif msg.name() == 'RXM_SFRBX':
|
||||
nav = gen_nav_data(msg, nav_frame_buffer)
|
||||
if nav is not None:
|
||||
nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
|
||||
f1.write(nav.to_bytes())
|
||||
saved_i += 1
|
||||
|
||||
msg = UBloxMessage()
|
||||
msg.debug_level = 0
|
||||
print('Parsed {} msgs'.format(i))
|
||||
print('Generated {} cereal events'.format(saved_i))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) < 3:
|
||||
print('Format: ubloxd_py_test.py file_path prefix')
|
||||
sys.exit(0)
|
||||
|
||||
fn = sys.argv[1]
|
||||
if not os.path.isfile(fn):
|
||||
print('File path invalid')
|
||||
sys.exit(0)
|
||||
|
||||
prefix = sys.argv[2]
|
||||
mkdirs_exists_ok(prefix)
|
||||
parser_test(fn, prefix)
|
||||
@@ -0,0 +1,96 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import argparse
|
||||
|
||||
from cereal import log
|
||||
from common.basedir import BASEDIR
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Compare two result files",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("dir1", nargs='?', default='/data/ubloxdc',
|
||||
help="Directory path 1 from which events are loaded")
|
||||
|
||||
parser.add_argument("dir2", nargs='?', default='/data/ubloxdpy',
|
||||
help="Directory path 2 from which msgs are loaded")
|
||||
|
||||
return parser
|
||||
|
||||
|
||||
def read_file(fn):
|
||||
with open(fn, 'rb') as f:
|
||||
return f.read()
|
||||
|
||||
|
||||
def compare_results(dir1, dir2):
|
||||
onlyfiles1 = [f for f in os.listdir(dir1) if os.path.isfile(os.path.join(dir1, f))]
|
||||
onlyfiles1.sort()
|
||||
|
||||
onlyfiles2 = [f for f in os.listdir(dir2) if os.path.isfile(os.path.join(dir2, f))]
|
||||
onlyfiles2.sort()
|
||||
|
||||
if len(onlyfiles1) != len(onlyfiles2):
|
||||
print('len mismatch: {} != {}'.format(len(onlyfiles1), len(onlyfiles2)))
|
||||
return -1
|
||||
events1 = [log.Event.from_bytes(read_file(os.path.join(dir1, f))) for f in onlyfiles1]
|
||||
events2 = [log.Event.from_bytes(read_file(os.path.join(dir2, f))) for f in onlyfiles2]
|
||||
|
||||
for i in range(len(events1)):
|
||||
if events1[i].which() != events2[i].which():
|
||||
print('event {} type mismatch: {} != {}'.format(i, events1[i].which(), events2[i].which()))
|
||||
return -2
|
||||
if events1[i].which() == 'gpsLocationExternal':
|
||||
old_gps = events1[i].gpsLocationExternal
|
||||
gps = events2[i].gpsLocationExternal
|
||||
# print(gps, old_gps)
|
||||
attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing',
|
||||
'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy']
|
||||
for attr in attrs:
|
||||
o = getattr(old_gps, attr)
|
||||
n = getattr(gps, attr)
|
||||
if attr == 'vNED':
|
||||
if len(o) != len(n):
|
||||
print('Gps vNED len mismatch', o, n)
|
||||
return -3
|
||||
else:
|
||||
for i in range(len(o)):
|
||||
if abs(o[i] - n[i]) > 1e-3:
|
||||
print('Gps vNED mismatch', o, n)
|
||||
return
|
||||
elif o != n:
|
||||
print('Gps mismatch', attr, o, n)
|
||||
return -4
|
||||
elif events1[i].which() == 'ubloxGnss':
|
||||
old_gnss = events1[i].ubloxGnss
|
||||
gnss = events2[i].ubloxGnss
|
||||
if old_gnss.which() == 'measurementReport' and gnss.which() == 'measurementReport':
|
||||
attrs = ['gpsWeek', 'leapSeconds', 'measurements', 'numMeas', 'rcvTow', 'receiverStatus', 'schema']
|
||||
for attr in attrs:
|
||||
o = getattr(old_gnss.measurementReport, attr)
|
||||
n = getattr(gnss.measurementReport, attr)
|
||||
if str(o) != str(n):
|
||||
print('measurementReport {} mismatched'.format(attr))
|
||||
return -5
|
||||
if not (str(old_gnss.measurementReport) == str(gnss.measurementReport)):
|
||||
print('Gnss measurementReport mismatched!')
|
||||
print('gnss measurementReport old', old_gnss.measurementReport.measurements)
|
||||
print('gnss measurementReport new', gnss.measurementReport.measurements)
|
||||
return -6
|
||||
elif old_gnss.which() == 'ephemeris' and gnss.which() == 'ephemeris':
|
||||
if not (str(old_gnss.ephemeris) == str(gnss.ephemeris)):
|
||||
print('Gnss ephemeris mismatched!')
|
||||
print('gnss ephemeris old', old_gnss.ephemeris)
|
||||
print('gnss ephemeris new', gnss.ephemeris)
|
||||
return -7
|
||||
print('All {} events matched!'.format(len(events1)))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
compare_results(args.dir1, args.dir2)
|
||||
Reference in New Issue
Block a user