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