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();
}