Ubloxd cleanup (#20637)
* clean out locationd/test * get rid of ubloxd_main * remove ubloxd_test.cc * less includespull/214/head
Willem Melching
5 years ago
committed by
GitHub
parent
2967e79296
commit
4dd4b12140
14 changed files with 105 additions and 978 deletions
@ -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) |
|
||||||
@ -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 |
|
||||||
Binary file not shown.
@ -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() |
|
||||||
@ -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) |
|
||||||
@ -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) |
|
||||||
@ -1,23 +1,100 @@ |
|||||||
#include <stdio.h> |
|
||||||
|
|
||||||
#include "messaging.hpp" |
#include "messaging.hpp" |
||||||
|
#include "common/util.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
|
||||||
#include "ublox_msg.h" |
#include "ublox_msg.h" |
||||||
|
|
||||||
Message * poll_ubloxraw_msg(Poller * poller) { |
ExitHandler do_exit; |
||||||
auto p = poller->poll(1000); |
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()) { |
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg)); |
||||||
return p[0]->receive(); |
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||||
} else { |
auto ubloxRaw = event.getUbloxRaw(); |
||||||
return NULL; |
|
||||||
|
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) { |
delete subscriber; |
||||||
return s->send((char*)buf, len); |
delete context; |
||||||
} |
|
||||||
|
|
||||||
int main() { |
return 0; |
||||||
return ubloxd_main(poll_ubloxraw_msg, send_gps_event); |
} |
||||||
} |
|
||||||
|
|||||||
@ -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; |
|
||||||
} |
|
||||||
@ -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; |
|
||||||
} |
|
||||||
Loading…
Reference in new issue