Vehicle Researcher
6 years ago
parent
790732bea3
commit
2f92d577f9
99 changed files with 2756 additions and 1619 deletions
Binary file not shown.
Binary file not shown.
@ -0,0 +1,76 @@ |
|||||||
|
#from common.numpy_fast import clip |
||||||
|
from common.realtime import sec_since_boot |
||||||
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
||||||
|
from selfdrive.car import apply_std_steer_torque_limits |
||||||
|
from selfdrive.car.subaru import subarucan |
||||||
|
from selfdrive.car.subaru.values import CAR, DBC |
||||||
|
from selfdrive.can.packer import CANPacker |
||||||
|
|
||||||
|
|
||||||
|
class CarControllerParams(): |
||||||
|
def __init__(self, car_fingerprint): |
||||||
|
self.STEER_MAX = 2047 # max_steer 4095 |
||||||
|
self.STEER_STEP = 2 # how often we update the steer cmd |
||||||
|
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max |
||||||
|
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh |
||||||
|
if car_fingerprint == CAR.IMPREZA: |
||||||
|
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting |
||||||
|
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily |
||||||
|
self.STEER_DRIVER_FACTOR = 1 # from dbc |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class CarController(object): |
||||||
|
def __init__(self, car_fingerprint): |
||||||
|
self.start_time = sec_since_boot() |
||||||
|
self.lkas_active = False |
||||||
|
self.steer_idx = 0 |
||||||
|
self.apply_steer_last = 0 |
||||||
|
self.car_fingerprint = car_fingerprint |
||||||
|
self.es_distance_cnt = -1 |
||||||
|
self.es_lkas_cnt = -1 |
||||||
|
|
||||||
|
# Setup detection helper. Routes commands to |
||||||
|
# an appropriate CAN bus number. |
||||||
|
self.params = CarControllerParams(car_fingerprint) |
||||||
|
print(DBC) |
||||||
|
self.packer = CANPacker(DBC[car_fingerprint]['pt']) |
||||||
|
|
||||||
|
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert): |
||||||
|
""" Controls thread """ |
||||||
|
|
||||||
|
P = self.params |
||||||
|
|
||||||
|
# Send CAN commands. |
||||||
|
can_sends = [] |
||||||
|
|
||||||
|
### STEER ### |
||||||
|
|
||||||
|
if (frame % P.STEER_STEP) == 0: |
||||||
|
|
||||||
|
final_steer = actuators.steer if enabled else 0. |
||||||
|
apply_steer = int(round(final_steer * P.STEER_MAX)) |
||||||
|
|
||||||
|
# limits due to driver torque |
||||||
|
|
||||||
|
apply_steer = int(round(apply_steer)) |
||||||
|
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) |
||||||
|
|
||||||
|
lkas_enabled = enabled and not CS.steer_not_allowed |
||||||
|
|
||||||
|
if not lkas_enabled: |
||||||
|
apply_steer = 0 |
||||||
|
|
||||||
|
can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP)) |
||||||
|
|
||||||
|
self.apply_steer_last = apply_steer |
||||||
|
|
||||||
|
if self.es_distance_cnt != CS.es_distance_msg["Counter"]: |
||||||
|
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd)) |
||||||
|
self.es_distance_cnt = CS.es_distance_msg["Counter"] |
||||||
|
|
||||||
|
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: |
||||||
|
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert)) |
||||||
|
self.es_lkas_cnt = CS.es_lkas_msg["Counter"] |
||||||
|
|
||||||
|
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) |
||||||
@ -0,0 +1,54 @@ |
|||||||
|
import copy |
||||||
|
from cereal import car |
||||||
|
from selfdrive.car.subaru.values import CAR |
||||||
|
|
||||||
|
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||||
|
|
||||||
|
def subaru_checksum(packer, values, addr): |
||||||
|
dat = packer.make_can_msg(addr, 0, values)[2] |
||||||
|
dat = [ord(i) for i in dat] |
||||||
|
return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff |
||||||
|
|
||||||
|
def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step): |
||||||
|
|
||||||
|
if car_fingerprint == CAR.IMPREZA: |
||||||
|
#counts from 0 to 15 then back to 0 + 16 for enable bit |
||||||
|
idx = ((frame / steer_step) % 16) |
||||||
|
|
||||||
|
values = { |
||||||
|
"Counter": idx, |
||||||
|
"LKAS_Output": apply_steer, |
||||||
|
"LKAS_Request": 1 if apply_steer != 0 else 0, |
||||||
|
"SET_1": 1 |
||||||
|
} |
||||||
|
values["Checksum"] = subaru_checksum(packer, values, 0x122) |
||||||
|
|
||||||
|
return packer.make_can_msg("ES_LKAS", 0, values) |
||||||
|
|
||||||
|
def create_steering_status(packer, car_fingerprint, apply_steer, frame, steer_step): |
||||||
|
|
||||||
|
if car_fingerprint == CAR.IMPREZA: |
||||||
|
values = {} |
||||||
|
values["Checksum"] = subaru_checksum(packer, {}, 0x322) |
||||||
|
|
||||||
|
return packer.make_can_msg("ES_LKAS_State", 0, values) |
||||||
|
|
||||||
|
def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd): |
||||||
|
|
||||||
|
values = copy.copy(es_distance_msg) |
||||||
|
if pcm_cancel_cmd: |
||||||
|
values["Main"] = 1 |
||||||
|
|
||||||
|
values["Checksum"] = subaru_checksum(packer, values, 545) |
||||||
|
|
||||||
|
return packer.make_can_msg("ES_Distance", 0, values) |
||||||
|
|
||||||
|
def create_es_lkas(packer, es_lkas_msg, visual_alert): |
||||||
|
|
||||||
|
values = copy.copy(es_lkas_msg) |
||||||
|
if visual_alert == VisualAlert.steerRequired: |
||||||
|
values["Keep_Hands_On_Wheel"] = 1 |
||||||
|
|
||||||
|
values["Checksum"] = subaru_checksum(packer, values, 802) |
||||||
|
|
||||||
|
return packer.make_can_msg("ES_LKAS_State", 0, values) |
||||||
@ -1 +1 @@ |
|||||||
#define COMMA_VERSION "0.5.10-release" |
#define COMMA_VERSION "0.5.11-release" |
||||||
|
|||||||
@ -0,0 +1,99 @@ |
|||||||
|
import psutil |
||||||
|
import time |
||||||
|
import os |
||||||
|
import sys |
||||||
|
import numpy as np |
||||||
|
import argparse |
||||||
|
import re |
||||||
|
|
||||||
|
''' |
||||||
|
System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs. |
||||||
|
Features: |
||||||
|
Use psutil library to sample cpu usage(avergage for all cores) of OpenPilot processes, at a rate of 5 samples/sec. |
||||||
|
Do cpu usage statistics periodically, 5 seconds as a cycle. |
||||||
|
Caculate the average cpu usage within this cycle. |
||||||
|
Caculate minumium/maximium/accumulated_average cpu usage as long term inspections. |
||||||
|
Monitor multiple processes simuteneously. |
||||||
|
Sample usage: |
||||||
|
root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd |
||||||
|
('Add monitored proc:', './boardd') |
||||||
|
('Add monitored proc:', 'python locationd/ubloxd.py') |
||||||
|
boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96% |
||||||
|
ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39% |
||||||
|
''' |
||||||
|
|
||||||
|
# Do statistics every 5 seconds |
||||||
|
PRINT_INTERVAL = 5 |
||||||
|
SLEEP_INTERVAL = 0.2 |
||||||
|
|
||||||
|
monitored_proc_names = [ |
||||||
|
'ubloxd', 'thermald', 'uploader', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned', |
||||||
|
'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'calibrationd', 'locationd', 'visiond', 'sensord', 'updated', 'gpsd', 'athena'] |
||||||
|
|
||||||
|
|
||||||
|
def get_arg_parser(): |
||||||
|
parser = argparse.ArgumentParser( |
||||||
|
description="Unlogger and UI", |
||||||
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter) |
||||||
|
|
||||||
|
parser.add_argument("proc_names", nargs="?", default='', |
||||||
|
help="Process names to be monitored, comma seperated") |
||||||
|
parser.add_argument("--list_all", nargs="?", type=bool, default=False, |
||||||
|
help="Show all running processes' cmdline") |
||||||
|
return parser |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
args = get_arg_parser().parse_args(sys.argv[1:]) |
||||||
|
if args.list_all: |
||||||
|
for p in psutil.process_iter(): |
||||||
|
print('cmdline', p.cmdline(), 'name', p.name()) |
||||||
|
sys.exit(0) |
||||||
|
|
||||||
|
if len(args.proc_names) > 0: |
||||||
|
monitored_proc_names = args.proc_names.split(',') |
||||||
|
monitored_procs = [] |
||||||
|
stats = {} |
||||||
|
for p in psutil.process_iter(): |
||||||
|
if p == psutil.Process(): |
||||||
|
continue |
||||||
|
matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])]) |
||||||
|
if matched: |
||||||
|
k = ' '.join(p.cmdline()) |
||||||
|
print('Add monitored proc:', k) |
||||||
|
stats[k] = {'cpu_samples': [], 'avg_cpu': None, 'min': None, 'max': None} |
||||||
|
monitored_procs.append(p) |
||||||
|
i = 0 |
||||||
|
interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL) |
||||||
|
while True: |
||||||
|
for p in monitored_procs: |
||||||
|
k = ' '.join(p.cmdline()) |
||||||
|
stats[k]['cpu_samples'].append(p.cpu_percent()) |
||||||
|
time.sleep(SLEEP_INTERVAL) |
||||||
|
i += 1 |
||||||
|
if i % interval_int == 0: |
||||||
|
l = [] |
||||||
|
avg_cpus = [] |
||||||
|
for k, stat in stats.items(): |
||||||
|
if len(stat['cpu_samples']) <= 0: |
||||||
|
continue |
||||||
|
avg_cpu = np.array(stat['cpu_samples']).mean() |
||||||
|
c = len(stat['cpu_samples']) |
||||||
|
stat['cpu_samples'] = [] |
||||||
|
if not stat['avg_cpu']: |
||||||
|
stat['avg_cpu'] = avg_cpu |
||||||
|
else: |
||||||
|
stat['avg_cpu'] = (stat['avg_cpu'] * (c + i) + avg_cpu * c) / (c + i + c) |
||||||
|
if not stat['min'] or avg_cpu < stat['min']: |
||||||
|
stat['min'] = avg_cpu |
||||||
|
if not stat['max'] or avg_cpu > stat['max']: |
||||||
|
stat['max'] = avg_cpu |
||||||
|
msg = 'avg: {1:.2f}%, min: {2:.2f}%, max: {3:.2f}% {0}'.format(os.path.basename(k), stat['avg_cpu'], stat['min'], stat['max']) |
||||||
|
l.append((os.path.basename(k), avg_cpu, msg)) |
||||||
|
avg_cpus.append(avg_cpu) |
||||||
|
l.sort(key= lambda x: -x[1]) |
||||||
|
for x in l: |
||||||
|
print(x[2]) |
||||||
|
print('avg sum: {0:.2f}%\n'.format( |
||||||
|
sum([stat['avg_cpu'] for k, stat in stats.items()]) |
||||||
|
)) |
||||||
@ -0,0 +1,86 @@ |
|||||||
|
CC = clang
|
||||||
|
CXX = clang++
|
||||||
|
|
||||||
|
ARCH := $(shell uname -m)
|
||||||
|
OS := $(shell uname -o)
|
||||||
|
|
||||||
|
BASEDIR = ../..
|
||||||
|
PHONELIBS = ../../phonelibs
|
||||||
|
|
||||||
|
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||||
|
-Werror=incompatible-pointer-types \
|
||||||
|
-Werror=int-conversion \
|
||||||
|
-Werror=return-type \
|
||||||
|
-Werror=format-extra-args
|
||||||
|
|
||||||
|
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
|
||||||
|
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
|
||||||
|
|
||||||
|
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||||
|
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
|
||||||
|
-l:libczmq.a -l:libzmq.a \
|
||||||
|
-lgnustl_shared
|
||||||
|
|
||||||
|
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
||||||
|
|
||||||
|
EXTRA_LIBS = -lpthread
|
||||||
|
|
||||||
|
ifeq ($(ARCH),x86_64) |
||||||
|
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
|
||||||
|
-l:libczmq.a -l:libzmq.a
|
||||||
|
endif |
||||||
|
|
||||||
|
.PHONY: all |
||||||
|
all: ubloxd |
||||||
|
|
||||||
|
include ../common/cereal.mk |
||||||
|
|
||||||
|
OBJS = ublox_msg.o \
|
||||||
|
ubloxd_main.o \
|
||||||
|
../common/swaglog.o \
|
||||||
|
../common/params.o \
|
||||||
|
../common/util.o \
|
||||||
|
$(PHONELIBS)/json/src/json.o \
|
||||||
|
$(CEREAL_OBJS)
|
||||||
|
|
||||||
|
DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d
|
||||||
|
|
||||||
|
ubloxd: ubloxd.o $(OBJS) |
||||||
|
@echo "[ LINK ] $@"
|
||||||
|
$(CXX) -fPIC -o '$@' $^ \
|
||||||
|
$(CEREAL_LIBS) \
|
||||||
|
$(ZMQ_LIBS) \
|
||||||
|
$(EXTRA_LIBS)
|
||||||
|
|
||||||
|
ubloxd_test: ubloxd_test.o $(OBJS) |
||||||
|
@echo "[ LINK ] $@"
|
||||||
|
$(CXX) -fPIC -o '$@' $^ \
|
||||||
|
$(CEREAL_LIBS) \
|
||||||
|
$(ZMQ_LIBS) \
|
||||||
|
$(EXTRA_LIBS)
|
||||||
|
|
||||||
|
%.o: %.cc |
||||||
|
@echo "[ CXX ] $@"
|
||||||
|
$(CXX) $(CXXFLAGS) -MMD \
|
||||||
|
-Iinclude -I.. -I../.. \
|
||||||
|
$(CEREAL_CXXFLAGS) \
|
||||||
|
$(ZMQ_FLAGS) \
|
||||||
|
$(JSON_FLAGS) \
|
||||||
|
-I../ \
|
||||||
|
-I../../ \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
%.o: %.c |
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CFLAGS) -MMD \
|
||||||
|
-Iinclude -I.. -I../.. \
|
||||||
|
$(CEREAL_CFLAGS) \
|
||||||
|
$(ZMQ_FLAGS) \
|
||||||
|
$(JSON_FLAGS) \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
.PHONY: clean |
||||||
|
clean: |
||||||
|
rm -f ubloxd ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(DEPS)
|
||||||
|
|
||||||
|
-include $(DEPS) |
||||||
@ -0,0 +1,82 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
import subprocess |
||||||
|
import os |
||||||
|
import sys |
||||||
|
import argparse |
||||||
|
import tempfile |
||||||
|
|
||||||
|
from ubloxd_py_test import parser_test |
||||||
|
from 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('Compiling test app...') |
||||||
|
subprocess.check_call(["make", "ubloxd_test"], cwd=ubloxd_dir) |
||||||
|
|
||||||
|
print('Run regression test - CC parser...') |
||||||
|
if args.valgrind: |
||||||
|
subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) |
||||||
|
else: |
||||||
|
subprocess.check_call([os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) |
||||||
|
|
||||||
|
print('Running regression test - py parser...') |
||||||
|
parser_test(stream_file_path, py_output_dir) |
||||||
|
|
||||||
|
print('Running regression test - compare result...') |
||||||
|
r = compare_results(cc_output_dir, py_output_dir) |
||||||
|
|
||||||
|
print('All done!') |
||||||
|
|
||||||
|
subprocess.check_call(["rm", stream_file_path]) |
||||||
|
subprocess.check_call(["rm", '-rf', cc_output_dir]) |
||||||
|
subprocess.check_call(["rm", '-rf', py_output_dir]) |
||||||
|
sys.exit(r) |
||||||
|
|
||||||
|
except subprocess.CalledProcessError as e: |
||||||
|
print('CI test failed with {}'.format(e.returncode)) |
||||||
|
sys.exit(e.returncode) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
parser = argparse.ArgumentParser(description="Ubloxd CI test", |
||||||
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter) |
||||||
|
|
||||||
|
parser.add_argument("stream_gz_file", nargs='?', default='ubloxRaw.tar.gz', |
||||||
|
help="UbloxRaw data stream zip file") |
||||||
|
|
||||||
|
parser.add_argument("output_dir", nargs='?', default='out', |
||||||
|
help="Output events temp directory") |
||||||
|
|
||||||
|
parser.add_argument("--valgrind", default=False, action='store_true', |
||||||
|
help="Run in valgrind") |
||||||
|
|
||||||
|
args = parser.parse_args() |
||||||
|
main(args) |
||||||
@ -0,0 +1,55 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import os |
||||||
|
import ublox |
||||||
|
from common import realtime |
||||||
|
from ubloxd import gen_raw, gen_solution |
||||||
|
import zmq |
||||||
|
import selfdrive.messaging as messaging |
||||||
|
from selfdrive.services import service_list |
||||||
|
|
||||||
|
|
||||||
|
unlogger = os.getenv("UNLOGGER") is not None # debug prints |
||||||
|
|
||||||
|
def main(gctx=None): |
||||||
|
context = zmq.Context() |
||||||
|
poller = zmq.Poller() |
||||||
|
|
||||||
|
context = zmq.Context() |
||||||
|
gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port) |
||||||
|
ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port) |
||||||
|
|
||||||
|
# ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port, 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() |
||||||
@ -0,0 +1,77 @@ |
|||||||
|
import sys |
||||||
|
import os |
||||||
|
|
||||||
|
from ublox import UBloxMessage |
||||||
|
from 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 xrange(1, 33): |
||||||
|
nav_frame_buffer[0][i] = {} |
||||||
|
|
||||||
|
if not os.path.exists(prefix): |
||||||
|
print('Prefix invalid') |
||||||
|
sys.exit(-1) |
||||||
|
|
||||||
|
with open(fn, 'rb') as f: |
||||||
|
i = 0 |
||||||
|
saved_i = 0 |
||||||
|
msg = UBloxMessage() |
||||||
|
while True: |
||||||
|
n = msg.needed_bytes() |
||||||
|
b = f.read(n) |
||||||
|
if not b: |
||||||
|
break |
||||||
|
msg.add(b) |
||||||
|
if msg.valid(): |
||||||
|
i += 1 |
||||||
|
if msg.name() == 'NAV_PVT': |
||||||
|
sol = gen_solution(msg) |
||||||
|
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) |
||||||
|
with open(os.path.join(prefix, str(saved_i)), 'wb') as f1: |
||||||
|
f1.write(sol.to_bytes()) |
||||||
|
saved_i += 1 |
||||||
|
elif msg.name() == 'RXM_RAW': |
||||||
|
raw = gen_raw(msg) |
||||||
|
raw.logMonoTime = int(realtime.sec_since_boot() * 1e9) |
||||||
|
with open(os.path.join(prefix, str(saved_i)), 'wb') as f1: |
||||||
|
f1.write(raw.to_bytes()) |
||||||
|
saved_i += 1 |
||||||
|
elif msg.name() == 'RXM_SFRBX': |
||||||
|
nav = gen_nav_data(msg, nav_frame_buffer) |
||||||
|
if nav is not None: |
||||||
|
nav.logMonoTime = int(realtime.sec_since_boot() * 1e9) |
||||||
|
with open(os.path.join(prefix, str(saved_i)), 'wb') as f1: |
||||||
|
f1.write(nav.to_bytes()) |
||||||
|
saved_i += 1 |
||||||
|
|
||||||
|
msg = UBloxMessage() |
||||||
|
msg.debug_level = 0 |
||||||
|
print('Parsed {} msgs'.format(i)) |
||||||
|
print('Generated {} cereal events'.format(saved_i)) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
if len(sys.argv) < 3: |
||||||
|
print('Format: ubloxd_py_test.py file_path prefix') |
||||||
|
sys.exit(0) |
||||||
|
|
||||||
|
fn = sys.argv[1] |
||||||
|
if not os.path.isfile(fn): |
||||||
|
print('File path invalid') |
||||||
|
sys.exit(0) |
||||||
|
|
||||||
|
prefix = sys.argv[2] |
||||||
|
mkdirs_exists_ok(prefix) |
||||||
|
parser_test(fn, prefix) |
||||||
@ -0,0 +1,96 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import os |
||||||
|
import sys |
||||||
|
import argparse |
||||||
|
|
||||||
|
from cereal import log |
||||||
|
from common.basedir import BASEDIR |
||||||
|
os.environ['BASEDIR'] = BASEDIR |
||||||
|
|
||||||
|
|
||||||
|
def get_arg_parser(): |
||||||
|
parser = argparse.ArgumentParser( |
||||||
|
description="Compare two result files", |
||||||
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter) |
||||||
|
|
||||||
|
parser.add_argument("dir1", nargs='?', default='/data/ubloxdc', |
||||||
|
help="Directory path 1 from which events are loaded") |
||||||
|
|
||||||
|
parser.add_argument("dir2", nargs='?', default='/data/ubloxdpy', |
||||||
|
help="Directory path 2 from which msgs are loaded") |
||||||
|
|
||||||
|
return parser |
||||||
|
|
||||||
|
|
||||||
|
def read_file(fn): |
||||||
|
with open(fn, 'rb') as f: |
||||||
|
return f.read() |
||||||
|
|
||||||
|
|
||||||
|
def compare_results(dir1, dir2): |
||||||
|
onlyfiles1 = [f for f in os.listdir(dir1) if os.path.isfile(os.path.join(dir1, f))] |
||||||
|
onlyfiles1.sort() |
||||||
|
|
||||||
|
onlyfiles2 = [f for f in os.listdir(dir2) if os.path.isfile(os.path.join(dir2, f))] |
||||||
|
onlyfiles2.sort() |
||||||
|
|
||||||
|
if len(onlyfiles1) != len(onlyfiles2): |
||||||
|
print('len mismatch: {} != {}'.format(len(onlyfiles1), len(onlyfiles2))) |
||||||
|
return -1 |
||||||
|
events1 = [log.Event.from_bytes(read_file(os.path.join(dir1, f))) for f in onlyfiles1] |
||||||
|
events2 = [log.Event.from_bytes(read_file(os.path.join(dir2, f))) for f in onlyfiles2] |
||||||
|
|
||||||
|
for i in range(len(events1)): |
||||||
|
if events1[i].which() != events2[i].which(): |
||||||
|
print('event {} type mismatch: {} != {}'.format(i, events1[i].which(), events2[i].which())) |
||||||
|
return -2 |
||||||
|
if events1[i].which() == 'gpsLocationExternal': |
||||||
|
old_gps = events1[i].gpsLocationExternal |
||||||
|
gps = events2[i].gpsLocationExternal |
||||||
|
# print(gps, old_gps) |
||||||
|
attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing', |
||||||
|
'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy'] |
||||||
|
for attr in attrs: |
||||||
|
o = getattr(old_gps, attr) |
||||||
|
n = getattr(gps, attr) |
||||||
|
if attr == 'vNED': |
||||||
|
if len(o) != len(n): |
||||||
|
print('Gps vNED len mismatch', o, n) |
||||||
|
return -3 |
||||||
|
else: |
||||||
|
for i in range(len(o)): |
||||||
|
if abs(o[i] - n[i]) > 1e-3: |
||||||
|
print('Gps vNED mismatch', o, n) |
||||||
|
return |
||||||
|
elif o != n: |
||||||
|
print('Gps mismatch', attr, o, n) |
||||||
|
return -4 |
||||||
|
elif events1[i].which() == 'ubloxGnss': |
||||||
|
old_gnss = events1[i].ubloxGnss |
||||||
|
gnss = events2[i].ubloxGnss |
||||||
|
if old_gnss.which() == 'measurementReport' and gnss.which() == 'measurementReport': |
||||||
|
attrs = ['gpsWeek', 'leapSeconds', 'measurements', 'numMeas', 'rcvTow', 'receiverStatus', 'schema'] |
||||||
|
for attr in attrs: |
||||||
|
o = getattr(old_gnss.measurementReport, attr) |
||||||
|
n = getattr(gnss.measurementReport, attr) |
||||||
|
if str(o) != str(n): |
||||||
|
print('measurementReport {} mismatched'.format(attr)) |
||||||
|
return -5 |
||||||
|
if not (str(old_gnss.measurementReport) == str(gnss.measurementReport)): |
||||||
|
print('Gnss measurementReport mismatched!') |
||||||
|
print('gnss measurementReport old', old_gnss.measurementReport.measurements) |
||||||
|
print('gnss measurementReport new', gnss.measurementReport.measurements) |
||||||
|
return -6 |
||||||
|
elif old_gnss.which() == 'ephemeris' and gnss.which() == 'ephemeris': |
||||||
|
if not (str(old_gnss.ephemeris) == str(gnss.ephemeris)): |
||||||
|
print('Gnss ephemeris mismatched!') |
||||||
|
print('gnss ephemeris old', old_gnss.ephemeris) |
||||||
|
print('gnss ephemeris new', gnss.ephemeris) |
||||||
|
return -7 |
||||||
|
print('All {} events matched!'.format(len(events1))) |
||||||
|
return 0 |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
args = get_arg_parser().parse_args(sys.argv[1:]) |
||||||
|
compare_results(args.dir1, args.dir2) |
||||||
@ -0,0 +1,375 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <sched.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <sys/cdefs.h> |
||||||
|
#include <sys/types.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <math.h> |
||||||
|
#include <ctime> |
||||||
|
#include <chrono> |
||||||
|
#include <map> |
||||||
|
#include <vector> |
||||||
|
#include <algorithm> |
||||||
|
|
||||||
|
#include <zmq.h> |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
|
||||||
|
#include "common/params.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
#include "common/timing.h" |
||||||
|
|
||||||
|
#include "ublox_msg.h" |
||||||
|
|
||||||
|
#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4]) |
||||||
|
#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) |
||||||
|
|
||||||
|
namespace ublox { |
||||||
|
|
||||||
|
inline int twos_complement(uint32_t v, uint32_t nb) { |
||||||
|
int sign = v >> (nb - 1); |
||||||
|
int value = v; |
||||||
|
if(sign != 0) |
||||||
|
value = value - (1 << nb); |
||||||
|
return value; |
||||||
|
} |
||||||
|
|
||||||
|
inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) { |
||||||
|
int v = GET_FIELD_U(w, nb, pos); |
||||||
|
return twos_complement(v, nb); |
||||||
|
} |
||||||
|
|
||||||
|
class EphemerisData { |
||||||
|
public: |
||||||
|
EphemerisData(uint8_t svId, subframes_map subframes) { |
||||||
|
this->svId = svId; |
||||||
|
int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20); |
||||||
|
int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6); |
||||||
|
int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( |
||||||
|
subframes[1][2+5], 8, 22); |
||||||
|
|
||||||
|
int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6); |
||||||
|
int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22); |
||||||
|
int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6); |
||||||
|
int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8); |
||||||
|
|
||||||
|
int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6); |
||||||
|
int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14); |
||||||
|
int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( |
||||||
|
subframes[2][2+2], 24, 6); |
||||||
|
int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14); |
||||||
|
int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6); |
||||||
|
int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14); |
||||||
|
uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( |
||||||
|
subframes[2][2+6], 24, 6); |
||||||
|
int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14); |
||||||
|
|
||||||
|
int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14); |
||||||
|
int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( |
||||||
|
subframes[3][2+1], 24, 6); |
||||||
|
int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14); |
||||||
|
int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( |
||||||
|
subframes[3][2+3], 24, 6); |
||||||
|
int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14); |
||||||
|
int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6); |
||||||
|
int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6); |
||||||
|
int idot = GET_FIELD_S(subframes[3][2+7], 14, 8); |
||||||
|
|
||||||
|
this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6); |
||||||
|
this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6); |
||||||
|
this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6); |
||||||
|
this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14); |
||||||
|
this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8); |
||||||
|
|
||||||
|
double gpsPi = 3.1415926535898; |
||||||
|
|
||||||
|
// now form variables in radians, meters and seconds etc
|
||||||
|
this->Tgd = t_gd * pow(2, -31); |
||||||
|
this->A = pow(a_powhalf * pow(2, -19), 2.0); |
||||||
|
this->cic = c_ic * pow(2, -29); |
||||||
|
this->cis = c_is * pow(2, -29); |
||||||
|
this->crc = c_rc * pow(2, -5); |
||||||
|
this->crs = c_rs * pow(2, -5); |
||||||
|
this->cuc = c_uc * pow(2, -29); |
||||||
|
this->cus = c_us * pow(2, -29); |
||||||
|
this->deltaN = delta_n * pow(2, -43) * gpsPi; |
||||||
|
this->ecc = e * pow(2, -33); |
||||||
|
this->i0 = i_0 * pow(2, -31) * gpsPi; |
||||||
|
this->idot = idot * pow(2, -43) * gpsPi; |
||||||
|
this->M0 = m_0 * pow(2, -31) * gpsPi; |
||||||
|
this->omega = w * pow(2, -31) * gpsPi; |
||||||
|
this->omega_dot = omega_dot * pow(2, -43) * gpsPi; |
||||||
|
this->omega0 = omega_0 * pow(2, -31) * gpsPi; |
||||||
|
this->toe = t_oe * pow(2, 4); |
||||||
|
|
||||||
|
this->toc = t_oc * pow(2, 4); |
||||||
|
this->gpsWeek = week_no; |
||||||
|
this->af0 = a_f0 * pow(2, -31); |
||||||
|
this->af1 = a_f1 * pow(2, -43); |
||||||
|
this->af2 = a_f2 * pow(2, -55); |
||||||
|
|
||||||
|
uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22); |
||||||
|
uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22); |
||||||
|
this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff)); |
||||||
|
this->iode = iode1; |
||||||
|
|
||||||
|
if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 && |
||||||
|
GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 && |
||||||
|
GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) { |
||||||
|
double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30); |
||||||
|
double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27); |
||||||
|
double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24); |
||||||
|
double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24); |
||||||
|
double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11); |
||||||
|
double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14); |
||||||
|
double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16); |
||||||
|
double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16); |
||||||
|
this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3; |
||||||
|
this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3; |
||||||
|
this->ionoCoeffsValid = true; |
||||||
|
} else { |
||||||
|
this->ionoCoeffsValid = false; |
||||||
|
} |
||||||
|
} |
||||||
|
uint16_t svId; |
||||||
|
double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc; |
||||||
|
uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo; |
||||||
|
double af0, af1, af2; |
||||||
|
bool valid; |
||||||
|
double ionoAlpha[4], ionoBeta[4]; |
||||||
|
bool ionoCoeffsValid; |
||||||
|
}; |
||||||
|
|
||||||
|
UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) { |
||||||
|
nav_frame_buffer[0U] = std::map<uint8_t, subframes_map>(); |
||||||
|
for(int i = 1;i < 33;i++) |
||||||
|
nav_frame_buffer[0U][i] = subframes_map(); |
||||||
|
} |
||||||
|
|
||||||
|
inline int UbloxMsgParser::needed_bytes() { |
||||||
|
// Msg header incomplete?
|
||||||
|
if(bytes_in_parse_buf < UBLOX_HEADER_SIZE) |
||||||
|
return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; |
||||||
|
uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE; |
||||||
|
// too much data
|
||||||
|
if(needed < (uint16_t)bytes_in_parse_buf) |
||||||
|
return -1; |
||||||
|
return needed - (uint16_t)bytes_in_parse_buf; |
||||||
|
} |
||||||
|
|
||||||
|
inline bool UbloxMsgParser::valid_cheksum() { |
||||||
|
uint8_t ck_a = 0, ck_b = 0; |
||||||
|
for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) { |
||||||
|
ck_a = (ck_a + msg_parse_buf[i]) & 0xFF; |
||||||
|
ck_b = (ck_b + ck_a) & 0xFF; |
||||||
|
} |
||||||
|
if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) { |
||||||
|
LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]); |
||||||
|
return false; |
||||||
|
} |
||||||
|
if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) { |
||||||
|
LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]); |
||||||
|
return false; |
||||||
|
} |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
inline bool UbloxMsgParser::valid() { |
||||||
|
return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE && |
||||||
|
needed_bytes() == 0 && |
||||||
|
valid_cheksum(); |
||||||
|
} |
||||||
|
|
||||||
|
inline bool UbloxMsgParser::valid_so_far() { |
||||||
|
if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) { |
||||||
|
//LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]);
|
||||||
|
return false; |
||||||
|
} |
||||||
|
if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) { |
||||||
|
//LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]);
|
||||||
|
return false; |
||||||
|
} |
||||||
|
if(needed_bytes() == 0 && !valid()) |
||||||
|
return false; |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
kj::Array<capnp::word> UbloxMsgParser::gen_solution() { |
||||||
|
nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; |
||||||
|
capnp::MallocMessageBuilder msg_builder; |
||||||
|
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
auto gpsLoc = event.initGpsLocationExternal(); |
||||||
|
gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); |
||||||
|
gpsLoc.setFlags(msg->flags); |
||||||
|
gpsLoc.setLatitude(msg->lat * 1e-07); |
||||||
|
gpsLoc.setLongitude(msg->lon * 1e-07); |
||||||
|
gpsLoc.setAltitude(msg->height * 1e-03); |
||||||
|
gpsLoc.setSpeed(msg->gSpeed * 1e-03); |
||||||
|
gpsLoc.setBearing(msg->headMot * 1e-5); |
||||||
|
gpsLoc.setAccuracy(msg->hAcc * 1e-03); |
||||||
|
std::tm timeinfo = std::tm(); |
||||||
|
timeinfo.tm_year = msg->year - 1900; |
||||||
|
timeinfo.tm_mon = msg->month - 1; |
||||||
|
timeinfo.tm_mday = msg->day; |
||||||
|
timeinfo.tm_hour = msg->hour; |
||||||
|
timeinfo.tm_min = msg->min; |
||||||
|
timeinfo.tm_sec = msg->sec; |
||||||
|
std::time_t utc_tt = timegm(&timeinfo); |
||||||
|
gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); |
||||||
|
float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; |
||||||
|
kj::ArrayPtr<const float> ap(&f[0], sizeof(f) / sizeof(f[0])); |
||||||
|
gpsLoc.setVNED(ap); |
||||||
|
gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); |
||||||
|
gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); |
||||||
|
gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05); |
||||||
|
return capnp::messageToFlatArray(msg_builder); |
||||||
|
} |
||||||
|
|
||||||
|
inline bool bit_to_bool(uint8_t val, int shifts) { |
||||||
|
return (val & (1 << shifts)) ? true : false; |
||||||
|
} |
||||||
|
|
||||||
|
kj::Array<capnp::word> UbloxMsgParser::gen_raw() { |
||||||
|
rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; |
||||||
|
if(bytes_in_parse_buf != ( |
||||||
|
UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE |
||||||
|
)) { |
||||||
|
LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); |
||||||
|
return kj::Array<capnp::word>(); |
||||||
|
} |
||||||
|
rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)]; |
||||||
|
capnp::MallocMessageBuilder msg_builder; |
||||||
|
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
auto gnss = event.initUbloxGnss(); |
||||||
|
auto mr = gnss.initMeasurementReport(); |
||||||
|
mr.setRcvTow(msg->rcvTow); |
||||||
|
mr.setGpsWeek(msg->week); |
||||||
|
mr.setLeapSeconds(msg->leapS); |
||||||
|
mr.setGpsWeek(msg->week); |
||||||
|
auto mb = mr.initMeasurements(msg->numMeas); |
||||||
|
for(int8_t i = 0; i < msg->numMeas; i++) { |
||||||
|
mb[i].setSvId(measurements[i].svId); |
||||||
|
mb[i].setSigId(measurements[i].sigId); |
||||||
|
mb[i].setPseudorange(measurements[i].prMes); |
||||||
|
mb[i].setCarrierCycles(measurements[i].cpMes); |
||||||
|
mb[i].setDoppler(measurements[i].doMes); |
||||||
|
mb[i].setGnssId(measurements[i].gnssId); |
||||||
|
mb[i].setGlonassFrequencyIndex(measurements[i].freqId); |
||||||
|
mb[i].setLocktime(measurements[i].locktime); |
||||||
|
mb[i].setCno(measurements[i].cno); |
||||||
|
mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong
|
||||||
|
mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15)); |
||||||
|
mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong
|
||||||
|
auto ts = mb[i].initTrackingStatus(); |
||||||
|
ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0)); |
||||||
|
ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1)); |
||||||
|
ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2)); |
||||||
|
ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3)); |
||||||
|
} |
||||||
|
|
||||||
|
mr.setNumMeas(msg->numMeas); |
||||||
|
auto rs = mr.initReceiverStatus(); |
||||||
|
rs.setLeapSecValid(bit_to_bool(msg->recStat, 0)); |
||||||
|
rs.setClkReset(bit_to_bool(msg->recStat, 2)); |
||||||
|
return capnp::messageToFlatArray(msg_builder); |
||||||
|
} |
||||||
|
|
||||||
|
kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() { |
||||||
|
rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; |
||||||
|
if(bytes_in_parse_buf != ( |
||||||
|
UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE |
||||||
|
)) { |
||||||
|
LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); |
||||||
|
return kj::Array<capnp::word>(); |
||||||
|
} |
||||||
|
rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)]; |
||||||
|
if(msg->gnssId == 0) { |
||||||
|
uint8_t subframeId = GET_FIELD_U(measurements[1].dwrd, 3, 8); |
||||||
|
std::vector<uint32_t> words; |
||||||
|
for(int i = 0; i < msg->numWords;i++) |
||||||
|
words.push_back(measurements[i].dwrd); |
||||||
|
|
||||||
|
if(subframeId == 1) { |
||||||
|
nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map(); |
||||||
|
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; |
||||||
|
} else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end()) |
||||||
|
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; |
||||||
|
if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) { |
||||||
|
EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]); |
||||||
|
capnp::MallocMessageBuilder msg_builder; |
||||||
|
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
auto gnss = event.initUbloxGnss(); |
||||||
|
auto eph = gnss.initEphemeris(); |
||||||
|
eph.setSvId(ephem_data.svId); |
||||||
|
eph.setToc(ephem_data.toc); |
||||||
|
eph.setGpsWeek(ephem_data.gpsWeek); |
||||||
|
eph.setAf0(ephem_data.af0); |
||||||
|
eph.setAf1(ephem_data.af1); |
||||||
|
eph.setAf2(ephem_data.af2); |
||||||
|
eph.setIode(ephem_data.iode); |
||||||
|
eph.setCrs(ephem_data.crs); |
||||||
|
eph.setDeltaN(ephem_data.deltaN); |
||||||
|
eph.setM0(ephem_data.M0); |
||||||
|
eph.setCuc(ephem_data.cuc); |
||||||
|
eph.setEcc(ephem_data.ecc); |
||||||
|
eph.setCus(ephem_data.cus); |
||||||
|
eph.setA(ephem_data.A); |
||||||
|
eph.setToe(ephem_data.toe); |
||||||
|
eph.setCic(ephem_data.cic); |
||||||
|
eph.setOmega0(ephem_data.omega0); |
||||||
|
eph.setCis(ephem_data.cis); |
||||||
|
eph.setI0(ephem_data.i0); |
||||||
|
eph.setCrc(ephem_data.crc); |
||||||
|
eph.setOmega(ephem_data.omega); |
||||||
|
eph.setOmegaDot(ephem_data.omega_dot); |
||||||
|
eph.setIDot(ephem_data.idot); |
||||||
|
eph.setTgd(ephem_data.Tgd); |
||||||
|
eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); |
||||||
|
if(ephem_data.ionoCoeffsValid) { |
||||||
|
kj::ArrayPtr<const double> apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0])); |
||||||
|
eph.setIonoAlpha(apa); |
||||||
|
kj::ArrayPtr<const double> apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0])); |
||||||
|
eph.setIonoBeta(apb); |
||||||
|
} else { |
||||||
|
eph.setIonoAlpha(kj::ArrayPtr<const double>()); |
||||||
|
eph.setIonoBeta(kj::ArrayPtr<const double>()); |
||||||
|
} |
||||||
|
return capnp::messageToFlatArray(msg_builder); |
||||||
|
} |
||||||
|
} |
||||||
|
return kj::Array<capnp::word>(); |
||||||
|
} |
||||||
|
|
||||||
|
bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { |
||||||
|
int needed = needed_bytes(); |
||||||
|
if(needed > 0) { |
||||||
|
bytes_consumed = min((size_t)needed, incoming_data_len ); |
||||||
|
// Add data to buffer
|
||||||
|
memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed); |
||||||
|
bytes_in_parse_buf += bytes_consumed; |
||||||
|
} else { |
||||||
|
bytes_consumed = incoming_data_len; |
||||||
|
} |
||||||
|
// Validate msg format, detect invalid header and invalid checksum.
|
||||||
|
while(!valid_so_far() && bytes_in_parse_buf != 0) { |
||||||
|
//LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf);
|
||||||
|
// Corrupted msg, drop a byte.
|
||||||
|
bytes_in_parse_buf -= 1; |
||||||
|
if(bytes_in_parse_buf > 0) |
||||||
|
memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf); |
||||||
|
} |
||||||
|
// There is redundant data at the end of buffer, reset the buffer.
|
||||||
|
if(needed_bytes() == -1) |
||||||
|
bytes_in_parse_buf = 0; |
||||||
|
return valid(); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,149 @@ |
|||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <stdint.h> |
||||||
|
|
||||||
|
#define min(x, y) ((x) <= (y) ? (x) : (y)) |
||||||
|
|
||||||
|
// NAV_PVT
|
||||||
|
typedef struct __attribute__((packed)) { |
||||||
|
uint32_t iTOW; |
||||||
|
uint16_t year; |
||||||
|
int8_t month; |
||||||
|
int8_t day; |
||||||
|
int8_t hour; |
||||||
|
int8_t min; |
||||||
|
int8_t sec; |
||||||
|
int8_t valid; |
||||||
|
uint32_t tAcc; |
||||||
|
int32_t nano; |
||||||
|
int8_t fixType; |
||||||
|
int8_t flags; |
||||||
|
int8_t flags2; |
||||||
|
int8_t numSV; |
||||||
|
int32_t lon; |
||||||
|
int32_t lat; |
||||||
|
int32_t height; |
||||||
|
int32_t hMSL; |
||||||
|
uint32_t hAcc; |
||||||
|
uint32_t vAcc; |
||||||
|
int32_t velN; |
||||||
|
int32_t velE; |
||||||
|
int32_t velD; |
||||||
|
int32_t gSpeed; |
||||||
|
int32_t headMot; |
||||||
|
uint32_t sAcc; |
||||||
|
uint32_t headAcc; |
||||||
|
uint16_t pDOP; |
||||||
|
int8_t reserverd1[6]; |
||||||
|
int32_t headVeh; |
||||||
|
int16_t magDec; |
||||||
|
uint16_t magAcc; |
||||||
|
} nav_pvt_msg; |
||||||
|
|
||||||
|
// RXM_RAW
|
||||||
|
typedef struct __attribute__((packed)) { |
||||||
|
double rcvTow; |
||||||
|
uint16_t week; |
||||||
|
int8_t leapS; |
||||||
|
int8_t numMeas; |
||||||
|
int8_t recStat; |
||||||
|
int8_t reserved1[3]; |
||||||
|
} rxm_raw_msg; |
||||||
|
|
||||||
|
// Extra data count is in numMeas
|
||||||
|
typedef struct __attribute__((packed)) { |
||||||
|
double prMes; |
||||||
|
double cpMes; |
||||||
|
float doMes; |
||||||
|
int8_t gnssId; |
||||||
|
int8_t svId; |
||||||
|
int8_t sigId; |
||||||
|
int8_t freqId; |
||||||
|
uint16_t locktime; |
||||||
|
int8_t cno; |
||||||
|
int8_t prStdev; |
||||||
|
int8_t cpStdev; |
||||||
|
int8_t doStdev; |
||||||
|
int8_t trkStat; |
||||||
|
int8_t reserved3; |
||||||
|
} rxm_raw_msg_extra; |
||||||
|
// RXM_SFRBX
|
||||||
|
typedef struct __attribute__((packed)) { |
||||||
|
int8_t gnssId; |
||||||
|
int8_t svid; |
||||||
|
int8_t reserved1; |
||||||
|
int8_t freqId; |
||||||
|
int8_t numWords; |
||||||
|
int8_t reserved2; |
||||||
|
int8_t version; |
||||||
|
int8_t reserved3; |
||||||
|
} rxm_sfrbx_msg; |
||||||
|
|
||||||
|
// Extra data count is in numWords
|
||||||
|
typedef struct __attribute__((packed)) { |
||||||
|
uint32_t dwrd; |
||||||
|
} rxm_sfrbx_msg_extra; |
||||||
|
|
||||||
|
namespace ublox { |
||||||
|
// protocol constants
|
||||||
|
const uint8_t PREAMBLE1 = 0xb5; |
||||||
|
const uint8_t PREAMBLE2 = 0x62; |
||||||
|
|
||||||
|
// message classes
|
||||||
|
const uint8_t CLASS_NAV = 0x01; |
||||||
|
const uint8_t CLASS_RXM = 0x02; |
||||||
|
|
||||||
|
// NAV messages
|
||||||
|
const uint8_t MSG_NAV_PVT = 0x7; |
||||||
|
|
||||||
|
// RXM messages
|
||||||
|
const uint8_t MSG_RXM_RAW = 0x15; |
||||||
|
const uint8_t MSG_RXM_SFRBX = 0x13; |
||||||
|
|
||||||
|
const int UBLOX_HEADER_SIZE = 6; |
||||||
|
const int UBLOX_CHECKSUM_SIZE = 2; |
||||||
|
const int UBLOX_MAX_MSG_SIZE = 65536; |
||||||
|
|
||||||
|
typedef std::map<uint8_t, std::vector<uint32_t>> subframes_map; |
||||||
|
|
||||||
|
class UbloxMsgParser { |
||||||
|
public: |
||||||
|
|
||||||
|
UbloxMsgParser(); |
||||||
|
kj::Array<capnp::word> gen_solution(); |
||||||
|
kj::Array<capnp::word> gen_raw(); |
||||||
|
|
||||||
|
kj::Array<capnp::word> gen_nav_data(); |
||||||
|
bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); |
||||||
|
inline void reset() {bytes_in_parse_buf = 0;} |
||||||
|
inline uint8_t msg_class() { |
||||||
|
return msg_parse_buf[2]; |
||||||
|
} |
||||||
|
|
||||||
|
inline uint8_t msg_id() { |
||||||
|
return msg_parse_buf[3]; |
||||||
|
} |
||||||
|
inline int needed_bytes(); |
||||||
|
|
||||||
|
void hexdump(uint8_t *d, int l) { |
||||||
|
for (int i = 0; i < l; i++) { |
||||||
|
if (i%0x10 == 0 && i != 0) printf("\n"); |
||||||
|
printf("%02X ", d[i]); |
||||||
|
} |
||||||
|
printf("\n"); |
||||||
|
} |
||||||
|
private: |
||||||
|
inline bool valid_cheksum(); |
||||||
|
inline bool valid(); |
||||||
|
inline bool valid_so_far(); |
||||||
|
|
||||||
|
uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE]; |
||||||
|
int bytes_in_parse_buf; |
||||||
|
std::map<uint8_t, std::map<uint8_t, subframes_map>> nav_frame_buffer; |
||||||
|
}; |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
typedef int (*poll_ubloxraw_msg_func)(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg); |
||||||
|
typedef int (*send_gps_event_func)(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags); |
||||||
|
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func); |
||||||
@ -0,0 +1,45 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <sched.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <sys/cdefs.h> |
||||||
|
#include <sys/types.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <math.h> |
||||||
|
#include <ctime> |
||||||
|
#include <chrono> |
||||||
|
#include <map> |
||||||
|
#include <vector> |
||||||
|
|
||||||
|
#include <zmq.h> |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
|
||||||
|
#include "common/params.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
#include "common/timing.h" |
||||||
|
|
||||||
|
#include "ublox_msg.h" |
||||||
|
|
||||||
|
const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds
|
||||||
|
|
||||||
|
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) { |
||||||
|
int err; |
||||||
|
zmq_pollitem_t item = {.socket = subscriber, .events = ZMQ_POLLIN}; |
||||||
|
err = zmq_poll (&item, 1, ZMQ_POLL_TIMEOUT); |
||||||
|
if(err <= 0) |
||||||
|
return err; |
||||||
|
return zmq_msg_recv(msg, subscriber, 0); |
||||||
|
} |
||||||
|
|
||||||
|
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) { |
||||||
|
return zmq_send(s, buf, len, flags); |
||||||
|
} |
||||||
|
|
||||||
|
int main() { |
||||||
|
return ubloxd_main(poll_ubloxraw_msg, send_gps_event); |
||||||
|
} |
||||||
@ -0,0 +1,113 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <sched.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <sys/cdefs.h> |
||||||
|
#include <sys/types.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <math.h> |
||||||
|
#include <ctime> |
||||||
|
#include <chrono> |
||||||
|
#include <map> |
||||||
|
#include <vector> |
||||||
|
|
||||||
|
#include <zmq.h> |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
|
||||||
|
#include "common/params.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
#include "common/timing.h" |
||||||
|
|
||||||
|
#include "ublox_msg.h" |
||||||
|
|
||||||
|
volatile int do_exit = 0; // Flag for process exit on signal
|
||||||
|
|
||||||
|
void set_do_exit(int sig) { |
||||||
|
do_exit = 1; |
||||||
|
} |
||||||
|
|
||||||
|
using namespace ublox; |
||||||
|
|
||||||
|
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) { |
||||||
|
LOGW("starting ubloxd"); |
||||||
|
signal(SIGINT, (sighandler_t) set_do_exit); |
||||||
|
signal(SIGTERM, (sighandler_t) set_do_exit); |
||||||
|
|
||||||
|
UbloxMsgParser parser; |
||||||
|
void *context = zmq_ctx_new(); |
||||||
|
void *gpsLocationExternal = zmq_socket(context, ZMQ_PUB); |
||||||
|
zmq_bind(gpsLocationExternal, "tcp://*:8032"); |
||||||
|
void *ubloxGnss = zmq_socket(context, ZMQ_PUB); |
||||||
|
zmq_bind(ubloxGnss, "tcp://*:8033"); |
||||||
|
// ubloxRaw = 8042
|
||||||
|
void *subscriber = zmq_socket(context, ZMQ_SUB); |
||||||
|
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); |
||||||
|
zmq_connect(subscriber, "tcp://127.0.0.1:8042"); |
||||||
|
while (!do_exit) { |
||||||
|
zmq_msg_t msg; |
||||||
|
zmq_msg_init(&msg); |
||||||
|
int err = poll_func(gpsLocationExternal, ubloxGnss, subscriber, &msg); |
||||||
|
if(err < 0) { |
||||||
|
LOGE_100("zmq_poll error %s in %s", strerror(errno ), __FUNCTION__); |
||||||
|
break; |
||||||
|
} else if(err == 0) { |
||||||
|
continue; |
||||||
|
} |
||||||
|
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||||
|
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); |
||||||
|
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); |
||||||
|
capnp::FlatArrayMessageReader cmsg(amsg); |
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||||
|
const uint8_t *data = event.getUbloxRaw().begin(); |
||||||
|
size_t len = event.getUbloxRaw().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(); |
||||||
|
send_func(parser.msg_class(), parser.msg_id(), gpsLocationExternal, bytes.begin(), bytes.size(), 0); |
||||||
|
} |
||||||
|
} 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(); |
||||||
|
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0); |
||||||
|
} |
||||||
|
} 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(); |
||||||
|
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0); |
||||||
|
} |
||||||
|
} else |
||||||
|
LOGW("Unknown rxm 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; |
||||||
|
} |
||||||
|
zmq_msg_close(&msg); |
||||||
|
} |
||||||
|
zmq_close(subscriber); |
||||||
|
zmq_close(gpsLocationExternal); |
||||||
|
zmq_close(ubloxGnss); |
||||||
|
zmq_ctx_destroy(context); |
||||||
|
return 0; |
||||||
|
} |
||||||
@ -0,0 +1,103 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <sched.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <sys/cdefs.h> |
||||||
|
#include <sys/types.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <math.h> |
||||||
|
#include <ctime> |
||||||
|
#include <chrono> |
||||||
|
#include <map> |
||||||
|
#include <vector> |
||||||
|
#include <iostream> |
||||||
|
|
||||||
|
#include <zmq.h> |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
|
||||||
|
#include "common/params.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
#include "common/timing.h" |
||||||
|
#include "common/util.h" |
||||||
|
#include "ublox_msg.h" |
||||||
|
|
||||||
|
using namespace ublox; |
||||||
|
|
||||||
|
void write_file(std::string fpath, uint8_t *data, int len) { |
||||||
|
FILE* f = fopen(fpath.c_str(), "wb"); |
||||||
|
if (!f) { |
||||||
|
std::cout << "Open " << fpath << " failed" << std::endl; |
||||||
|
return; |
||||||
|
} |
||||||
|
fwrite(data, len, 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; |
||||||
|
static void *gps_sock, *ublox_gnss_sock; |
||||||
|
|
||||||
|
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) { |
||||||
|
gps_sock = gpsLocationExternal; |
||||||
|
ublox_gnss_sock = ubloxGnss; |
||||||
|
size_t consuming = min(len - consumed, 128); |
||||||
|
if(consumed < len) { |
||||||
|
// create message
|
||||||
|
capnp::MallocMessageBuilder msg_builder; |
||||||
|
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
auto ublox_raw = event.initUbloxRaw(consuming); |
||||||
|
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming); |
||||||
|
auto words = capnp::messageToFlatArray(msg_builder); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
zmq_msg_init_size (msg, bytes.size()); |
||||||
|
memcpy (zmq_msg_data(msg), (void *)bytes.begin(), bytes.size()); |
||||||
|
consumed += consuming; |
||||||
|
return 1; |
||||||
|
} else |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) { |
||||||
|
if(msg_cls == CLASS_NAV && msg_id == MSG_NAV_PVT) |
||||||
|
assert(s == gps_sock); |
||||||
|
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_RAW) |
||||||
|
assert(s == ublox_gnss_sock); |
||||||
|
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_SFRBX) |
||||||
|
assert(s == ublox_gnss_sock); |
||||||
|
else |
||||||
|
assert(0); |
||||||
|
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len); |
||||||
|
save_idx ++; |
||||||
|
return len; |
||||||
|
} |
||||||
|
|
||||||
|
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; |
||||||
|
} |
||||||
Binary file not shown.
@ -1,8 +0,0 @@ |
|||||||
orbd |
|
||||||
orbd_cpu |
|
||||||
test/turbocv_profile |
|
||||||
test/turbocv_test |
|
||||||
dspout/* |
|
||||||
dumb_test |
|
||||||
bilinear_lut.h |
|
||||||
orb_lut.h |
|
||||||
@ -1,105 +0,0 @@ |
|||||||
# CPU
|
|
||||||
|
|
||||||
CC = clang
|
|
||||||
CXX = clang++
|
|
||||||
|
|
||||||
WARN_FLAGS = -Werror=implicit-function-declaration \
|
|
||||||
-Werror=incompatible-pointer-types \
|
|
||||||
-Werror=int-conversion \
|
|
||||||
-Werror=return-type \
|
|
||||||
-Werror=format-extra-args
|
|
||||||
|
|
||||||
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
|
||||||
|
|
||||||
CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
|
||||||
CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
|
||||||
LDFLAGS =
|
|
||||||
|
|
||||||
# profile
|
|
||||||
# CXXFLAGS += -DTURBOCV_PROFILE=1
|
|
||||||
|
|
||||||
PHONELIBS = ../../phonelibs
|
|
||||||
BASEDIR = ../..
|
|
||||||
EXTERNAL = ../../external
|
|
||||||
PYTHONLIBS =
|
|
||||||
|
|
||||||
UNAME_M := $(shell uname -m)
|
|
||||||
|
|
||||||
ifeq ($(UNAME_M),x86_64) |
|
||||||
# computer
|
|
||||||
|
|
||||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
|
||||||
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
|
|
||||||
-l:libczmq.a -l:libzmq.a -lpthread
|
|
||||||
|
|
||||||
OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc
|
|
||||||
|
|
||||||
CXXFLAGS += -fopenmp
|
|
||||||
LDFLAGS += -lomp
|
|
||||||
|
|
||||||
else |
|
||||||
# phone
|
|
||||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
|
||||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
|
|
||||||
-l:libczmq.a -l:libzmq.a \
|
|
||||||
-lgnustl_shared
|
|
||||||
|
|
||||||
OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include
|
|
||||||
OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so
|
|
||||||
|
|
||||||
endif |
|
||||||
|
|
||||||
.PHONY: all |
|
||||||
all: orbd |
|
||||||
|
|
||||||
include ../common/cereal.mk |
|
||||||
|
|
||||||
DEP_OBJS = ../common/visionipc.o ../common/ipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
|
|
||||||
|
|
||||||
orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o |
|
||||||
@echo "[ LINK ] $@"
|
|
||||||
$(CXX) -fPIC -o '$@' $^ \
|
|
||||||
$(LDFLAGS) \
|
|
||||||
$(ZMQ_LIBS) \
|
|
||||||
$(CEREAL_LIBS) \
|
|
||||||
-L/usr/lib \
|
|
||||||
-L/system/vendor/lib64 \
|
|
||||||
-ladsprpc \
|
|
||||||
-lm -lz -llog
|
|
||||||
|
|
||||||
%.o: %.c |
|
||||||
@echo "[ CC ] $@"
|
|
||||||
$(CC) $(CFLAGS) \
|
|
||||||
$(ZMQ_FLAGS) \
|
|
||||||
-I../ \
|
|
||||||
-I../../ \
|
|
||||||
-c -o '$@' '$<'
|
|
||||||
|
|
||||||
orbd_dsp.o: orbd.cc |
|
||||||
@echo "[ CXX ] $@"
|
|
||||||
$(CXX) $(CXXFLAGS) \
|
|
||||||
$(CEREAL_CXXFLAGS) \
|
|
||||||
$(ZMQ_FLAGS) \
|
|
||||||
$(OPENCV_FLAGS) \
|
|
||||||
-DDSP \
|
|
||||||
-I../ \
|
|
||||||
-I../../ \
|
|
||||||
-I../../../ \
|
|
||||||
-I./include \
|
|
||||||
-c -o '$@' '$<'
|
|
||||||
|
|
||||||
freethedsp.o: dsp/freethedsp.c |
|
||||||
@echo "[ CC ] $@"
|
|
||||||
$(CC) $(CFLAGS) \
|
|
||||||
-c -o '$@' '$<'
|
|
||||||
|
|
||||||
calculator_stub.o: dsp/gen/calculator_stub.c |
|
||||||
@echo "[ CC ] $@"
|
|
||||||
$(CC) $(CFLAGS) -I./include -c -o '$@' '$<'
|
|
||||||
|
|
||||||
-include internal.mk |
|
||||||
|
|
||||||
.PHONY: clean |
|
||||||
clean: |
|
||||||
rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h
|
|
||||||
|
|
||||||
@ -1,119 +0,0 @@ |
|||||||
// freethedsp by geohot
|
|
||||||
// (because the DSP should be free)
|
|
||||||
// released under MIT License
|
|
||||||
|
|
||||||
// usage instructions:
|
|
||||||
// 1. Compile an example from the Qualcomm Hexagon SDK
|
|
||||||
// 2. Try to run it on your phone
|
|
||||||
// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat
|
|
||||||
// ...here is where people would give up before freethedsp
|
|
||||||
// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program)
|
|
||||||
// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./<your_prog>'
|
|
||||||
// 6. OMG THE DSP WORKS
|
|
||||||
// 7. Be happy.
|
|
||||||
|
|
||||||
// *** patch may have to change for your phone ***
|
|
||||||
|
|
||||||
// this is patching /dsp/fastrpc_shell_0
|
|
||||||
// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8"
|
|
||||||
// patch to return 0xFFFFFFFF from is_test_enabled instead of 0
|
|
||||||
// your fastrpc_shell_0 may vary
|
|
||||||
#define PATCH_ADDR 0x5200c |
|
||||||
#define PATCH_OLD "\x40\x3f\x20\x50" |
|
||||||
#define PATCH_NEW "\x40\x3f\x00\x5a" |
|
||||||
#define PATCH_LEN (sizeof(PATCH_OLD)-1) |
|
||||||
#define _BITS_IOCTL_H_ |
|
||||||
|
|
||||||
// under 100 lines of code begins now
|
|
||||||
#include <stdio.h> |
|
||||||
#include <dlfcn.h> |
|
||||||
#include <assert.h> |
|
||||||
#include <stdlib.h> |
|
||||||
#include <unistd.h> |
|
||||||
|
|
||||||
// ioctl stuff
|
|
||||||
#define IOC_OUT 0x40000000 /* copy out parameters */ |
|
||||||
#define IOC_IN 0x80000000 /* copy in parameters */ |
|
||||||
#define IOC_INOUT (IOC_IN|IOC_OUT) |
|
||||||
#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */ |
|
||||||
|
|
||||||
#define _IOC(inout,group,num,len) \ |
|
||||||
(inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num)) |
|
||||||
#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t)) |
|
||||||
|
|
||||||
// ion ioctls
|
|
||||||
#include <linux/ion.h> |
|
||||||
#define ION_IOC_MSM_MAGIC 'M' |
|
||||||
#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \ |
|
||||||
struct ion_flush_data) |
|
||||||
|
|
||||||
struct ion_flush_data { |
|
||||||
ion_user_handle_t handle; |
|
||||||
int fd; |
|
||||||
void *vaddr; |
|
||||||
unsigned int offset; |
|
||||||
unsigned int length; |
|
||||||
}; |
|
||||||
|
|
||||||
// fastrpc ioctls
|
|
||||||
#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init) |
|
||||||
|
|
||||||
struct fastrpc_ioctl_init { |
|
||||||
uint32_t flags; /* one of FASTRPC_INIT_* macros */ |
|
||||||
uintptr_t __user file; /* pointer to elf file */ |
|
||||||
int32_t filelen; /* elf file length */ |
|
||||||
int32_t filefd; /* ION fd for the file */ |
|
||||||
uintptr_t __user mem; /* mem for the PD */ |
|
||||||
int32_t memlen; /* mem length */ |
|
||||||
int32_t memfd; /* ION fd for the mem */ |
|
||||||
}; |
|
||||||
|
|
||||||
int ioctl(int fd, unsigned long request, void *arg) { |
|
||||||
static void *handle = NULL; |
|
||||||
static int (*orig_ioctl)(int, int, void*); |
|
||||||
|
|
||||||
if (handle == NULL) { |
|
||||||
handle = dlopen("/system/lib64/libc.so", RTLD_LAZY); |
|
||||||
assert(handle != NULL); |
|
||||||
orig_ioctl = dlsym(handle, "ioctl"); |
|
||||||
} |
|
||||||
|
|
||||||
int ret = orig_ioctl(fd, request, arg); |
|
||||||
|
|
||||||
// carefully modify this one
|
|
||||||
if (request == FASTRPC_IOCTL_INIT) { |
|
||||||
struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; |
|
||||||
|
|
||||||
// confirm patch is correct and do the patch
|
|
||||||
assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0); |
|
||||||
memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN); |
|
||||||
|
|
||||||
// flush cache
|
|
||||||
int ionfd = open("/dev/ion", O_RDONLY); |
|
||||||
assert(ionfd > 0); |
|
||||||
|
|
||||||
struct ion_fd_data fd_data; |
|
||||||
fd_data.fd = init->memfd; |
|
||||||
int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
struct ion_flush_data flush_data; |
|
||||||
flush_data.handle = fd_data.handle; |
|
||||||
flush_data.vaddr = (void*)init->mem; |
|
||||||
flush_data.offset = 0; |
|
||||||
flush_data.length = init->memlen; |
|
||||||
ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
struct ion_handle_data handle_data; |
|
||||||
handle_data.handle = fd_data.handle; |
|
||||||
ret = ioctl(ionfd, ION_IOC_FREE, &handle_data); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
// cleanup
|
|
||||||
close(ionfd); |
|
||||||
} |
|
||||||
|
|
||||||
return ret; |
|
||||||
} |
|
||||||
|
|
||||||
@ -1,39 +0,0 @@ |
|||||||
#ifndef _CALCULATOR_H |
|
||||||
#define _CALCULATOR_H |
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
typedef uint8_t uint8; |
|
||||||
typedef uint32_t uint32; |
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER |
|
||||||
#define __QAIC_HEADER(ff) ff |
|
||||||
#endif //__QAIC_HEADER
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_EXPORT |
|
||||||
#define __QAIC_HEADER_EXPORT |
|
||||||
#endif // __QAIC_HEADER_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_ATTRIBUTE |
|
||||||
#define __QAIC_HEADER_ATTRIBUTE |
|
||||||
#endif // __QAIC_HEADER_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL |
|
||||||
#define __QAIC_IMPL(ff) ff |
|
||||||
#endif //__QAIC_IMPL
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_EXPORT |
|
||||||
#define __QAIC_IMPL_EXPORT |
|
||||||
#endif // __QAIC_IMPL_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_ATTRIBUTE |
|
||||||
#define __QAIC_IMPL_ATTRIBUTE |
|
||||||
#endif // __QAIC_IMPL_ATTRIBUTE
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE; |
|
||||||
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE; |
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
#endif //_CALCULATOR_H
|
|
||||||
@ -1,613 +0,0 @@ |
|||||||
#ifndef _CALCULATOR_STUB_H |
|
||||||
#define _CALCULATOR_STUB_H |
|
||||||
#include "calculator.h" |
|
||||||
|
|
||||||
// remote.h
|
|
||||||
#include <stdint.h> |
|
||||||
#include <sys/types.h> |
|
||||||
|
|
||||||
typedef uint32_t remote_handle; |
|
||||||
typedef uint64_t remote_handle64; |
|
||||||
|
|
||||||
typedef struct { |
|
||||||
void *pv; |
|
||||||
size_t nLen; |
|
||||||
} remote_buf; |
|
||||||
|
|
||||||
typedef struct { |
|
||||||
int32_t fd; |
|
||||||
uint32_t offset; |
|
||||||
} remote_dma_handle; |
|
||||||
|
|
||||||
typedef union { |
|
||||||
remote_buf buf; |
|
||||||
remote_handle h; |
|
||||||
remote_handle64 h64; |
|
||||||
remote_dma_handle dma; |
|
||||||
} remote_arg; |
|
||||||
|
|
||||||
int remote_handle_open(const char* name, remote_handle *ph); |
|
||||||
int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra); |
|
||||||
int remote_handle_close(remote_handle h); |
|
||||||
|
|
||||||
#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \ |
|
||||||
((((uint32_t) (nAttr) & 0x7) << 29) | \
|
|
||||||
(((uint32_t) (nMethod) & 0x1f) << 24) | \
|
|
||||||
(((uint32_t) (nIn) & 0xff) << 16) | \
|
|
||||||
(((uint32_t) (nOut) & 0xff) << 8) | \
|
|
||||||
(((uint32_t) (noIn) & 0x0f) << 4) | \
|
|
||||||
((uint32_t) (noOut) & 0x0f)) |
|
||||||
|
|
||||||
#ifndef _QAIC_ENV_H |
|
||||||
#define _QAIC_ENV_H |
|
||||||
|
|
||||||
#ifdef __GNUC__ |
|
||||||
#ifdef __clang__ |
|
||||||
#pragma GCC diagnostic ignored "-Wunknown-pragmas" |
|
||||||
#else |
|
||||||
#pragma GCC diagnostic ignored "-Wpragmas" |
|
||||||
#endif |
|
||||||
#pragma GCC diagnostic ignored "-Wuninitialized" |
|
||||||
#pragma GCC diagnostic ignored "-Wunused-parameter" |
|
||||||
#pragma GCC diagnostic ignored "-Wunused-function" |
|
||||||
#endif |
|
||||||
|
|
||||||
#ifndef _ATTRIBUTE_UNUSED |
|
||||||
|
|
||||||
#ifdef _WIN32 |
|
||||||
#define _ATTRIBUTE_UNUSED |
|
||||||
#else |
|
||||||
#define _ATTRIBUTE_UNUSED __attribute__ ((unused)) |
|
||||||
#endif |
|
||||||
|
|
||||||
#endif // _ATTRIBUTE_UNUSED
|
|
||||||
|
|
||||||
#ifndef __QAIC_REMOTE |
|
||||||
#define __QAIC_REMOTE(ff) ff |
|
||||||
#endif //__QAIC_REMOTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER |
|
||||||
#define __QAIC_HEADER(ff) ff |
|
||||||
#endif //__QAIC_HEADER
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_EXPORT |
|
||||||
#define __QAIC_HEADER_EXPORT |
|
||||||
#endif // __QAIC_HEADER_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_ATTRIBUTE |
|
||||||
#define __QAIC_HEADER_ATTRIBUTE |
|
||||||
#endif // __QAIC_HEADER_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL |
|
||||||
#define __QAIC_IMPL(ff) ff |
|
||||||
#endif //__QAIC_IMPL
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_EXPORT |
|
||||||
#define __QAIC_IMPL_EXPORT |
|
||||||
#endif // __QAIC_IMPL_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_ATTRIBUTE |
|
||||||
#define __QAIC_IMPL_ATTRIBUTE |
|
||||||
#endif // __QAIC_IMPL_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_STUB |
|
||||||
#define __QAIC_STUB(ff) ff |
|
||||||
#endif //__QAIC_STUB
|
|
||||||
|
|
||||||
#ifndef __QAIC_STUB_EXPORT |
|
||||||
#define __QAIC_STUB_EXPORT |
|
||||||
#endif // __QAIC_STUB_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_STUB_ATTRIBUTE |
|
||||||
#define __QAIC_STUB_ATTRIBUTE |
|
||||||
#endif // __QAIC_STUB_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_SKEL |
|
||||||
#define __QAIC_SKEL(ff) ff |
|
||||||
#endif //__QAIC_SKEL__
|
|
||||||
|
|
||||||
#ifndef __QAIC_SKEL_EXPORT |
|
||||||
#define __QAIC_SKEL_EXPORT |
|
||||||
#endif // __QAIC_SKEL_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_SKEL_ATTRIBUTE |
|
||||||
#define __QAIC_SKEL_ATTRIBUTE |
|
||||||
#endif // __QAIC_SKEL_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifdef __QAIC_DEBUG__ |
|
||||||
#ifndef __QAIC_DBG_PRINTF__ |
|
||||||
#include <stdio.h> |
|
||||||
#define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0) |
|
||||||
#endif |
|
||||||
#else |
|
||||||
#define __QAIC_DBG_PRINTF__( ee ) (void)0 |
|
||||||
#endif |
|
||||||
|
|
||||||
|
|
||||||
#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof))) |
|
||||||
|
|
||||||
#define _COPY(dst, dof, src, sof, sz) \ |
|
||||||
do {\
|
|
||||||
struct __copy { \
|
|
||||||
char ar[sz]; \
|
|
||||||
};\
|
|
||||||
*(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
#define _COPYIF(dst, dof, src, sof, sz) \ |
|
||||||
do {\
|
|
||||||
if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\
|
|
||||||
_COPY(dst, dof, src, sof, sz); \
|
|
||||||
} \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline void _qaic_memmove(void* dst, void* src, int size) { |
|
||||||
int i; |
|
||||||
for(i = 0; i < size; ++i) { |
|
||||||
((char*)dst)[i] = ((char*)src)[i]; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
#define _MEMMOVEIF(dst, src, sz) \ |
|
||||||
do {\
|
|
||||||
if(dst != src) {\
|
|
||||||
_qaic_memmove(dst, src, sz);\
|
|
||||||
} \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
|
|
||||||
#define _ASSIGN(dst, src, sof) \ |
|
||||||
do {\
|
|
||||||
dst = OFFSET(src, sof); \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str)) |
|
||||||
|
|
||||||
#define AEE_SUCCESS 0 |
|
||||||
#define AEE_EOFFSET 0x80000400 |
|
||||||
#define AEE_EBADPARM (AEE_EOFFSET + 0x00E) |
|
||||||
|
|
||||||
#define _TRY(ee, func) \ |
|
||||||
do { \
|
|
||||||
if (AEE_SUCCESS != ((ee) = func)) {\
|
|
||||||
__QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\
|
|
||||||
goto ee##bail;\
|
|
||||||
} \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS) |
|
||||||
|
|
||||||
#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS) |
|
||||||
|
|
||||||
#ifdef __QAIC_DEBUG__ |
|
||||||
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv)) |
|
||||||
#else |
|
||||||
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv)) |
|
||||||
#endif |
|
||||||
|
|
||||||
|
|
||||||
#endif // _QAIC_ENV_H
|
|
||||||
|
|
||||||
#ifndef _ALLOCATOR_H |
|
||||||
#define _ALLOCATOR_H |
|
||||||
|
|
||||||
#include <stdlib.h> |
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
typedef struct _heap _heap; |
|
||||||
struct _heap { |
|
||||||
_heap* pPrev; |
|
||||||
const char* loc; |
|
||||||
uint64_t buf; |
|
||||||
}; |
|
||||||
|
|
||||||
typedef struct _allocator { |
|
||||||
_heap* pheap; |
|
||||||
uint8_t* stack; |
|
||||||
uint8_t* stackEnd; |
|
||||||
int nSize; |
|
||||||
} _allocator; |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) { |
|
||||||
_heap* pn = 0; |
|
||||||
pn = malloc(size + sizeof(_heap) - sizeof(uint64_t)); |
|
||||||
if(pn != 0) { |
|
||||||
pn->pPrev = *ppa; |
|
||||||
pn->loc = loc; |
|
||||||
*ppa = pn; |
|
||||||
*ppbuf = (void*)&(pn->buf); |
|
||||||
return 0; |
|
||||||
} else { |
|
||||||
return -1; |
|
||||||
} |
|
||||||
} |
|
||||||
#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1)) |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline int _allocator_alloc(_allocator* me, |
|
||||||
const char* loc, |
|
||||||
int size, |
|
||||||
unsigned int al, |
|
||||||
void** ppbuf) { |
|
||||||
if(size < 0) { |
|
||||||
return -1; |
|
||||||
} else if (size == 0) { |
|
||||||
*ppbuf = 0; |
|
||||||
return 0; |
|
||||||
} |
|
||||||
if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) { |
|
||||||
*ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al); |
|
||||||
me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size; |
|
||||||
return 0; |
|
||||||
} else { |
|
||||||
return _heap_alloc(&me->pheap, loc, size, ppbuf); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline void _allocator_deinit(_allocator* me) { |
|
||||||
_heap* pa = me->pheap; |
|
||||||
while(pa != 0) { |
|
||||||
_heap* pn = pa; |
|
||||||
const char* loc = pn->loc; |
|
||||||
(void)loc; |
|
||||||
pa = pn->pPrev; |
|
||||||
free(pn); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) { |
|
||||||
me->stack = stack; |
|
||||||
me->stackEnd = stack + stackSize; |
|
||||||
me->nSize = stackSize; |
|
||||||
me->pheap = 0; |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
#endif // _ALLOCATOR_H
|
|
||||||
|
|
||||||
#ifndef SLIM_H |
|
||||||
#define SLIM_H |
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
//a C data structure for the idl types that can be used to implement
|
|
||||||
//static and dynamic language bindings fairly efficiently.
|
|
||||||
//
|
|
||||||
//the goal is to have a minimal ROM and RAM footprint and without
|
|
||||||
//doing too many allocations. A good way to package these things seemed
|
|
||||||
//like the module boundary, so all the idls within one module can share
|
|
||||||
//all the type references.
|
|
||||||
|
|
||||||
|
|
||||||
#define PARAMETER_IN 0x0 |
|
||||||
#define PARAMETER_OUT 0x1 |
|
||||||
#define PARAMETER_INOUT 0x2 |
|
||||||
#define PARAMETER_ROUT 0x3 |
|
||||||
#define PARAMETER_INROUT 0x4 |
|
||||||
|
|
||||||
//the types that we get from idl
|
|
||||||
#define TYPE_OBJECT 0x0 |
|
||||||
#define TYPE_INTERFACE 0x1 |
|
||||||
#define TYPE_PRIMITIVE 0x2 |
|
||||||
#define TYPE_ENUM 0x3 |
|
||||||
#define TYPE_STRING 0x4 |
|
||||||
#define TYPE_WSTRING 0x5 |
|
||||||
#define TYPE_STRUCTURE 0x6 |
|
||||||
#define TYPE_UNION 0x7 |
|
||||||
#define TYPE_ARRAY 0x8 |
|
||||||
#define TYPE_SEQUENCE 0x9 |
|
||||||
|
|
||||||
//these require the pack/unpack to recurse
|
|
||||||
//so it's a hint to those languages that can optimize in cases where
|
|
||||||
//recursion isn't necessary.
|
|
||||||
#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE) |
|
||||||
#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION) |
|
||||||
#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY) |
|
||||||
#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE) |
|
||||||
|
|
||||||
|
|
||||||
typedef struct Type Type; |
|
||||||
|
|
||||||
#define INHERIT_TYPE\ |
|
||||||
int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\
|
|
||||||
union {\
|
|
||||||
struct {\
|
|
||||||
const uintptr_t p1;\
|
|
||||||
const uintptr_t p2;\
|
|
||||||
} _cast;\
|
|
||||||
struct {\
|
|
||||||
uint32_t iid;\
|
|
||||||
uint32_t bNotNil;\
|
|
||||||
} object;\
|
|
||||||
struct {\
|
|
||||||
const Type *arrayType;\
|
|
||||||
int32_t nItems;\
|
|
||||||
} array;\
|
|
||||||
struct {\
|
|
||||||
const Type *seqType;\
|
|
||||||
int32_t nMaxLen;\
|
|
||||||
} seqSimple; \
|
|
||||||
struct {\
|
|
||||||
uint32_t bFloating;\
|
|
||||||
uint32_t bSigned;\
|
|
||||||
} prim; \
|
|
||||||
const SequenceType* seqComplex;\
|
|
||||||
const UnionType *unionType;\
|
|
||||||
const StructType *structType;\
|
|
||||||
int32_t stringMaxLen;\
|
|
||||||
uint8_t bInterfaceNotNil;\
|
|
||||||
} param;\
|
|
||||||
uint8_t type;\
|
|
||||||
uint8_t nativeAlignment\
|
|
||||||
|
|
||||||
typedef struct UnionType UnionType; |
|
||||||
typedef struct StructType StructType; |
|
||||||
typedef struct SequenceType SequenceType; |
|
||||||
struct Type { |
|
||||||
INHERIT_TYPE; |
|
||||||
}; |
|
||||||
|
|
||||||
struct SequenceType { |
|
||||||
const Type * seqType; |
|
||||||
uint32_t nMaxLen; |
|
||||||
uint32_t inSize; |
|
||||||
uint32_t routSizePrimIn; |
|
||||||
uint32_t routSizePrimROut; |
|
||||||
}; |
|
||||||
|
|
||||||
//byte offset from the start of the case values for
|
|
||||||
//this unions case value array. it MUST be aligned
|
|
||||||
//at the alignment requrements for the descriptor
|
|
||||||
//
|
|
||||||
//if negative it means that the unions cases are
|
|
||||||
//simple enumerators, so the value read from the descriptor
|
|
||||||
//can be used directly to find the correct case
|
|
||||||
typedef union CaseValuePtr CaseValuePtr; |
|
||||||
union CaseValuePtr { |
|
||||||
const uint8_t* value8s; |
|
||||||
const uint16_t* value16s; |
|
||||||
const uint32_t* value32s; |
|
||||||
const uint64_t* value64s; |
|
||||||
}; |
|
||||||
|
|
||||||
//these are only used in complex cases
|
|
||||||
//so I pulled them out of the type definition as references to make
|
|
||||||
//the type smaller
|
|
||||||
struct UnionType { |
|
||||||
const Type *descriptor; |
|
||||||
uint32_t nCases; |
|
||||||
const CaseValuePtr caseValues; |
|
||||||
const Type * const *cases; |
|
||||||
int32_t inSize; |
|
||||||
int32_t routSizePrimIn; |
|
||||||
int32_t routSizePrimROut; |
|
||||||
uint8_t inAlignment; |
|
||||||
uint8_t routAlignmentPrimIn; |
|
||||||
uint8_t routAlignmentPrimROut; |
|
||||||
uint8_t inCaseAlignment; |
|
||||||
uint8_t routCaseAlignmentPrimIn; |
|
||||||
uint8_t routCaseAlignmentPrimROut; |
|
||||||
uint8_t nativeCaseAlignment; |
|
||||||
uint8_t bDefaultCase; |
|
||||||
}; |
|
||||||
|
|
||||||
struct StructType { |
|
||||||
uint32_t nMembers; |
|
||||||
const Type * const *members; |
|
||||||
int32_t inSize; |
|
||||||
int32_t routSizePrimIn; |
|
||||||
int32_t routSizePrimROut; |
|
||||||
uint8_t inAlignment; |
|
||||||
uint8_t routAlignmentPrimIn; |
|
||||||
uint8_t routAlignmentPrimROut; |
|
||||||
}; |
|
||||||
|
|
||||||
typedef struct Parameter Parameter; |
|
||||||
struct Parameter { |
|
||||||
INHERIT_TYPE; |
|
||||||
uint8_t mode; |
|
||||||
uint8_t bNotNil; |
|
||||||
}; |
|
||||||
|
|
||||||
#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64)) |
|
||||||
#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff) |
|
||||||
|
|
||||||
typedef struct Method Method; |
|
||||||
struct Method { |
|
||||||
uint32_t uScalars; //no method index
|
|
||||||
int32_t primInSize; |
|
||||||
int32_t primROutSize; |
|
||||||
int maxArgs; |
|
||||||
int numParams; |
|
||||||
const Parameter * const *params; |
|
||||||
uint8_t primInAlignment; |
|
||||||
uint8_t primROutAlignment; |
|
||||||
}; |
|
||||||
|
|
||||||
typedef struct Interface Interface; |
|
||||||
|
|
||||||
struct Interface { |
|
||||||
int nMethods; |
|
||||||
const Method * const *methodArray; |
|
||||||
int nIIds; |
|
||||||
const uint32_t *iids; |
|
||||||
const uint16_t* methodStringArray; |
|
||||||
const uint16_t* methodStrings; |
|
||||||
const char* strings; |
|
||||||
}; |
|
||||||
|
|
||||||
|
|
||||||
#endif //SLIM_H
|
|
||||||
|
|
||||||
|
|
||||||
#ifndef _CALCULATOR_SLIM_H |
|
||||||
#define _CALCULATOR_SLIM_H |
|
||||||
|
|
||||||
// remote.h
|
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
#ifndef __QAIC_SLIM |
|
||||||
#define __QAIC_SLIM(ff) ff |
|
||||||
#endif |
|
||||||
#ifndef __QAIC_SLIM_EXPORT |
|
||||||
#define __QAIC_SLIM_EXPORT |
|
||||||
#endif |
|
||||||
|
|
||||||
static const Type types[1]; |
|
||||||
static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}}; |
|
||||||
static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}}; |
|
||||||
static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))}; |
|
||||||
static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}}; |
|
||||||
static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])}; |
|
||||||
static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0"; |
|
||||||
static const uint16_t methodStrings[5] = {0,37,18,32,27}; |
|
||||||
static const uint16_t methodStringsArrays[2] = {3,0}; |
|
||||||
__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings}; |
|
||||||
#endif //_CALCULATOR_SLIM_H
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
|
|
||||||
#ifndef _const_calculator_handle |
|
||||||
#define _const_calculator_handle ((remote_handle)-1) |
|
||||||
#endif //_const_calculator_handle
|
|
||||||
|
|
||||||
static void _calculator_pls_dtor(void* data) { |
|
||||||
remote_handle* ph = (remote_handle*)data; |
|
||||||
if(_const_calculator_handle != *ph) { |
|
||||||
(void)__QAIC_REMOTE(remote_handle_close)(*ph); |
|
||||||
*ph = _const_calculator_handle; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
static int _calculator_pls_ctor(void* ctx, void* data) { |
|
||||||
remote_handle* ph = (remote_handle*)data; |
|
||||||
*ph = _const_calculator_handle; |
|
||||||
if(*ph == (remote_handle)-1) { |
|
||||||
return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph); |
|
||||||
} |
|
||||||
return 0; |
|
||||||
} |
|
||||||
|
|
||||||
#if (defined __qdsp6__) || (defined __hexagon__) |
|
||||||
#pragma weak adsp_pls_add_lookup |
|
||||||
extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
|
||||||
#pragma weak HAP_pls_add_lookup |
|
||||||
extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
|
||||||
|
|
||||||
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
|
||||||
remote_handle* ph; |
|
||||||
if(adsp_pls_add_lookup) { |
|
||||||
if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
|
||||||
return *ph; |
|
||||||
} |
|
||||||
return (remote_handle)-1; |
|
||||||
} else if(HAP_pls_add_lookup) { |
|
||||||
if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
|
||||||
return *ph; |
|
||||||
} |
|
||||||
return (remote_handle)-1; |
|
||||||
} |
|
||||||
return(remote_handle)-1; |
|
||||||
} |
|
||||||
|
|
||||||
#else //__qdsp6__ || __hexagon__
|
|
||||||
|
|
||||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare); |
|
||||||
|
|
||||||
#ifdef _WIN32 |
|
||||||
#include "Windows.h" |
|
||||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
|
||||||
return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare); |
|
||||||
} |
|
||||||
#elif __GNUC__ |
|
||||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
|
||||||
return __sync_val_compare_and_swap(puDest, uCompare, uExchange); |
|
||||||
} |
|
||||||
#endif //_WIN32
|
|
||||||
|
|
||||||
|
|
||||||
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
|
||||||
static remote_handle handle = _const_calculator_handle; |
|
||||||
if((remote_handle)-1 != handle) { |
|
||||||
return handle; |
|
||||||
} else { |
|
||||||
remote_handle tmp; |
|
||||||
int nErr = _calculator_pls_ctor("calculator", (void*)&tmp); |
|
||||||
if(nErr) { |
|
||||||
return (remote_handle)-1; |
|
||||||
} |
|
||||||
if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) { |
|
||||||
_calculator_pls_dtor(&tmp); |
|
||||||
} |
|
||||||
return handle; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
#endif //__qdsp6__
|
|
||||||
|
|
||||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE { |
|
||||||
return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra); |
|
||||||
} |
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
|
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
extern int remote_register_dma_handle(int, uint32_t); |
|
||||||
static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) { |
|
||||||
int _numIn[1]; |
|
||||||
remote_arg _pra[1]; |
|
||||||
uint32_t _primROut[1]; |
|
||||||
int _nErr = 0; |
|
||||||
_numIn[0] = 0; |
|
||||||
_pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut; |
|
||||||
_pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut); |
|
||||||
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra)); |
|
||||||
_COPY(_rout0, 0, _primROut, 0, 4); |
|
||||||
_CATCH(_nErr) {} |
|
||||||
return _nErr; |
|
||||||
} |
|
||||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE { |
|
||||||
uint32_t _mid = 0; |
|
||||||
return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet); |
|
||||||
} |
|
||||||
static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) { |
|
||||||
int _numIn[1]; |
|
||||||
remote_arg _pra[3]; |
|
||||||
uint32_t _primIn[2]; |
|
||||||
remote_arg* _praIn; |
|
||||||
remote_arg* _praROut; |
|
||||||
int _nErr = 0; |
|
||||||
_numIn[0] = 1; |
|
||||||
_pra[0].buf.pv = (void*)_primIn; |
|
||||||
_pra[0].buf.nLen = sizeof(_primIn); |
|
||||||
_COPY(_primIn, 0, _in0Len, 0, 4); |
|
||||||
_praIn = (_pra + 1); |
|
||||||
_praIn[0].buf.pv = _in0[0]; |
|
||||||
_praIn[0].buf.nLen = (1 * _in0Len[0]); |
|
||||||
_COPY(_primIn, 4, _rout1Len, 0, 4); |
|
||||||
_praROut = (_praIn + _numIn[0] + 0); |
|
||||||
_praROut[0].buf.pv = _rout1[0]; |
|
||||||
_praROut[0].buf.nLen = (1 * _rout1Len[0]); |
|
||||||
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra)); |
|
||||||
_CATCH(_nErr) {} |
|
||||||
return _nErr; |
|
||||||
} |
|
||||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE { |
|
||||||
uint32_t _mid = 1; |
|
||||||
return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen); |
|
||||||
} |
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
#endif //_CALCULATOR_STUB_H
|
|
||||||
Binary file not shown.
@ -1,38 +0,0 @@ |
|||||||
#ifndef EXTRACTOR_H |
|
||||||
#define EXTRACTOR_H |
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
#define ORBD_KEYPOINTS 3000 |
|
||||||
#define ORBD_DESCRIPTOR_LENGTH 32 |
|
||||||
#define ORBD_HEIGHT 874 |
|
||||||
#define ORBD_WIDTH 1164 |
|
||||||
#define ORBD_FOCAL 910 |
|
||||||
|
|
||||||
// matches OrbFeatures from log.capnp
|
|
||||||
struct orb_features { |
|
||||||
// align this
|
|
||||||
uint16_t n_corners; |
|
||||||
uint16_t xy[ORBD_KEYPOINTS][2]; |
|
||||||
uint8_t octave[ORBD_KEYPOINTS]; |
|
||||||
uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH]; |
|
||||||
int16_t matches[ORBD_KEYPOINTS]; |
|
||||||
}; |
|
||||||
|
|
||||||
// forward declare this
|
|
||||||
struct pyramid; |
|
||||||
|
|
||||||
// manage the pyramids in extractor.c
|
|
||||||
void init_gpyrs(); |
|
||||||
int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *); |
|
||||||
int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *); |
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
|
|
||||||
#endif // EXTRACTOR_H
|
|
||||||
@ -1,191 +0,0 @@ |
|||||||
#include <stdio.h> |
|
||||||
#include <stdlib.h> |
|
||||||
#include <signal.h> |
|
||||||
#include <unistd.h> |
|
||||||
#include <stdint.h> |
|
||||||
#include <assert.h> |
|
||||||
#include <sys/resource.h> |
|
||||||
|
|
||||||
#include "common/visionipc.h" |
|
||||||
#include "common/swaglog.h" |
|
||||||
|
|
||||||
#include "extractor.h" |
|
||||||
|
|
||||||
#ifdef DSP |
|
||||||
#include "dsp/gen/calculator.h" |
|
||||||
#else |
|
||||||
#include "turbocv.h" |
|
||||||
#endif |
|
||||||
|
|
||||||
#include <zmq.h> |
|
||||||
#include <capnp/serialize.h> |
|
||||||
#include "cereal/gen/cpp/log.capnp.h" |
|
||||||
|
|
||||||
#ifndef PATH_MAX |
|
||||||
#include <linux/limits.h> |
|
||||||
#endif |
|
||||||
|
|
||||||
volatile int do_exit = 0; |
|
||||||
|
|
||||||
static void set_do_exit(int sig) { |
|
||||||
do_exit = 1; |
|
||||||
} |
|
||||||
|
|
||||||
int main(int argc, char *argv[]) { |
|
||||||
int err; |
|
||||||
setpriority(PRIO_PROCESS, 0, -13); |
|
||||||
printf("starting orbd\n"); |
|
||||||
|
|
||||||
#ifdef DSP |
|
||||||
uint32_t test_leet = 0; |
|
||||||
char my_path[PATH_MAX+1]; |
|
||||||
memset(my_path, 0, sizeof(my_path)); |
|
||||||
|
|
||||||
ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path)); |
|
||||||
assert(len > 5); |
|
||||||
my_path[len-5] = '\0'; |
|
||||||
LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX); |
|
||||||
|
|
||||||
char adsp_path[PATH_MAX+1]; |
|
||||||
snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path); |
|
||||||
assert(putenv(adsp_path) == 0); |
|
||||||
|
|
||||||
assert(calculator_init(&test_leet) == 0); |
|
||||||
assert(test_leet == 0x1337); |
|
||||||
LOGW("orbd init complete"); |
|
||||||
#else |
|
||||||
init_gpyrs(); |
|
||||||
#endif |
|
||||||
|
|
||||||
signal(SIGINT, (sighandler_t) set_do_exit); |
|
||||||
signal(SIGTERM, (sighandler_t) set_do_exit); |
|
||||||
|
|
||||||
void *ctx = zmq_ctx_new(); |
|
||||||
|
|
||||||
void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB); |
|
||||||
assert(orb_features_sock); |
|
||||||
zmq_bind(orb_features_sock, "tcp://*:8058"); |
|
||||||
|
|
||||||
void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB); |
|
||||||
assert(orb_features_summary_sock); |
|
||||||
zmq_bind(orb_features_summary_sock, "tcp://*:8062"); |
|
||||||
|
|
||||||
struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features)); |
|
||||||
int last_frame_id = 0; |
|
||||||
uint64_t frame_count = 0; |
|
||||||
|
|
||||||
// every other frame
|
|
||||||
const int RATE = 2; |
|
||||||
|
|
||||||
VisionStream stream; |
|
||||||
while (!do_exit) { |
|
||||||
VisionStreamBufs buf_info; |
|
||||||
err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); |
|
||||||
if (err) { |
|
||||||
printf("visionstream connect fail\n"); |
|
||||||
usleep(100000); |
|
||||||
continue; |
|
||||||
} |
|
||||||
uint64_t timestamp_last_eof = 0; |
|
||||||
while (!do_exit) { |
|
||||||
VIPCBuf *buf; |
|
||||||
VIPCBufExtra extra; |
|
||||||
buf = visionstream_get(&stream, &extra); |
|
||||||
if (buf == NULL) { |
|
||||||
printf("visionstream get failed\n"); |
|
||||||
break; |
|
||||||
} |
|
||||||
|
|
||||||
// every other frame
|
|
||||||
frame_count++; |
|
||||||
if ((frame_count%RATE) != 0) { |
|
||||||
continue; |
|
||||||
} |
|
||||||
|
|
||||||
uint64_t start = nanos_since_boot(); |
|
||||||
#ifdef DSP |
|
||||||
int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features)); |
|
||||||
#else |
|
||||||
int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features); |
|
||||||
#endif |
|
||||||
uint64_t end = nanos_since_boot(); |
|
||||||
LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
if (last_frame_id+RATE != extra.frame_id) { |
|
||||||
LOGW("dropped frame!"); |
|
||||||
} |
|
||||||
|
|
||||||
last_frame_id = extra.frame_id; |
|
||||||
|
|
||||||
if (timestamp_last_eof == 0) { |
|
||||||
timestamp_last_eof = extra.timestamp_eof; |
|
||||||
continue; |
|
||||||
} |
|
||||||
|
|
||||||
int match_count = 0; |
|
||||||
|
|
||||||
// *** send OrbFeatures ***
|
|
||||||
{ |
|
||||||
// create capnp message
|
|
||||||
capnp::MallocMessageBuilder msg; |
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
|
||||||
event.setLogMonoTime(nanos_since_boot()); |
|
||||||
|
|
||||||
auto orb_features = event.initOrbFeatures(); |
|
||||||
|
|
||||||
// set timestamps
|
|
||||||
orb_features.setTimestampEof(extra.timestamp_eof); |
|
||||||
orb_features.setTimestampLastEof(timestamp_last_eof); |
|
||||||
|
|
||||||
// init descriptors for send
|
|
||||||
kj::ArrayPtr<capnp::byte> descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners); |
|
||||||
orb_features.setDescriptors(descriptorsPtr); |
|
||||||
|
|
||||||
auto xs = orb_features.initXs(features->n_corners); |
|
||||||
auto ys = orb_features.initYs(features->n_corners); |
|
||||||
auto octaves = orb_features.initOctaves(features->n_corners); |
|
||||||
auto matches = orb_features.initMatches(features->n_corners); |
|
||||||
|
|
||||||
// copy out normalized keypoints
|
|
||||||
for (int i = 0; i < features->n_corners; i++) { |
|
||||||
xs.set(i, (features->xy[i][0] * 1.0f - ORBD_WIDTH / 2) / ORBD_FOCAL); |
|
||||||
ys.set(i, (features->xy[i][1] * 1.0f - ORBD_HEIGHT / 2) / ORBD_FOCAL); |
|
||||||
octaves.set(i, features->octave[i]); |
|
||||||
matches.set(i, features->matches[i]); |
|
||||||
match_count += features->matches[i] != -1; |
|
||||||
} |
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg); |
|
||||||
auto bytes = words.asBytes(); |
|
||||||
zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0); |
|
||||||
} |
|
||||||
|
|
||||||
// *** send OrbFeaturesSummary ***
|
|
||||||
|
|
||||||
{ |
|
||||||
// create capnp message
|
|
||||||
capnp::MallocMessageBuilder msg; |
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
|
||||||
event.setLogMonoTime(nanos_since_boot()); |
|
||||||
|
|
||||||
auto orb_features_summary = event.initOrbFeaturesSummary(); |
|
||||||
|
|
||||||
orb_features_summary.setTimestampEof(extra.timestamp_eof); |
|
||||||
orb_features_summary.setTimestampLastEof(timestamp_last_eof); |
|
||||||
orb_features_summary.setFeatureCount(features->n_corners); |
|
||||||
orb_features_summary.setMatchCount(match_count); |
|
||||||
orb_features_summary.setComputeNs(end-start); |
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg); |
|
||||||
auto bytes = words.asBytes(); |
|
||||||
zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0); |
|
||||||
} |
|
||||||
|
|
||||||
timestamp_last_eof = extra.timestamp_eof; |
|
||||||
} |
|
||||||
} |
|
||||||
visionstream_destroy(&stream); |
|
||||||
return 0; |
|
||||||
} |
|
||||||
|
|
||||||
@ -1,13 +0,0 @@ |
|||||||
#!/bin/sh |
|
||||||
finish() { |
|
||||||
echo "exiting orbd" |
|
||||||
pkill -SIGINT -P $$ |
|
||||||
} |
|
||||||
|
|
||||||
trap finish EXIT |
|
||||||
|
|
||||||
while true; do |
|
||||||
./orbd & |
|
||||||
wait $! |
|
||||||
done |
|
||||||
|
|
||||||
Binary file not shown.
Binary file not shown.
@ -0,0 +1,53 @@ |
|||||||
|
#include <string.h> |
||||||
|
#include <assert.h> |
||||||
|
|
||||||
|
#include "clutil.h" |
||||||
|
|
||||||
|
#include "rgb_to_yuv.h" |
||||||
|
|
||||||
|
void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { |
||||||
|
int err = 0; |
||||||
|
memset(s, 0, sizeof(*s)); |
||||||
|
assert(width % 2 == 0); |
||||||
|
assert(height % 2 == 0); |
||||||
|
s->width = width; |
||||||
|
s->height = height; |
||||||
|
char args[1024]; |
||||||
|
snprintf(args, sizeof(args), |
||||||
|
"-cl-fast-relaxed-math -cl-denorms-are-zero " |
||||||
|
#ifdef CL_DEBUG |
||||||
|
"-DCL_DEBUG " |
||||||
|
#endif |
||||||
|
"-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d", |
||||||
|
width, height, width/ 2, height / 2, rgb_stride, width * height); |
||||||
|
cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "rgb_to_yuv.cl", args); |
||||||
|
|
||||||
|
s->rgb_to_yuv_krnl = clCreateKernel(prg, "rgb_to_yuv", &err); |
||||||
|
assert(err == 0); |
||||||
|
// done with this
|
||||||
|
err = clReleaseProgram(prg); |
||||||
|
assert(err == 0); |
||||||
|
} |
||||||
|
|
||||||
|
void rgb_to_yuv_destroy(RGBToYUVState* s) { |
||||||
|
int err = 0; |
||||||
|
err = clReleaseKernel(s->rgb_to_yuv_krnl); |
||||||
|
assert(err == 0); |
||||||
|
} |
||||||
|
|
||||||
|
void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) { |
||||||
|
int err = 0; |
||||||
|
err = clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl); |
||||||
|
assert(err == 0); |
||||||
|
err = clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl); |
||||||
|
assert(err == 0); |
||||||
|
const size_t work_size[2] = { |
||||||
|
(size_t)(s->width + (s->width % 4 == 0 ? 0 : (4 - s->width % 4))) / 4,
|
||||||
|
(size_t)(s->height + (s->height % 4 == 0 ? 0 : (4 - s->height % 4))) / 4 |
||||||
|
}; |
||||||
|
cl_event event; |
||||||
|
err = clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event); |
||||||
|
assert(err == 0); |
||||||
|
clWaitForEvents(1, &event); |
||||||
|
clReleaseEvent(event); |
||||||
|
} |
||||||
@ -0,0 +1,127 @@ |
|||||||
|
#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) |
||||||
|
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) |
||||||
|
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) |
||||||
|
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) |
||||||
|
|
||||||
|
inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) { |
||||||
|
uchar2 yy = (uchar2)( |
||||||
|
RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), |
||||||
|
RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3) |
||||||
|
); |
||||||
|
#ifdef CL_DEBUG |
||||||
|
if(yi >= RGB_SIZE) |
||||||
|
printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE); |
||||||
|
#endif |
||||||
|
vstore2(yy, 0, out_yuv + yi); |
||||||
|
} |
||||||
|
|
||||||
|
inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) { |
||||||
|
const uchar4 yy = (uchar4)( |
||||||
|
RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), |
||||||
|
RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3), |
||||||
|
RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6), |
||||||
|
RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1) |
||||||
|
); |
||||||
|
#ifdef CL_DEBUG |
||||||
|
if(yi > RGB_SIZE - 4) |
||||||
|
printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4); |
||||||
|
#endif |
||||||
|
vstore4(yy, 0, out_yuv + yi); |
||||||
|
} |
||||||
|
|
||||||
|
inline void convert_uv(__global uchar * out_yuv, int ui, int vi, |
||||||
|
const uchar8 rgbs1, const uchar8 rgbs2) { |
||||||
|
// U & V: average of 2x2 pixels square |
||||||
|
const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); |
||||||
|
const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); |
||||||
|
const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); |
||||||
|
#ifdef CL_DEBUG |
||||||
|
if(ui >= RGB_SIZE + RGB_SIZE / 4) |
||||||
|
printf("U overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4); |
||||||
|
if(vi >= RGB_SIZE + RGB_SIZE / 2) |
||||||
|
printf("V overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2); |
||||||
|
#endif |
||||||
|
out_yuv[ui] = RGB_TO_U(ar, ag, ab); |
||||||
|
out_yuv[vi] = RGB_TO_V(ar, ag, ab); |
||||||
|
} |
||||||
|
|
||||||
|
inline void convert_2_uvs(__global uchar * out_yuv, int ui, int vi, |
||||||
|
const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) { |
||||||
|
// U & V: average of 2x2 pixels square |
||||||
|
const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); |
||||||
|
const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); |
||||||
|
const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); |
||||||
|
const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1); |
||||||
|
const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2); |
||||||
|
const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3); |
||||||
|
uchar2 u2 = (uchar2)( |
||||||
|
RGB_TO_U(ar1, ag1, ab1), |
||||||
|
RGB_TO_U(ar2, ag2, ab2) |
||||||
|
); |
||||||
|
uchar2 v2 = (uchar2)( |
||||||
|
RGB_TO_V(ar1, ag1, ab1), |
||||||
|
RGB_TO_V(ar2, ag2, ab2) |
||||||
|
); |
||||||
|
#ifdef CL_DEBUG1 |
||||||
|
if(ui > RGB_SIZE + RGB_SIZE / 4 - 2) |
||||||
|
printf("U 2 overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4 - 2); |
||||||
|
if(vi > RGB_SIZE + RGB_SIZE / 2 - 2) |
||||||
|
printf("V 2 overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2 - 2); |
||||||
|
#endif |
||||||
|
vstore2(u2, 0, out_yuv + ui); |
||||||
|
vstore2(v2, 0, out_yuv + vi); |
||||||
|
} |
||||||
|
|
||||||
|
__kernel void rgb_to_yuv(__global uchar const * const rgb, |
||||||
|
__global uchar * out_yuv) |
||||||
|
{ |
||||||
|
const int dx = get_global_id(0); |
||||||
|
const int dy = get_global_id(1); |
||||||
|
const int col = mul24(dx, 4); // Current column in rgb image |
||||||
|
const int row = mul24(dy, 4); // Current row in rgb image |
||||||
|
const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted |
||||||
|
const int yi_start = mad24(row, WIDTH, col); // Start offset in the target yuv buffer |
||||||
|
int ui = mad24(row / 2, UV_WIDTH, RGB_SIZE + col / 2); |
||||||
|
int vi = mad24(row / 2 , UV_WIDTH, RGB_SIZE + UV_WIDTH * UV_HEIGHT + col / 2); |
||||||
|
int num_col = min(WIDTH - col, 4); |
||||||
|
int num_row = min(HEIGHT - row, 4); |
||||||
|
if(num_row == 4) { |
||||||
|
const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); |
||||||
|
const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); |
||||||
|
const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); |
||||||
|
const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); |
||||||
|
const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2); |
||||||
|
const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8); |
||||||
|
const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3); |
||||||
|
const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8); |
||||||
|
if(num_col == 4) { |
||||||
|
convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); |
||||||
|
convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); |
||||||
|
convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1); |
||||||
|
convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1); |
||||||
|
convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); |
||||||
|
convert_2_uvs(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1); |
||||||
|
} else if(num_col == 2) { |
||||||
|
convert_2_ys(out_yuv, yi_start, rgbs0_0); |
||||||
|
convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); |
||||||
|
convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0); |
||||||
|
convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0); |
||||||
|
convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); |
||||||
|
convert_uv(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0); |
||||||
|
} |
||||||
|
} else { |
||||||
|
const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); |
||||||
|
const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); |
||||||
|
const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); |
||||||
|
const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); |
||||||
|
if(num_col == 4) { |
||||||
|
convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); |
||||||
|
convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); |
||||||
|
convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); |
||||||
|
} else if(num_col == 2) { |
||||||
|
convert_2_ys(out_yuv, yi_start, rgbs0_0); |
||||||
|
convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); |
||||||
|
convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,32 @@ |
|||||||
|
#ifndef RGB_TO_YUV_H |
||||||
|
#define RGB_TO_YUV_H |
||||||
|
|
||||||
|
#include <inttypes.h> |
||||||
|
#include <stdbool.h> |
||||||
|
|
||||||
|
#ifdef __APPLE__ |
||||||
|
#include <OpenCL/cl.h> |
||||||
|
#else |
||||||
|
#include <CL/cl.h> |
||||||
|
#endif |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
|
||||||
|
typedef struct { |
||||||
|
int width, height; |
||||||
|
cl_kernel rgb_to_yuv_krnl; |
||||||
|
} RGBToYUVState; |
||||||
|
|
||||||
|
void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride); |
||||||
|
|
||||||
|
void rgb_to_yuv_destroy(RGBToYUVState* s); |
||||||
|
|
||||||
|
void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl); |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
#endif // RGB_TO_YUV_H
|
||||||
@ -0,0 +1,201 @@ |
|||||||
|
#include <memory.h> |
||||||
|
#include <iostream> |
||||||
|
#include <getopt.h> |
||||||
|
#include <math.h> |
||||||
|
#include <fstream> |
||||||
|
#include <cstdlib> |
||||||
|
#include <vector> |
||||||
|
#include <string> |
||||||
|
#include <iomanip> |
||||||
|
#include <thread> |
||||||
|
#include <fcntl.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <cassert> |
||||||
|
#include <cstdint> |
||||||
|
|
||||||
|
#ifdef ANDROID |
||||||
|
|
||||||
|
#define MAXE 0 |
||||||
|
#include <unistd.h> |
||||||
|
|
||||||
|
#else |
||||||
|
// The libyuv implementation on ARM is slightly different than on x86
|
||||||
|
// Our implementation matches the ARM version, so accept errors of 1
|
||||||
|
#define MAXE 1 |
||||||
|
|
||||||
|
#endif |
||||||
|
|
||||||
|
#include <libyuv.h> |
||||||
|
|
||||||
|
#include <CL/cl.h> |
||||||
|
|
||||||
|
#include "clutil.h" |
||||||
|
#include "rgb_to_yuv.h" |
||||||
|
|
||||||
|
|
||||||
|
static inline double millis_since_boot() { |
||||||
|
struct timespec t; |
||||||
|
clock_gettime(CLOCK_BOOTTIME, &t); |
||||||
|
return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6; |
||||||
|
} |
||||||
|
|
||||||
|
void cl_init(cl_device_id &device_id, cl_context &context) { |
||||||
|
int err; |
||||||
|
cl_platform_id platform_id = NULL; |
||||||
|
cl_uint num_devices; |
||||||
|
cl_uint num_platforms; |
||||||
|
|
||||||
|
err = clGetPlatformIDs(1, &platform_id, &num_platforms); |
||||||
|
err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, |
||||||
|
&device_id, &num_devices); |
||||||
|
cl_print_info(platform_id, device_id); |
||||||
|
context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
bool compare_results(uint8_t *a, uint8_t *b, int len, int stride, int width, int height, uint8_t *rgb) { |
||||||
|
int min_diff = 0., max_diff = 0., max_e = 0.; |
||||||
|
int e1 = 0, e0 = 0; |
||||||
|
int e0y = 0, e0u = 0, e0v = 0, e1y = 0, e1u = 0, e1v = 0; |
||||||
|
int max_e_i = 0; |
||||||
|
for (int i = 0;i < len;i++) { |
||||||
|
int e = ((int)a[i]) - ((int)b[i]); |
||||||
|
if(e < min_diff) { |
||||||
|
min_diff = e; |
||||||
|
} |
||||||
|
if(e > max_diff) { |
||||||
|
max_diff = e; |
||||||
|
} |
||||||
|
int e_abs = std::abs(e); |
||||||
|
if(e_abs > max_e) { |
||||||
|
max_e = e_abs; |
||||||
|
max_e_i = i; |
||||||
|
} |
||||||
|
if(e_abs < 1) { |
||||||
|
e0++; |
||||||
|
if(i < stride * height) |
||||||
|
e0y++; |
||||||
|
else if(i < stride * height + stride * height / 4) |
||||||
|
e0u++; |
||||||
|
else |
||||||
|
e0v++; |
||||||
|
} else { |
||||||
|
e1++; |
||||||
|
if(i < stride * height) |
||||||
|
e1y++; |
||||||
|
else if(i < stride * height + stride * height / 4) |
||||||
|
e1u++; |
||||||
|
else |
||||||
|
e1v++; |
||||||
|
} |
||||||
|
} |
||||||
|
//printf("max diff : %d, min diff : %d, e < 1: %d, e >= 1: %d\n", max_diff, min_diff, e0, e1);
|
||||||
|
//printf("Y: e < 1: %d, e >= 1: %d, U: e < 1: %d, e >= 1: %d, V: e < 1: %d, e >= 1: %d\n", e0y, e1y, e0u, e1u, e0v, e1v);
|
||||||
|
if(max_e <= MAXE) { |
||||||
|
return true; |
||||||
|
} |
||||||
|
int row = max_e_i / stride; |
||||||
|
if(row < height) { |
||||||
|
printf("max error is Y: %d = (libyuv: %u - cl: %u), row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], row, max_e_i % stride); |
||||||
|
} else if(row >= height && row < (height + height / 4)) { |
||||||
|
printf("max error is U: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height) / 2, max_e_i % stride / 2); |
||||||
|
} else { |
||||||
|
printf("max error is V: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height - height / 4) / 2, max_e_i % stride / 2); |
||||||
|
} |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
int main(int argc, char** argv) { |
||||||
|
srand(1337); |
||||||
|
|
||||||
|
clu_init(); |
||||||
|
cl_device_id device_id; |
||||||
|
cl_context context; |
||||||
|
cl_init(device_id, context) ; |
||||||
|
|
||||||
|
int err; |
||||||
|
const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
|
||||||
|
cl_command_queue q = clCreateCommandQueueWithProperties(context, device_id, props, &err); |
||||||
|
if(err != 0) { |
||||||
|
std::cout << "clCreateCommandQueueWithProperties error: " << err << std::endl; |
||||||
|
} |
||||||
|
|
||||||
|
int width = 1164; |
||||||
|
int height = 874; |
||||||
|
|
||||||
|
int opt = 0; |
||||||
|
while ((opt = getopt(argc, argv, "f")) != -1) |
||||||
|
{ |
||||||
|
switch (opt) |
||||||
|
{ |
||||||
|
case 'f': |
||||||
|
std::cout << "Using front camera dimensions" << std::endl; |
||||||
|
int width = 1152; |
||||||
|
int height = 846; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
std::cout << "Width: " << width << " Height: " << height << std::endl; |
||||||
|
uint8_t *rgb_frame = new uint8_t[width * height * 3]; |
||||||
|
|
||||||
|
|
||||||
|
RGBToYUVState rgb_to_yuv_state; |
||||||
|
rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, width, height, width * 3); |
||||||
|
|
||||||
|
int frame_yuv_buf_size = width * height * 3 / 2; |
||||||
|
cl_mem yuv_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err); |
||||||
|
uint8_t *frame_yuv_buf = new uint8_t[frame_yuv_buf_size]; |
||||||
|
uint8_t *frame_yuv_ptr_y = frame_yuv_buf; |
||||||
|
uint8_t *frame_yuv_ptr_u = frame_yuv_buf + (width * height); |
||||||
|
uint8_t *frame_yuv_ptr_v = frame_yuv_ptr_u + ((width/2) * (height/2)); |
||||||
|
|
||||||
|
cl_mem rgb_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err); |
||||||
|
int mismatched = 0; |
||||||
|
int counter = 0; |
||||||
|
srand (time(NULL)); |
||||||
|
|
||||||
|
for (int i = 0; i < 100; i++){ |
||||||
|
for (int i = 0; i < width * height * 3; i++){ |
||||||
|
rgb_frame[i] = (uint8_t)rand(); |
||||||
|
} |
||||||
|
|
||||||
|
double t1 = millis_since_boot(); |
||||||
|
libyuv::RGB24ToI420((uint8_t*)rgb_frame, width * 3, |
||||||
|
frame_yuv_ptr_y, width, |
||||||
|
frame_yuv_ptr_u, width/2, |
||||||
|
frame_yuv_ptr_v, width/2, |
||||||
|
width, height); |
||||||
|
double t2 = millis_since_boot(); |
||||||
|
//printf("Libyuv: rgb to yuv: %.2fms\n", t2-t1);
|
||||||
|
|
||||||
|
clEnqueueWriteBuffer(q, rgb_cl, CL_TRUE, 0, width * height * 3, (void *)rgb_frame, 0, NULL, NULL); |
||||||
|
t1 = millis_since_boot(); |
||||||
|
rgb_to_yuv_queue(&rgb_to_yuv_state, q, rgb_cl, yuv_cl); |
||||||
|
t2 = millis_since_boot(); |
||||||
|
|
||||||
|
//printf("OpenCL: rgb to yuv: %.2fms\n", t2-t1);
|
||||||
|
uint8_t *yyy = (uint8_t *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, |
||||||
|
CL_MAP_READ, 0, frame_yuv_buf_size, |
||||||
|
0, NULL, NULL, &err); |
||||||
|
if(!compare_results(frame_yuv_ptr_y, yyy, frame_yuv_buf_size, width, width, height, (uint8_t*)rgb_frame)) |
||||||
|
mismatched++; |
||||||
|
clEnqueueUnmapMemObject(q, yuv_cl, yyy, 0, NULL, NULL); |
||||||
|
|
||||||
|
// std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
||||||
|
if(counter++ % 100 == 0) |
||||||
|
printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched); |
||||||
|
|
||||||
|
} |
||||||
|
printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched); |
||||||
|
|
||||||
|
delete[] frame_yuv_buf; |
||||||
|
rgb_to_yuv_destroy(&rgb_to_yuv_state); |
||||||
|
clReleaseContext(context); |
||||||
|
delete[] rgb_frame; |
||||||
|
|
||||||
|
if (mismatched == 0) |
||||||
|
return 0; |
||||||
|
else |
||||||
|
return -1; |
||||||
|
} |
||||||
Loading…
Reference in new issue