Ubloxd cleanup (#20637)
* clean out locationd/test
* get rid of ubloxd_main
* remove ubloxd_test.cc
* less includes
old-commit-hash: 4dd4b12140
commatwo_master
Willem Melching
4 years ago
committed by
GitHub
parent
51e12611ce
commit
2d6d92402a
14 changed files with 105 additions and 981 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 |
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:a15bc80894142f5b959fe83b627edf0c658ce881bac9447126b2979fdf5a2e1a |
||||
size 2069996 |
||||
@ -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 "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"}); |
||||
|
||||
if (p.size()) { |
||||
return p[0]->receive(); |
||||
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 { |
||||
return NULL; |
||||
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); |
||||
return 0; |
||||
} |
||||
@ -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