diff --git a/RELEASES.md b/RELEASES.md index 2c63e90833..34837bcc8c 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,13 @@ +Version 0.4.5 (2018-04-27) +========================== + * Release notes added to the update popup + * Improve auto shut-off logic to disallow empty battery + * Added onboarding instructions + * Include orbd, the first piece of new calibration algorithm + * Show remaining upload data instead of file numbers + * Fix UI bugs + * Fix memory leaks + Version 0.4.4 (2018-04-13) ========================== * EON are flipped! Flip your EON's mount! diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index d2985f2aab..845f9482be 100644 --- a/apk/ai.comma.plus.frame.apk +++ b/apk/ai.comma.plus.frame.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:70323271320417698bb8640f18bbed920199fb61553ae85ff8a44a08a39e34b5 -size 2108896 +oid sha256:f1542ec60d6c18b0bef02f0dff85989814e99b9b35f331ae8416b6c0367fe368 +size 2116281 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 2cc6bd8e5f..56e288f15a 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0ddd24ea3ab1349196d6413eed5432d5292d5912c641b6c3e76a46913b284080 -size 14337199 +oid sha256:8aa566ee67a720686f86bcde0eb4905d26735a6fa1d08459f254c8bddad95270 +size 17455432 diff --git a/cereal/.gitignore b/cereal/.gitignore index 4f62b849d5..57b5883f38 100644 --- a/cereal/.gitignore +++ b/cereal/.gitignore @@ -1 +1,3 @@ gen +node_modules +package-lock.json diff --git a/cereal/Makefile b/cereal/Makefile index cb064ddb4c..b86fa68f58 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -1,40 +1,48 @@ PWD := $(shell pwd) -SRCS := log.capnp car.capnp mapd.capnp +SRCS := log.capnp car.capnp -GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/mapd.capnp.c++ +GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ +JS := gen/js/car.capnp.js gen/js/log.capnp.js UNAME_M ?= $(shell uname -m) # only generate C++ for docker tests ifneq ($(OPTEST),1) - GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/mapd.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h + GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h -# Dont build java on the phone... -ifeq ($(UNAME_M),x86_64) - GENS += gen/java/Car.java gen/java/Log.java -endif + ifeq ($(UNAME_M),x86_64) + GENS += gen/java/Car.java gen/java/Log.java + endif endif ifeq ($(UNAME_M),aarch64) -CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc + CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc else -CAPNPC=capnpc + CAPNPC=capnpc endif .PHONY: all all: $(GENS) +js: $(JS) .PHONY: clean clean: rm -rf gen + rm -rf node_modules + rm -rf package-lock.json gen/c/%.capnp.c: %.capnp @echo "[ CAPNPC C ] $@" mkdir -p gen/c/ $(CAPNPC) '$<' -o c:gen/c/ +gen/js/%.capnp.js: %.capnp + @echo "[ CAPNPC JavaScript ] $@" + mkdir -p gen/js/ + sh ./generate_javascript.sh + gen/cpp/%.capnp.c++: %.capnp @echo "[ CAPNPC C++ ] $@" mkdir -p gen/cpp/ @@ -46,7 +54,6 @@ gen/java/Car.java gen/java/Log.java: $(SRCS) $(CAPNPC) $^ -o java:gen/java # c-capnproto needs some empty headers -gen/c/c++.capnp.h gen/c/java.capnp.h: - mkdir -p gen/c/ +gen/c/include/c++.capnp.h gen/c/include/java.capnp.h: + mkdir -p gen/c/include touch '$@' - diff --git a/cereal/__init__.py b/cereal/__init__.py index 2809e623e3..2d3b48526b 100644 --- a/cereal/__init__.py +++ b/cereal/__init__.py @@ -6,4 +6,3 @@ capnp.remove_import_hook() log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) -mapd = capnp.load(os.path.join(CEREAL_PATH, "mapd.capnp")) diff --git a/cereal/car.capnp b/cereal/car.capnp index 7e94de7ed0..c290edb8f6 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -1,7 +1,7 @@ -using Cxx = import "c++.capnp"; +using Cxx = import "./include/c++.capnp"; $Cxx.namespace("cereal"); -using Java = import "java.capnp"; +using Java = import "./include/java.capnp"; $Java.package("ai.comma.openpilot.cereal"); $Java.outerClassname("Car"); @@ -293,6 +293,7 @@ struct CarParams { elm327 @3; gm @4; hondaBosch @5; + ford @6; } # things about the car in the manual diff --git a/cereal/c++.capnp b/cereal/include/c++.capnp similarity index 100% rename from cereal/c++.capnp rename to cereal/include/c++.capnp diff --git a/cereal/java.capnp b/cereal/include/java.capnp similarity index 100% rename from cereal/java.capnp rename to cereal/include/java.capnp diff --git a/cereal/log.capnp b/cereal/log.capnp index 5d001a611e..84bf1b3b5e 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1,7 +1,7 @@ -using Cxx = import "c++.capnp"; +using Cxx = import "./include/c++.capnp"; $Cxx.namespace("cereal"); -using Java = import "java.capnp"; +using Java = import "./include/java.capnp"; $Java.package("ai.comma.openpilot.cereal"); $Java.outerClassname("Log"); @@ -1497,6 +1497,15 @@ struct OrbFeatures { matches @6: List(Int16); } +struct OrbFeaturesSummary { + timestampEof @0 :UInt64; + timestampLastEof @1 :UInt64; + + featureCount @2 :UInt16; + matchCount @3 :UInt16; + computeNs @4 :UInt64; +} + struct OrbKeyFrame { # this is a globally unique id for the KeyFrame id @0: UInt64; @@ -1572,5 +1581,6 @@ struct Event { applanixLocation @55 :LiveLocationData; orbKeyFrame @56 :OrbKeyFrame; uiLayoutState @57 :UiLayoutState; + orbFeaturesSummary @58 :OrbFeaturesSummary; } } diff --git a/cereal/mapd.capnp b/cereal/mapd.capnp deleted file mode 100644 index 2fbf286dd4..0000000000 --- a/cereal/mapd.capnp +++ /dev/null @@ -1,18 +0,0 @@ -using Cxx = import "c++.capnp"; -$Cxx.namespace("cereal"); - -using Java = import "java.capnp"; -$Java.package("ai.comma.openpilot.cereal"); -$Java.outerClassname("Map"); - -using Log = import "log.capnp"; - -@0xe1a6ab330ea7205f; - -struct MapdRequest { - pos @0 :Log.ECEFPoint; -} - -struct MapdResponse { - kfs @0 :List(Log.OrbKeyFrame); -} diff --git a/common/params.py b/common/params.py index 7955c88ed9..58b881f308 100755 --- a/common/params.py +++ b/common/params.py @@ -46,10 +46,11 @@ class UnknownKeyName(Exception): keys = { # written: manager -# read: loggerd, uploaderd, baseui +# read: loggerd, uploaderd, offroad "DongleId": TxType.PERSISTANT, "AccessToken": TxType.PERSISTANT, "Version": TxType.PERSISTANT, + "TrainingVersion": TxType.PERSISTANT, "GitCommit": TxType.PERSISTANT, "GitBranch": TxType.PERSISTANT, "GitRemote": TxType.PERSISTANT, @@ -59,6 +60,7 @@ keys = { "IsRearViewMirror": TxType.PERSISTANT, "IsFcwEnabled": TxType.PERSISTANT, "HasAcceptedTerms": TxType.PERSISTANT, + "CompletedTrainingVersion": TxType.PERSISTANT, "IsUploadVideoOverCellularEnabled": TxType.PERSISTANT, # written: visiond # read: visiond, controlsd diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py index cdb2bc7e31..1cd7a8e9d5 100644 --- a/common/transformations/coordinates.py +++ b/common/transformations/coordinates.py @@ -71,7 +71,7 @@ class LocalCoord(object): """ def __init__(self, init_geodetic, init_ecef): self.init_ecef = init_ecef - lat, lon, _ = (np.pi/180)*init_geodetic + lat, lon, _ = (np.pi/180)*np.array(init_geodetic) self.ned2ecef_matrix = np.array([[-np.sin(lat)*np.cos(lon), -np.sin(lon), -np.cos(lat)*np.cos(lon)], [-np.sin(lat)*np.sin(lon), np.cos(lon), -np.cos(lat)*np.sin(lon)], [np.cos(lat), 0, -np.sin(lat)]]) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index e325e76310..6da41e182d 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -384,6 +384,8 @@ void *thermal_thread(void *crap) { pthread_mutex_lock(&usb_lock); libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); + + zmq_msg_close(&msg); } // turn the fan off when we exit @@ -462,8 +464,8 @@ void _pigeon_send(const char *dat, int len) { pthread_mutex_lock(&usb_lock); err = libusb_bulk_transfer(dev_handle, 2, a, ll+1, &sent, TIMEOUT); if (err < 0) { handle_usb_issue(err, __func__); } - assert(err == 0); - assert(sent == ll+1); + /*assert(err == 0); + assert(sent == ll+1);*/ //hexdump(a, ll+1); pthread_mutex_unlock(&usb_lock); } diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 52d7ceba7b..7f23c159eb 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -294,6 +294,7 @@ class CANParser { UpdateValid(sec); + zmq_msg_close(&msg); } std::vector query(uint64_t sec) { diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 1a3692d0c1..d7c80a11d1 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -188,7 +188,7 @@ class CarInterface(object): ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car - is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310'] + is_fw_modified = os.getenv("DONGLE_ID") in ['ff83f397542ab647'] ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 136faef674..2ce828d551 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.4.4-release" +#define COMMA_VERSION "0.4.5-release" diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index f67976222f..afa79d48a9 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -72,31 +72,31 @@ class AlertManager(object): "fcw": Alert( "Brake!", - "Risk of collision detected", + "Risk of Collision", AlertStatus.critical, AlertSize.full, Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( "TAKE CONTROL", - "Turn exceeds steering limit", + "Turn Exceeds Steering Limit", AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), "steerTempUnavailable": Alert( "TAKE CONTROL", - "Steering temporarily unavailable", + "Steering Temporarily Unavailable", AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), "preDriverDistracted": Alert( "TAKE CONTROL", - "User appears distracted", + "User Appears Distracted", AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, "steerRequired", None, 0., .1, .1), "driverDistracted": Alert( "TAKE CONTROL TO REGAIN SPEED", - "User appears distracted", + "User Appears Distracted", AlertStatus.critical, AlertSize.full, Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), @@ -113,62 +113,62 @@ class AlertManager(object): Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), "steerTempUnavailableNoEntry": Alert( - "Comma Unavailable", - "Steering temporarily unavailable", + "openpilot Unavailable", + "Steering Temporarily Unavailable", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "manualRestart": Alert( "TAKE CONTROL", - "Resume driving manually", + "Resume Driving Manually", AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, None, None, 0., 0., .2), # Non-entry only alerts "wrongCarModeNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Main Switch Off", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "dataNeededNoEntry": Alert( - "Comma Unavailable", - "Data needed for calibration. Upload drive, try again", + "openpilot Unavailable", + "Data Needed for Calibration. Upload Drive, Try Again", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "outOfSpaceNoEntry": Alert( - "Comma Unavailable", - "Out of storage space", + "openpilot Unavailable", + "Out of Storage Space", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 0., 3.), "pedalPressedNoEntry": Alert( - "Comma Unavailable", - "Pedal pressed during attempt", + "openpilot Unavailable", + "Pedal Pressed During Attempt", AlertStatus.normal, AlertSize.mid, Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), "speedTooLowNoEntry": Alert( - "Comma Unavailable", - "Speed too low", + "openpilot Unavailable", + "Speed Too Low", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeHoldNoEntry": Alert( - "Comma Unavailable", - "Brake hold active", + "openpilot Unavailable", + "Brake Hold Active", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "parkBrakeNoEntry": Alert( - "Comma Unavailable", - "Park brake engaged", + "openpilot Unavailable", + "Park Brake Engaged", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "lowSpeedLockoutNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Cruise Fault: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), @@ -249,7 +249,7 @@ class AlertManager(object): "commIssue": Alert( "TAKE CONTROL IMMEDIATELY", - "CAN Error: Restart the Car", + "CAN Error: Check Connections", AlertStatus.critical, AlertSize.full, Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), @@ -291,128 +291,128 @@ class AlertManager(object): # not loud cancellations (user is in control) "noTarget": Alert( - "Comma Canceled", + "openpilot Canceled", "No close lead car", AlertStatus.normal, AlertSize.mid, Priority.HIGH, None, "chimeDouble", .4, 2., 3.), "speedTooLow": Alert( - "Comma Canceled", + "openpilot Canceled", "Speed too low", AlertStatus.normal, AlertSize.mid, Priority.HIGH, None, "chimeDouble", .4, 2., 3.), # Cancellation alerts causing non-entry "overheatNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "System overheated", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "wrongGearNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Gear not D", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInvalidNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Calibration Invalid: Reposition EON and Recalibrate", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInProgressNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Calibration in Progress", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "doorOpenNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Door open", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "seatbeltNotLatchedNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Seatbelt unlatched", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "espDisabledNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "ESP Off", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarCommIssueNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Radar Error: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarFaultNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Radar Error: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "modelCommIssueNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Model Error: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "controlsFailedNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Controls Failed", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "commIssueNoEntry": Alert( - "Comma Unavailable", - "CAN Error: Restart the Car", + "openpilot Unavailable", + "CAN Error: Check Connections", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "steerUnavailableNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Steer Fault: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeUnavailableNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Brake Fault: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "gasUnavailableNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Gas Error: Restart the Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "reverseGearNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Reverse Gear", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "cruiseDisabledNoEntry": Alert( - "Comma Unavailable", - "Cruise is off", + "openpilot Unavailable", + "Cruise is Off", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "noTargetNoEntry": Alert( - "Comma Unavailable", - "No close lead car", + "openpilot Unavailable", + "No Close Lead Car", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), "plannerErrorNoEntry": Alert( - "Comma Unavailable", + "openpilot Unavailable", "Planner Solution Error", AlertStatus.normal, AlertSize.mid, Priority.LOW, None, "chimeDouble", .4, 2., 3.), diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 732a43759f..ad66f5f118 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:900915b281d311e13e98ca6c0dcb16966f1127af00d1523b38c8a0fc029c97c1 +oid sha256:86a303c99ff57d29f0c3cc8656ae2cbd413a01894f89c718b5b9481c71b1ba4d size 1416464 diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 1d9c69cc2d..f45303a16e 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -70,7 +70,7 @@ def clear_locks(root): def is_on_wifi(): # ConnectivityManager.getActiveNetworkInfo() result = subprocess.check_output(["service", "call", "connectivity", "2"]).strip().split("\n") - data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:]) + data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:]) return "\x00".join("WIFI") in data @@ -126,15 +126,17 @@ class Uploader(object): return (key, fn, 0) if with_video: - # then upload compressed camera file + # then upload compressed rear and front camera files for name, key, fn in self.gen_upload_files(): - if name in ["fcamera.hevc"]: + if name == "fcamera.hevc": return (key, fn, 1) + elif name == "dcamera.hevc": + return (key, fn, 2) # then upload other files for name, key, fn in self.gen_upload_files(): if not name.endswith('.lock') and not name.endswith(".tmp"): - return (key, fn, 1) + return (key, fn, 3) return None diff --git a/selfdrive/manager.py b/selfdrive/manager.py index f010c632a4..6ea25ddca0 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -62,7 +62,7 @@ from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging from selfdrive.thermal import read_thermal from selfdrive.registration import register -from selfdrive.version import version, dirty +from selfdrive.version import version, dirty, training_version import selfdrive.crash as crash from selfdrive.loggerd.config import ROOT @@ -87,6 +87,7 @@ managed_processes = { "visiond": ("selfdrive/visiond", ["./visiond"]), "sensord": ("selfdrive/sensord", ["./sensord"]), "gpsd": ("selfdrive/sensord", ["./gpsd"]), + "orbd": ("selfdrive/orbd", ["./orbd"]), "updated": "selfdrive.updated", #"gpsplanner": "selfdrive.controls.gps_plannerd", } @@ -120,6 +121,7 @@ car_started_processes = [ 'radard', 'visiond', 'proclogd', + 'orbd', # 'gpsplanner, ] @@ -393,11 +395,12 @@ def manager_thread(): passive_starter = LocationStarter() started_ts = None + off_ts = None logger_dead = False count = 0 fan_speed = 0 ignition_seen = False - battery_was_high = False + started_seen = False panda_seen = False health_sock.RCVTIMEO = 1500 @@ -460,6 +463,7 @@ def manager_thread(): do_uninstall = params.get("DoUninstall") == "1" accepted_terms = params.get("HasAcceptedTerms") == "1" + completed_training = params.get("CompletedTrainingVersion") == training_version should_start = ignition @@ -477,7 +481,7 @@ def manager_thread(): # require usb power should_start = should_start and msg.thermal.usbOnline - should_start = should_start and accepted_terms and (not do_uninstall) + should_start = should_start and accepted_terms and completed_training and (not do_uninstall) # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 @@ -486,9 +490,11 @@ def manager_thread(): should_start = False if should_start: - if not started_ts: + off_ts = None + if started_ts is None: params.car_start() started_ts = sec_since_boot() + started_seen = True for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) @@ -496,15 +502,17 @@ def manager_thread(): start_managed_process(p) else: started_ts = None + if off_ts is None: + off_ts = sec_since_boot() logger_dead = False for p in car_started_processes: kill_managed_process(p) - # shutdown if the battery gets lower than 5%, we aren't running, and we are discharging - if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high: + # shutdown if the battery gets lower than 3%, t's discharging, we aren't running for + # more than a minute but we were running + if msg.thermal.batteryPercent < 3 and msg.thermal.batteryStatus == "Discharging" and \ + started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') - if msg.thermal.batteryPercent > 10: - battery_was_high = True # check the status of all processes, did any of them die? for p in running: diff --git a/selfdrive/orbd/Makefile b/selfdrive/orbd/Makefile new file mode 100644 index 0000000000..8847ef5d25 --- /dev/null +++ b/selfdrive/orbd/Makefile @@ -0,0 +1,98 @@ +# CPU + +CC = clang +CXX = clang++ + +JSON_FLAGS = -I$(PHONELIBS)/json/src + +CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I. +CXXFLAGS = -std=c++11 -g -fPIC -O2 $(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/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) \ + -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 + diff --git a/selfdrive/orbd/dsp/freethedsp.c b/selfdrive/orbd/dsp/freethedsp.c new file mode 100644 index 0000000000..9c5ef05983 --- /dev/null +++ b/selfdrive/orbd/dsp/freethedsp.c @@ -0,0 +1,118 @@ +// 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 ./' +// 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) + +// under 100 lines of code begins now +#include +#include +#include +#include +#include + +// 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 +#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, 4) == 0); + memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, 4); + + // 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 = PATCH_ADDR; + flush_data.length = PATCH_LEN; + 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; +} + diff --git a/selfdrive/orbd/dsp/gen/calculator.h b/selfdrive/orbd/dsp/gen/calculator.h new file mode 100644 index 0000000000..86a3de6717 --- /dev/null +++ b/selfdrive/orbd/dsp/gen/calculator.h @@ -0,0 +1,39 @@ +#ifndef _CALCULATOR_H +#define _CALCULATOR_H + +#include +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 diff --git a/selfdrive/orbd/dsp/gen/calculator_stub.c b/selfdrive/orbd/dsp/gen/calculator_stub.c new file mode 100644 index 0000000000..66e4a0f822 --- /dev/null +++ b/selfdrive/orbd/dsp/gen/calculator_stub.c @@ -0,0 +1,613 @@ +#ifndef _CALCULATOR_STUB_H +#define _CALCULATOR_STUB_H +#include "calculator.h" + +// remote.h +#include +#include + +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 + #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 +#include + +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 + +//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 + +#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 diff --git a/selfdrive/orbd/extractor.h b/selfdrive/orbd/extractor.h new file mode 100644 index 0000000000..c6392d446b --- /dev/null +++ b/selfdrive/orbd/extractor.h @@ -0,0 +1,37 @@ +#ifndef EXTRACTOR_H +#define EXTRACTOR_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define ORBD_KEYPOINTS 3000 +#define ORBD_DESCRIPTOR_LENGTH 32 +#define ORBD_HEIGHT 874 +#define ORBD_WIDTH 1164 + +// 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 diff --git a/selfdrive/orbd/orbd.cc b/selfdrive/orbd/orbd.cc new file mode 100644 index 0000000000..7bbd34d9f9 --- /dev/null +++ b/selfdrive/orbd/orbd.cc @@ -0,0 +1,174 @@ +#include +#include +#include +#include +#include +#include + +#include "common/visionipc.h" +#include "common/swaglog.h" + +#include "extractor.h" + +#ifdef DSP +#include "dsp/gen/calculator.h" +#else +#include "turbocv.h" +#endif + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#ifndef PATH_MAX +#include +#endif + +volatile int do_exit = 0; + +static void set_do_exit(int sig) { + do_exit = 1; +} + +int main(int argc, char *argv[]) { + int err; + printf("starting orbd\n"); + +#ifdef DSP + char my_path[PATH_MAX+1]; + readlink("/proc/self/exe", my_path, PATH_MAX); + my_path[strlen(my_path)-4] = '\0'; + LOGW("running from %s", my_path); + + char adsp_path[PATH_MAX+1]; + snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen;/dsp;/system/lib/rfsa/adsp;/system/vendor/lib/rfsa/adsp", my_path); + putenv(adsp_path); + + uint32_t test_leet = 0; + assert(calculator_init(&test_leet) == 0); + assert(test_leet == 0x1337); +#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; + + 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; + } + + 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); + + if (last_frame_id+1 != 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(); + 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 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 - 0.5f); + ys.set(i, features->xy[i][1] * 1.0f / ORBD_HEIGHT - 0.5f); + 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(); + 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; +} + diff --git a/selfdrive/registration.py b/selfdrive/registration.py index d444ded20a..ed22e5dfa5 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -2,7 +2,7 @@ import json import subprocess from selfdrive.swaglog import cloudlog -from selfdrive.version import version +from selfdrive.version import version, training_version from common.api import api_get from common.params import Params @@ -24,6 +24,7 @@ def get_git_remote(): def register(): params = Params() params.put("Version", version) + params.put("TrainingVersion", training_version) params.put("GitCommit", get_git_commit()) params.put("GitBranch", get_git_branch()) params.put("GitRemote", get_git_remote()) diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 12ec16640b..5a49578aa8 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:536c91fab8a36e7cdb06ade1599cad406a206c1b681bb1873fdb7c39e6213a58 +oid sha256:e694582549cfe8d1ed27d2e6e34b3f8dc1c01bf2a257c30251bd1af65229bf5d size 981400 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 4bddc648f2..22c9696e46 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:da501e3c27b8e736b6c99dac2b7c27ffb38aa43febe56b8134668250eeebb4aa +oid sha256:2cc5877fcc3d8ba9bc1ac50b4effcf62b3a865081d5c18b56528cfbef42eda4c size 972296 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 55c77e4cc9..a5d7fd69c9 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -64,10 +64,11 @@ applanixLocation: [8053, true] liveLocationKalman: [8054, true] uiNavigationEvent: [8055, true] orbOdometry: [8057, true] -orbFeatures: [8058, true] +orbFeatures: [8058, false] orbKeyFrame: [8059, true] uiLayoutState: [8060, true] frontEncodeIdx: [8061, true] +orbFeaturesSummary: [8062, true] testModel: [8040, false] testLiveLocation: [8045, false] diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index ca20ce9acf..783c05fb6d 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -211,6 +211,7 @@ class Plant(object): print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel) # ******** publish the car ******** + # TODO: the order is this list should not matter, but currently everytime we change carstate we break this test. Fix it! vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.angle_steer, self.angle_steer_rate, 0, 0, 0, 0, 0, # Doors diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 78441856db..d29ec18a13 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -1465,10 +1465,10 @@ static void ui_update(UIState *s) { assert(glGetError() == GL_NO_ERROR); - // Default UI Measurements (Assumes sidebar visible) - s->scene.ui_viz_rx = box_x; - s->scene.ui_viz_rw = box_w; - s->scene.ui_viz_ro = -(sbr_w - 4*bdr_s); + // Default UI Measurements (Assumes sidebar collapsed) + s->scene.ui_viz_rx = (box_x-sbr_w+bdr_s*2); + s->scene.ui_viz_rw = (box_w+sbr_w-(bdr_s*2)); + s->scene.ui_viz_ro = 0; s->vision_connect_firstrun = false; } diff --git a/selfdrive/version.py b/selfdrive/version.py index cbef487ac7..97d960678c 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -16,3 +16,7 @@ try: dirty = True except subprocess.CalledProcessError: dirty = True + +# put this here +training_version = "0.1.0" + diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index f8a8fe1d18..be97be3323 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:120cc9f11d56e4b67a2b9600b04500601ad597f8f4516e1c22cf32330a891442 -size 13386048 +oid sha256:53701cb3f9469c23d4e48e8bb417c307f1b41a3de3172fe3031d85613d8b31c1 +size 13386064