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

openpilot v0.6.2 release

Browse Source 
old-commit-hash: e90c41c576
commatwo_master
Vehicle Researcher 6 years ago
parent b059840baf
commit 3835061760
56 changed files with 848 additions and 1087 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      Pipfile
  2. 4
      Pipfile.lock
  3. 12
      RELEASES.md
  4. 4
      apk/ai.comma.plus.offroad.apk
  5. 253
      common/kalman/ekf.py
  6. 116
      common/kalman/tests/test_ekf.py
  7. 2
      common/params.py
  8. 1
      common/realtime.py
  9. 128
      common/vin.py
  10. 4
      models/driving_model.dlc
  11. 4
      models/monitoring_model.dlc
  12. 66
      selfdrive/boardd/boardd.cc
  13. 21
      selfdrive/can/tests/test_packer_honda.py
  14. 133
      selfdrive/car/car_helpers.py
  15. 4
      selfdrive/car/chrysler/carstate.py
  16. 5
      selfdrive/car/chrysler/interface.py
  17. 2
      selfdrive/car/chrysler/values.py
  18. 5
      selfdrive/car/ford/interface.py
  19. 5
      selfdrive/car/gm/interface.py
  20. 10
      selfdrive/car/honda/carcontroller.py
  21. 8
      selfdrive/car/honda/carstate.py
  22. 40
      selfdrive/car/honda/hondacan.py
  23. 13
      selfdrive/car/honda/interface.py
  24. 9
      selfdrive/car/honda/values.py
  25. 5
      selfdrive/car/hyundai/interface.py
  26. 7
      selfdrive/car/mock/interface.py
  27. 3
      selfdrive/car/subaru/interface.py
  28. 17
      selfdrive/car/toyota/carstate.py
  29. 11
      selfdrive/car/toyota/interface.py
  30. 9
      selfdrive/common/modeldata.h
  31. 3
      selfdrive/common/params.cc
  32. 2
      selfdrive/common/version.h
  33. 32
      selfdrive/controls/controlsd.py
  34. 14
      selfdrive/controls/lib/alerts.py
  35. 28
      selfdrive/controls/lib/driver_monitor.py
  36. 1
      selfdrive/controls/lib/pathplanner.py
  37. 84
      selfdrive/controls/lib/planner.py
  38. 160
      selfdrive/controls/lib/radar_helpers.py
  39. 6
      selfdrive/controls/plannerd.py
  40. 217
      selfdrive/controls/radard.py
  41. 18
      selfdrive/locationd/locationd_yawrate.cc
  42. 4
      selfdrive/locationd/locationd_yawrate.h
  43. 22
      selfdrive/locationd/paramsd.cc
  44. 4
      selfdrive/locationd/test/test_params_learner.py
  45. 8
      selfdrive/manager.py
  46. 4
      selfdrive/service_list.yaml
  47. 36
      selfdrive/test/plant/maneuver.py
  48. 37
      selfdrive/test/plant/plant.py
  49. 136
      selfdrive/test/tests/plant/test_longitudinal.py
  50. 14
      selfdrive/thermald.py
  51. 19
      selfdrive/ui/ui.c
  52. 65
      selfdrive/visiond/models/commonmodel.c
  53. 74
      selfdrive/visiond/models/driving.cc
  54. 8
      selfdrive/visiond/models/monitoring.cc
  55. 13
      selfdrive/visiond/models/monitoring.h
  56. 21
      selfdrive/visiond/visiond.cc

4
Pipfile
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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()

128
common/vin.py
Unescape 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
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]]
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__":
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
print get_vin(logcan, sendcan)
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
Unescape 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
Unescape 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
Unescape 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
Unescape 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)

133
selfdrive/car/car_helpers.py
Unescape 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
# 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]]
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 = ""
params = Params()
car_params = params.get("CarParams")
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
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)
if can_seen_frame is None and can_seen:
can_seen_frame = frame
# 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
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 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]
# 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape View File

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

32
selfdrive/controls/controlsd.py
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
#map_age = cur_time - rcv_times['liveMapData']
map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0
# 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 not self.model_v_kf_ready:
self.model_v_kf.x = [[v_ego],[0.0]]
self.model_v_kf_ready = True
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)
if len(sm['model'].speed):
self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
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
Unescape 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
# 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 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
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
Unescape 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
Unescape 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'])
# 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]
self.ready = True
# *** 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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)
print("maneuver end")
fcw=log['fcw'])
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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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*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.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_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
Unescape 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
Unescape 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
Unescape 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;

Write Preview
Loading…
Cancel
Save