openpilot v0.6.1 release
Vehicle Researcher
parent
cd98235644
commit
94053536b4
3
Pipfile
3
Pipfile
|
|
@ -96,6 +96,9 @@ fastcluster = "==1.1.25"
|
|||
backports-abc = "*"
|
||||
pygame = "*"
|
||||
simplejson = "*"
|
||||
python-logstash-async = "*"
|
||||
pandas = "*"
|
||||
seaborn = "*"
|
||||
|
||||
[packages]
|
||||
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
16
README.md
16
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 | 25mph<sup>1</sup>| 25mph | Nidec |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
|
||||
| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Buick<sup>3</sup> | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| 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 |
|
||||
| GMC<sup>3</sup> | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
|
|
@ -84,9 +84,9 @@ Supported Cars
|
|||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| 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 | Custom<sup>6</sup>|
|
||||
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
|
|
@ -96,8 +96,9 @@ Supported Cars
|
|||
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
|
||||
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
|
||||
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Camry 2018 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-18 | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Avalon 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-19 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 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 | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Sienna 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
|
||||
<sup>1</sup>[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).*** <br />
|
||||
<sup>2</sup>When 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). <br />
|
||||
|
|
|
|||
|
|
@ -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!
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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],
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
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);
|
||||
|
||||
// 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 {
|
||||
tcp_addr_str = "tcp://" + tcp_addr + ":8006";
|
||||
}
|
||||
const char *tcp_addr_char = tcp_addr_str.c_str();
|
||||
|
||||
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);
|
||||
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<capnp::word>((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<cereal::Event>();
|
||||
|
||||
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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 = <can_init_with_vectors_func>dlsym(libdbc, 'can_init_with_vectors')
|
||||
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
|
||||
self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
|
||||
self.can_update_string = <can_update_string_func>dlsym(libdbc, 'can_update_string')
|
||||
self.can_query_vector = <can_query_vector_func>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)
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -50,6 +50,8 @@ def _get_interface_names():
|
|||
# imports from directory selfdrive/car/<name>/
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
self.trigger_msg = LAST_RADAR_MSG
|
||||
self.updated_messages = set()
|
||||
|
||||
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_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()
|
||||
while 1:
|
||||
|
||||
if self.rcp is None:
|
||||
time.sleep(0.05) # nothing to do
|
||||
return ret
|
||||
|
||||
tm = int(sec_since_boot() * 1e9)
|
||||
_, vls = self.rcp.update(tm, True)
|
||||
updated_messages.update(vls)
|
||||
if LAST_RADAR_MSG in updated_messages:
|
||||
break
|
||||
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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']
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
while 1:
|
||||
tm = int(sec_since_boot() * 1e9)
|
||||
_, vls = self.rcp.update(tm, True)
|
||||
updated_messages.update(vls)
|
||||
if 0x445 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
|
||||
|
||||
rr = self._update(self.updated_messages)
|
||||
self.updated_messages.clear()
|
||||
return rr
|
||||
|
||||
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
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
|
||||
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
|
||||
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.6-release"
|
||||
#define COMMA_VERSION "0.6.1-release"
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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]))
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,6 +48,185 @@ class EKFV1D(EKF):
|
|||
tfj = tf
|
||||
return tf, tfj
|
||||
|
||||
class RadarD(object):
|
||||
def __init__(self, VM, mocked):
|
||||
self.VM = VM
|
||||
self.mocked = mocked
|
||||
|
||||
self.MP = ModelParser()
|
||||
self.tracks = defaultdict(dict)
|
||||
|
||||
self.last_md_ts = 0
|
||||
self.last_controls_state_ts = 0
|
||||
|
||||
self.active = 0
|
||||
self.steer_angle = 0.
|
||||
self.steer_override = False
|
||||
|
||||
# Kalman filter stuff:
|
||||
self.ekfv = EKFV1D()
|
||||
self.speedSensorV = SimpleSensor(XV, 1, 2)
|
||||
|
||||
# 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]
|
||||
|
||||
if sm.updated['liveParameters']:
|
||||
self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
|
||||
|
||||
if sm.updated['controlsState']:
|
||||
self.active = sm['controlsState'].active
|
||||
self.v_ego = sm['controlsState'].vEgo
|
||||
self.steer_angle = sm['controlsState'].angleSteers
|
||||
self.steer_override = sm['controlsState'].steerOverride
|
||||
|
||||
self.v_ego_hist_v.append(self.v_ego)
|
||||
self.v_ego_hist_t.append(float(frame)/rate)
|
||||
|
||||
self.last_controls_state_ts = sm.logMonoTime['controlsState']
|
||||
|
||||
if sm.updated['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 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 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(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])),
|
||||
float(self.ekfv.state[SPEEDV]), False)
|
||||
else:
|
||||
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 (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(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(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 self.tracks.keys():
|
||||
if ids not in ar_pts:
|
||||
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 self.mocked:
|
||||
continue
|
||||
rpt = ar_pts[ids]
|
||||
|
||||
# align v_ego by a fixed time to align it with the radar measurement
|
||||
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 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 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 = self.tracks[ids].vision_score
|
||||
|
||||
if fused_id is not None:
|
||||
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(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:
|
||||
cluster_idxs = cluster_points_centroid(track_pts, 2.5)
|
||||
clusters = [None] * (max(cluster_idxs) + 1)
|
||||
|
||||
for idx in xrange(len(track_pts)):
|
||||
cluster_i = cluster_idxs[idx]
|
||||
if clusters[cluster_i] is None:
|
||||
clusters[cluster_i] = Cluster()
|
||||
clusters[cluster_i].add(self.tracks[idens[idx]])
|
||||
|
||||
elif len(track_pts) == 1:
|
||||
# TODO: why do we need this?
|
||||
clusters = [Cluster()]
|
||||
clusters[0].add(self.tracks[idens[0]])
|
||||
else:
|
||||
clusters = []
|
||||
|
||||
if DEBUG:
|
||||
for i in clusters:
|
||||
print(i)
|
||||
# *** extract the lead car ***
|
||||
lead_clusters = [c for c in clusters
|
||||
if c.is_potential_lead(self.v_ego)]
|
||||
lead_clusters.sort(key=lambda x: x.dRel)
|
||||
lead_len = len(lead_clusters)
|
||||
|
||||
# *** extract the second lead from the whole set of leads ***
|
||||
lead2_clusters = [c for c in lead_clusters
|
||||
if c.is_potential_lead2(lead_clusters)]
|
||||
lead2_clusters.sort(key=lambda x: x.dRel)
|
||||
lead2_len = len(lead2_clusters)
|
||||
|
||||
# *** publish radarState ***
|
||||
dat = messaging.new_message()
|
||||
dat.init('radarState')
|
||||
dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
|
||||
dat.radarState.mdMonoTime = self.last_md_ts
|
||||
dat.radarState.canMonoTimes = list(rr.canMonoTimes)
|
||||
dat.radarState.radarErrors = list(rr.errors)
|
||||
dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
|
||||
if lead_len > 0:
|
||||
dat.radarState.leadOne = lead_clusters[0].toRadarState()
|
||||
if lead2_len > 0:
|
||||
dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
|
||||
else:
|
||||
dat.radarState.leadTwo.status = False
|
||||
else:
|
||||
dat.radarState.leadOne.status = False
|
||||
|
||||
return dat
|
||||
|
||||
## fuses camera and radar data for best lead detection
|
||||
def radard_thread(gctx=None):
|
||||
|
|
@ -59,210 +243,34 @@ def radard_thread(gctx=None):
|
|||
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'])
|
||||
|
||||
# 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
|
||||
|
||||
MP = ModelParser()
|
||||
RI = RadarInterface(CP)
|
||||
|
||||
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()
|
||||
RD = RadarD(VM, mocked)
|
||||
|
||||
ar_pts = {}
|
||||
for pt in rr.points:
|
||||
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
|
||||
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)
|
||||
|
||||
if sm.updated['liveParameters']:
|
||||
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
|
||||
|
||||
v_ego_hist_v.append(v_ego)
|
||||
v_ego_hist_t.append(float(rk.frame)/rate)
|
||||
|
||||
last_controls_state_ts = sm.logMonoTime['controlsState']
|
||||
|
||||
if sm.updated['model']:
|
||||
last_md_ts = sm.logMonoTime['model']
|
||||
MP.update(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)
|
||||
|
||||
# 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.
|
||||
|
||||
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
|
||||
float(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.
|
||||
|
||||
if VISION_POINT in ar_pts:
|
||||
del ar_pts[VISION_POINT]
|
||||
|
||||
# *** compute the likely path_y ***
|
||||
if (active and not steer_override) or mocked:
|
||||
# use path from model (always when mocking as steering is too noisy)
|
||||
path_y = np.polyval(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]
|
||||
|
||||
# *** remove missing points from meta data ***
|
||||
for ids in tracks.keys():
|
||||
if ids not in ar_pts:
|
||||
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:
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
# 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:
|
||||
fused_id = ids
|
||||
best_score = tracks[ids].vision_score
|
||||
|
||||
if fused_id is not None:
|
||||
tracks[fused_id].vision_cnt += 1
|
||||
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])
|
||||
|
||||
# If we have multiple points, cluster them
|
||||
if len(track_pts) > 1:
|
||||
cluster_idxs = cluster_points_centroid(track_pts, 2.5)
|
||||
clusters = [None] * (max(cluster_idxs) + 1)
|
||||
|
||||
for idx in xrange(len(track_pts)):
|
||||
cluster_i = cluster_idxs[idx]
|
||||
if clusters[cluster_i] is None:
|
||||
clusters[cluster_i] = Cluster()
|
||||
clusters[cluster_i].add(tracks[idens[idx]])
|
||||
|
||||
elif len(track_pts) == 1:
|
||||
# TODO: why do we need this?
|
||||
clusters = [Cluster()]
|
||||
clusters[0].add(tracks[idens[0]])
|
||||
else:
|
||||
clusters = []
|
||||
|
||||
if DEBUG:
|
||||
for i in clusters:
|
||||
print(i)
|
||||
# *** extract the lead car ***
|
||||
lead_clusters = [c for c in clusters
|
||||
if c.is_potential_lead(v_ego)]
|
||||
lead_clusters.sort(key=lambda x: x.dRel)
|
||||
lead_len = len(lead_clusters)
|
||||
|
||||
# *** extract the second lead from the whole set of leads ***
|
||||
lead2_clusters = [c for c in lead_clusters
|
||||
if c.is_potential_lead2(lead_clusters)]
|
||||
lead2_clusters.sort(key=lambda x: x.dRel)
|
||||
lead2_len = len(lead2_clusters)
|
||||
|
||||
# *** publish radarState ***
|
||||
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.canMonoTimes = list(rr.canMonoTimes)
|
||||
dat.radarState.radarErrors = list(rr.errors)
|
||||
dat.radarState.controlsStateMonoTime = last_controls_state_ts
|
||||
if lead_len > 0:
|
||||
dat.radarState.leadOne = lead_clusters[0].toRadarState()
|
||||
if lead2_len > 0:
|
||||
dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
|
||||
else:
|
||||
dat.radarState.leadTwo.status = False
|
||||
else:
|
||||
dat.radarState.leadOne.status = False
|
||||
|
||||
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))
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
ubloxd
|
||||
ubloxd_test
|
||||
params_learner
|
||||
params_learner
|
||||
paramsd
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -1,287 +1,96 @@
|
|||
#include <iostream>
|
||||
#include <csignal>
|
||||
#include <cmath>
|
||||
|
||||
#include <czmq.h>
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#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<double, 1, 2> C_posenet;
|
||||
Eigen::Matrix<double, 1, 2> C_gyro;
|
||||
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &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;
|
||||
}
|
||||
|
||||
double R_gyro;
|
||||
// x = A * x;
|
||||
// P = A * P * A.transpose() + dt * Q;
|
||||
// Simplify because A is unity
|
||||
P = P + dt * Q;
|
||||
|
||||
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
|
||||
double dt = current_time - prev_update_time;
|
||||
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 Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::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::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 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;
|
||||
}
|
||||
|
||||
|
||||
Localizer::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 Localizer::handle_log(const unsigned char* msg_dat, size_t msg_size) {
|
||||
const kj::ArrayPtr<const capnp::word> view((const capnp::word*)msg_dat, msg_size);
|
||||
capnp::FlatArrayMessageReader msg(view);
|
||||
cereal::Event::Reader event = msg.getRoot<cereal::Event>();
|
||||
double current_time = event.getLogMonoTime() / 1.0e9;
|
||||
|
||||
if (prev_update_time < 0) {
|
||||
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<cereal::SensorEventData>::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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
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<const capnp::word> view((const capnp::word*)msg_dat, msg_size);
|
||||
capnp::FlatArrayMessageReader msg(view);
|
||||
cereal::Event::Reader event = msg.getRoot<cereal::Event>();
|
||||
double current_time = event.getLogMonoTime() / 1.0e9;
|
||||
|
||||
if (prev_update_time < 0) {
|
||||
prev_update_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;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
// make copy due to alignment issues
|
||||
auto amsg = kj::heapArray<capnp::word>((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<cereal::CarParams>();
|
||||
|
||||
// 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<capnp::word>((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<cereal::Event>();
|
||||
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;
|
||||
return type;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,34 @@
|
|||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#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<double, 1, 2> C_posenet;
|
||||
Eigen::Matrix<double, 1, 2> C_gyro;
|
||||
|
||||
double R_gyro;
|
||||
|
||||
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
|
||||
void handle_sensor_events(capnp::List<cereal::SensorEventData>::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);
|
||||
};
|
||||
|
|
@ -2,6 +2,8 @@
|
|||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
#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<capnp::word>((len / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), params, len);
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,174 @@
|
|||
#include <czmq.h>
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
|
||||
#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<capnp::word>((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<cereal::CarParams>();
|
||||
|
||||
// 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<capnp::word>((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<cereal::Event>();
|
||||
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;
|
||||
}
|
||||
|
|
@ -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()
|
||||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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():
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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())
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ except subprocess.CalledProcessError:
|
|||
dirty = True
|
||||
|
||||
training_version = "0.1.0"
|
||||
terms_version = "2"
|
||||
|
||||
if __name__ == "__main__":
|
||||
print("Dirty: %s" % dirty)
|
||||
|
|
|
|||
|
|
@ -135,7 +135,14 @@ void poly_fit(float *in_pts, float *in_stds, float *out) {
|
|||
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> lhs = vander.array().colwise() / std.array();
|
||||
Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();
|
||||
|
||||
// Improve numerical stability
|
||||
Eigen::Matrix<float, POLYFIT_DEGREE, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
|
||||
lhs = lhs * scale.asDiagonal();
|
||||
|
||||
// Solve inplace
|
||||
Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
|
||||
p = qr.solve(rhs);
|
||||
|
||||
// Apply scale to output
|
||||
p = p.transpose() * scale.asDiagonal();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue