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

Ubloxd cleanup (#20637)

Browse Source 
* clean out locationd/test

* get rid of ubloxd_main

* remove ubloxd_test.cc

* less includes
old-commit-hash: 4dd4b1214061ebade872e7de73fce1800a9b74ca
This commit is contained in:
Willem Melching
2021-04-09 18:01:24 +02:00
committed by GitHub
parent 51e12611ce
commit 2d6d92402a
14 changed files with 107 additions and 983 deletions
Download Patch File Download Diff File Expand all files Collapse all files
release/files_common
-1
View File
@@ -281,7 +281,6 @@ selfdrive/locationd/__init__.py
selfdrive/locationd/.gitignore
selfdrive/locationd/SConscript
selfdrive/locationd/ubloxd.cc
selfdrive/locationd/ubloxd_main.cc
selfdrive/locationd/ublox_msg.cc
selfdrive/locationd/ublox_msg.h
selfdrive/boardd/pigeon.cc
+1 -2
View File
@@ -56,9 +56,8 @@ void Pigeon::init() {
util::sleep_for(100);
// init from ubloxd
// To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py
// To generate this data, run test/ubloxd.py
send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s);
send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"s);
send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s);
send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s);
send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s);
selfdrive/locationd/SConscript
+1 -4
View File
@@ -2,7 +2,4 @@ Import('env', 'common', 'cereal', 'messaging')
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread']
env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs)
if GetOption("test"):
env.Program("ubloxd_test", ["ubloxd_test.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs)
env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc"], LIBS=loc_libs)
selfdrive/locationd/test/ci_test.py
-80
View File
@@ -1,80 +0,0 @@
#!/usr/bin/env python3
# type: ignore
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)
selfdrive/locationd/test/ephemeris.py
-136
View File
@@ -1,136 +0,0 @@
import math
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):
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 * math.pow(2, -31)
self.A = math.pow(a_powhalf * math.pow(2, -19), 2.0)
self.cic = c_ic * math.pow(2, -29)
self.cis = c_is * math.pow(2, -29)
self.crc = c_rc * math.pow(2, -5)
self.crs = c_rs * math.pow(2, -5)
self.cuc = c_uc * math.pow(2, -29)
self.cus = c_us * math.pow(2, -29)
self.deltaN = delta_n * math.pow(2, -43) * gpsPi
self.ecc = e * math.pow(2, -33)
self.i0 = i_0 * math.pow(2, -31) * gpsPi
self.idot = idot * math.pow(2, -43) * gpsPi
self.M0 = m_0 * math.pow(2, -31) * gpsPi
self.omega = w * math.pow(2, -31) * gpsPi
self.omega_dot = omega_dot * math.pow(2, -43) * gpsPi
self.omega0 = omega_0 * math.pow(2, -31) * gpsPi
self.toe = t_oe * math.pow(2, 4)
# clock correction information
self.toc = t_oc * math.pow(2, 4)
self.gpsWeek = week_no
self.af0 = a_f0 * math.pow(2, -31)
self.af1 = a_f1 * math.pow(2, -43)
self.af2 = a_f2 * math.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) * math.pow(2, -30)
a1 = GET_FIELD_S(subframes[4][2], 8, 6) * math.pow(2, -27)
a2 = GET_FIELD_S(subframes[4][3], 8, 22) * math.pow(2, -24)
a3 = GET_FIELD_S(subframes[4][3], 8, 14) * math.pow(2, -24)
b0 = GET_FIELD_S(subframes[4][3], 8, 6) * math.pow(2, 11)
b1 = GET_FIELD_S(subframes[4][4], 8, 22) * math.pow(2, 14)
b2 = GET_FIELD_S(subframes[4][4], 8, 14) * math.pow(2, 16)
b3 = GET_FIELD_S(subframes[4][4], 8, 6) * math.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
selfdrive/locationd/test/ublox.py
+3 -72
View File
@@ -13,7 +13,6 @@ for ublox version 8, not all functions may work.
import struct
import os
import time
# protocol constants
@@ -689,82 +688,14 @@ class UBloxMessage:
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
def __init__(self, dev, baudrate):
self.dev = dev
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 = b""
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
selfdrive/locationd/test/ubloxRaw.tar.gz
-3
View File
@@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a15bc80894142f5b959fe83b627edf0c658ce881bac9447126b2979fdf5a2e1a
size 2069996
selfdrive/locationd/test/ubloxd.py
+8 -238
View File
@@ -2,37 +2,16 @@
# type: ignore
# pylint: skip-file
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
panda = True
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
@@ -82,220 +61,11 @@ def configure_ublox(dev):
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 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 = {'bearingDeg': 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
'bearingAccuracyDeg': 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 gen_hw_status(msg):
msg_data = msg.unpack()[0]
ublox_hw_status = {'hwStatus': {
'noisePerMS': msg_data['noisePerMS'],
'agcCnt': msg_data['agcCnt'],
'aStatus': msg_data['aStatus'],
'aPower': msg_data['aPower'],
'jamInd': msg_data['jamInd']
}}
return log.Event.new_message(ubloxGnss=ublox_hw_status)
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())
elif msg.name() == 'MON_HW':
hw = gen_hw_status(msg)
hw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
ubloxGnss.send(hw.to_bytes())
else:
print("UNKNOWN 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():
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()
class Device:
def write(self, s):
d = '"{}"s'.format(''.join('\\x{:02X}'.format(b) for b in s))
print(f"send({d});")
dev = ublox.UBlox(Device(), baudrate=baudrate)
configure_ublox(dev)
selfdrive/locationd/test/ubloxd_easy.py
-54
View File
@@ -1,54 +0,0 @@
#!/usr/bin/env python3
# type: ignore
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():
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()
selfdrive/locationd/test/ubloxd_py_test.py
-79
View File
@@ -1,79 +0,0 @@
# type: ignore
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)
selfdrive/locationd/test/ubloxd_regression_test.py
-96
View File
@@ -1,96 +0,0 @@
#!/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', 'bearingDeg',
'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracyDeg', '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)
selfdrive/locationd/ubloxd.cc
+94 -17
View File
@@ -1,23 +1,100 @@
#include <stdio.h>
#include "messaging.hpp"
#include "common/util.h"
#include "common/swaglog.h"
#include "ublox_msg.h"
Message * poll_ubloxraw_msg(Poller * poller) {
auto p = poller->poll(1000);
if (p.size()) {
return p[0]->receive();
} else {
return NULL;
}
}
int send_gps_event(PubSocket *s, const void *buf, size_t len) {
return s->send((char*)buf, len);
}
ExitHandler do_exit;
using namespace ublox;
int main() {
return ubloxd_main(poll_ubloxraw_msg, send_gps_event);
}
LOGW("starting ubloxd");
AlignedBuffer aligned_buf;
UbloxMsgParser parser;
Context * context = Context::create();
SubSocket * subscriber = SubSocket::create(context, "ubloxRaw");
assert(subscriber != NULL);
subscriber->setTimeout(100);
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
while (!do_exit) {
Message * msg = subscriber->receive();
if (!msg){
if (errno == EINTR) {
do_exit = true;
}
continue;
}
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto ubloxRaw = event.getUbloxRaw();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
size_t bytes_consumed = 0;
while(bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
// New message available
if(parser.msg_class() == CLASS_NAV) {
if(parser.msg_id() == MSG_NAV_PVT) {
//LOGD("MSG_NAV_PVT");
auto words = parser.gen_solution();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("gpsLocationExternal", bytes.begin(), bytes.size());
}
} else
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_RXM) {
if(parser.msg_id() == MSG_RXM_RAW) {
//LOGD("MSG_RXM_RAW");
auto words = parser.gen_raw();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
//LOGD("MSG_RXM_SFRBX");
auto words = parser.gen_nav_data();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else
LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_MON) {
if(parser.msg_id() == MSG_MON_HW) {
//LOGD("MSG_MON_HW");
auto words = parser.gen_mon_hw();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else if(parser.msg_id() == MSG_MON_HW2) {
//LOGD("MSG_MON_HW2");
auto words = parser.gen_mon_hw2();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else {
LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
}
} else
LOGW("Unknown msg class: 0x%02X", parser.msg_class());
parser.reset();
}
bytes_consumed += bytes_consumed_this_time;
}
delete msg;
}
delete subscriber;
delete context;
return 0;
}
selfdrive/locationd/ubloxd_main.cc
-114
View File
@@ -1,114 +0,0 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <assert.h>
#include <math.h>
#include <ctime>
#include <chrono>
#include "messaging.hpp"
#include "common/util.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "ublox_msg.h"
ExitHandler do_exit;
using namespace ublox;
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
LOGW("starting ubloxd");
AlignedBuffer aligned_buf;
UbloxMsgParser parser;
Context * context = Context::create();
SubSocket * subscriber = SubSocket::create(context, "ubloxRaw");
assert(subscriber != NULL);
subscriber->setTimeout(100);
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
while (!do_exit) {
Message * msg = subscriber->receive();
if (!msg){
if (errno == EINTR) {
do_exit = true;
}
continue;
}
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto ubloxRaw = event.getUbloxRaw();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
size_t bytes_consumed = 0;
while(bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
// New message available
if(parser.msg_class() == CLASS_NAV) {
if(parser.msg_id() == MSG_NAV_PVT) {
//LOGD("MSG_NAV_PVT");
auto words = parser.gen_solution();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("gpsLocationExternal", bytes.begin(), bytes.size());
}
} else
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_RXM) {
if(parser.msg_id() == MSG_RXM_RAW) {
//LOGD("MSG_RXM_RAW");
auto words = parser.gen_raw();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
//LOGD("MSG_RXM_SFRBX");
auto words = parser.gen_nav_data();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else
LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_MON) {
if(parser.msg_id() == MSG_MON_HW) {
//LOGD("MSG_MON_HW");
auto words = parser.gen_mon_hw();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else if(parser.msg_id() == MSG_MON_HW2) {
//LOGD("MSG_MON_HW2");
auto words = parser.gen_mon_hw2();
if(words.size() > 0) {
auto bytes = words.asBytes();
pm.send("ubloxGnss", bytes.begin(), bytes.size());
}
} else {
LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
}
} else
LOGW("Unknown msg class: 0x%02X", parser.msg_class());
parser.reset();
}
bytes_consumed += bytes_consumed_this_time;
}
delete msg;
}
delete subscriber;
delete context;
return 0;
}
selfdrive/locationd/ubloxd_test.cc
-87
View File
@@ -1,87 +0,0 @@
#include <stdio.h>
#include <iostream>
#include <cassert>
#include <algorithm>
#include "messaging.hpp"
#include "impl_zmq.hpp"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "ublox_msg.h"
using namespace ublox;
extern volatile sig_atomic_t do_exit;
void write_file(std::string fpath, uint8_t *to_write, int length) {
FILE* f = fopen(fpath.c_str(), "wb");
if (!f) {
std::cout << "Open " << fpath << " failed" << std::endl;
return;
}
fwrite(to_write, length, 1, f);
fclose(f);
}
static size_t len = 0U;
static size_t consumed = 0U;
static uint8_t *data = NULL;
static int save_idx = 0;
static std::string prefix;
Message * poll_ubloxraw_msg(Poller * poller) {
assert(poller);
size_t consuming = std::min((int)(len - consumed), 128);
if(consumed < len) {
// create message
MessageBuilder msg_builder;
auto ublox_raw = msg_builder.initEvent().initUbloxRaw(consuming);
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
auto bytes = msg_builder.toBytes();
Message * msg = new ZMQMessage();
msg->init((char*)bytes.begin(), bytes.size());
consumed += consuming;
return msg;
} else {
do_exit = 1;
return NULL;
}
}
int send_gps_event(PubSocket *s, const void *buf, size_t length) {
assert(s);
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, length);
save_idx++;
return length;
}
int main(int argc, char** argv) {
if(argc < 3) {
printf("Format: ubloxd_test stream_file_path save_prefix\n");
return 0;
}
// Parse 11360 msgs, generate 9452 events
data = (uint8_t *)read_file(argv[1], &len);
if(data == NULL) {
LOGE("Read file %s failed\n", argv[1]);
return -1;
}
prefix = argv[2];
ubloxd_main(poll_ubloxraw_msg, send_gps_event);
free(data);
printf("Generated %d cereal events\n", save_idx);
if(save_idx != 9452) {
printf("Event count error: %d\n", save_idx);
return -1;
}
return 0;
}