From 07aa8b1bf30271aee177ade3a1bd5269b7bcb738 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Mon, 22 Jul 2019 19:17:47 +0000 Subject: [PATCH] openpilot v0.6.1 release old-commit-hash: 94053536b4bfa7a8c4760db99063093aae69374d --- Pipfile | 4 +- Pipfile.lock | 4 +- README.md | 16 +- RELEASES.md | 7 + apk/ai.comma.plus.offroad.apk | 4 +- common/file_helpers.py | 17 +- common/fingerprints.py | 4 +- common/params.py | 3 + selfdrive/athena/athenad.py | 116 ++++++- selfdrive/boardd/boardd.cc | 9 +- selfdrive/can/parser.cc | 64 ++-- selfdrive/can/parser_pyx.pxd | 2 + selfdrive/can/parser_pyx.pyx | 16 +- selfdrive/can/tests/test_parser.py | 17 +- selfdrive/car/car_helpers.py | 18 +- selfdrive/car/chrysler/carstate.py | 4 +- selfdrive/car/chrysler/interface.py | 10 +- selfdrive/car/chrysler/radar_interface.py | 30 +- selfdrive/car/ford/carstate.py | 2 +- selfdrive/car/ford/interface.py | 9 +- selfdrive/car/ford/radar_interface.py | 29 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/gm/interface.py | 6 +- selfdrive/car/gm/radar_interface.py | 36 +- selfdrive/car/honda/carstate.py | 7 +- selfdrive/car/honda/interface.py | 10 +- selfdrive/car/honda/radar_interface.py | 43 +-- selfdrive/car/hyundai/carstate.py | 4 +- selfdrive/car/hyundai/interface.py | 10 +- selfdrive/car/hyundai/radar_interface.py | 10 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/mock/radar_interface.py | 10 +- selfdrive/car/subaru/carstate.py | 4 +- selfdrive/car/subaru/interface.py | 8 +- selfdrive/car/subaru/radar_interface.py | 9 +- selfdrive/car/toyota/carstate.py | 9 +- selfdrive/car/toyota/interface.py | 26 +- selfdrive/car/toyota/radar_interface.py | 39 +-- selfdrive/car/toyota/values.py | 27 +- selfdrive/common/version.h | 2 +- selfdrive/controls/controlsd.py | 28 +- selfdrive/controls/lib/alerts.py | 7 + selfdrive/controls/lib/driver_monitor.py | 10 +- selfdrive/controls/lib/fcw.py | 11 +- selfdrive/controls/lib/planner.py | 4 +- selfdrive/controls/radard.py | 222 ++++++------ selfdrive/locationd/.gitignore | 3 +- selfdrive/locationd/Makefile | 8 +- selfdrive/locationd/locationd_yawrate.cc | 319 ++++-------------- selfdrive/locationd/locationd_yawrate.h | 34 ++ selfdrive/locationd/params_learner.cc | 59 +++- selfdrive/locationd/paramsd.cc | 174 ++++++++++ .../locationd/test/test_params_learner.py | 53 +++ selfdrive/loggerd/uploader.py | 4 +- selfdrive/manager.py | 84 ++++- selfdrive/messaging.py | 34 +- selfdrive/registration.py | 3 +- selfdrive/test/plant/plant.py | 2 + selfdrive/thermald.py | 4 +- selfdrive/version.py | 1 + selfdrive/visiond/models/driving.cc | 7 + 61 files changed, 1064 insertions(+), 656 deletions(-) create mode 100644 selfdrive/locationd/locationd_yawrate.h create mode 100644 selfdrive/locationd/paramsd.cc create mode 100755 selfdrive/locationd/test/test_params_learner.py diff --git a/Pipfile b/Pipfile index 84eea415bd..37f4754e34 100644 --- a/Pipfile +++ b/Pipfile @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c8eab0ec610fccd29115e8451f1088ed71db5ce7575956d08fd76baf62172e28 -size 2565 +oid sha256:6f453d0dd767bc785836bf53d1ba3f4175188b4ca7b9fdcf8d146bb28c0cafb6 +size 2620 diff --git a/Pipfile.lock b/Pipfile.lock index d4a2173f48..3557a42d17 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:14e21ae221670a6b9178371274d1b30428f6d136195e8320943e8988764d9bd4 -size 152571 +oid sha256:4e0fb1640f01f83f792d10e8957dc191edd1ff195a1188229730e08ee2198fdc +size 155198 diff --git a/README.md b/README.md index dfbb768030..f6e3fbe797 100644 --- a/README.md +++ b/README.md @@ -60,13 +60,13 @@ Supported Cars | Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe | | ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------| -| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec | -| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec | +| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec | +| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec | | Buick3 | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| Chrysler | Pacifica 2018 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | +| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | | Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | | Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA | | GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| @@ -84,9 +84,9 @@ Supported Cars | Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | | Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | | Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6| -| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| +| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| | Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6| -| Jeep | Grand Cherokee 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | +| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | | Jeep | Grand Cherokee 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA | | Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| @@ -96,8 +96,9 @@ Supported Cars | Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4| | Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4| | Toyota | Avalon 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | -| Toyota | Camry 2018 | All | Yes | Stock | 0mph5 | 0mph | Toyota | -| Toyota | C-HR 2017-18 | All | Yes | Stock | 0mph | 0mph | Toyota | +| Toyota | Avalon 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph5 | 0mph | Toyota | +| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota | | Toyota | Corolla 2017-19 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | Toyota | | Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | @@ -110,6 +111,7 @@ Supported Cars | Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Sienna 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | 1[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai).***
2When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota).
diff --git a/RELEASES.md b/RELEASES.md index f73b8daf83..07bbc6208d 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,10 @@ +Version 0.6.1 (2019-07-21) +======================== + * Remote SSH with comma prime and [ssh.comma.ai](https://ssh.comma.ai) + * Panda code Misra-c2012 compliance, tested against cppcheck coverage + * Lockout openpilot after 3 terminal alerts for driver distracted or unresponsive + * Toyota Sienna support thanks to wocsor! + Version 0.6 (2019-07-01) ======================== * New model, with double the pixels and ten times the temporal context! diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index e6a70e2cea..96302d19a1 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:3eaf6c7d0666f721967fdf5441ea254568cbe898ee71f31e55e41c0cdd6cc41b -size 18310731 +oid sha256:01a583ce80c2bf9e1a48c55b485cea03d6160358e0f5a528317040d30b9c0978 +size 17042125 diff --git a/common/file_helpers.py b/common/file_helpers.py index bdfe9d0ecf..3300ae595a 100644 --- a/common/file_helpers.py +++ b/common/file_helpers.py @@ -28,7 +28,7 @@ def get_tmpdir_on_same_filesystem(path): normpath = os.path.normpath(path) parts = normpath.split("/") if len(parts) > 1: - if parts[1].startswith("raid"): + if parts[1].startswith("raid") or parts[1].startswith("datasets"): if len(parts) > 2 and parts[2] == "runner": return "/{}/runner/tmp".format(parts[1]) elif len(parts) > 2 and parts[2] == "aws": @@ -101,3 +101,18 @@ def atomic_write_in_dir(path, **kwargs): writer = AtomicWriter(path, **kwargs) return writer._open(_get_fileobject_func(writer, os.path.dirname(path))) +def atomic_write_in_dir_neos(path, contents, mode=None): + """ + Atomically writes contents to path using a temporary file in the same directory + as path. Useful on NEOS, where `os.link` (required by atomic_write_in_dir) is missing. + """ + + f = tempfile.NamedTemporaryFile(delete=False, prefix=".tmp", dir=os.path.dirname(path)) + f.write(contents) + f.flush() + if mode is not None: + os.fchmod(f.fileno(), mode) + os.fsync(f.fileno()) + f.close() + + os.rename(f.name, path) diff --git a/common/fingerprints.py b/common/fingerprints.py index ddc3bceb66..0e29e6c1e7 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -28,10 +28,8 @@ _DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes def is_valid_for_fingerprint(msg, car_fingerprint): adr = msg.address - bus = msg.src # ignore addresses that are more than 11 bits - return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or \ - bus != 0 or adr >= 0x800 + return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800 def eliminate_incompatible_cars(msg, candidate_cars): diff --git a/common/params.py b/common/params.py index c374d5233e..963debd091 100755 --- a/common/params.py +++ b/common/params.py @@ -50,12 +50,14 @@ class UnknownKeyName(Exception): keys = { "AccessToken": [TxType.PERSISTENT], + "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CompletedTrainingVersion": [TxType.PERSISTENT], "ControlsParams": [TxType.PERSISTENT], "DoUninstall": [TxType.CLEAR_ON_MANAGER_START], "DongleId": [TxType.PERSISTENT], + "GithubSshKeys": [TxType.PERSISTENT], "GitBranch": [TxType.PERSISTENT], "GitCommit": [TxType.PERSISTENT], "GitRemote": [TxType.PERSISTENT], @@ -75,6 +77,7 @@ keys = { "ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], "SpeedLimitOffset": [TxType.PERSISTENT], "SubscriberInfo": [TxType.PERSISTENT], + "TermsVersion": [TxType.PERSISTENT], "TrainingVersion": [TxType.PERSISTENT], "Version": [TxType.PERSISTENT], } diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 58918de81a..d6d429d92b 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -1,15 +1,22 @@ #!/usr/bin/env python2.7 import json +import jwt import os import random +import re +import select +import subprocess +import socket import time import threading import traceback import zmq import requests import six.moves.queue +from datetime import datetime, timedelta +from functools import partial from jsonrpc import JSONRPCResponseManager, dispatcher -from websocket import create_connection, WebSocketTimeoutException +from websocket import create_connection, WebSocketTimeoutException, ABNF from selfdrive.loggerd.config import ROOT import selfdrive.crash as crash @@ -21,6 +28,7 @@ from selfdrive.version import version, dirty ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4) +LOCAL_PORT_WHITELIST = set([8022]) dispatcher["echo"] = lambda s: s payload_queue = six.moves.queue.Queue() @@ -49,6 +57,7 @@ def handle_long_poll(ws): thread.join() def jsonrpc_handler(end_event): + dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event) while not end_event.is_set(): try: data = payload_queue.get(timeout=1) @@ -85,6 +94,109 @@ def uploadFileToUrl(fn, url, headers): ret = requests.put(url, data=f, headers=headers, timeout=10) return ret.status_code +def startLocalProxy(global_end_event, remote_ws_uri, local_port): + try: + cloudlog.event("athena startLocalProxy", remote_ws_uri=remote_ws_uri, local_port=local_port) + + if local_port not in LOCAL_PORT_WHITELIST: + raise Exception("Requested local port not whitelisted") + + params = Params() + dongle_id = params.get("DongleId") + private_key = open("/persist/comma/id_rsa").read() + identity_token = jwt.encode({'identity':dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256') + + ws = create_connection(remote_ws_uri, + cookie="jwt=" + identity_token, + enable_multithread=True) + + ssock, csock = socket.socketpair() + local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + local_sock.connect(('127.0.0.1', local_port)) + local_sock.setblocking(0) + + proxy_end_event = threading.Event() + threads = [ + threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)), + threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event)) + ] + + map(lambda thread: thread.start(), threads) + + return {"success": 1} + except Exception as e: + traceback.print_exc() + raise e + +@dispatcher.add_method +def getPublicKey(): + if not os.path.isfile('/persist/comma/id_rsa.pub'): + return None + + with open('/persist/comma/id_rsa.pub', 'r') as f: + return f.read() + +@dispatcher.add_method +def getSshAuthorizedKeys(): + with open('/system/comma/home/.ssh/authorized_keys', 'r') as f: + return f.read() + +@dispatcher.add_method +def getSimInfo(): + sim_state = subprocess.check_output(['getprop', 'gsm.sim.state']).strip().split(',') + network_type = subprocess.check_output(['getprop', 'gsm.network.type']).strip().split(',') + mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric']).strip() or None + + sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11']) + sim_id_aidl_lines = sim_id_aidl_out.split('\n') + if len(sim_id_aidl_lines) > 3: + sim_id_lines = sim_id_aidl_lines[1:4] + sim_id_fragments = [re.search(r"'([0-9\.]+)'", line).group(1) for line in sim_id_lines] + sim_id = reduce(lambda frag1, frag2: frag1.replace('.', '') + frag2.replace('.', ''), sim_id_fragments) + else: + sim_id = None + + return { + 'sim_id': sim_id, + 'mcc_mnc': mcc_mnc, + 'network_type': network_type, + 'sim_state': sim_state + } + +def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): + while not (end_event.is_set() or global_end_event.is_set()): + try: + data = ws.recv() + local_sock.sendall(data) + except WebSocketTimeoutException: + pass + except Exception: + traceback.print_exc() + break + + ssock.close() + end_event.set() + +def ws_proxy_send(ws, local_sock, signal_sock, end_event): + while not end_event.is_set(): + try: + r, _, _ = select.select((local_sock, signal_sock), (), ()) + if r: + if r[0].fileno() == signal_sock.fileno(): + # got end signal from ws_proxy_recv + end_event.set() + break + data = local_sock.recv(4096) + if not data: + # local_sock is dead + end_event.set() + break + + ws.send(data, ABNF.OPCODE_BINARY) + except Exception: + traceback.print_exc() + end_event.set() + def ws_recv(ws, end_event): while not end_event.is_set(): try: @@ -138,5 +250,7 @@ def main(gctx=None): time.sleep(backoff(conn_retries)) + params.delete("AthenadPid") + if __name__ == "__main__": main() diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1a64416e9f..779ca7b279 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -283,8 +283,9 @@ void can_health(void *s) { uint8_t started; uint8_t controls_allowed; uint8_t gas_interceptor_detected; - uint8_t started_signal_detected; - uint8_t started_alt; + uint32_t can_send_errs; + uint32_t can_fwd_errs; + uint32_t gmlan_send_errs; } health; // recv from board @@ -313,8 +314,10 @@ void can_health(void *s) { } healthData.setControlsAllowed(health.controls_allowed); healthData.setGasInterceptorDetected(health.gas_interceptor_detected); - healthData.setStartedSignalDetected(health.started_signal_detected); healthData.setIsGreyPanda(is_grey_panda); + healthData.setCanSendErrs(health.can_send_errs); + healthData.setCanFwdErrs(health.can_fwd_errs); + healthData.setGmlanSendErrs(health.gmlan_send_errs); // send to health auto words = capnp::messageToFlatArray(msg); diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index e3225181ba..69b30fb511 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -194,32 +194,36 @@ class CANParser { : bus(abus) { // connect to can on 8006 context = zmq_ctx_new(); - subscriber = zmq_socket(context, ZMQ_SUB); - zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); - zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int)); - std::string tcp_addr_str; + if (tcp_addr.length() > 0) { + subscriber = zmq_socket(context, ZMQ_SUB); + zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); + zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int)); - if (sendcan) { - tcp_addr_str = "tcp://" + tcp_addr + ":8017"; - } else { - tcp_addr_str = "tcp://" + tcp_addr + ":8006"; - } - const char *tcp_addr_char = tcp_addr_str.c_str(); + std::string tcp_addr_str; + + if (sendcan) { + tcp_addr_str = "tcp://" + tcp_addr + ":8017"; + } else { + tcp_addr_str = "tcp://" + tcp_addr + ":8006"; + } + const char *tcp_addr_char = tcp_addr_str.c_str(); - zmq_connect(subscriber, tcp_addr_char); + zmq_connect(subscriber, tcp_addr_char); - // drain sendcan to delete any stale messages from previous runs - zmq_msg_t msgDrain; - zmq_msg_init(&msgDrain); - int err = 0; - while(err >= 0) { - err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT); + // drain sendcan to delete any stale messages from previous runs + zmq_msg_t msgDrain; + zmq_msg_init(&msgDrain); + int err = 0; + while(err >= 0) { + err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT); + } + } else { + subscriber = NULL; } dbc = dbc_lookup(dbc_name); - assert(dbc); - + assert(dbc); for (const auto& op : options) { MessageState state = { .address = op.address, @@ -326,6 +330,21 @@ class CANParser { } } + void update_string(uint64_t sec, std::string data) { + // format for board, make copy due to alignment issues, will be freed on out of scope + auto amsg = kj::heapArray((data.length() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), data.data(), data.length()); + + // extract the messages + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + auto cans = event.getCan(); + UpdateCans(sec, cans); + + UpdateValid(sec); + } + int update(uint64_t sec, bool wait) { int err; int result = 0; @@ -336,7 +355,7 @@ class CANParser { // multiple recv is fine bool first = wait; - while (1) { + while (subscriber != NULL) { if (first) { err = zmq_msg_recv(&msg, subscriber, 0); first = false; @@ -432,6 +451,11 @@ int can_update(void* can, uint64_t sec, bool wait) { return cp->update(sec, wait); } +void can_update_string(void *can, uint64_t sec, const char* dat, int len) { + CANParser* cp = (CANParser*)can; + cp->update_string(sec, std::string(dat, len)); +} + size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) { CANParser* cp = (CANParser*)can; diff --git a/selfdrive/can/parser_pyx.pxd b/selfdrive/can/parser_pyx.pxd index 9d8efa318c..ac619707ac 100644 --- a/selfdrive/can/parser_pyx.pxd +++ b/selfdrive/can/parser_pyx.pxd @@ -67,6 +67,7 @@ ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name, const char* tcp_addr, int timeout) ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait); +ctypedef void (*can_update_string_func)(void* can, uint64_t sec, const char* dat, int len); ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values); ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid, vector[SignalValue] &values) @@ -77,6 +78,7 @@ cdef class CANParser: dbc_lookup_func dbc_lookup can_init_with_vectors_func can_init_with_vectors can_update_func can_update + can_update_string_func can_update_string can_query_vector_func can_query_vector map[string, uint32_t] msg_name_to_address map[uint32_t, string] address_to_msg_name diff --git a/selfdrive/can/parser_pyx.pyx b/selfdrive/can/parser_pyx.pyx index 65c6f5ab21..c6f1f58e03 100644 --- a/selfdrive/can/parser_pyx.pyx +++ b/selfdrive/can/parser_pyx.pyx @@ -8,7 +8,7 @@ import numbers cdef int CAN_INVALID_CNT = 5 cdef class CANParser: - def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1): + def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="", timeout=-1): self.test_mode_enabled = False can_dir = os.path.dirname(os.path.abspath(__file__)) libdbc_fn = os.path.join(can_dir, "libdbc.so") @@ -17,6 +17,7 @@ cdef class CANParser: self.can_init_with_vectors = dlsym(libdbc, 'can_init_with_vectors') self.dbc_lookup = dlsym(libdbc, 'dbc_lookup') self.can_update = dlsym(libdbc, 'can_update') + self.can_update_string = dlsym(libdbc, 'can_update_string') self.can_query_vector = dlsym(libdbc, 'can_query_vector') if checks is None: checks = [] @@ -99,6 +100,19 @@ cdef class CANParser: return updated_val + def update_string(self, uint64_t sec, dat): + self.can_update_string(self.can, sec, dat, len(dat)) + return self.update_vl(sec) + + def update_strings(self, uint64_t sec, strings): + updated_vals = set() + + for s in strings: + updated_val = self.update_string(sec, s) + updated_vals.update(updated_val) + + return updated_vals + def update(self, uint64_t sec, bool wait): r = (self.can_update(self.can, sec, wait) >= 0) updated_val = self.update_vl(sec) diff --git a/selfdrive/can/tests/test_parser.py b/selfdrive/can/tests/test_parser.py index bb00d042fc..53c95ce912 100755 --- a/selfdrive/can/tests/test_parser.py +++ b/selfdrive/can/tests/test_parser.py @@ -47,8 +47,9 @@ def run_route(route): CP = CarInterface.get_params(CAR.CIVIC, {}) signals, checks = get_can_signals(CP) - parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1) - parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1) + parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") + parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") + parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1) if dict_keys_differ(parser_old.vl, parser_new.vl): return False @@ -61,19 +62,29 @@ def run_route(route): for msg in lr: if msg.which() == 'can': t += DT - can.send(msg.as_builder().to_bytes()) + msg_bytes = msg.as_builder().to_bytes() + can.send(msg_bytes) _, updated_old = parser_old.update(t, True) _, updated_new = parser_new.update(t, True) + updated_string = parser_string.update_string(t, msg_bytes) if updated_old != updated_new: route_ok = False print(t, "Diff in seen") + if updated_new != updated_string: + route_ok = False + print(t, "Diff in seen string") + if dicts_vals_differ(parser_old.vl, parser_new.vl): print(t, "Diff in dict") route_ok = False + if dicts_vals_differ(parser_new.vl, parser_string.vl): + print(t, "Diff in dict string") + route_ok = False + return route_ok class TestCanParser(unittest.TestCase): diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index f7e8c53757..155e443e1c 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -50,6 +50,8 @@ def _get_interface_names(): # imports from directory selfdrive/car// interfaces = load_interfaces(_get_interface_names()) +def only_toyota_left(candidate_cars): + return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) # BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai # **** for use live only **** @@ -59,7 +61,7 @@ def fingerprint(logcan, sendcan): elif os.getenv("SIMULATOR") is not None: return ("simulator", None, "") - finger = {} + finger = {0: {}, 2:{}} # collect on bus 0 or 2 cloudlog.warning("waiting for fingerprint...") candidate_cars = all_known_cars() can_seen_frame = None @@ -79,6 +81,7 @@ def fingerprint(logcan, sendcan): vin = "" frame = 0 + while True: a = messaging.recv_one(logcan) @@ -98,9 +101,12 @@ def fingerprint(logcan, sendcan): # ignore everything not on bus 0 and with more than 11 bits, # which are ussually sporadic and hard to include in fingerprints. - # also exclude VIN query response on 0x7e8 - if can.src == 0 and can.address < 0x800 and can.address != 0x7e8: - finger[can.address] = len(can.dat) + # also exclude VIN query response on 0x7e8. + # Include bus 2 for toyotas to disambiguate cars using camera messages + # (ideally should be done for all cars but we can't for Honda Bosch) + if (can.src == 0 or (only_toyota_left(candidate_cars) and can.src == 2)) and \ + can.address < 0x800 and can.address != 0x7e8: + finger[can.src][can.address] = len(can.dat) candidate_cars = eliminate_incompatible_cars(can, candidate_cars) if can_seen_frame is None and can_seen: @@ -110,7 +116,7 @@ def fingerprint(logcan, sendcan): # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not # broadcast immediately if len(candidate_cars) == 1 and can_seen_frame is not None: - time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1 + time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1 if (frame - can_seen_frame) > (time_fingerprint * 100): break @@ -146,6 +152,6 @@ def get_car(logcan, sendcan): candidate = "mock" CarInterface, CarController = interfaces[candidate] - params = CarInterface.get_params(candidate, fingerprints, vin) + params = CarInterface.get_params(candidate, fingerprints[0], vin) return CarInterface(params, CarController), params diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 1d38b8cdab..24acc76a4b 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -60,7 +60,7 @@ def get_can_parser(CP): ("ACC_2", 50), ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) def get_camera_parser(CP): signals = [ @@ -72,7 +72,7 @@ def get_camera_parser(CP): ] checks = [] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) class CarState(object): diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 793f732084..65a60904f5 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -112,18 +112,17 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): + def update(self, c, can_strings): # ******************* do can recv ******************* - canMonoTimes = [] - can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True) - cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False) + self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) + self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid and self.cp_cam.can_valid + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid # speeds ret.vEgo = self.CS.v_ego @@ -222,7 +221,6 @@ class CarInterface(object): events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events - ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index b4970b2223..43f5c6105e 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -51,27 +51,24 @@ class RadarInterface(object): self.pts = {} self.delay = 0.0 # Delay of radar #TUNE self.rcp = _create_radar_can_parser() + self.updated_messages = set() + self.trigger_msg = LAST_MSG - def update(self): - canMonoTimes = [] + def update(self, can_strings): + tm = int(sec_since_boot() * 1e9) + vls = self.rcp.update_strings(tm, can_strings) + self.updated_messages.update(vls) - updated_messages = set() # set of message IDs (sig_addresses) we've seen - - while 1: - tm = int(sec_since_boot() * 1e9) - _, vls = self.rcp.update(tm, True) - updated_messages.update(vls) - if LAST_MSG in updated_messages: - break + if self.trigger_msg not in self.updated_messages: + return None ret = car.RadarData.new_message() errors = [] if not self.rcp.can_valid: errors.append("canError") ret.errors = errors - ret.canMonoTimes = canMonoTimes - for ii in updated_messages: # ii should be the message ID as a number + for ii in self.updated_messages: # ii should be the message ID as a number cpt = self.rcp.vl[ii] trackId = _address_to_track(ii) @@ -92,11 +89,6 @@ class RadarInterface(object): # We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid. ret.points = [x for x in self.pts.values() if x.dRel != 0] - return ret -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") # clear screen - print(ret) + self.updated_messages.clear() + return ret diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index dc6d824ff5..5e87a2c878 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -29,7 +29,7 @@ def get_can_parser(CP): checks = [ ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) class CarState(object): diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 0f3bbc33cc..ca0f9f830f 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -105,18 +105,16 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): + def update(self, c, can_strings): # ******************* do can recv ******************* - canMonoTimes = [] - - can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True) + self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and self.cp.can_valid + ret.canValid = self.cp.can_valid # speeds ret.vEgo = self.CS.v_ego @@ -167,7 +165,6 @@ class CarInterface(object): events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) ret.events = events - ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 08b54723d6..04cab9c66d 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -28,28 +28,25 @@ class RadarInterface(object): # Nidec self.rcp = _create_radar_can_parser() + self.trigger_msg = 0x53f + self.updated_messages = set() - def update(self): - canMonoTimes = [] + def update(self, can_strings): + tm = int(sec_since_boot() * 1e9) + vls = self.rcp.update_strings(tm, can_strings) + self.updated_messages.update(vls) - updated_messages = set() - while 1: - tm = int(sec_since_boot() * 1e9) - _, vls = self.rcp.update(tm, True) - updated_messages.update(vls) + if self.trigger_msg not in self.updated_messages: + return None - # TODO: do not hardcode last msg - if 0x53f in updated_messages: - break ret = car.RadarData.new_message() errors = [] if not self.rcp.can_valid: errors.append("canError") ret.errors = errors - ret.canMonoTimes = canMonoTimes - for ii in updated_messages: + for ii in self.updated_messages: cpt = self.rcp.vl[ii] if cpt['X_Rel'] > 0.00001: @@ -78,11 +75,5 @@ class RadarInterface(object): del self.pts[ii] ret.points = self.pts.values() + self.updated_messages.clear() return ret - -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 174d11b46b..2501598dd6 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -47,7 +47,7 @@ def get_powertrain_can_parser(CP, canbus): ("CruiseState", "AcceleratorPedal2", 0), ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) class CarState(object): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index c066a6523f..71b0efad29 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -170,15 +170,15 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): - can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True) + def update(self, c, can_strings): + self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.pt_cp) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and self.pt_cp.can_valid + ret.canValid = self.pt_cp.can_valid # speeds ret.vEgo = self.CS.v_ego diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 12fb7c2342..6788e1ce74 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -52,21 +52,23 @@ class RadarInterface(object): print "Using %d as obstacle CAN bus ID" % canbus.obstacle self.rcp = create_radar_can_parser(canbus, CP.carFingerprint) - def update(self): - updated_messages = set() - ret = car.RadarData.new_message() - while 1: + self.trigger_msg = LAST_RADAR_MSG + self.updated_messages = set() - if self.rcp is None: - time.sleep(0.05) # nothing to do - return ret + def update(self, can_strings): + if self.rcp is None: + time.sleep(0.05) # nothing to do + return car.RadarData.new_message() - tm = int(sec_since_boot() * 1e9) - _, vls = self.rcp.update(tm, True) - updated_messages.update(vls) - if LAST_RADAR_MSG in updated_messages: - break + tm = int(sec_since_boot() * 1e9) + vls = self.rcp.update_strings(tm, can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() header = self.rcp.vl[RADAR_HEADER_MSG] fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \ header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \ @@ -83,7 +85,7 @@ class RadarInterface(object): # Not all radar messages describe targets, # no need to monitor all of the self.rcp.msgs_upd - for ii in updated_messages: + for ii in self.updated_messages: if ii == RADAR_HEADER_MSG: continue @@ -112,11 +114,5 @@ class RadarInterface(object): del self.pts[oldTarget] ret.points = self.pts.values() + self.updated_messages.clear() return ret - -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6c0aec52a0..e2bb82c73c 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -146,6 +146,7 @@ def get_can_signals(CP): # add gas interceptor reading if we are using it if CP.enableGasInterceptor: signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) checks.append(("GAS_SENSOR", 50)) return signals, checks @@ -153,7 +154,7 @@ def get_can_signals(CP): def get_can_parser(CP): signals, checks = get_can_signals(CP) - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) def get_cam_can_parser(CP): @@ -166,7 +167,7 @@ def get_cam_can_parser(CP): cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2 - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus) class CarState(object): def __init__(self, CP): @@ -261,7 +262,7 @@ class CarState(object): # this is a hack for the interceptor. This is now only used in the simulation # TODO: Replace tests by toyota so this can go away if self.CP.enableGasInterceptor: - self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 93aaf0182b..7ad72cdc48 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -358,18 +358,17 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): + def update(self, c, can_strings): # ******************* do can recv ******************* - canMonoTimes = [] - can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True) - cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False) + self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) + self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid + ret.canValid = self.cp.can_valid # speeds ret.vEgo = self.CS.v_ego @@ -547,7 +546,6 @@ class CarInterface(object): events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events - ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 94e98cf2cb..f8cecd6de4 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -31,25 +31,30 @@ class RadarInterface(object): # Nidec self.rcp = _create_nidec_can_parser() + self.trigger_msg = 0x445 + self.updated_messages = set() - def update(self): - canMonoTimes = [] - - updated_messages = set() - ret = car.RadarData.new_message() - + def update(self, can_strings): # in Bosch radar and we are only steering for now, so sleep 0.05s to keep # radard at 20Hz and return no points if self.radar_off_can: time.sleep(0.05) - return ret + return car.RadarData.new_message() + + tm = int(sec_since_boot() * 1e9) + vls = self.rcp.update_strings(tm, can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + rr = self._update(self.updated_messages) + self.updated_messages.clear() + return rr - while 1: - tm = int(sec_since_boot() * 1e9) - _, vls = self.rcp.update(tm, True) - updated_messages.update(vls) - if 0x445 in updated_messages: - break + + def _update(self, updated_messages): + ret = car.RadarData.new_message() for ii in updated_messages: cpt = self.rcp.vl[ii] @@ -80,19 +85,7 @@ class RadarInterface(object): if self.radar_wrong_config: errors.append("wrongConfig") ret.errors = errors - ret.canMonoTimes = canMonoTimes ret.points = self.pts.values() return ret - - -if __name__ == "__main__": - class CarParams: - radarOffCan = False - - RI = RadarInterface(CarParams) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 8c900d73f8..f993bd2a0f 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -93,7 +93,7 @@ def get_can_parser(CP): ("SAS11", 100) ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) def get_camera_parser(CP): @@ -119,7 +119,7 @@ def get_camera_parser(CP): checks = [] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) class CarState(object): diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 67ca2261ff..4e1b2e6be2 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -151,17 +151,16 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): + def update(self, c, can_strings): # ******************* do can recv ******************* - canMonoTimes = [] - can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True) - cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False) + self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) + self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid # TODO: check cp_cam validity + ret.canValid = self.cp.can_valid # TODO: check cp_cam validity # speeds ret.vEgo = self.CS.v_ego @@ -269,7 +268,6 @@ class CarInterface(object): events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events - ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 75256683de..1d7772fd3b 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -9,16 +9,8 @@ class RadarInterface(object): self.pts = {} self.delay = 0.1 - def update(self): - + def update(self, can_strings): ret = car.RadarData.new_message() time.sleep(0.05) # radard runs on RI updates return ret - -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index a18d2bf244..3f47b9b00a 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -80,7 +80,7 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): + def update(self, c, can_strings): self.rk.keep_time() # get basic data from phone and gps since CAN isn't connected diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py index 437bb0538a..8e5f7b7fca 100755 --- a/selfdrive/car/mock/radar_interface.py +++ b/selfdrive/car/mock/radar_interface.py @@ -9,15 +9,7 @@ class RadarInterface(object): self.pts = {} self.delay = 0.1 - def update(self): - + def update(self, can_strings): ret = car.RadarData.new_message() time.sleep(0.05) # radard runs on RI updates return ret - -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 9a92777548..6c5f678266 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -37,7 +37,7 @@ def get_powertrain_can_parser(CP): ("BodyInfo", 10), ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) def get_camera_can_parser(CP): @@ -79,7 +79,7 @@ def get_camera_can_parser(CP): ("ES_DashStatus", 10), ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) class CarState(object): diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index e9d9c117fc..ad8d1d5a48 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -94,16 +94,16 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): - can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True) - cam_rcv_valid, _ = self.cam_cp.update(int(sec_since_boot() * 1e9), False) + def update(self, c, can_strings): + self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings) + self.cam_cp.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.pt_cp, self.cam_cp) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and cam_rcv_valid and self.pt_cp.can_valid and self.cam_cp.can_valid + ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid # speeds ret.vEgo = self.CS.v_ego diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py index 75256683de..0f81087711 100644 --- a/selfdrive/car/subaru/radar_interface.py +++ b/selfdrive/car/subaru/radar_interface.py @@ -9,16 +9,9 @@ class RadarInterface(object): self.pts = {} self.delay = 0.1 - def update(self): + def update(self, can_strings): ret = car.RadarData.new_message() time.sleep(0.05) # radard runs on RI updates return ret - -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index ee7c61d940..40be422db1 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,7 +1,7 @@ import numpy as np from common.kalman.simple_kalman import KF1D -from selfdrive.can.parser import CANParser from selfdrive.can.can_define import CANDefine +from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR @@ -70,9 +70,10 @@ def get_can_parser(CP): # add gas interceptor reading if we are using it if CP.enableGasInterceptor: signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) checks.append(("GAS_SENSOR", 50)) - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) def get_cam_can_parser(CP): @@ -82,7 +83,7 @@ def get_cam_can_parser(CP): # use steering message to check if panda is connected to frc checks = [("STEERING_LKA", 42)] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) class CarState(object): @@ -118,7 +119,7 @@ class CarState(object): self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] if self.CP.enableGasInterceptor: - self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. else: self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] self.car_gas = self.pedal_gas diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 29abfeb846..5e5cb2c839 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -9,7 +9,6 @@ from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness from selfdrive.swaglog import cloudlog - class CarInterface(object): def __init__(self, CP, CarController): self.CP = CP @@ -175,6 +174,16 @@ class CarInterface(object): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 + elif candidate == CAR.SIENNA: + stop_and_go = True + ret.safetyParam = 73 + ret.wheelbase = 3.03 + ret.steerRatio = 16.0 + tire_stiffness_factor = 0.444 + ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] + ret.lateralTuning.pid.kf = 0.00007818594 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 @@ -201,8 +210,8 @@ class CarInterface(object): # steer, gas, brake limitations VS speed ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph - ret.brakeMaxBP = [5., 20.] - ret.brakeMaxV = [1., 0.8] + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [1.] ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU) @@ -236,22 +245,20 @@ class CarInterface(object): return ret # returns a car.CarState - def update(self, c): + def update(self, c, can_strings): # ******************* do can recv ******************* - canMonoTimes = [] - - can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True) + self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) # run the cam can update for 10s as we just need to know if the camera is alive if self.frame < 1000: - self.cp_cam.update(int(sec_since_boot() * 1e9), False) + self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings) self.CS.update(self.cp) # create message ret = car.CarState.new_message() - ret.canValid = can_rcv_valid and self.cp.can_valid + ret.canValid = self.cp.can_valid # speeds ret.vEgo = self.CS.v_ego @@ -368,7 +375,6 @@ class CarInterface(object): events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events - ret.canMonoTimes = canMonoTimes self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index c160e751aa..4e0a0e809b 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -46,32 +46,36 @@ class RadarInterface(object): self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS} self.rcp = _create_radar_can_parser(CP.carFingerprint) + self.trigger_msg = self.RADAR_B_MSGS[-1] + self.updated_messages = set() + # No radar dbc for cars without DSU which are not TSS 2.0 # TODO: make a adas dbc file for dsu-less models self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR - def update(self): - - ret = car.RadarData.new_message() - + def update(self, can_strings): if self.no_radar: time.sleep(0.05) - return ret + return car.RadarData.new_message() + + tm = int(sec_since_boot() * 1e9) + vls = self.rcp.update_strings(tm, can_strings) + self.updated_messages.update(vls) - canMonoTimes = [] - updated_messages = set() - while 1: - tm = int(sec_since_boot() * 1e9) - _, vls = self.rcp.update(tm, True) - updated_messages.update(vls) - if self.RADAR_B_MSGS[-1] in updated_messages: - break + if self.trigger_msg not in self.updated_messages: + return None + rr = self._update(self.updated_messages) + self.updated_messages.clear() + + return rr + + def _update(self, updated_messages): + ret = car.RadarData.new_message() errors = [] if not self.rcp.can_valid: errors.append("canError") ret.errors = errors - ret.canMonoTimes = canMonoTimes for ii in updated_messages: if ii in self.RADAR_A_MSGS: @@ -105,10 +109,3 @@ class RadarInterface(object): ret.points = self.pts.values() return ret - -if __name__ == "__main__": - RI = RadarInterface(None) - while 1: - ret = RI.update() - print(chr(27) + "[2J") - print(ret) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index da8b02dcd7..df535eab05 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -16,6 +16,7 @@ class CAR: RAV4_TSS2 = "TOYOTA RAV4 2019" COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019" + SIENNA = "TOYOTA SIENNA XLE 2018" class ECU: @@ -42,23 +43,23 @@ STATIC_MSGS = [ (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'), - (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 3, '\x03\x00\x20\x00\x00\x52'), - (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 2, '\x00\x00\x00\x46'), - (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA), 1, 3, '\x03\x00\x20\x00\x00\x52'), + (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 2, '\x00\x00\x00\x46'), + (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), - (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), + (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'), + (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), (0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), - (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), + (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), + (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), (0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'), - (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H), 1, 100, '\x00\x00\x01\x79'), - (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA), 1, 100, '\x00\x00\x01\x79'), + (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), (0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), (0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), @@ -178,6 +179,9 @@ FINGERPRINTS = { { 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.SIENNA: [{ + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], } STEER_THRESHOLD = 100 @@ -198,8 +202,9 @@ DBC = { CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'), } NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2] TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2] -NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2] # no resume button press required +NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index ad486deed5..5c21a9db87 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6-release" +#define COMMA_VERSION "0.6.1-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f1c556ba12..5c5ba2f661 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -22,7 +22,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.controls.lib.driver_monitor import DriverStatus +from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter @@ -49,16 +49,22 @@ def events_to_bytes(events): return ret -def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery, +def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events - CS = CI.update(CC) + can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) + CS = CI.update(CC, can_strs) + + sm.update(0) + events = list(CS.events) enabled = isEnabled(state) - sm.update(0) + # Check for CAN timeout + if not can_strs: + events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if sm.updated['thermal']: overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red @@ -73,6 +79,7 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) + # Handle calibration if sm.updated['liveCalibration']: cal_status = sm['liveCalibration'].calStatus @@ -102,6 +109,9 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt if sm.updated['driverMonitoring']: driver_status.get_pose(sm['driverMonitoring'], params) + if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS: + events.append(create_event("tooDistracted", [ET.NO_ENTRY])) + return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter @@ -402,6 +412,7 @@ def controlsd_thread(gctx=None): params = Params() + # Pub Sockets sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) @@ -414,10 +425,15 @@ def controlsd_thread(gctx=None): passive = params.get("Passive") != "0" sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan']) + logcan = messaging.sub_sock(service_list['can'].port) + CI, CP = get_car(logcan, sendcan) + logcan.close() + + # TODO: Use the logcan socket from above, but that will currenly break the tests + can_sock = messaging.sub_sock(service_list['can'].port, timeout=100) CC = car.CarControl.new_message() - CI, CP = get_car(logcan, sendcan) AM = AlertManager() car_recognized = CP.carName != 'mock' @@ -469,7 +485,7 @@ def controlsd_thread(gctx=None): # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ - data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery, + data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index da7e88acbd..0b0d5b1d54 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -298,6 +298,13 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + Alert( + "tooDistractedNoEntry", + "openpilot Unavailable", + "Distraction Level Too High", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + # Cancellation alerts causing soft disabling Alert( "overheat", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index e813d0b176..d38a115f97 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -14,11 +14,12 @@ _PITCH_WEIGHT = 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 _PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch _PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up -_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) +_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) _STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid _DISTRACTED_FILTER_TS = 0.25 # 0.6Hz _VARIANCE_FILTER_TS = 20. # 0.008Hz +MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts RESIZED_FOCAL = 320.0 H, W, FULL_W = 320, 160, 426 @@ -68,6 +69,7 @@ class DriverStatus(): self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON) self.ts_last_check = 0. self.face_detected = False + self.terminal_alert_cnt = 0 self._set_timers() def _reset_filters(self): @@ -133,6 +135,7 @@ class DriverStatus(): def update(self, events, driver_engaged, ctrl_active, standstill): driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on) + awareness_prev = self.awareness if (driver_engaged and self.awareness > 0.) or not ctrl_active: # always reset if driver is in control (unless we are in red alert state) or op isn't active @@ -144,15 +147,18 @@ class DriverStatus(): self.awareness = max(self.awareness - self.step_change, -0.1) alert = None - if self.awareness <= 0.: + if self.awareness < 0.: # terminal red alert: disengagement required alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive' + if awareness_prev >= 0.: + self.terminal_alert_cnt += 1 elif self.awareness <= self.threshold_prompt: # prompt orange alert alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive' elif self.awareness <= self.threshold_pre: # pre green alert alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive' + if alert is not None: events.append(create_event(alert, [ET.WARNING])) diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index f93a72cfcc..8180fadeba 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -43,8 +43,10 @@ class FCWChecker(object): ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc) return ttc - def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): + def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): mpc_solution_a = list(mpc_solution[0].a_ego) + a_target = mpc_solution_a[1] + self.last_min_a = min(mpc_solution_a) self.v_lead_max = max(self.v_lead_max, v_lead) @@ -62,8 +64,11 @@ class FCWChecker(object): a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V) a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego) - fcw_allowed = all(c >= 10 for c in self.counters.values()) - if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time: + future_fcw_allowed = all(c >= 10 for c in self.counters.values()) + future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed + current_fcw = a_target < -3.0 and active + + if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time): self.last_fcw_time = cur_time self.last_fcw_a = self.last_min_a return True diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 9aaec513be..94cc2fc135 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -201,7 +201,9 @@ class Planner(object): self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker - fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, sm['carState'].aEgo, + fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, + sm['controlsState'].active, + v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index b37ed8f755..afb4ffd2bc 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -26,6 +26,11 @@ DIMSV = 2 XV, SPEEDV = 0, 1 VISION_POINT = -1 +path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max + +# Time-alignment +rate = 1. / DT_MDL # model and radar are both at 20Hz +v_len = 20 # how many speed data points to remember for t alignment with rdr data class EKFV1D(EKF): def __init__(self): @@ -43,171 +48,133 @@ class EKFV1D(EKF): tfj = tf return tf, tfj +class RadarD(object): + def __init__(self, VM, mocked): + self.VM = VM + self.mocked = mocked -## fuses camera and radar data for best lead detection -def radard_thread(gctx=None): - set_realtime_priority(2) + self.MP = ModelParser() + self.tracks = defaultdict(dict) - # wait for stats about the car to come in from controls - cloudlog.info("radard is waiting for CarParams") - CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) - mocked = CP.carName == "mock" - VM = VehicleModel(CP) - cloudlog.info("radard got CarParams") - - # import the radar from the fingerprint - cloudlog.info("radard is importing %s", CP.carName) - RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface - - sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) + self.last_md_ts = 0 + self.last_controls_state_ts = 0 - # Default parameters - live_parameters = messaging.new_message() - live_parameters.init('liveParameters') - live_parameters.liveParameters.valid = True - live_parameters.liveParameters.steerRatio = CP.steerRatio - live_parameters.liveParameters.stiffnessFactor = 1.0 + self.active = 0 + self.steer_angle = 0. + self.steer_override = False - MP = ModelParser() - RI = RadarInterface(CP) + # Kalman filter stuff: + self.ekfv = EKFV1D() + self.speedSensorV = SimpleSensor(XV, 1, 2) - last_md_ts = 0 - last_controls_state_ts = 0 - - # *** publish radarState and liveTracks - radarState = messaging.pub_sock(service_list['radarState'].port) - liveTracks = messaging.pub_sock(service_list['liveTracks'].port) - - path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max - - # Time-alignment - rate = 1. / DT_MDL # model and radar are both at 20Hz - v_len = 20 # how many speed data points to remember for t alignment with rdr data - - active = 0 - steer_angle = 0. - steer_override = False - - tracks = defaultdict(dict) - - # Kalman filter stuff: - ekfv = EKFV1D() - speedSensorV = SimpleSensor(XV, 1, 2) - - # v_ego - v_ego = 0. - v_ego_hist_t = deque([0], maxlen=v_len) - v_ego_hist_v = deque([0], maxlen=v_len) - v_ego_t_aligned = 0. - - rk = Ratekeeper(rate, print_delay_threshold=None) - while 1: - rr = RI.update() + # v_ego + self.v_ego = 0. + self.v_ego_hist_t = deque([0], maxlen=v_len) + self.v_ego_hist_v = deque([0], maxlen=v_len) + self.v_ego_t_aligned = 0. + def update(self, frame, delay, sm, rr): ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured] - sm.update(0) - if sm.updated['liveParameters']: - VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) + self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) if sm.updated['controlsState']: - active = sm['controlsState'].active - v_ego = sm['controlsState'].vEgo - steer_angle = sm['controlsState'].angleSteers - steer_override = sm['controlsState'].steerOverride + self.active = sm['controlsState'].active + self.v_ego = sm['controlsState'].vEgo + self.steer_angle = sm['controlsState'].angleSteers + self.steer_override = sm['controlsState'].steerOverride - v_ego_hist_v.append(v_ego) - v_ego_hist_t.append(float(rk.frame)/rate) + self.v_ego_hist_v.append(self.v_ego) + self.v_ego_hist_t.append(float(frame)/rate) - last_controls_state_ts = sm.logMonoTime['controlsState'] + self.last_controls_state_ts = sm.logMonoTime['controlsState'] if sm.updated['model']: - last_md_ts = sm.logMonoTime['model'] - MP.update(v_ego, sm['model']) - + self.last_md_ts = sm.logMonoTime['model'] + self.MP.update(self.v_ego, sm['model']) # run kalman filter only if prob is high enough - if MP.lead_prob > 0.7: - reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var)) - ekfv.update_scalar(reading) - ekfv.predict(DT_MDL) + if self.MP.lead_prob > 0.7: + reading = self.speedSensorV.read(self.MP.lead_dist, covar=np.matrix(self.MP.lead_var)) + self.ekfv.update_scalar(reading) + self.ekfv.predict(DT_MDL) # When changing lanes the distance to the lead car can suddenly change, # which makes the Kalman filter output large relative acceleration - if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0: - ekfv.state[XV] = MP.lead_dist - ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) - ekfv.state[SPEEDV] = 0. + if self.mocked and abs(self.MP.lead_dist - self.ekfv.state[XV]) > 2.0: + self.ekfv.state[XV] = self.MP.lead_dist + self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init])) + self.ekfv.state[SPEEDV] = 0. - ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])), - float(ekfv.state[SPEEDV]), False) + ar_pts[VISION_POINT] = (float(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])), + float(self.ekfv.state[SPEEDV]), False) else: - ekfv.state[XV] = MP.lead_dist - ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) - ekfv.state[SPEEDV] = 0. + self.ekfv.state[XV] = self.MP.lead_dist + self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init])) + self.ekfv.state[SPEEDV] = 0. if VISION_POINT in ar_pts: del ar_pts[VISION_POINT] # *** compute the likely path_y *** - if (active and not steer_override) or mocked: + if (self.active and not self.steer_override) or self.mocked: # use path from model (always when mocking as steering is too noisy) - path_y = np.polyval(MP.d_poly, path_x) + path_y = np.polyval(self.MP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 it does not only report the physical offset - path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0] + path_y = calc_lookahead_offset(self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0] # *** remove missing points from meta data *** - for ids in tracks.keys(): + for ids in self.tracks.keys(): if ids not in ar_pts: - tracks.pop(ids, None) + self.tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: # ignore standalone vision point, unless we are mocking the radar - if ids == VISION_POINT and not mocked: + if ids == VISION_POINT and not self.mocked: continue rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement - cur_time = float(rk.frame)/rate - v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v) + cur_time = float(frame)/rate + self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v) d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) # add sign d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y)) # create the track if it doesn't exist or it's a new track - if ids not in tracks: - tracks[ids] = Track() - tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override) + if ids not in self.tracks: + self.tracks[ids] = Track() + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: fused_id = None best_score = NO_FUSION_SCORE - for ids in tracks: - dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2) - rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel) - tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) - if best_score > tracks[ids].vision_score: + for ids in self.tracks: + dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - self.tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - self.tracks[ids].yRel)) ** 2) + rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel) + self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) + if best_score > self.tracks[ids].vision_score: fused_id = ids - best_score = tracks[ids].vision_score + best_score = self.tracks[ids].vision_score if fused_id is not None: - tracks[fused_id].vision_cnt += 1 - tracks[fused_id].update_vision_fusion() + self.tracks[fused_id].vision_cnt += 1 + self.tracks[fused_id].update_vision_fusion() if DEBUG: print("NEW CYCLE") if VISION_POINT in ar_pts: print("vision", ar_pts[VISION_POINT]) - idens = list(tracks.keys()) - track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) + idens = list(self.tracks.keys()) + track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them if len(track_pts) > 1: @@ -218,12 +185,12 @@ def radard_thread(gctx=None): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: clusters[cluster_i] = Cluster() - clusters[cluster_i].add(tracks[idens[idx]]) + clusters[cluster_i].add(self.tracks[idens[idx]]) elif len(track_pts) == 1: # TODO: why do we need this? clusters = [Cluster()] - clusters[0].add(tracks[idens[0]]) + clusters[0].add(self.tracks[idens[0]]) else: clusters = [] @@ -232,7 +199,7 @@ def radard_thread(gctx=None): print(i) # *** extract the lead car *** lead_clusters = [c for c in clusters - if c.is_potential_lead(v_ego)] + if c.is_potential_lead(self.v_ego)] lead_clusters.sort(key=lambda x: x.dRel) lead_len = len(lead_clusters) @@ -246,10 +213,10 @@ def radard_thread(gctx=None): dat = messaging.new_message() dat.init('radarState') dat.valid = sm.all_alive_and_valid(service_list=['controlsState']) - dat.radarState.mdMonoTime = last_md_ts + dat.radarState.mdMonoTime = self.last_md_ts dat.radarState.canMonoTimes = list(rr.canMonoTimes) dat.radarState.radarErrors = list(rr.errors) - dat.radarState.controlsStateMonoTime = last_controls_state_ts + dat.radarState.controlsStateMonoTime = self.last_controls_state_ts if lead_len > 0: dat.radarState.leadOne = lead_clusters[0].toRadarState() if lead2_len > 0: @@ -259,10 +226,51 @@ def radard_thread(gctx=None): else: dat.radarState.leadOne.status = False + return dat + +## fuses camera and radar data for best lead detection +def radard_thread(gctx=None): + set_realtime_priority(2) + + # wait for stats about the car to come in from controls + cloudlog.info("radard is waiting for CarParams") + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + mocked = CP.carName == "mock" + VM = VehicleModel(CP) + cloudlog.info("radard got CarParams") + + # import the radar from the fingerprint + cloudlog.info("radard is importing %s", CP.carName) + RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface + + can_sock = messaging.sub_sock(service_list['can'].port) + sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) + + RI = RadarInterface(CP) + + # *** publish radarState and liveTracks + radarState = messaging.pub_sock(service_list['radarState'].port) + liveTracks = messaging.pub_sock(service_list['liveTracks'].port) + + rk = Ratekeeper(rate, print_delay_threshold=None) + RD = RadarD(VM, mocked) + + while 1: + can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + rr = RI.update(can_strings) + + if rr is None: + continue + + sm.update(0) + + dat = RD.update(rk.frame, RI.delay, sm, rr) dat.radarState.cumLagMs = -rk.remaining*1000. + radarState.send(dat.to_bytes()) # *** publish tracks for UI debugging (keep last) *** + tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 8cdb0d2305..6ea757462e 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -1,3 +1,4 @@ ubloxd ubloxd_test -params_learner \ No newline at end of file +params_learner +paramsd \ No newline at end of file diff --git a/selfdrive/locationd/Makefile b/selfdrive/locationd/Makefile index f7652648e1..ff46847689 100644 --- a/selfdrive/locationd/Makefile +++ b/selfdrive/locationd/Makefile @@ -40,11 +40,11 @@ EXTRA_LIBS += -llog -luuid endif .PHONY: all -all: ubloxd params_learner +all: ubloxd paramsd include ../common/cereal.mk -LOC_OBJS = locationd_yawrate.o params_learner.o \ +LOC_OBJS = locationd_yawrate.o params_learner.o paramsd.o \ ../common/swaglog.o \ ../common/params.o \ ../common/util.o \ @@ -71,7 +71,7 @@ liblocationd.so: $(LOC_OBJS) $(ZMQ_SHARED_LIBS) \ $(EXTRA_LIBS) -params_learner: $(LOC_OBJS) +paramsd: $(LOC_OBJS) @echo "[ LINK ] $@" $(CXX) -fPIC -o '$@' $^ \ $(CEREAL_LIBS) \ @@ -115,7 +115,7 @@ ubloxd_test: ubloxd_test.o $(OBJS) .PHONY: clean clean: - rm -f ubloxd params_learner liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS) + rm -f ubloxd paramsd liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS) -include $(DEPS) -include $(LOC_DEPS) diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index f03f808ffd..ae4b05a71a 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -1,287 +1,96 @@ #include -#include #include -#include #include #include #include -#include "json11.hpp" -#include "cereal/gen/cpp/log.capnp.h" -#include "common/swaglog.h" -#include "common/messaging.h" -#include "common/params.h" -#include "common/timing.h" -#include "params_learner.h" +#include "locationd_yawrate.h" -const int num_polls = 3; - -class Localizer -{ - Eigen::Matrix2d A; - Eigen::Matrix2d I; - Eigen::Matrix2d Q; - Eigen::Matrix2d P; - Eigen::Matrix C_posenet; - Eigen::Matrix C_gyro; - - double R_gyro; - - void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas) { - double dt = current_time - prev_update_time; - prev_update_time = current_time; - if (dt < 1.0e-9) { - return; - } - - // x = A * x; - // P = A * P * A.transpose() + dt * Q; - // Simplify because A is unity - P = P + dt * Q; - - double y = meas - C * x; - double S = R + C * P * C.transpose(); - Eigen::Vector2d K = P * C.transpose() * (1.0 / S); - x = x + K * y; - P = (I - K * C) * P; - } - - void handle_sensor_events(capnp::List::Reader sensor_events, double current_time) { - for (cereal::SensorEventData::Reader sensor_event : sensor_events){ - if (sensor_event.getType() == 4) { - sensor_data_time = current_time; - - double meas = -sensor_event.getGyro().getV()[0]; - update_state(C_gyro, R_gyro, current_time, meas); - } - } +void Localizer::update_state(const Eigen::Matrix &C, const double R, double current_time, double meas) { + double dt = current_time - prev_update_time; + prev_update_time = current_time; + if (dt < 1.0e-9) { + return; } - void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) { - double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2); - double meas = camera_odometry.getRot()[2]; - update_state(C_posenet, R, current_time, meas); - } - - void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) { - steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS; - car_speed = controls_state.getVEgo(); - controls_state_time = current_time; - } + // x = A * x; + // P = A * P * A.transpose() + dt * Q; + // Simplify because A is unity + P = P + dt * Q; + double y = meas - C * x; + double S = R + C * P * C.transpose(); + Eigen::Vector2d K = P * C.transpose() * (1.0 / S); + x = x + K * y; + P = (I - K * C) * P; +} -public: - Eigen::Vector2d x; - double steering_angle = 0; - double car_speed = 0; - double prev_update_time = -1; - double controls_state_time = -1; - double sensor_data_time = -1; - - Localizer() { - A << 1, 0, 0, 1; - I << 1, 0, 0, 1; - - Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0); - P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0); - - C_posenet << 1, 0; - C_gyro << 1, 1; - x << 0, 0; - - R_gyro = pow(0.05, 2.0); - } - - cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size) { - const kj::ArrayPtr view((const capnp::word*)msg_dat, msg_size); - capnp::FlatArrayMessageReader msg(view); - cereal::Event::Reader event = msg.getRoot(); - double current_time = event.getLogMonoTime() / 1.0e9; - - if (prev_update_time < 0) { - prev_update_time = current_time; - } +void Localizer::handle_sensor_events(capnp::List::Reader sensor_events, double current_time) { + for (cereal::SensorEventData::Reader sensor_event : sensor_events){ + if (sensor_event.getType() == 4) { + sensor_data_time = current_time; - auto type = event.which(); - switch(type) { - case cereal::Event::CONTROLS_STATE: - handle_controls_state(event.getControlsState(), current_time); - break; - case cereal::Event::CAMERA_ODOMETRY: - handle_camera_odometry(event.getCameraOdometry(), current_time); - break; - case cereal::Event::SENSOR_EVENTS: - handle_sensor_events(event.getSensorEvents(), current_time); - break; - default: - break; + double meas = -sensor_event.getGyro().getV()[0]; + update_state(C_gyro, R_gyro, current_time, meas); } - - return type; } -}; - - - -int main(int argc, char *argv[]) { - auto ctx = zmq_ctx_new(); - auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007"); - auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003"); - auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066"); - - auto live_parameters_sock = zsock_new_pub("@tcp://*:8064"); - assert(live_parameters_sock); - auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock); - - int err; - Localizer localizer; - - zmq_pollitem_t polls[num_polls] = {{0}}; - polls[0].socket = controls_state_sock; - polls[0].events = ZMQ_POLLIN; - polls[1].socket = sensor_events_sock; - polls[1].events = ZMQ_POLLIN; - polls[2].socket = camera_odometry_sock; - polls[2].events = ZMQ_POLLIN; - - // Read car params - char *value; - size_t value_sz = 0; +} - LOGW("waiting for params to set vehicle model"); - while (true) { - read_db_value(NULL, "CarParams", &value, &value_sz); - if (value_sz > 0) break; - usleep(100*1000); - } - LOGW("got %d bytes CarParams", value_sz); +void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) { + double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2); + double meas = camera_odometry.getRot()[2]; + update_state(C_posenet, R, current_time, meas); +} - // make copy due to alignment issues - auto amsg = kj::heapArray((value_sz / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), value, value_sz); - free(value); +void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) { + steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS; + car_speed = controls_state.getVEgo(); + controls_state_time = current_time; +} - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - // Read params from previous run - const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz); +Localizer::Localizer() { + A << 1, 0, 0, 1; + I << 1, 0, 0, 1; - std::string fingerprint = car_params.getCarFingerprint(); - std::string vin = car_params.getCarVin(); - double sR = car_params.getSteerRatio(); - double x = 1.0; - double ao = 0.0; + Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0); + P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0); - if (result == 0){ - auto str = std::string(value, value_sz); - free(value); + C_posenet << 1, 0; + C_gyro << 1, 1; + x << 0, 0; - std::string err; - auto json = json11::Json::parse(str, err); - if (json.is_null() || !err.empty()) { - std::string log = "Error parsing json: " + err; - LOGW(log.c_str()); - } else { - std::string new_fingerprint = json["carFingerprint"].string_value(); - std::string new_vin = json["carVin"].string_value(); + R_gyro = pow(0.05, 2.0); +} - if (fingerprint == new_fingerprint && vin == new_vin) { - std::string log = "Parameter starting with: " + str; - LOGW(log.c_str()); +cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t msg_size) { + const kj::ArrayPtr view((const capnp::word*)msg_dat, msg_size); + capnp::FlatArrayMessageReader msg(view); + cereal::Event::Reader event = msg.getRoot(); + double current_time = event.getLogMonoTime() / 1.0e9; - sR = json["steerRatio"].number_value(); - x = json["stiffnessFactor"].number_value(); - ao = json["angleOffsetAverage"].number_value(); - } - } + if (prev_update_time < 0) { + prev_update_time = current_time; } - ParamsLearner learner(car_params, ao, x, sR, 1.0); - - // Main loop - int save_counter = 0; - while (true){ - int ret = zmq_poll(polls, num_polls, 100); - - if (ret == 0){ - continue; - } else if (ret < 0){ - break; - } - - for (int i=0; i < num_polls; i++) { - if (polls[i].revents) { - zmq_msg_t msg; - err = zmq_msg_init(&msg); - assert(err == 0); - err = zmq_msg_recv(&msg, polls[i].socket, 0); - assert(err >= 0); - // make copy due to alignment issues, will be freed on out of scope - auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); - - auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size()); - zmq_msg_close(&msg); - - if (which == cereal::Event::CONTROLS_STATE){ - save_counter++; - - double yaw_rate = -localizer.x[0]; - bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle); - - // TODO: Fix in replay - double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time; - - double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; - double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; - - // Send parameters at 10 Hz - if (save_counter % 10 == 0){ - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto live_params = event.initLiveParameters(); - live_params.setValid(valid); - live_params.setYawRate(localizer.x[0]); - live_params.setGyroBias(localizer.x[1]); - live_params.setSensorValid(sensor_data_age < 5.0); - live_params.setAngleOffset(angle_offset_degrees); - live_params.setAngleOffsetAverage(angle_offset_average_degrees); - live_params.setStiffnessFactor(learner.x); - live_params.setSteerRatio(learner.sR); - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); - } - - - // Save parameters every minute - if (save_counter % 6000 == 0) { - json11::Json json = json11::Json::object { - {"carVin", vin}, - {"carFingerprint", fingerprint}, - {"steerRatio", learner.sR}, - {"stiffnessFactor", learner.x}, - {"angleOffsetAverage", angle_offset_average_degrees}, - }; - - std::string out = json.dump(); - write_db_value(NULL, "LiveParameters", out.c_str(), out.length()); - } - } - } - } + auto type = event.which(); + switch(type) { + case cereal::Event::CONTROLS_STATE: + handle_controls_state(event.getControlsState(), current_time); + break; + case cereal::Event::CAMERA_ODOMETRY: + handle_camera_odometry(event.getCameraOdometry(), current_time); + break; + case cereal::Event::SENSOR_EVENTS: + handle_sensor_events(event.getSensorEvents(), current_time); + break; + default: + break; } - zmq_close(controls_state_sock); - zmq_close(sensor_events_sock); - zmq_close(camera_odometry_sock); - zmq_close(live_parameters_sock_raw); - return 0; + return type; } diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h new file mode 100644 index 0000000000..323e87de4e --- /dev/null +++ b/selfdrive/locationd/locationd_yawrate.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include "cereal/gen/cpp/log.capnp.h" + +#define DEGREES_TO_RADIANS 0.017453292519943295 + +class Localizer +{ + Eigen::Matrix2d A; + Eigen::Matrix2d I; + Eigen::Matrix2d Q; + Eigen::Matrix2d P; + Eigen::Matrix C_posenet; + Eigen::Matrix C_gyro; + + double R_gyro; + + void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas); + void handle_sensor_events(capnp::List::Reader sensor_events, double current_time); + void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time); + void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time); + +public: + Eigen::Vector2d x; + double steering_angle = 0; + double car_speed = 0; + double prev_update_time = -1; + double controls_state_time = -1; + double sensor_data_time = -1; + + Localizer(); + cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size); +}; diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc index a150805d86..912abde357 100644 --- a/selfdrive/locationd/params_learner.cc +++ b/selfdrive/locationd/params_learner.cc @@ -2,6 +2,8 @@ #include #include +#include +#include #include "cereal/gen/cpp/log.capnp.h" #include "cereal/gen/cpp/car.capnp.h" #include "params_learner.h" @@ -14,14 +16,15 @@ T clip(const T& n, const T& lower, const T& upper) { } ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params, - double angle_offset, - double stiffness_factor, - double steer_ratio, - double learning_rate) : - ao(angle_offset * DEGREES_TO_RADIANS), - slow_ao(angle_offset * DEGREES_TO_RADIANS), - x(stiffness_factor), - sR(steer_ratio) { + double angle_offset, + double stiffness_factor, + double steer_ratio, + double learning_rate) : + ao(angle_offset * DEGREES_TO_RADIANS), + slow_ao(angle_offset * DEGREES_TO_RADIANS), + x(stiffness_factor), + sR(steer_ratio) { + cF0 = car_params.getTireStiffnessFront(); cR0 = car_params.getTireStiffnessRear(); @@ -73,3 +76,43 @@ bool ParamsLearner::update(double psi, double u, double sa) { valid = valid && sR < max_sr_th; return valid; } + + +extern "C" { + void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) { + + auto amsg = kj::heapArray((len / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), params, len); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate); + return (void*)p; + } + + bool params_learner_update(void * params_learner, double psi, double u, double sa) { + ParamsLearner * p = (ParamsLearner*) params_learner; + return p->update(psi, u, sa); + } + + double params_learner_get_ao(void * params_learner){ + ParamsLearner * p = (ParamsLearner*) params_learner; + return p->ao; + } + + double params_learner_get_x(void * params_learner){ + ParamsLearner * p = (ParamsLearner*) params_learner; + return p->x; + } + + double params_learner_get_slow_ao(void * params_learner){ + ParamsLearner * p = (ParamsLearner*) params_learner; + return p->slow_ao; + } + + double params_learner_get_sR(void * params_learner){ + ParamsLearner * p = (ParamsLearner*) params_learner; + return p->sR; + } +} diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc new file mode 100644 index 0000000000..9fc3ad625d --- /dev/null +++ b/selfdrive/locationd/paramsd.cc @@ -0,0 +1,174 @@ +#include +#include +#include + +#include "locationd_yawrate.h" +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/swaglog.h" +#include "common/messaging.h" +#include "common/params.h" +#include "common/timing.h" +#include "params_learner.h" +#include "json11.hpp" + +const int num_polls = 3; + +int main(int argc, char *argv[]) { + auto ctx = zmq_ctx_new(); + auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007"); + auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003"); + auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066"); + + auto live_parameters_sock = zsock_new_pub("@tcp://*:8064"); + assert(live_parameters_sock); + auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock); + + int err; + Localizer localizer; + + zmq_pollitem_t polls[num_polls] = {{0}}; + polls[0].socket = controls_state_sock; + polls[0].events = ZMQ_POLLIN; + polls[1].socket = sensor_events_sock; + polls[1].events = ZMQ_POLLIN; + polls[2].socket = camera_odometry_sock; + polls[2].events = ZMQ_POLLIN; + + // Read car params + char *value; + size_t value_sz = 0; + + LOGW("waiting for params to set vehicle model"); + while (true) { + read_db_value(NULL, "CarParams", &value, &value_sz); + if (value_sz > 0) break; + usleep(100*1000); + } + LOGW("got %d bytes CarParams", value_sz); + + // make copy due to alignment issues + auto amsg = kj::heapArray((value_sz / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), value, value_sz); + free(value); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + // Read params from previous run + const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz); + + std::string fingerprint = car_params.getCarFingerprint(); + std::string vin = car_params.getCarVin(); + double sR = car_params.getSteerRatio(); + double x = 1.0; + double ao = 0.0; + + if (result == 0){ + auto str = std::string(value, value_sz); + free(value); + + std::string err; + auto json = json11::Json::parse(str, err); + if (json.is_null() || !err.empty()) { + std::string log = "Error parsing json: " + err; + LOGW(log.c_str()); + } else { + std::string new_fingerprint = json["carFingerprint"].string_value(); + std::string new_vin = json["carVin"].string_value(); + + if (fingerprint == new_fingerprint && vin == new_vin) { + std::string log = "Parameter starting with: " + str; + LOGW(log.c_str()); + + sR = json["steerRatio"].number_value(); + x = json["stiffnessFactor"].number_value(); + ao = json["angleOffsetAverage"].number_value(); + } + } + } + + ParamsLearner learner(car_params, ao, x, sR, 1.0); + + // Main loop + int save_counter = 0; + while (true){ + int ret = zmq_poll(polls, num_polls, 100); + + if (ret == 0){ + continue; + } else if (ret < 0){ + break; + } + + for (int i=0; i < num_polls; i++) { + if (polls[i].revents) { + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, polls[i].socket, 0); + assert(err >= 0); + // make copy due to alignment issues, will be freed on out of scope + auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); + + auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size()); + zmq_msg_close(&msg); + + if (which == cereal::Event::CONTROLS_STATE){ + save_counter++; + + double yaw_rate = -localizer.x[0]; + bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle); + + // TODO: Fix in replay + double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time; + + double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; + double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; + + // Send parameters at 10 Hz + if (save_counter % 10 == 0){ + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto live_params = event.initLiveParameters(); + live_params.setValid(valid); + live_params.setYawRate(localizer.x[0]); + live_params.setGyroBias(localizer.x[1]); + live_params.setSensorValid(sensor_data_age < 5.0); + live_params.setAngleOffset(angle_offset_degrees); + live_params.setAngleOffsetAverage(angle_offset_average_degrees); + live_params.setStiffnessFactor(learner.x); + live_params.setSteerRatio(learner.sR); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); + } + + + // Save parameters every minute + if (save_counter % 6000 == 0) { + json11::Json json = json11::Json::object { + {"carVin", vin}, + {"carFingerprint", fingerprint}, + {"steerRatio", learner.sR}, + {"stiffnessFactor", learner.x}, + {"angleOffsetAverage", angle_offset_average_degrees}, + }; + + std::string out = json.dump(); + write_db_value(NULL, "LiveParameters", out.c_str(), out.length()); + } + } + } + } + } + + zmq_close(controls_state_sock); + zmq_close(sensor_events_sock); + zmq_close(camera_odometry_sock); + zmq_close(live_parameters_sock_raw); + return 0; +} diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py new file mode 100755 index 0000000000..e2a6913c0c --- /dev/null +++ b/selfdrive/locationd/test/test_params_learner.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +import numpy as np +import unittest + +from selfdrive.car.honda.interface import CarInterface +from selfdrive.car.honda.values import CAR +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error + + +class TestParamsLearner(unittest.TestCase): + def setUp(self): + + self.CP = CarInterface.get_params(CAR.CIVIC, {}) + bts = self.CP.to_bytes() + + self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0) + + def test_convergence(self): + # Setup vehicle model with wrong parameters + VM_sim = VehicleModel(self.CP) + x_target = 0.75 + sr_target = 14 + ao_target = -1.0 + VM_sim.update_params(x_target, sr_target) + + # Run simulation + times = np.arange(0, 10*3600, 0.01) + angle_offset = np.radians(ao_target) + steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset + speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25 + + for i, t in enumerate(times): + u = speeds[i] + sa = steering_angles[i] + psi = VM_sim.yaw_rate(sa - angle_offset, u) + liblocationd.params_learner_update(self.params_learner, psi, u, sa) + + # Verify learned parameters + sr = liblocationd.params_learner_get_sR(self.params_learner) + ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner)) + x = liblocationd.params_learner_get_x(self.params_learner) + self.assertAlmostEqual(x_target, x, places=1) + self.assertAlmostEqual(ao_target, ao_slow, places=1) + self.assertAlmostEqual(sr_target, sr, places=1) + + + + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 7cad71bf7d..0bd2ff18f1 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -168,7 +168,7 @@ class Uploader(object): def do_upload(self, key, fn): try: - url_resp = api_get("v1.2/"+self.dongle_id+"/upload_url/", timeout=2, path=key, access_token=self.access_token) + url_resp = api_get("v1.2/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.access_token) url_resp_json = json.loads(url_resp.text) url = url_resp_json['url'] headers = url_resp_json['headers'] @@ -223,7 +223,7 @@ class Uploader(object): try: os.unlink(fn) except OSError: - cloudlog.exception("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz) + cloudlog.event("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz) success = True else: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index c3be2c094d..4e52f2beb4 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -72,12 +72,15 @@ import glob import shutil import hashlib import importlib +import re +import stat import subprocess import traceback from multiprocessing import Process from setproctitle import setproctitle #pylint: disable=no-name-in-module +from common.file_helpers import atomic_write_in_dir_neos from common.params import Params import cereal ThermalStatus = cereal.log.ThermalData.ThermalStatus @@ -109,12 +112,14 @@ managed_processes = { "pandad": "selfdrive.pandad", "ui": ("selfdrive/ui", ["./start.py"]), "calibrationd": "selfdrive.locationd.calibrationd", - "params_learner": ("selfdrive/locationd", ["./params_learner"]), + "paramsd": ("selfdrive/locationd", ["./paramsd"]), "visiond": ("selfdrive/visiond", ["./visiond"]), "sensord": ("selfdrive/sensord", ["./start_sensord.py"]), "gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]), "updated": "selfdrive.updated", - "athena": "selfdrive.athena.athenad", +} +daemon_processes = { + "athenad": "selfdrive.athena.athenad", } android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") @@ -136,7 +141,6 @@ persistent_processes = [ 'uploader', 'ui', 'updated', - 'athena', ] car_started_processes = [ @@ -146,7 +150,7 @@ car_started_processes = [ 'sensord', 'radard', 'calibrationd', - 'params_learner', + 'paramsd', 'visiond', 'proclogd', 'ubloxd', @@ -209,6 +213,29 @@ def start_managed_process(name): running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd)) running[name].start() +def start_daemon_process(name, params): + proc = daemon_processes[name] + pid_param = name.capitalize() + 'Pid' + pid = params.get(pid_param) + + if pid is not None: + try: + os.kill(int(pid), 0) + # process is running (kill is a poorly-named system call) + return + except OSError: + # process is dead + pass + + cloudlog.info("starting daemon %s" % name) + proc = subprocess.Popen(['python', '-m', proc], + cwd='/', + stdout=open('/dev/null', 'w'), + stderr=open('/dev/null', 'w'), + preexec_fn=os.setpgrp) + + params.put(pid_param, str(proc.pid)) + def prepare_managed_process(p): proc = managed_processes[p] if isinstance(proc, str): @@ -321,6 +348,12 @@ def manager_thread(): # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) + params = Params() + + # start daemon processes + for p in daemon_processes: + start_daemon_process(p, params) + # start persistent processes for p in persistent_processes: start_managed_process(p) @@ -332,7 +365,6 @@ def manager_thread(): if os.getenv("NOBOARD") is None: start_managed_process("pandad") - params = Params() logger_dead = False while 1: @@ -420,10 +452,46 @@ def update_apks(): assert success +def update_ssh(): + ssh_home_dirpath = "/system/comma/home/.ssh/" + auth_keys_path = os.path.join(ssh_home_dirpath, "authorized_keys") + auth_keys_persist_path = os.path.join(ssh_home_dirpath, "authorized_keys.persist") + auth_keys_mode = stat.S_IREAD | stat.S_IWRITE + + params = Params() + github_keys = params.get("GithubSshKeys") or '' + + old_keys = open(auth_keys_path).read() + has_persisted_keys = os.path.exists(auth_keys_persist_path) + if has_persisted_keys: + persisted_keys = open(auth_keys_persist_path).read() + else: + # add host filter + persisted_keys = re.sub(r'^(?!.+?from.+? )(ssh|ecdsa)', 'from="10.0.0.0/8,172.16.0.0/12,192.168.0.0/16" \\1', old_keys, flags=re.MULTILINE) + + new_keys = persisted_keys + '\n' + github_keys + + if has_persisted_keys and new_keys == old_keys and os.stat(auth_keys_path)[stat.ST_MODE] == auth_keys_mode: + # nothing to do - let's avoid remount + return + + try: + subprocess.check_call(["mount", "-o", "rw,remount", "/system"]) + if not has_persisted_keys: + atomic_write_in_dir_neos(auth_keys_persist_path, persisted_keys, mode=auth_keys_mode) + + atomic_write_in_dir_neos(auth_keys_path, new_keys, mode=auth_keys_mode) + finally: + try: + subprocess.check_call(["mount", "-o", "ro,remount", "/system"]) + except: + cloudlog.exception("Failed to remount as read-only") + # this can fail due to "Device busy" - reboot if so + os.system("reboot") + raise RuntimeError + def manager_update(): - if os.path.exists(os.path.join(BASEDIR, "vpn")): - cloudlog.info("installing vpn") - os.system(os.path.join(BASEDIR, "vpn", "install.sh")) + update_ssh() update_apks() def manager_prepare(): diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py index 4e78f66c81..cad7e2e8bf 100644 --- a/selfdrive/messaging.py +++ b/selfdrive/messaging.py @@ -16,17 +16,34 @@ def pub_sock(port, addr="*"): sock.bind("tcp://%s:%d" % (addr, port)) return sock -def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False): +def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False, timeout=None): context = zmq.Context.instance() sock = context.socket(zmq.SUB) if conflate: sock.setsockopt(zmq.CONFLATE, 1) sock.connect("tcp://%s:%d" % (addr, port)) sock.setsockopt(zmq.SUBSCRIBE, b"") + + if timeout is not None: + sock.RCVTIMEO = timeout + if poller is not None: poller.register(sock, zmq.POLLIN) return sock +def drain_sock_raw(sock, wait_for_one=False): + ret = [] + while 1: + try: + if wait_for_one and len(ret) == 0: + dat = sock.recv() + else: + dat = sock.recv(zmq.NOBLOCK) + ret.append(dat) + except zmq.error.Again: + break + return ret + def drain_sock(sock, wait_for_one=False): ret = [] while 1: @@ -82,24 +99,29 @@ class SubMaster(): self.valid = {} for s in services: # TODO: get address automatically from service_list - self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True) + if addr is not None: + self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True) self.freq[s] = service_list[s].frequency data = new_message() data.init(s) self.data[s] = getattr(data, s) - self.logMonoTime[s] = data.logMonoTime + self.logMonoTime[s] = 0 self.valid[s] = data.valid def __getitem__(self, s): return self.data[s] def update(self, timeout=-1): + msgs = [] + for sock, _ in self.poller.poll(timeout): + msgs.append(recv_one(sock)) + self.update_msgs(sec_since_boot(), msgs) + + def update_msgs(self, cur_time, msgs): # TODO: add optional input that specify the service to wait for self.frame += 1 self.updated = dict.fromkeys(self.updated, False) - cur_time = sec_since_boot() - for sock, _ in self.poller.poll(timeout): - msg = recv_one(sock) + for msg in msgs: s = msg.which() self.updated[s] = True self.rcv_time[s] = cur_time diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 9f68998499..f8a084bd36 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -5,7 +5,7 @@ import struct from datetime import datetime, timedelta from selfdrive.swaglog import cloudlog -from selfdrive.version import version, training_version, get_git_commit, get_git_branch, get_git_remote +from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote from common.api import api_get from common.params import Params from common.file_helpers import mkdirs_exists_ok @@ -53,6 +53,7 @@ def get_subscriber_info(): def register(): params = Params() params.put("Version", version) + params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) params.put("GitCommit", get_git_commit()) params.put("GitBranch", get_git_branch()) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index e39bafd792..930f83be87 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -244,6 +244,7 @@ class Plant(object): 'EPB_STATE', 'BRAKE_HOLD_ACTIVE', 'INTERCEPTOR_GAS', + 'INTERCEPTOR_GAS2', 'IMPERIAL_UNIT', ]) vls = vls_tuple( @@ -276,6 +277,7 @@ class Plant(object): 0, # EPB State 0, # Brake hold 0, # Interceptor feedback + 0, # Interceptor 2 feedback False ) diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index ef75cfffd9..9a29b96fec 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -2,7 +2,7 @@ import os from smbus2 import SMBus from cereal import log -from selfdrive.version import training_version +from selfdrive.version import terms_version, training_version from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging from selfdrive.services import service_list @@ -216,7 +216,7 @@ def thermald_thread(): ignition = True do_uninstall = params.get("DoUninstall") == "1" - accepted_terms = params.get("HasAcceptedTerms") == "1" + accepted_terms = params.get("HasAcceptedTerms") == terms_version completed_training = params.get("CompletedTrainingVersion") == training_version should_start = ignition diff --git a/selfdrive/version.py b/selfdrive/version.py index 4acc183502..2eb39dc976 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -59,6 +59,7 @@ except subprocess.CalledProcessError: dirty = True training_version = "0.1.0" +terms_version = "2" if __name__ == "__main__": print("Dirty: %s" % dirty) diff --git a/selfdrive/visiond/models/driving.cc b/selfdrive/visiond/models/driving.cc index bf5933f428..28876c81b1 100644 --- a/selfdrive/visiond/models/driving.cc +++ b/selfdrive/visiond/models/driving.cc @@ -135,7 +135,14 @@ void poly_fit(float *in_pts, float *in_stds, float *out) { Eigen::Matrix lhs = vander.array().colwise() / std.array(); Eigen::Matrix rhs = pts.array() / std.array(); + // Improve numerical stability + Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); + lhs = lhs * scale.asDiagonal(); + // Solve inplace Eigen::ColPivHouseholderQR > qr(lhs); p = qr.solve(rhs); + + // Apply scale to output + p = p.transpose() * scale.asDiagonal(); }