mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.6.2 release 

old-commit-hash: e90c41c576
This commit is contained in:
Vehicle Researcher 2019-07-30 02:27:48 +00:00
parent b059840baf
commit 3835061760
56 changed files with 845 additions and 1084 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Pipfile
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6f453d0dd767bc785836bf53d1ba3f4175188b4ca7b9fdcf8d146bb28c0cafb6
size 2620
oid sha256:9c487f33783e0fc4a6aee30c18c5e5b5391dca8cf360c531a69d46baa5f5a922
size 2668

4
Pipfile.lock generated
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:4e0fb1640f01f83f792d10e8957dc191edd1ff195a1188229730e08ee2198fdc
size 155198
oid sha256:795c35481c2671b9d4555fc684eb9a2b91e5cfdbc66be7a5650a7fef92b0d736
size 157287

12
RELEASES.md
View File

@ -1,3 +1,15 @@
Version 0.6.2 (2019-07-29)
========================
* New driving model!
* Improve lane tracking with double lines
* Strongly improve stationary vehicle detection
* Strongly reduce cases of braking due to false leads
* Better lead tracking around turns
* Improve cut-in prediction by using neural network
* Improve lateral control on Toyota Camry and C-HR thanks to zorrobyte!
* Fix unintended openpilot disengagements on Jeep thanks to adhintz!
* Fix delayed transition to offroad when car is turned off
Version 0.6.1 (2019-07-21)
========================
* Remote SSH with comma prime and [ssh.comma.ai](https://ssh.comma.ai)

4
apk/ai.comma.plus.offroad.apk
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:01a583ce80c2bf9e1a48c55b485cea03d6160358e0f5a528317040d30b9c0978
size 17042125
oid sha256:3bf24d5dd606d364e339c8823a5b71b0a5271716b551a187ca8a8ea457a275d0
size 17042107

253
common/kalman/ekf.py
View File

@ -1,253 +0,0 @@
# pylint: skip-file
from __future__ import print_function
import abc
import numpy as np
# The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.
# A subclass must implement:
# 1) calc_transfer_fun(); see bottom of file for more info.
# 2) __init__() to initialize self.state, self.covar, and self.process_noise appropriately
# Alternatively, the existing implementations of EKF can be used (e.g. EKF2D)
# Sensor classes are optionally used to pass measurement information into the EKF, to keep
# sensor parameters and processing methods for a each sensor together.
# Sensor classes have a read() method which takes raw sensor data and returns
# a SensorReading object, which can be passed to the EKF update() method.
# For usage, see run_ekf1d.py in selfdrive/new for a simple example.
# ekf.predict(dt) should be called between update cycles with the time since it was last called.
# Ideally, predict(dt) should be called at a relatively constant rate.
# update() should be called once per sensor, and can be called multiple times between predict steps.
# Access and set the state of the filter directly with ekf.state and ekf.covar.
class SensorReading:
# Given a perfect model and no noise, data = obs_model * state
def __init__(self, data, covar, obs_model):
self.data = data
self.obs_model = obs_model
self.covar = covar
def __repr__(self):
return "SensorReading(data={}, covar={}, obs_model={})".format(
repr(self.data), repr(self.covar), repr(self.obs_model))
# A generic sensor class that does no pre-processing of data
class SimpleSensor:
# obs_model can be
# a full observation model matrix, or
# an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
# covar can be
# a full covariance matrix
# a float or tuple of individual covars for each component of the sensor reading
# dims is the number of states in the EKF
def __init__(self, obs_model, covar, dims):
# Allow for integer covar/obs_model
if not hasattr(obs_model, "__len__"):
obs_model = (obs_model, )
if not hasattr(covar, "__len__"):
covar = (covar, )
# Full observation model passed
if dims in np.array(obs_model).shape:
self.obs_model = np.asmatrix(obs_model)
self.covar = np.asmatrix(covar)
# Indices of unit observations passed
else:
self.obs_model = np.matlib.zeros((len(obs_model), dims))
self.obs_model[:, list(obs_model)] = np.identity(len(obs_model))
if np.asarray(covar).ndim == 2:
self.covar = np.asmatrix(covar)
elif len(covar) == len(obs_model):
self.covar = np.matlib.diag(covar)
else:
self.covar = np.matlib.identity(len(obs_model)) * covar
def read(self, data, covar=None):
if covar:
self.covar = covar
return SensorReading(data, self.covar, self.obs_model)
class EKF:
__metaclass__ = abc.ABCMeta
def __init__(self, debug=False):
self.DEBUG = debug
def __str__(self):
return "EKF(state={}, covar={})".format(self.state, self.covar)
# Measurement update
# Reading should be a SensorReading object with data, covar, and obs_model attributes
def update(self, reading):
# Potential improvements:
# deal with negative covars
# add noise to really low covars to ensure stability
# use mahalanobis distance to reject outliers
# wrap angles after state updates and innovation
# y = z - H*x
innovation = reading.data - reading.obs_model * self.state
if self.DEBUG:
print("reading:\n",reading.data)
print("innovation:\n",innovation)
# S = H*P*H' + R
innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar
# K = P*H'*S^-1
kalman_gain = self.covar * reading.obs_model.T * np.linalg.inv(
innovation_covar)
if self.DEBUG:
print("gain:\n", kalman_gain)
print("innovation_covar:\n", innovation_covar)
print("innovation: ", innovation)
print("test: ", self.covar * reading.obs_model.T * (
reading.obs_model * self.covar * reading.obs_model.T + reading.covar *
0).I)
# x = x + K*y
self.state += kalman_gain*innovation
# print "covar", np.diag(self.covar)
#self.state[(roll_vel, yaw_vel, pitch_vel),:] = reading.data
# Standard form: P = (I - K*H)*P
# self.covar = (self.identity - kalman_gain*reading.obs_model) * self.covar
# Use the Joseph form for numerical stability: P = (I-K*H)*P*(I - K*H)' + K*R*K'
aux_mtrx = (self.identity - kalman_gain * reading.obs_model)
self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T
if self.DEBUG:
print("After update")
print("state\n", self.state)
print("covar:\n",self.covar)
def update_scalar(self, reading):
# like update but knowing that measurement is a scalar
# this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
# innovation = reading.data - np.matmul(reading.obs_model, self.state)
# innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
# kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
# self.state += np.matmul(kalman_gain, innovation)
# aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
# self.covar = np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
# written without np.matmul
es = np.einsum
ABC_T = "ij,jk,lk->il"
AB_T = "ij,kj->ik"
AB = "ij,jk->ik"
innovation = reading.data - es(AB, reading.obs_model, self.state)
innovation_covar = es(ABC_T, reading.obs_model, self.covar,
reading.obs_model) + reading.covar
kalman_gain = es(AB_T, self.covar, reading.obs_model) / innovation_covar
self.state += es(AB, kalman_gain, innovation)
aux_mtrx = self.identity - es(AB, kalman_gain, reading.obs_model)
self.covar = es(ABC_T, aux_mtrx, self.covar, aux_mtrx) + \
es(ABC_T, kalman_gain, reading.covar, kalman_gain)
# Prediction update
def predict(self, dt):
es = np.einsum
ABC_T = "ij,jk,lk->il"
AB = "ij,jk->ik"
# State update
transfer_fun, transfer_fun_jacobian = self.calc_transfer_fun(dt)
# self.state = np.matmul(transfer_fun, self.state)
# self.covar = np.matmul(np.matmul(transfer_fun_jacobian, self.covar), transfer_fun_jacobian.T) + self.process_noise * dt
# x = f(x, u), written in the form x = A(x, u)*x
self.state = es(AB, transfer_fun, self.state)
# P = J*P*J' + Q
self.covar = es(ABC_T, transfer_fun_jacobian, self.covar,
transfer_fun_jacobian) + self.process_noise * dt #!dt
#! Clip covariance to avoid explosions
self.covar = np.clip(self.covar,-1e10,1e10)
@abc.abstractmethod
def calc_transfer_fun(self, dt):
"""Return a tuple with the transfer function and transfer function jacobian
The transfer function and jacobian should both be a numpy matrix of size DIMSxDIMS
The transfer function matrix A should satisfy the state-update equation
x_(k+1) = A * x_k
The jacobian J is the direct jacobian A*x_k. For linear systems J=A.
Current implementations calculate A and J as functions of state. Control input
can be added trivially by adding a control parameter to predict() and calc_tranfer_update(),
and using it during calculation of A and J
"""
class FastEKF1D(EKF):
"""Fast version of EKF for 1D problems with scalar readings."""
def __init__(self, dt, var_init, Q):
super(FastEKF1D, self).__init__(False)
self.state = [0, 0]
self.covar = [var_init, var_init, 0]
# Process Noise
self.dtQ0 = dt * Q[0]
self.dtQ1 = dt * Q[1]
def update(self, reading):
raise NotImplementedError
def update_scalar(self, reading):
# TODO(mgraczyk): Delete this for speed.
# assert np.all(reading.obs_model == [1, 0])
rcov = reading.covar[0, 0]
x = self.state
S = self.covar
innovation = reading.data - x[0]
innovation_covar = S[0] + rcov
k0 = S[0] / innovation_covar
k1 = S[2] / innovation_covar
x[0] += k0 * innovation
x[1] += k1 * innovation
mk = 1 - k0
S[1] += k1 * (k1 * (S[0] + rcov) - 2 * S[2])
S[2] = mk * (S[2] - k1 * S[0]) + rcov * k0 * k1
S[0] = mk * mk * S[0] + rcov * k0 * k0
def predict(self, dt):
# State update
x = self.state
x[0] += dt * x[1]
# P = J*P*J' + Q
S = self.covar
S[0] += dt * (2 * S[2] + dt * S[1]) + self.dtQ0
S[2] += dt * S[1]
S[1] += self.dtQ1
# Clip covariance to avoid explosions
S = max(-1e10, min(S, 1e10))
def calc_transfer_fun(self, dt):
tf = np.identity(2)
tf[0, 1] = dt
tfj = tf
return tf, tfj

116
common/kalman/tests/test_ekf.py
View File

@ -1,116 +0,0 @@
import numpy as np
import numpy.matlib
import unittest
import timeit
from common.kalman.ekf import EKF, SimpleSensor, FastEKF1D
class TestEKF(EKF):
def __init__(self, var_init, Q):
super(TestEKF, self).__init__(False)
self.identity = numpy.matlib.identity(2)
self.state = numpy.matlib.zeros((2, 1))
self.covar = self.identity * var_init
self.process_noise = numpy.matlib.diag(Q)
def calc_transfer_fun(self, dt):
tf = numpy.matlib.identity(2)
tf[0, 1] = dt
return tf, tf
class EKFTest(unittest.TestCase):
def test_update_scalar(self):
ekf = TestEKF(1e3, [0.1, 1])
dt = 1. / 100
sensor = SimpleSensor(0, 1, 2)
readings = map(sensor.read, np.arange(100, 300))
for reading in readings:
ekf.update_scalar(reading)
ekf.predict(dt)
np.testing.assert_allclose(ekf.state, [[300], [100]], 1e-4)
np.testing.assert_allclose(
ekf.covar,
np.asarray([[0.0563, 0.10278], [0.10278, 0.55779]]),
atol=1e-4)
def test_unbiased(self):
ekf = TestEKF(1e3, [0., 0.])
dt = np.float64(1. / 100)
sensor = SimpleSensor(0, 1, 2)
readings = map(sensor.read, np.arange(1000))
for reading in readings:
ekf.update_scalar(reading)
ekf.predict(dt)
np.testing.assert_allclose(ekf.state, [[1000.], [100.]], 1e-4)
class FastEKF1DTest(unittest.TestCase):
def test_correctness(self):
dt = 1. / 100
reading = SimpleSensor(0, 1, 2).read(100)
ekf = TestEKF(1e3, [0.1, 1])
fast_ekf = FastEKF1D(dt, 1e3, [0.1, 1])
ekf.update_scalar(reading)
fast_ekf.update_scalar(reading)
self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0])
self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1])
self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0])
self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2])
self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1])
ekf.predict(dt)
fast_ekf.predict(dt)
self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0])
self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1])
self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0])
self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2])
self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1])
def test_speed(self):
setup = """
import numpy as np
from common.kalman.tests.test_ekf import TestEKF
from common.kalman.ekf import SimpleSensor, FastEKF1D
dt = 1. / 100
reading = SimpleSensor(0, 1, 2).read(100)
var_init, Q = 1e3, [0.1, 1]
ekf = TestEKF(var_init, Q)
fast_ekf = FastEKF1D(dt, var_init, Q)
"""
timeit.timeit("""
ekf.update_scalar(reading)
ekf.predict(dt)
""", setup=setup, number=1000)
ekf_speed = timeit.timeit("""
ekf.update_scalar(reading)
ekf.predict(dt)
""", setup=setup, number=20000)
timeit.timeit("""
fast_ekf.update_scalar(reading)
fast_ekf.predict(dt)
""", setup=setup, number=1000)
fast_ekf_speed = timeit.timeit("""
fast_ekf.update_scalar(reading)
fast_ekf.predict(dt)
""", setup=setup, number=20000)
assert fast_ekf_speed < ekf_speed / 4
if __name__ == "__main__":
unittest.main()

2
common/params.py
View File

@ -53,6 +53,7 @@ keys = {
"AthenadPid": [TxType.PERSISTENT],
"CalibrationParams": [TxType.PERSISTENT],
"CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CompletedTrainingVersion": [TxType.PERSISTENT],
"ControlsParams": [TxType.PERSISTENT],
"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
@ -70,6 +71,7 @@ keys = {
"IsUploadRawEnabled": [TxType.PERSISTENT],
"IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT],
"LimitSetSpeed": [TxType.PERSISTENT],
"LimitSetSpeedNeural": [TxType.PERSISTENT],
"LiveParameters": [TxType.PERSISTENT],
"LongitudinalControl": [TxType.PERSISTENT],
"Passive": [TxType.PERSISTENT],

1
common/realtime.py
View File

@ -20,6 +20,7 @@ DT_CTRL = 0.01 # controlsd
DT_PLAN = 0.05 # mpc
DT_MDL = 0.05 # model
DT_DMON = 0.1 # driver monitoring
DT_TRML = 0.5 # thermald and manager
ffi = FFI()

130
common/vin.py
View File

@ -1,61 +1,8 @@
#!/usr/bin/env python
import time
from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.boardd.boardd import can_list_to_can_capnp
def get_vin(logcan, sendcan):
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
cnts = [1, 2] # Number of messages to wait for at each iteration
vin_valid = True
dat = []
for i in range(len(query_msg)):
cnt = 0
sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan'))
got_response = False
t_start = sec_since_boot()
while sec_since_boot() - t_start < 0.05 and not got_response:
for a in messaging.drain_sock(logcan):
for can in a.can:
if can.src == 0 and can.address == 0x7e8:
vin_valid = vin_valid and is_vin_response_valid(can.dat, i, cnt)
dat += can.dat[2:] if i == 0 else can.dat[1:]
cnt += 1
if cnt == cnts[i]:
got_response = True
time.sleep(0.01)
return "".join(dat[3:]) if vin_valid else ""
"""
if 'vin' not in gctx:
print "getting vin"
gctx['vin'] = query_vin()[3:]
print "got VIN %s" % (gctx['vin'],)
cloudlog.info("got VIN %s" % (gctx['vin'],))
# *** determine platform based on VIN ****
if vin.startswith("19UDE2F36G"):
print "ACURA ILX 2016"
self.civic = False
else:
# TODO: add Honda check explicitly
print "HONDA CIVIC 2016"
self.civic = True
# *** special case VIN of Acura test platform
if vin == "19UDE2F36GA001322":
print "comma.ai test platform detected"
# it has a gas interceptor and a torque mod
self.torque_mod = True
"""
VIN_UNKNOWN = "0" * 17
# sanity checks on response messages from vin query
def is_vin_response_valid(can_dat, step, cnt):
@ -84,12 +31,71 @@ def is_vin_response_valid(can_dat, step, cnt):
return True
if __name__ == "__main__":
import zmq
from selfdrive.services import service_list
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
time.sleep(1.) # give time to sendcan socket to start
class VinQuery():
def __init__(self, bus):
self.bus = bus
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
self.query_ext_msgs = [[0x18DB33F1, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus],
[0x18DA10f1, 0, '\x30'.ljust(8, "\x00"), bus]]
self.query_nor_msgs = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), bus]]
print get_vin(logcan, sendcan)
self.cnts = [1, 2] # number of messages to wait for at each iteration
self.step = 0
self.cnt = 0
self.responded = False
self.never_responded = True
self.dat = []
self.vin = VIN_UNKNOWN
def check_response(self, msg):
# have we got a VIN query response?
if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]:
self.never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(msg.dat, self.step, self.cnt):
self.dat += msg.dat[2:] if self.step == 0 else msg.dat[1:]
self.cnt += 1
if self.cnt == self.cnts[self.step]:
self.responded = True
self.step += 1
def send_query(self, sendcan):
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
if self.never_responded or (self.responded and self.step < len(self.cnts)):
sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan'))
sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan'))
self.responded = False
self.cnt = 0
def get_vin(self):
# only report vin if procedure is finished
if self.step == len(self.cnts) and self.cnt == self.cnts[-1]:
self.vin = "".join(self.dat[3:])
return self.vin
def get_vin(logcan, sendcan, bus, query_time=1.):
vin_query = VinQuery(bus)
frame = 0
# 1s max of VIN query time
while frame < query_time * 100:
a = messaging.recv_one(logcan)
for can in a.can:
vin_query.check_response(can)
vin_query.send_query(sendcan)
frame += 1
return vin_query.get_vin()
if __name__ == "__main__":
from selfdrive.services import service_list
logcan = messaging.sub_sock(service_list['can'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
print get_vin(logcan, sendcan, 0)

4
models/driving_model.dlc
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:46b4433f49e6d54ced1049e03aed650f6cb6de6d243ae64167945150935d7cd8
size 12039472
oid sha256:32aef4d710c994e0c8a019b64eb2bd495f493be0dc0daa9f312f7dfeeb7f1568
size 14761531

4
models/monitoring_model.dlc
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f681047cee0e546c1438aecc99f3039456b4044f50e74745171a6aa0fab6503e
size 428211
oid sha256:eb047afc34b4b5b9888dc1dfc796d4f83fc695734decac5f6a6057a70e468623
size 544528

66
selfdrive/boardd/boardd.cc
View File

@ -59,7 +59,8 @@ pthread_mutex_t usb_lock;
bool spoofing_started = false;
bool fake_send = false;
bool loopback_can = false;
bool is_grey_panda = false;
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
bool is_pigeon = false;
pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
@ -69,6 +70,29 @@ void pigeon_init();
void *pigeon_thread(void *crap);
void *safety_setter_thread(void *s) {
char *value_vin;
size_t value_vin_sz = 0;
// switch to no_output when CarVin param is read
while (1) {
if (do_exit) return NULL;
const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz);
if (value_vin_sz > 0) {
// sanity check VIN format
assert(value_vin_sz == 17);
break;
}
usleep(100*1000);
}
LOGW("got CarVin %s", value_vin);
pthread_mutex_lock(&usb_lock);
// VIN qury done, stop listening to OBDII
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
char *value;
size_t value_sz = 0;
@ -151,7 +175,7 @@ void *safety_setter_thread(void *s) {
// must be called before threads or with mutex
bool usb_connect() {
int err;
unsigned char is_pigeon[1] = {0};
unsigned char hw_query[1] = {0};
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { goto fail; }
@ -184,11 +208,12 @@ bool usb_connect() {
assert(err == 0);
}
libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, is_pigeon, 1, TIMEOUT);
libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT);
if (is_pigeon[0]) {
LOGW("grey panda detected");
is_grey_panda = true;
hw_type = (cereal::HealthData::HwType)(hw_query[0]);
is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA);
if (is_pigeon) {
LOGW("panda with gps detected");
pigeon_needs_init = true;
if (pigeon_thread_handle == -1) {
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
@ -280,12 +305,13 @@ void can_health(void *s) {
struct __attribute__((packed)) health {
uint32_t voltage;
uint32_t current;
uint8_t started;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
uint32_t can_send_errs;
uint32_t can_fwd_errs;
uint32_t gmlan_send_errs;
uint8_t started;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
uint8_t car_harness_status_pkt;
} health;
// recv from board
@ -293,7 +319,9 @@ void can_health(void *s) {
do {
cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT);
if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); }
if (cnt != sizeof(health)) {
handle_usb_issue(cnt, __func__);
}
} while(cnt != sizeof(health));
pthread_mutex_unlock(&usb_lock);
@ -314,15 +342,23 @@ void can_health(void *s) {
}
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
healthData.setIsGreyPanda(is_grey_panda);
healthData.setHasGps(is_pigeon);
healthData.setCanSendErrs(health.can_send_errs);
healthData.setCanFwdErrs(health.can_fwd_errs);
healthData.setGmlanSendErrs(health.gmlan_send_errs);
healthData.setHwType(hw_type);
// send to health
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
pthread_mutex_lock(&usb_lock);
// send heartbeat back to panda
libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
}
@ -447,10 +483,10 @@ void *can_health_thread(void *crap) {
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8011");
// run at 1hz
// run at 2hz
while (!do_exit) {
can_health(publisher);
usleep(1000*1000);
usleep(500*1000);
}
return NULL;
}
@ -502,7 +538,7 @@ void pigeon_set_baud(int baud) {
void pigeon_init() {
usleep(1000*1000);
LOGW("grey panda start");
LOGW("panda GPS start");
// power off pigeon
pigeon_set_power(0);
@ -543,7 +579,7 @@ void pigeon_init() {
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
LOGW("grey panda is ready to fly");
LOGW("panda GPS on");
}
static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {

21
selfdrive/can/tests/test_packer_honda.py
View File

@ -16,6 +16,9 @@ class TestPackerMethods(unittest.TestCase):
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
is_panda_black = False
car_fingerprint = HONDA_BOSCH[0]
apply_brake = (random.randint(0, 2) % 2 == 0)
pump_on = (random.randint(0, 2) % 2 == 0)
pcm_override = (random.randint(0, 2) % 2 == 0)
@ -23,33 +26,31 @@ class TestPackerMethods(unittest.TestCase):
chime = random.randint(0, 65536)
fcw = random.randint(0, 65536)
idx = random.randint(0, 65536)
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
lkas_active = (random.randint(0, 2) % 2 == 0)
car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx)
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx)
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
self.assertEqual(m_old, m)
pcm_speed = random.randint(0, 65536)
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536),
random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
is_metric = (random.randint(0, 2) % 2 == 0)
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx)
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx)
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
self.assertEqual(m_old, m)
button_val = random.randint(0, 65536)
idx = random.randint(0, 65536)
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx)
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx)
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black)
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black)
self.assertEqual(m_old, m)

125
selfdrive/car/car_helpers.py
View File

@ -1,8 +1,9 @@
import os
from common.vin import is_vin_response_valid
from cereal import car
from common.params import Params
from common.vin import get_vin, VIN_UNKNOWN
from common.basedir import BASEDIR
from common.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
@ -51,107 +52,85 @@ def _get_interface_names():
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)
return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
# **** for use live only ****
def fingerprint(logcan, sendcan):
def fingerprint(logcan, sendcan, is_panda_black):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None, "")
finger = {0: {}, 2:{}} # collect on bus 0 or 2
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
can_seen_frame = None
can_seen = False
params = Params()
car_params = params.get("CarParams")
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
if car_params is not None:
# use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode
car_params = car.CarParams.from_bytes(car_params)
vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin
elif is_panda_black:
# Vin query only reliably works thorugh OBDII
vin = get_vin(logcan, sendcan, 1)
else:
vin = VIN_UNKNOWN
vin_cnts = [1, 2] # number of messages to wait for at each iteration
vin_step = 0
vin_cnt = 0
vin_responded = False
vin_never_responded = True
vin_dat = []
vin = ""
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
finger = {i: {} for i in range(0, 4)} # collect on all buses
candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
frame = 0
frame_fingerprint = 10 # 0.1s
car_fingerprint = None
done = False
while True:
while not done:
a = messaging.recv_one(logcan)
for can in a.can:
can_seen = True
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
# 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.
# need to independently try to fingerprint both bus 0 and 1 to work
# for the combo black_panda and honda_bosch. Ignore extended messages
# and VIN query response.
# 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)
for b in candidate_cars:
if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
finger[can.src][can.address] = len(can.dat)
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
if can_seen_frame is None and can_seen:
can_seen_frame = frame
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
# Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
if only_toyota_left(candidate_cars[b]):
frame_fingerprint = 100 # 1s
if len(candidate_cars[b]) == 1:
if frame > frame_fingerprint:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# if we only have one car choice and the time_fingerprint since we got our first
# 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 only_toyota_left(candidate_cars) else 0.1
if (frame - can_seen_frame) > (time_fingerprint * 100):
break
# bail if no cars left or we've been waiting for more than 2s since can_seen
elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200):
return None, finger, ""
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
# TODO: VIN query temporarily disabled until we have the harness
if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan'))
vin_responded = False
vin_cnt = 0
# bail if no cars left or we've been waiting for more than 2s
failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
# only report vin if procedure is finished
if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
vin = "".join(vin_dat[3:])
cloudlog.warning("fingerprinted %s", candidate_cars[0])
cloudlog.warning("VIN %s", vin)
return candidate_cars[0], finger, vin
cloudlog.warning("fingerprinted %s", car_fingerprint)
return car_fingerprint, finger, vin
def get_car(logcan, sendcan):
def get_car(logcan, sendcan, is_panda_black=False):
candidate, fingerprints, vin = fingerprint(logcan, sendcan)
candidate, fingerprints, vin = fingerprint(logcan, sendcan, is_panda_black)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
candidate = "mock"
CarInterface, CarController = interfaces[candidate]
params = CarInterface.get_params(candidate, fingerprints[0], vin)
car_params = CarInterface.get_params(candidate, fingerprints[0], vin, is_panda_black)
return CarInterface(params, CarController), params
return CarInterface(car_params, CarController), car_params

4
selfdrive/car/chrysler/carstate.py
View File

@ -27,7 +27,7 @@ def get_can_parser(CP):
("DOOR_OPEN_RL", "DOORS", 0),
("DOOR_OPEN_RR", "DOORS", 0),
("BRAKE_PRESSED_2", "BRAKE_2", 0),
("ACCEL_PEDAL", "ACCEL_PEDAL_MSG", 0),
("ACCEL_134", "ACCEL_GAS_134", 0),
("SPEED_LEFT", "SPEED_1", 0),
("SPEED_RIGHT", "SPEED_1", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
@ -112,7 +112,7 @@ class CarState(object):
self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
self.pedal_gas = cp.vl["ACCEL_PEDAL_MSG"]['ACCEL_PEDAL']
self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
self.car_gas = self.pedal_gas
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)

5
selfdrive/car/chrysler/interface.py
View File

@ -38,13 +38,14 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.chrysler
@ -94,7 +95,7 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
print("ECU Camera Simulated: {0}".format(ret.enableCamera))
ret.openpilotLongitudinalControl = False

2
selfdrive/car/chrysler/values.py
View File

@ -44,7 +44,7 @@ FINGERPRINTS = {
],
CAR.JEEP_CHEROKEE: [
# JEEP GRAND CHEROKEE V6 2018
{55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
{55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
# Jeep Grand Cherokee 2017 Trailhawk
{257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
],

5
selfdrive/car/ford/interface.py
View File

@ -38,13 +38,14 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.ford
@ -87,7 +88,7 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint)
ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = False
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)

5
selfdrive/car/gm/interface.py
View File

@ -46,19 +46,20 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "gm"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.enableCruise = False
# Presence of a camera on the object bus is ok.
# Have to go to read_only if ASCM is online (ACC-enabled cars),
# or camera is on powertrain bus (LKA cars without ACC).
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = ret.enableCamera
tire_stiffness_factor = 0.444 # not optimized yet

10
selfdrive/car/honda/carcontroller.py
View File

@ -149,19 +149,19 @@ class CarController(object):
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint, idx))
lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx))
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx))
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
elif CS.stopped:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx))
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
@ -170,7 +170,7 @@ class CarController(object):
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:

8
selfdrive/car/honda/carstate.py
View File

@ -154,7 +154,8 @@ 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)
bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt)
def get_cam_can_parser(CP):
@ -165,9 +166,8 @@ def get_cam_can_parser(CP):
if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]:
checks = [(0x194, 100)]
cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
class CarState(object):
def __init__(self, CP):

40
selfdrive/car/honda/hondacan.py
View File

@ -19,7 +19,15 @@ def fix(msg, addr):
return msg2
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
def get_pt_bus(car_fingerprint, is_panda_black):
return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0
def get_lkas_cmd_bus(car_fingerprint, is_panda_black):
return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black):
# TODO: do we loose pressure if we keep pump off for long?
brakelights = apply_brake > 0
brake_rq = apply_brake > 0
@ -38,27 +46,25 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
# TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
"FCW": fcw << 1,
}
return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
bus = get_pt_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
"STEER_TORQUE_REQUEST": lkas_active,
}
# Set bus 2 for accord and new crv.
bus = 2 if car_fingerprint in HONDA_BOSCH else 0
bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black):
commands = []
bus = 0
bus_pt = get_pt_bus(car_fingerprint, is_panda_black)
bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
# Bosch sends commands to bus 2.
if car_fingerprint in HONDA_BOSCH:
bus = 2
else:
if car_fingerprint not in HONDA_BOSCH:
acc_hud_values = {
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
'PCM_GAS': hud.pcm_accel,
@ -70,7 +76,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
'SET_ME_X01_2': 1,
'SET_ME_X01': 1,
}
commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
lkas_hud_values = {
'SET_ME_X41': 0x41,
@ -79,23 +85,23 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
'SOLID_LANES': hud.lanes,
'BEEP': hud.beep,
}
commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values, idx))
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY):
radar_hud_values = {
'ACC_ALERTS': hud.acc_alert,
'LEAD_SPEED': 0x1fe, # What are these magic values
'LEAD_STATE': 0x7,
'LEAD_DISTANCE': 0x1e,
}
commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx))
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
return commands
def spam_buttons_command(packer, button_val, idx):
def spam_buttons_command(packer, button_val, idx, car_fingerprint, is_panda_black):
values = {
'CRUISE_BUTTONS': button_val,
'CRUISE_SETTING': 0,
}
return packer.make_can_msg("SCM_BUTTONS", 0, values, idx)
bus = get_pt_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("SCM_BUTTONS", bus, values, idx)

13
selfdrive/car/honda/interface.py
View File

@ -129,12 +129,13 @@ class CarInterface(object):
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "honda"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
if candidate in HONDA_BOSCH:
ret.safetyModel = car.CarParams.SafetyModel.hondaBosch
@ -143,7 +144,7 @@ class CarInterface(object):
ret.openpilotLongitudinalControl = False
else:
ret.safetyModel = car.CarParams.SafetyModel.honda
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
ret.enableGasInterceptor = 0x201 in fingerprint
ret.openpilotLongitudinalControl = ret.enableCamera
@ -167,7 +168,7 @@ class CarInterface(object):
ret.mass = CivicParams.MASS
ret.wheelbase = CivicParams.WHEELBASE
ret.centerToFront = CivicParams.CENTER_TO_FRONT
ret.steerRatio = 14.63 # 10.93 is end-to-end spec
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
tire_stiffness_factor = 1.
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
@ -187,7 +188,7 @@ class CarInterface(object):
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.83
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 15.96 # 11.82 is spec end-to-end
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
tire_stiffness_factor = 0.8467
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
@ -213,7 +214,7 @@ class CarInterface(object):
ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.3 # as spec
ret.steerRatio = 16.89 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
@ -293,7 +294,7 @@ class CarInterface(object):
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
ret.wheelbase = 2.82
ret.centerToFront = ret.wheelbase * 0.428 # average weight distribution
ret.steerRatio = 16.0 # as spec
ret.steerRatio = 17.25 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]

9
selfdrive/car/honda/values.py
View File

@ -73,6 +73,9 @@ class CAR:
PILOT_2019 = "HONDA PILOT 2019 ELITE"
RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION"
# diag message that in some Nidec cars only appear with 1s freq if VIN query is performed
DIAG_MSGS = {1600: 5, 1601: 8}
FINGERPRINTS = {
CAR.ACCORD: [{
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
@ -139,6 +142,12 @@ FINGERPRINTS = {
}]
}
# add DIAG_MSGS to fingerprints
for c in FINGERPRINTS:
for f, _ in enumerate(FINGERPRINTS[c]):
for d in DIAG_MSGS:
FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),

5
selfdrive/car/hyundai/interface.py
View File

@ -40,13 +40,14 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
@ -141,7 +142,7 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = False
ret.steerLimitAlert = False

7
selfdrive/car/mock/interface.py
View File

@ -4,7 +4,6 @@ from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
from common.realtime import Ratekeeper
# mocked car interface to work with chffrplus
TS = 0.01 # 100Hz
@ -30,8 +29,6 @@ class CarInterface(object):
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
self.rk = Ratekeeper(100, print_delay_threshold=2. / 1000)
@staticmethod
def compute_gb(accel, speed):
return accel
@ -41,7 +38,7 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
@ -81,8 +78,6 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
self.rk.keep_time()
# get basic data from phone and gps since CAN isn't connected
sensors = messaging.recv_sock(self.sensor)
if sensors is not None:

3
selfdrive/car/subaru/interface.py
View File

@ -38,12 +38,13 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.subaru
ret.enableCruise = True

17
selfdrive/car/toyota/carstate.py
View File

@ -3,7 +3,7 @@ from common.kalman.simple_kalman import KF1D
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
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
def parse_gear_shifter(gear, vals):
@ -19,6 +19,7 @@ def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
("GAS_PEDAL", "GAS_PEDAL", 0),
@ -59,10 +60,8 @@ def get_can_parser(CP):
("EPS_STATUS", 25),
]
if CP.carFingerprint in TSS2_CAR:
if CP.carFingerprint in NO_DSU_CAR:
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
else:
signals += [("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0)]
if CP.carFingerprint == CAR.PRIUS:
signals += [("STATE", "AUTOPARK_STATUS", 0)]
@ -94,6 +93,8 @@ class CarState(object):
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.angle_offset = 0.
self.init_angle_offset = False
# initialize can parser
self.car_fingerprint = CP.carFingerprint
@ -144,6 +145,14 @@ class CarState(object):
if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
elif self.CP.carFingerprint in NO_DSU_CAR:
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
# need to apply an offset as soon as the steering angle measurements are both received
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
self.init_angle_offset = True
self.angle_offset = self.angle_steers - angle_wheel
else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']

11
selfdrive/car/toyota/interface.py
View File

@ -40,13 +40,14 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint, vin=""):
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.toyota
@ -62,7 +63,7 @@ class CarInterface(object):
stop_and_go = True
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70
ret.steerRatio = 15.00 # unknown end-to-end spec
ret.steerRatio = 15.74 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
@ -78,7 +79,7 @@ class CarInterface(object):
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.safetyParam = 73
ret.wheelbase = 2.65
ret.steerRatio = 16.30 # 14.5 is spec end-to-end
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
tire_stiffness_factor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
@ -88,7 +89,7 @@ class CarInterface(object):
stop_and_go = False
ret.safetyParam = 100
ret.wheelbase = 2.70
ret.steerRatio = 17.8
ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
@ -213,7 +214,7 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu

9
selfdrive/common/modeldata.h
View File

@ -1,8 +1,9 @@
#ifndef MODELDATA_H
#define MODELDATA_H
#define MODEL_PATH_DISTANCE 100
#define MODEL_PATH_DISTANCE 192
#define POLYFIT_DEGREE 4
#define SPEED_PERCENTILES 10
typedef struct PathData {
float points[MODEL_PATH_DISTANCE];
@ -16,8 +17,12 @@ typedef struct LeadData {
float dist;
float prob;
float std;
float rel_y;
float rel_y_std;
float rel_v;
float rel_v_std;
float rel_a;
float rel_a_std;
} LeadData;
typedef struct ModelData {
@ -25,6 +30,8 @@ typedef struct ModelData {
PathData left_lane;
PathData right_lane;
LeadData lead;
LeadData lead_future;
float speed[SPEED_PERCENTILES];
} ModelData;
#endif

3
selfdrive/common/params.cc
View File

@ -184,6 +184,9 @@ int read_db_all(const char* params_path, std::map<std::string, std::string> *par
while ((de = readdir(d))) {
if (!isalnum(de->d_name[0])) continue;
std::string key = std::string(de->d_name);
if (key == "AccessToken") continue;
std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str()));
(*params)[key] = value;

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.6.1-release"
#define COMMA_VERSION "0.6.2-release"

32
selfdrive/controls/controlsd.py
View File

@ -48,6 +48,10 @@ def events_to_bytes(events):
ret.append(e.to_bytes())
return ret
def wait_for_can(logcan):
print("Waiting for CAN messages...")
while len(messaging.recv_one(logcan).can) == 0:
pass
def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params):
@ -358,7 +362,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"angleModelBias": 0.,
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
"decelForTurn": sm['plan'].decelForTurn,
"decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": int(start_time * 1e9),
"mapValid": sm['plan'].mapValid,
@ -412,7 +416,6 @@ 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)
@ -427,15 +430,18 @@ def controlsd_thread(gctx=None):
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
logcan = messaging.sub_sock(service_list['can'].port)
CI, CP = get_car(logcan, sendcan)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
wait_for_can(logcan)
CI, CP = get_car(logcan, sendcan, is_panda_black)
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()
AM = AlertManager()
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
@ -443,6 +449,13 @@ def controlsd_thread(gctx=None):
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
@ -456,10 +469,6 @@ def controlsd_thread(gctx=None):
driver_status = DriverStatus()
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
@ -473,6 +482,7 @@ def controlsd_thread(gctx=None):
events_prev = []
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
@ -498,6 +508,8 @@ def controlsd_thread(gctx=None):
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['plan'].radarValid:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:

14
selfdrive/controls/lib/alerts.py
View File

@ -383,6 +383,13 @@ ALERTS = [
AlertStatus.critical, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
Alert(
"posenetInvalid",
"TAKE CONTROL IMMEDIATELY",
"Vision Failure: Check Camera View",
AlertStatus.critical, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
# Cancellation alerts causing immediate disabling
Alert(
"controlsFailed",
@ -540,6 +547,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"posenetInvalidNoEntry",
"openpilot Unavailable",
"Vision Failure: Check Camera View",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"controlsFailedNoEntry",
"openpilot Unavailable",

28
selfdrive/controls/lib/driver_monitor.py
View File

@ -10,6 +10,7 @@ _DISTRACTED_TIME = 7.
_DISTRACTED_PRE_TIME = 4.
_DISTRACTED_PROMPT_TIME = 2.
# model output refers to center of cropped image, so need to apply the x displacement offset
_FACE_THRESHOLD = 0.4
_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
@ -23,16 +24,15 @@ MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
RESIZED_FOCAL = 320.0
H, W, FULL_W = 320, 160, 426
def head_orientation_from_descriptor(desc):
def head_orientation_from_descriptor(angles_desc, pos_desc):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
# TODO this should be calibrated
pitch_prnet = desc[0]
yaw_prnet = desc[1]
roll_prnet = desc[2]
pitch_prnet = angles_desc[0]
yaw_prnet = angles_desc[1]
roll_prnet = angles_desc[2]
face_pixel_position = ((desc[3] + .5)*W - W + FULL_W, (desc[4]+.5)*H)
face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL)
pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL)
@ -96,24 +96,22 @@ class DriverStatus():
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
pitch_error *= _PITCH_WEIGHT
metric = np.sqrt(yaw_error**2 + pitch_error**2)
#print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric
# TODO: do something with the eye states and combine them with head pose
return 1 if metric > _METRIC_THRESHOLD else 0
def get_pose(self, driver_monitoring, params):
if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
return
self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.descriptor)
# TODO: DM data should not be in a list if they are not homogeneous
if len(driver_monitoring.descriptor) > 6:
self.face_detected = driver_monitoring.descriptor[6] > 0.
else:
self.face_detected = True
self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition)
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
self.driver_distracted = self._is_driver_distracted(self.pose)
# first order filters
self.driver_distraction_filter.update(self.driver_distracted)
self.variance_high = driver_monitoring.std > _STD_THRESHOLD
self.variance_high = False #driver_monitoring.std > _STD_THRESHOLD
self.variance_filter.update(self.variance_high)
monitor_param_on_prev = self.monitor_param_on

1
selfdrive/controls/lib/pathplanner.py
View File

@ -126,6 +126,7 @@ class PathPlanner(object):
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
self.plan.send(plan_send.to_bytes())

84
selfdrive/controls/lib/planner.py
View File

@ -3,6 +3,7 @@ import math
import numpy as np
from common.params import Params
from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
import selfdrive.messaging as messaging
from cereal import car
@ -15,7 +16,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.fcw import FCWChecker
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
MAX_SPEED = 255.0
LON_MPC_STEP = 0.2 # first step is 0.2s
MAX_SPEED_ERROR = 2.0
@ -37,6 +38,16 @@ _A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.]
# Model speed kalman stuff
_MODEL_V_A = [[1.0, DT_PLAN], [0.0, 1.0]]
_MODEL_V_C = [1.0, 0]
# calculated with observation std of 2m/s and accel proc noise of 2m/s**2
_MODEL_V_K = [[0.07068858], [0.04826294]]
# 75th percentile
SPEED_PERCENTILE_IDX = 7
def calc_cruise_accel_limits(v_ego, following):
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
@ -57,8 +68,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
a_target[1] = min(a_target[1], a_x_allowed)
return a_target
return [a_target[0], min(a_target[1], a_x_allowed)]
class Planner(object):
@ -79,16 +89,21 @@ class Planner(object):
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
self.model_v_kf_ready = False
self.params = Params()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
solutions = {'cruise': self.v_cruise, 'model': self.v_model}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
@ -108,10 +123,13 @@ class Planner(object):
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.v_model
self.a_acc = self.a_model
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
def update(self, sm, CP, VM, PP, live_map_data):
def update(self, sm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
@ -127,51 +145,43 @@ class Planner(object):
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
if not self.model_v_kf_ready:
self.model_v_kf.x = [[v_ego],[0.0]]
self.model_v_kf_ready = True
#map_age = cur_time - rcv_times['liveMapData']
map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0
if len(sm['model'].speed):
self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
# Speed limit and curvature
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
if set_speed_limit_active and map_valid:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
if self.params.get("LimitSetSpeedNeural") == "1":
model_speed = self.model_v_kf.x[0][0]
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
if decel_for_turn:
time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# accel and jerk up limits are higher here to make model not limiting accel
# mainly done to prevent flickering of slowdown icon
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
model_speed,
2*accel_limits[1], 3*accel_limits[0],
2*jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
@ -234,10 +244,6 @@ class Planner(object):
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = v_curvature
plan_send.plan.decelForTurn = decel_for_turn
plan_send.plan.mapValid = map_valid
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)

160
selfdrive/controls/lib/radar_helpers.py
View File

@ -1,48 +1,35 @@
import numpy as np
from common.numpy_fast import clip, interp
from common.realtime import DT_MDL
from common.kalman.simple_kalman import KF1D
from selfdrive.config import RADAR_TO_CENTER
# the longer lead decels, the more likely it will keep decelerating
# TODO is this a good default?
_LEAD_ACCEL_TAU = 1.5
NO_FUSION_SCORE = 100 # bad default fusion score
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
rate, ratev = 20., 20. # model and radar are both at 20Hz
ts = 1./rate
freq_v_lat = 0.2 # Hz
k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
freq_a_lead = .5 # Hz
k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
# stationary qualification parameters
v_stationary_thr = 4. # objects moving below this speed are classified as stationary
v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
v_ego_stationary = 4. # no stationary object flag below this speed
# Lead Kalman Filter params
_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]]
_VLEAD_C = [1.0, 0.0]
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]]
RDR_TO_LDR = 2.7
_VLEAD_K = [[0.1988689], [0.28555364]]
class Track(object):
def __init__(self):
self.ekf = None
self.stationary = True
self.initted = False
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
if self.initted:
# pylint: disable=access-member-before-definition
self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
self.vRelPrev = self.vRel
@ -52,9 +39,6 @@ class Track(object):
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
# compute distance to path
self.dPath = d_path
# computed velocity and accelerations
self.vLead = self.vRel + v_ego_t_aligned
@ -64,21 +48,8 @@ class Track(object):
self.cnt = 1
self.vision_cnt = 0
self.vision = False
self.aRel = 0. # nidec gives no information about this
self.vLat = 0.
self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
# estimate acceleration
# TODO: use Kalman filter
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
# TODO: use Kalman filter
# neglect steer override cases as dPath is too noisy
v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
self.kf.update(self.vLead)
self.cnt += 1
@ -86,33 +57,12 @@ class Track(object):
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
if self.stationary:
# stationary objects can become non stationary, but not the other way around
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr
self.vision_score = NO_FUSION_SCORE
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau = _LEAD_ACCEL_TAU
else:
self.aLeadTau *= 0.9
def update_vision_score(self, dist_to_vision, rel_speed_diff):
# rel speed is very hard to estimate from vision
if dist_to_vision < 4.0 and rel_speed_diff < 10.:
self.vision_score = dist_to_vision + rel_speed_diff
else:
self.vision_score = NO_FUSION_SCORE
def update_vision_fusion(self):
# vision point is never stationary
# don't trust 1 or 2 fusions until model quality is much better
if self.vision_cnt >= 3:
self.vision = True
self.stationary = False
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
return [self.dRel, self.yRel*2, self.vRel]
@ -171,89 +121,47 @@ class Cluster(object):
def aLeadTau(self):
return mean([t.aLeadTau for t in self.tracks])
@property
def vision(self):
return any([t.vision for t in self.tracks])
@property
def measured(self):
return any([t.measured for t in self.tracks])
@property
def vision_cnt(self):
return max([t.vision_cnt for t in self.tracks])
@property
def stationary(self):
return all([t.stationary for t in self.tracks])
@property
def oncoming(self):
return all([t.oncoming for t in self.tracks])
def toRadarState(self):
def get_RadarState(self, model_prob=0.0):
return {
"dRel": float(self.dRel) - RDR_TO_LDR,
"dRel": float(self.dRel),
"yRel": float(self.yRel),
"vRel": float(self.vRel),
"aRel": float(self.aRel),
"vLead": float(self.vLead),
"dPath": float(self.dPath),
"vLat": float(self.vLat),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"status": True,
"fcw": self.is_potential_fcw(),
"fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob,
"radar": True,
"aLeadTau": float(self.aLeadTau)
}
def get_RadarState_from_vision(self, lead_msg, v_ego):
return {
"dRel": float(lead_msg.dist - RADAR_TO_CENTER),
"yRel": float(lead_msg.relY),
"vRel": float(lead_msg.relVel),
"vLead": float(v_ego + lead_msg.relVel),
"vLeadK": float(v_ego + lead_msg.relVel),
"aLeadK": float(0),
"aLeadTau": _LEAD_ACCEL_TAU,
"fcw": False,
"modelProb": float(lead_msg.prob),
"radar": False,
"status": True
}
def __str__(self):
ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath)
if self.stationary:
ret += " stationary"
if self.vision:
ret += " vision"
if self.oncoming:
ret += " oncoming"
if self.vision_cnt > 0:
ret += " vision_cnt: %6.0f" % self.vision_cnt
ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
return ret
def is_potential_lead(self, v_ego):
# predict cut-ins by extrapolating lateral speed by a lookahead time
# lookahead time depends on cut-in distance. more attentive for close cut-ins
# also, above 50 meters the predicted path isn't very reliable
def potential_low_speed_lead(self, v_ego):
# stop for stuff in front of you and low speed, even without model confirmation
return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25
# the distance at which v_lat matters is higher at higher speed
lookahead_dist = 40. + v_ego/1.2 #40m at 0mph, ~70m at 80mph
t_lookahead_v = [1., 0.]
t_lookahead_bp = [10., lookahead_dist]
# average dist
d_path = self.dPath
# lat_corr used to be gated on enabled, now always running
t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact.
lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0.
# consider only cut-ins
d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path))
return abs(d_path) < 1.5 and not self.stationary and not self.oncoming
def is_potential_lead2(self, lead_clusters):
if len(lead_clusters) > 0:
lead_cluster = lead_clusters[0]
# check if the new lead is too close and roughly at the same speed of the first lead:
# it might just be the second axle of the same vehicle
return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1.
else:
return False
def is_potential_fcw(self):
# is this cluster trustrable enough for triggering fcw?
# fcw can trigger only on clusters that have been fused vision model for at least 20 frames
return self.vision_cnt >= 20
def is_potential_fcw(self, model_prob):
return model_prob > .9

6
selfdrive/controls/plannerd.py
View File

@ -37,8 +37,6 @@ def plannerd_thread():
sm['liveParameters'].sensorValid = True
sm['liveParameters'].steerRatio = CP.steerRatio
sm['liveParameters'].stiffnessFactor = 1.0
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
while True:
sm.update()
@ -46,9 +44,7 @@ def plannerd_thread():
if sm.updated['model']:
PP.update(sm, CP, VM)
if sm.updated['radarState']:
PL.update(sm, CP, VM, PP, live_map_data.liveMapData)
# elif socket is live_map_data_sock:
# live_map_data = msg
PL.update(sm, CP, VM, PP)
def main(gctx=None):

217
selfdrive/controls/radard.py
View File

@ -6,18 +6,13 @@ from collections import defaultdict, deque
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.model_parser import ModelParser
from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
RDR_TO_LDR, NO_FUSION_SCORE
from selfdrive.controls.lib.radar_helpers import Track, Cluster
from selfdrive.config import RADAR_TO_CENTER
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
from common.params import Params
from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL
from common.kalman.ekf import EKF, SimpleSensor
DEBUG = False
@ -26,106 +21,94 @@ 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):
super(EKFV1D, self).__init__(False)
self.identity = numpy.matlib.identity(DIMSV)
self.state = np.matlib.zeros((DIMSV, 1))
self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point
self.covar = self.identity * self.var_init
self.process_noise = np.matlib.diag([0.5, 1])
def laplacian_cdf(x, mu, b):
b = np.max([b, 1e-4])
return np.exp(-abs(x-mu)/b)
def match_vision_to_cluster(v_ego, lead, clusters):
# match vision point to best statistical cluster match
probs = []
offset_vision_dist = lead.dist - RADAR_TO_CENTER
for c in clusters:
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
# This is isn't exactly right, but good heuristic
combined_prob = prob_d * prob_y * prob_v
probs.append(combined_prob)
idx = np.argmax(probs)
# if no 'sane' match is found return -1
# stationary radar points can be false positives
dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2)
if dist_sane and vel_sane:
return idx
else:
return None
def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
# Determine leads, this is where the essential logic happens
if len(clusters) > 0 and ready and lead_msg.prob > .5:
lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters)
else:
lead_idx = None
lead_dict = {'status': False}
if lead_idx is not None:
lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob)
elif (lead_idx is None) and ready and (lead_msg.prob > .5):
lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
if low_speed_override:
low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
if len(low_speed_clusters) > 0:
lead_idx = np.argmin([c.dRel for c in low_speed_clusters])
if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']):
lead_dict = low_speed_clusters[lead_idx].get_RadarState()
return lead_dict
def calc_transfer_fun(self, dt):
tf = np.matlib.identity(DIMSV)
tf[XV, SPEEDV] = dt
tfj = tf
return tf, tfj
class RadarD(object):
def __init__(self, VM, mocked):
self.VM = VM
def __init__(self, mocked):
self.current_time = 0
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.
self.ready = False
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)
def update(self, frame, delay, sm, rr, has_radar):
self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
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'])
self.ready = True
# 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]
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
# *** remove missing points from meta data ***
for ids in self.tracks.keys():
@ -134,48 +117,21 @@ class RadarD(object):
# *** 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])
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3])
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)
@ -186,29 +142,12 @@ class RadarD(object):
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')
@ -217,18 +156,14 @@ class RadarD(object):
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
if has_radar:
dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
return dat
## fuses camera and radar data for best lead detection
# fuses camera and radar data for best lead detection
def radard_thread(gctx=None):
set_realtime_priority(2)
@ -236,7 +171,6 @@ def radard_thread(gctx=None):
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked = CP.carName == "mock"
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
@ -253,7 +187,9 @@ def radard_thread(gctx=None):
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(VM, mocked)
RD = RadarD(mocked)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
@ -264,7 +200,7 @@ def radard_thread(gctx=None):
sm.update(0)
dat = RD.update(rk.frame, RI.delay, sm, rr)
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining*1000.
radarState.send(dat.to_bytes())
@ -275,29 +211,20 @@ def radard_thread(gctx=None):
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary,
tracks[ids].measured))
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
"aRel": float(tracks[ids].aRel),
"stationary": bool(tracks[ids].stationary),
"oncoming": bool(tracks[ids].oncoming),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def main(gctx=None):
radard_thread(gctx)
if __name__ == "__main__":
main()

18
selfdrive/locationd/locationd_yawrate.cc
View File

@ -5,6 +5,8 @@
#include <capnp/serialize-packed.h>
#include <eigen3/Eigen/Dense>
#include "cereal/gen/cpp/log.capnp.h"
#include "locationd_yawrate.h"
@ -42,6 +44,9 @@ void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odo
double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
auto trans = camera_odometry.getTrans();
posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
}
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
@ -65,10 +70,7 @@ Localizer::Localizer() {
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>();
void Localizer::handle_log(cereal::Event::Reader event) {
double current_time = event.getLogMonoTime() / 1.0e9;
if (prev_update_time < 0) {
@ -89,8 +91,6 @@ cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t
default:
break;
}
return type;
}
@ -101,8 +101,12 @@ extern "C" {
}
void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) {
const kj::ArrayPtr<const capnp::word> view((const capnp::word*)data, len);
capnp::FlatArrayMessageReader msg(view);
cereal::Event::Reader event = msg.getRoot<cereal::Event>();
Localizer * loc = (Localizer*) localizer;
loc->handle_log(data, len);
loc->handle_log(event);
}
double localizer_get_yaw(void * localizer) {

4
selfdrive/locationd/locationd_yawrate.h
View File

@ -25,10 +25,12 @@ public:
Eigen::Vector2d x;
double steering_angle = 0;
double car_speed = 0;
double posenet_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);
void handle_log(cereal::Event::Reader event);
};

22
selfdrive/locationd/paramsd.cc
View File

@ -1,3 +1,4 @@
#include <iostream>
#include <czmq.h>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
@ -63,6 +64,7 @@ int main(int argc, char *argv[]) {
double sR = car_params.getSteerRatio();
double x = 1.0;
double ao = 0.0;
double posenet_invalid_count = 0;
if (result == 0){
auto str = std::string(value, value_sz);
@ -108,14 +110,25 @@ int main(int argc, char *argv[]) {
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){
capnp::FlatArrayMessageReader capnp_msg(amsg);
cereal::Event::Reader event = capnp_msg.getRoot<cereal::Event>();
localizer.handle_log(event);
auto which = event.which();
if (which == cereal::Event::CAMERA_ODOMETRY){
if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.5 * localizer.car_speed, 5.0)) {
posenet_invalid_count++;
} else {
posenet_invalid_count = 0;
}
} else if (which == cereal::Event::CONTROLS_STATE){
save_counter++;
double yaw_rate = -localizer.x[0];
@ -141,13 +154,14 @@ int main(int argc, char *argv[]) {
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
live_params.setPosenetSpeed(localizer.posenet_speed);
live_params.setPosenetValid(posenet_invalid_count < 5);
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 {

4
selfdrive/locationd/test/test_params_learner.py
View File

@ -21,12 +21,12 @@ class TestParamsLearner(unittest.TestCase):
# Setup vehicle model with wrong parameters
VM_sim = VehicleModel(self.CP)
x_target = 0.75
sr_target = 14
sr_target = self.CP.steerRatio - 0.5
ao_target = -1.0
VM_sim.update_params(x_target, sr_target)
# Run simulation
times = np.arange(0, 10*3600, 0.01)
times = np.arange(0, 15*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

8
selfdrive/manager.py
View File

@ -133,6 +133,9 @@ unkillable_processes = ['visiond']
# processes to end with SIGINT instead of SIGTERM
interrupt_processes = []
# processes to end with SIGKILL instead of SIGTERM
kill_processes = ['sensord']
persistent_processes = [
'thermald',
'logmessaged',
@ -261,6 +264,8 @@ def kill_managed_process(name):
if running[name].exitcode is None:
if name in interrupt_processes:
os.kill(running[name].pid, signal.SIGINT)
elif name in kill_processes:
os.kill(running[name].pid, signal.SIGKILL)
else:
running[name].terminate()
@ -368,7 +373,6 @@ def manager_thread():
logger_dead = False
while 1:
# get health of board, log this in "thermal"
msg = messaging.recv_sock(thermal_sock, wait=True)
# uploader is gated based on the phone temperature
@ -566,6 +570,8 @@ def main():
params.put("LongitudinalControl", "0")
if params.get("LimitSetSpeed") is None:
params.put("LimitSetSpeed", "0")
if params.get("LimitSetSpeedNeural") is None:
params.put("LimitSetSpeedNeural", "0")
# is this chffrplus?
if os.getenv("PASSIVE") is not None:

4
selfdrive/service_list.yaml
View File

@ -11,14 +11,14 @@ sensorEvents: [8003, true, 100., 100]
# GPS data, also global timestamp
gpsNMEA: [8004, true, 9.] # 9 msgs each sec
# CPU+MEM+GPU+BAT temps
thermal: [8005, true, 1., 1]
thermal: [8005, true, 2., 1]
# List(CanData), list of can messages
can: [8006, true, 100.]
controlsState: [8007, true, 100., 100]
#liveEvent: [8008, true, 0.]
model: [8009, true, 20.]
features: [8010, true, 0.]
health: [8011, true, 1., 1]
health: [8011, true, 2., 1]
radarState: [8012, true, 20.]
#liveUI: [8014, true, 0.]
encodeIdx: [8015, true, 20.]

36
selfdrive/test/plant/maneuver.py
View File

@ -1,3 +1,4 @@
from collections import defaultdict
from selfdrive.test.plant.maneuverplots import ManeuverPlot
from selfdrive.test.plant.plant import Plant
import numpy as np
@ -16,6 +17,7 @@ class Maneuver(object):
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
self.checks = kwargs.get("checks", [])
self.duration = duration
self.title = title
@ -28,6 +30,7 @@ class Maneuver(object):
distance_lead = self.distance_lead
)
logs = defaultdict(list)
last_controls_state = None
plot = ManeuverPlot(self.title)
@ -42,20 +45,24 @@ class Maneuver(object):
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
if controls_state:
last_controls_state = controls_state[-1]
# distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
log = plant.step(speed_lead, current_button, grade)
d_rel = distance_lead - distance if self.lead_relevancy else 200.
v_rel = speed_lead - speed if self.lead_relevancy else 0.
if log['controls_state_msgs']:
last_controls_state = log['controls_state_msgs'][-1]
d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
log['d_rel'] = d_rel
log['v_rel'] = v_rel
if last_controls_state:
# print(last_controls_state)
#develop plots
plot.add_data(
time=plant.current_time(),
gas=gas, brake=brake, steer_torque=steer_torque,
distance=distance, speed=speed, acceleration=acceleration,
gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
uf_accel_cmd=last_controls_state.ufAccelCmd,
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
@ -63,10 +70,17 @@ class Maneuver(object):
cruise_speed=last_controls_state.vCruise,
jerk_factor=last_controls_state.jerkFactor,
a_target=last_controls_state.aTarget,
fcw=fcw)
fcw=log['fcw'])
print("maneuver end")
return (None, plot)
for k, v in log.items():
logs[k].append(v)
valid = True
for check in self.checks:
c = check(logs)
if not c:
print check.__name__ + " not valid!"
valid = valid and c
print("maneuver end", valid)
return (plot, valid)

37
selfdrive/test/plant/plant.py
View File

@ -332,13 +332,15 @@ class Plant(object):
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.sensorValid = True
live_parameters.liveParameters.posenetValid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes())
driver_monitoring = messaging.new_message()
driver_monitoring.init('driverMonitoring')
driver_monitoring.driverMonitoring.descriptor = [0.] * 7
driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3
driver_monitoring.driverMonitoring.facePosition = [0.] * 2
Plant.driverMonitoring.send(driver_monitoring.to_bytes())
health = messaging.new_message()
@ -364,9 +366,26 @@ class Plant(object):
x.points = [0.0]*50
x.prob = 1.0
x.std = 1.0
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
prob = 1.0
else:
d_rel = 200.
v_rel = 0.
prob = 0.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
md.model.lead.prob = prob
md.model.lead.relY = 0.0
md.model.lead.relYStd = 1.
md.model.lead.relVel = float(v_rel)
md.model.lead.relVelStd = 1.
md.model.lead.relA = 0.0
md.model.lead.relAStd = 10.
md.model.lead.std = 1.0
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
@ -383,7 +402,17 @@ class Plant(object):
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs)
return {
"distance": distance,
"speed": speed,
"acceleration": acceleration,
"distance_lead": distance_lead,
"brake": brake,
"gas": gas,
"steer_torque": steer_torque,
"fcw": fcw,
"controls_state_msgs": controls_state_msgs,
}
# simple engage in standalone mode
def plant_thread(rate=100):

136
selfdrive/test/tests/plant/test_longitudinal.py
View File

@ -22,22 +22,34 @@ def create_dir(path):
except OSError:
pass
def check_no_collision(log):
return min(log['d_rel']) > 0
def check_fcw(log):
return any(log['fcw'])
def check_engaged(log):
return log['controls_state_msgs'][-1][-1].active
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.RES_ACCEL, 10.), (0, 10.1),
(CB.RES_ACCEL, 10.2), (0, 10.3)]
(CB.RES_ACCEL, 10.), (0, 10.1),
(CB.RES_ACCEL, 10.2), (0, 10.3)],
checks=[check_engaged],
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.DECEL_SET, 10.), (0, 10.1),
(CB.DECEL_SET, 10.2), (0, 10.3)]
(CB.DECEL_SET, 10.), (0, 10.1),
(CB.DECEL_SET, 10.2), (0, 10.3)],
checks=[check_engaged],
),
Maneuver(
'while cruising at 20mph, grade change +10%',
@ -45,7 +57,8 @@ maneuvers = [
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
grade_breakpoints = [0., 10., 11.]
grade_breakpoints = [0., 10., 11.],
checks=[check_engaged],
),
Maneuver(
'while cruising at 20mph, grade change -10%',
@ -53,7 +66,8 @@ maneuvers = [
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
grade_breakpoints = [0., 10., 11.]
grade_breakpoints = [0., 10., 11.],
checks=[check_engaged],
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
@ -63,7 +77,8 @@ maneuvers = [
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
@ -73,7 +88,8 @@ maneuvers = [
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
@ -83,7 +99,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 35.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@ -93,7 +110,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 25.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@ -103,7 +121,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 21.66],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
checks=[check_engaged, check_fcw],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
@ -113,7 +132,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 19.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
checks=[check_engaged, check_fcw],
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
@ -122,9 +142,10 @@ maneuvers = [
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
@ -135,8 +156,9 @@ maneuvers = [
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
@ -147,8 +169,9 @@ maneuvers = [
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
@ -159,11 +182,12 @@ maneuvers = [
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
@ -175,7 +199,8 @@ maneuvers = [
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
(CB.RES_ACCEL, 1.6), (0.0, 1.7)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
@ -186,8 +211,9 @@ maneuvers = [
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
@ -198,11 +224,12 @@ maneuvers = [
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
@ -213,11 +240,12 @@ maneuvers = [
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)],
checks=[check_engaged, check_no_collision],
),
Maneuver(
"fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
@ -227,7 +255,8 @@ maneuvers = [
initial_distance_lead=100.,
speed_lead_values=[20.],
speed_lead_breakpoints=[1.],
cruise_button_presses = []
cruise_button_presses = [],
checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
@ -237,7 +266,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 23.],
cruise_button_presses = []
cruise_button_presses = [],
checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
@ -247,7 +277,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 9.6],
cruise_button_presses = []
cruise_button_presses = [],
checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
@ -257,7 +288,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 7.],
cruise_button_presses = []
cruise_button_presses = [],
checks=[check_fcw],
)
]
@ -314,26 +346,30 @@ class LongitudinalControl(unittest.TestCase):
def test_longitudinal_setup(self):
pass
WORKERS = 8
def run_maneuver_worker(k):
man = maneuvers[k]
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
for i, man in enumerate(maneuvers[k::WORKERS]):
def run(self):
print(man.title)
manager.start_managed_process('radard')
manager.start_managed_process('controlsd')
manager.start_managed_process('plannerd')
score, plot = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2))
plot, valid = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(k+1).zfill(2))
manager.kill_managed_process('radard')
manager.kill_managed_process('controlsd')
manager.kill_managed_process('plannerd')
time.sleep(5)
for k in xrange(WORKERS):
setattr(LongitudinalControl,
"test_longitudinal_maneuvers_%d" % (k+1),
lambda self, k=k: run_maneuver_worker(k))
self.assertTrue(valid)
return run
for k in range(len(maneuvers)):
setattr(LongitudinalControl, "test_longitudinal_maneuvers_%d" % (k+1), run_maneuver_worker(k))
if __name__ == "__main__":
unittest.main()
unittest.main(failfast=True)

14
selfdrive/thermald.py
View File

@ -8,7 +8,7 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.loggerd.config import get_available_percent
from common.params import Params
from common.realtime import sec_since_boot
from common.realtime import sec_since_boot, DT_TRML
from common.numpy_fast import clip
from common.filter_simple import FirstOrderFilter
@ -138,8 +138,8 @@ def thermald_thread():
ignition_seen = False
started_seen = False
thermal_status = ThermalStatus.green
health_sock.RCVTIMEO = 1500
current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML) # 2x the expected health frequency
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
health_prev = None
# Make sure charging is enabled
@ -189,13 +189,13 @@ def thermald_thread():
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 95. or bat_temp > 60.:
elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
elif max_cpu_temp > 90.0:
elif max_cpu_temp > 87.5:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red)
elif max_cpu_temp > 85.0:
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
@ -266,7 +266,7 @@ def thermald_thread():
print(msg)
# report to server once per minute
if (count%60) == 0:
if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),

19
selfdrive/ui/ui.c
View File

@ -126,8 +126,7 @@ typedef struct UIScene {
float v_cruise;
uint64_t v_cruise_update_ts;
float v_ego;
float v_curvature;
bool decel_for_turn;
bool decel_for_model;
float speedlimit;
bool speedlimit_valid;
@ -1153,17 +1152,6 @@ static void ui_draw_vision_maxspeed(UIState *s) {
nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL);
}
#ifdef DEBUG_TURN
if (s->scene.decel_for_turn && s->scene.engaged){
int v_curvature = s->scene.v_curvature * 2.2369363 + 0.5;
snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", v_curvature);
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
nvgFontSize(s->vg, 25*2.5);
nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "TURN", NULL);
nvgFontSize(s->vg, 50*2.5);
nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL);
}
#endif
}
static void ui_draw_vision_speedlimit(UIState *s) {
@ -1294,7 +1282,7 @@ static void ui_draw_vision_event(UIState *s) {
const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2)));
const int viz_event_y = (box_y + (bdr_s*1.5));
const int viz_event_h = (header_h - (bdr_s*1.5));
if (s->scene.decel_for_turn && s->scene.engaged && s->limit_set_speed) {
if (s->scene.decel_for_model && s->scene.engaged) {
// draw winding road sign
const int img_turn_size = 160*1.5;
const int img_turn_x = viz_event_x-(img_turn_size/4);
@ -1643,8 +1631,7 @@ void handle_message(UIState *s, void *which) {
s->scene.frontview = datad.rearViewCam;
s->scene.v_curvature = datad.vCurvature;
s->scene.decel_for_turn = datad.decelForTurn;
s->scene.decel_for_model = datad.decelForModel;
if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) {
char* error = NULL;

65
selfdrive/visiond/models/commonmodel.c
View File

@ -97,39 +97,70 @@ static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, con
for (int i=0; i<POLYFIT_DEGREE; i++) {
capn_set32(poly_ptr, i, capn_from_f32(data.poly[i]));
}
if (getenv("DEBUG_MODEL")){
capn_list32 points_ptr = capn_new_list32(cs, MODEL_PATH_DISTANCE);
capn_list32 stds_ptr = capn_new_list32(cs, MODEL_PATH_DISTANCE);
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
capn_set32(points_ptr, i, capn_from_f32(data.points[i]));
capn_set32(stds_ptr, i, capn_from_f32(data.stds[i]));
}
struct cereal_ModelData_PathData d = {
.points = points_ptr,
.stds = stds_ptr,
.prob = data.prob,
.std = data.std,
.poly = poly_ptr,
};
cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
cereal_write_ModelData_PathData(&d, ret);
return ret;
} else {
struct cereal_ModelData_PathData d = {
.prob = data.prob,
.std = data.std,
.poly = poly_ptr,
};
cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
cereal_write_ModelData_PathData(&d, ret);
return ret;
}
}
cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
struct cereal_ModelData_PathData d = {
static cereal_ModelData_LeadData_ptr lead_to_cereal(struct capn_segment *cs, const LeadData data) {
cereal_ModelData_LeadData_ptr leadp = cereal_new_ModelData_LeadData(cs);
struct cereal_ModelData_LeadData leadd = (struct cereal_ModelData_LeadData){
.dist = data.dist,
.prob = data.prob,
.std = data.std,
.poly = poly_ptr,
.relY = data.rel_y,
.relYStd = data.rel_y_std,
.relVel = data.rel_v,
.relVelStd = data.rel_v_std,
.relA = data.rel_a,
.relAStd = data.rel_a_std,
};
cereal_write_ModelData_PathData(&d, ret);
return ret;
cereal_write_ModelData_LeadData(&leadd, leadp);
return leadp;
}
void model_publish(void* sock, uint32_t frame_id,
const mat3 transform, const ModelData data) {
struct capn rc;
capn_init_malloc(&rc);
struct capn_segment *cs = capn_root(&rc).seg;
cereal_ModelData_LeadData_ptr leadp = cereal_new_ModelData_LeadData(cs);
struct cereal_ModelData_LeadData leadd = (struct cereal_ModelData_LeadData){
.dist = data.lead.dist,
.prob = data.lead.prob,
.std = data.lead.std,
.relVel = data.lead.rel_v,
.relVelStd = data.lead.rel_v_std,
};
cereal_write_ModelData_LeadData(&leadd, leadp);
capn_list32 input_transform_ptr = capn_new_list32(cs, 3*3);
for (int i = 0; i < 3 * 3; i++) {
capn_set32(input_transform_ptr, i, capn_from_f32(transform.v[i]));
}
capn_list32 speed_perc_ptr = capn_new_list32(cs, SPEED_PERCENTILES);
for (int i=0; i<SPEED_PERCENTILES; i++) {
capn_set32(speed_perc_ptr, i, capn_from_f32(data.speed[i]));
}
cereal_ModelData_ModelSettings_ptr settingsp = cereal_new_ModelData_ModelSettings(cs);
struct cereal_ModelData_ModelSettings settingsd = {
.inputTransform = input_transform_ptr,
@ -142,8 +173,10 @@ void model_publish(void* sock, uint32_t frame_id,
.path = path_to_cereal(cs, data.path),
.leftLane = path_to_cereal(cs, data.left_lane),
.rightLane = path_to_cereal(cs, data.right_lane),
.lead = leadp,
.lead = lead_to_cereal(cs, data.lead),
.leadFuture = lead_to_cereal(cs, data.lead_future),
.settings = settingsp,
.speed = speed_perc_ptr,
};
cereal_write_ModelData(&modeld, modelp);

74
selfdrive/visiond/models/driving.cc
View File

@ -22,9 +22,12 @@
#define MODEL_NAME "driving_model_dlc"
#endif
#define OUTPUT_SIZE (200 + 2*201 + 26)
#define LEAD_MDN_N 5
#define LEAD_MDN_N 5 // probs for 5 groups
#define MDN_VALS 4 // output xyva for each lead group
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
#define MDN_GROUP_SIZE 11
#define SPEED_BUCKETS 100
#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + SPEED_BUCKETS)
#ifdef TEMPORAL
#define TEMPORAL_SIZE 512
#else
@ -60,6 +63,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
float *left_lane;
float *right_lane;
float *lead;
float *speed;
} net_outputs = {NULL};
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
@ -71,7 +75,8 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
//fclose(f);
//sleep(1);
//printf("done \n");
//printf("%i \n",OUTPUT_SIZE);
//printf("%i \n",MDN_GROUP_SIZE);
s->m->execute(net_input_buf);
// net outputs
@ -79,6 +84,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
ModelData model = {0};
@ -91,9 +97,9 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
}
model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.path.prob = 1.;
model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
@ -105,17 +111,59 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
// current lead
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*5 + 4] > net_outputs.lead[mdn_max_idx*5 + 4]) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
mdn_max_idx = i;
}
}
model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*5]);
model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist;
model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist;
model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel;
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel;
model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE]);
model.lead.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
model.lead.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
model.lead.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
model.lead.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
model.lead.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// lead in 2s
mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
mdn_max_idx = i;
}
}
model.lead_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// get speed percentiles numbers represent 5th, 15th, ... 95th percentile
for (int i=0; i < SPEED_PERCENTILES; i++) {
model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
}
float sum = 0;
for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
sum += net_outputs.speed[idx];
int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
if (idx_percentile < SPEED_PERCENTILES ){
model.speed[idx_percentile] = ((float)idx)/2.0;
}
}
// make sure no percentiles are skipped
for (int i=SPEED_PERCENTILES-1; i > 0; i--){
if (model.speed[i-1] > model.speed[i]){
model.speed[i-1] = model.speed[i];
}
}
return model;
}

8
selfdrive/visiond/models/monitoring.cc
View File

@ -37,9 +37,11 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
s->m->execute(net_input_buf);
MonitoringResult ret = {0};
memcpy(ret.vs, s->output, sizeof(ret.vs));
ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1];
memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position);
memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
return ret;
}

13
selfdrive/visiond/models/monitoring.h
View File

@ -8,11 +8,18 @@
extern "C" {
#endif
#define OUTPUT_SIZE 8
#define OUTPUT_SIZE_DEPRECATED 8
#define OUTPUT_SIZE 31
typedef struct MonitoringResult {
float vs[OUTPUT_SIZE - 1];
float std;
float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
float std_DEPRECATED;
float face_orientation[3];
float face_position[2];
float face_prob;
float left_eye_prob;
float right_eye_prob;
} MonitoringResult;
typedef struct MonitoringState {

21
selfdrive/visiond/visiond.cc
View File

@ -716,6 +716,8 @@ void* monitoring_thread(void *arg) {
MonitoringResult res = monitoring_eval_frame(&s->monitoring, q,
s->yuv_front_cl[buf_idx], s->yuv_front_width, s->yuv_front_height);
double t2 = millis_since_boot();
// send driver monitoring packet
{
capnp::MallocMessageBuilder msg;
@ -725,17 +727,28 @@ void* monitoring_thread(void *arg) {
auto framed = event.initDriverMonitoring();
framed.setFrameId(frame_data.frame_id);
kj::ArrayPtr<const float> descriptor_vs(&res.vs[0], ARRAYSIZE(res.vs));
framed.setDescriptor(descriptor_vs);
// junk 0s from legacy model
//kj::ArrayPtr<const float> descriptor_DEPRECATED(&res.descriptor_DEPRECATED[0], ARRAYSIZE(res.descriptor_DEPRECATED));
//framed.setDescriptor(descriptor_DEPRECATED);
//framed.setStd(res.std_DEPRECATED);
// why not use this junk space for reporting inference time instead
// framed.setStdDEPRECATED(static_cast<float>(t2-t1));
kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
kj::ArrayPtr<const float> face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
framed.setFaceOrientation(face_orientation);
framed.setFacePosition(face_position);
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);
framed.setStd(res.std);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s->monitoring_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
double t2 = millis_since_boot();
t2 = millis_since_boot();
//LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;