diff --git a/release/files_common b/release/files_common index ca1cf34b36..598815386e 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index 3f2588f252..336548383f 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -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); diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 43e5d1fecb..38f0d02653 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -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) diff --git a/selfdrive/locationd/test/ci_test.py b/selfdrive/locationd/test/ci_test.py deleted file mode 100755 index b715a499af..0000000000 --- a/selfdrive/locationd/test/ci_test.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/test/ephemeris.py b/selfdrive/locationd/test/ephemeris.py deleted file mode 100644 index 2793e3c62a..0000000000 --- a/selfdrive/locationd/test/ephemeris.py +++ /dev/null @@ -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 - 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 diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py index 5f561f2efd..006382dba7 100644 --- a/selfdrive/locationd/test/ublox.py +++ b/selfdrive/locationd/test/ublox.py @@ -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 diff --git a/selfdrive/locationd/test/ubloxRaw.tar.gz b/selfdrive/locationd/test/ubloxRaw.tar.gz deleted file mode 100644 index 9812f026c9..0000000000 --- a/selfdrive/locationd/test/ubloxRaw.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a15bc80894142f5b959fe83b627edf0c658ce881bac9447126b2979fdf5a2e1a -size 2069996 diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 6e26c12e72..470e679da8 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -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) diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py deleted file mode 100755 index 0f90b012bb..0000000000 --- a/selfdrive/locationd/test/ubloxd_easy.py +++ /dev/null @@ -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() diff --git a/selfdrive/locationd/test/ubloxd_py_test.py b/selfdrive/locationd/test/ubloxd_py_test.py deleted file mode 100755 index 7a573631b4..0000000000 --- a/selfdrive/locationd/test/ubloxd_py_test.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/test/ubloxd_regression_test.py b/selfdrive/locationd/test/ubloxd_regression_test.py deleted file mode 100644 index 4f496ccd6f..0000000000 --- a/selfdrive/locationd/test/ubloxd_regression_test.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc index 5fdcebba0b..566b82025a 100644 --- a/selfdrive/locationd/ubloxd.cc +++ b/selfdrive/locationd/ubloxd.cc @@ -1,23 +1,100 @@ -#include - #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); +ExitHandler do_exit; +using namespace ublox; + +int main() { + 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; + } - if (p.size()) { - return p[0]->receive(); - } else { - return NULL; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg)); + cereal::Event::Reader event = cmsg.getRoot(); + 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; } -} -int send_gps_event(PubSocket *s, const void *buf, size_t len) { - return s->send((char*)buf, len); -} + delete subscriber; + delete context; -int main() { - return ubloxd_main(poll_ubloxraw_msg, send_gps_event); -} \ No newline at end of file + return 0; +} diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc deleted file mode 100644 index 7fd3954345..0000000000 --- a/selfdrive/locationd/ubloxd_main.cc +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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(); - 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; -} diff --git a/selfdrive/locationd/ubloxd_test.cc b/selfdrive/locationd/ubloxd_test.cc deleted file mode 100644 index 85ebb3c53c..0000000000 --- a/selfdrive/locationd/ubloxd_test.cc +++ /dev/null @@ -1,87 +0,0 @@ -#include -#include -#include -#include - -#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; -}