mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-11 16:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'devel-en' into devel-en-unsupported-cars

Browse Source
This commit is contained in:
dragonpilot
2019-08-11 02:01:13 +10:00
parent d2cfd239d5 2861467183
commit fd79368f3b
100 changed files with 3125 additions and 1902 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Pipfile
+2
View File
@@ -99,6 +99,8 @@ simplejson = "*"
python-logstash-async = "*"
pandas = "*"
seaborn = "*"
tensorflow-estimator = "==1.10.12"
pyproj = "*"
[packages]
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
Pipfile.lock
Generated
+44 -17
View File
@@ -1,7 +1,7 @@
{
"_meta": {
"hash": {
"sha256": "89070d7d9478ac9e6ad2c9848aaf724cb7362a1de4af9f8fb8c40be5f37c043d"
"sha256": "43f96719ff9a1b7eae6c7a88f903c4f5a137c2b195e3ba148f32767827b98710"
},
"pipfile-spec": 6,
"requires": {
@@ -656,12 +656,12 @@
},
"aenum": {
"hashes": [
"sha256:058f0cfaf911899dc21b334362047df74ce989335dd8dff8e4be1a6313b15232",
"sha256:6af970173d9b4ac0384ad7d1cfe9523eeb9a3578793e1664090c13cb59df6469",
"sha256:80f14366578d84f6bccb0670259744cb3a7f2ab504480c306238a23cdd569457"
"sha256:0e3589654ef090784971f7778dcb74b08c9b4ef80b33267c00f82ddeedac179a",
"sha256:b12a7be3d89b270f266f8643aaa126404e5cdc0929bd6f09548b8eaed85e2aa1",
"sha256:e4dab068cbe00295bbc3660cd562221b008687d0f7a4c40fc2dd7a80002126a7"
],
"index": "pypi",
"version": "==2.2.0"
"version": "==2.2.1"
},
"amqp": {
"hashes": [
@@ -817,18 +817,18 @@
},
"boto3": {
"hashes": [
"sha256:34a8ddb7247316be6ea94c7eeee41212312d250d99bf668fcd6748629b578622",
"sha256:71f3554cc69fa20be06cf20d6c9e0d8095d7c40695b48618676c3cd9a5ba0783"
"sha256:6a950bf98b22812896ea0f833a26d448acfdf43179f41f389d501af7a9fae328",
"sha256:cfbc062a76a7781af8e6a4ea26ebcafa3866872a8cceb05fdbf588c36e7848f0"
],
"index": "pypi",
"version": "==1.9.189"
"version": "==1.9.195"
},
"botocore": {
"hashes": [
"sha256:4febbf206d1dc8b8299aa211d8e382d5bf3f22097855b9f98d5e8c401ef8192b",
"sha256:b62ab3e4e98e075fc9e8e8fd4e8f5b92ebf311a6dc9f7578650938c7bc94e592"
"sha256:691627c2aeff0fcbd9237985717c28404a628181fd3e86b7be500bf2ee156007",
"sha256:c59e9981db9dfc54f0d22f731ca8de904760049a9c60d86dcedde84ae64ef4f0"
],
"version": "==1.12.189"
"version": "==1.12.195"
},
"celery": {
"hashes": [
@@ -1734,7 +1734,7 @@
"sha256:23953d55076df038541e648a53676fb24980f7a1be290cdda21300b3bc21dfb0",
"sha256:552a91f381532e33cbd07c6a2655a21908088962bb8fa7239ecbcc6ad1140cc7"
],
"markers": "python_version == '2.7'",
"markers": "python_version == '2.6' or python_version == '2.7' or python_version == '3.0' or python_version == '3.1' or python_version == '3.2'",
"version": "==1.5"
},
"mpld3": {
@@ -2264,6 +2264,32 @@
"index": "pypi",
"version": "==0.1.17"
},
"pyproj": {
"hashes": [
"sha256:1303b343a1c4d83d1ac17494571d958335c0a4597d4aa623cd64df1f36fb18bf",
"sha256:1fd2f756f43ba762f5a1ae769c6cc9ae6f919996bf80c621f331e4a5d89ae74f",
"sha256:2b3f59b16cde23c0d95b1d9ba3dd1c0be3937d877df3b47a522d3c9a74d31668",
"sha256:2e5d8b892782db22c3d6ffcb272c8ac0a18995e22d45db762a893ec64cc039d6",
"sha256:512a59bc20587f88df78a69872ed8cb4ec7905415ba85b4de0af35601770fba1",
"sha256:5aa89b04855e439e2b96caf1f49fa2ab60e6e7c407034284d31d808abed0618a",
"sha256:75b4c5eba36060a00867e16e81755a44e93a95b0bbaa4d6ee40f10c330b62108",
"sha256:860fde3429f364767c88a8e7f14f330e3fdace9f438e0ef1b112108a44c1eee8",
"sha256:86886f21af819553990f0da9ad7737d52cd32076dd277db34e40cf805919e839",
"sha256:9417d46f2b5e6ba79bd6adea0aae9659501c8a0350a5a98c0d9cb1442d45b0b4",
"sha256:9bb81141715832bb8682dda3c2f8f1ad0f43cf8aec5db52e0992c16c7664e227",
"sha256:afe150d8f0b351df7a357cfce50a8a5c0a44bdfeec5e08075f873cfeddf63004",
"sha256:cc2e21c09245c3eb47014c3d4c58888cc8d48a168a03533f9ed3f8b2ee82a352",
"sha256:cefb4a1a5d51cd04c9dd31bb18047c2dafcb86bc33a0347746b5a0ef82c071d8",
"sha256:cfa2cd0ee23bcfc9f165b4f8e9c5b7ba829c7b49175fb73420544619148d91af",
"sha256:d3a78ce50e6c50e701d65929dc04ab751edaeae660455a9fef3c2231d609f8c1",
"sha256:d772ee53c8d316de1b7f68244ab654f864553bca177357f8787a65dc09816ae3",
"sha256:ddd99d6f2c813e44721c22039ddf333e4b95e8d0794b03d9962c94c01c823934",
"sha256:ec89e68a0cf210af0cc2724b5f8601d4b6809ff0f556e16efc8c955e79672f7a",
"sha256:f118762ed0abe8e988a20020241fe1e6133edb960d2665a44d6f48acc6d8e7c7"
],
"index": "pypi",
"version": "==2.2.1"
},
"pyrsistent": {
"hashes": [
"sha256:50cffebc87ca91b9d4be2dcc2e479272bcb466b5a0487b6c271f7ddea6917e14"
@@ -2424,10 +2450,10 @@
},
"qtconsole": {
"hashes": [
"sha256:4af84facdd6f00a6b9b2927255f717bb23ae4b7a20ba1d9ef0a5a5a8dbe01ae2",
"sha256:60d61d93f7d67ba2b265c6d599d413ffec21202fec999a952f658ff3a73d252b"
"sha256:6a85456af7a98b0f554d140922b7b6a219757b039adb2b95e847cf115eaa20ae",
"sha256:767eb9ec3f9943bc84270198b5ff95d2d86d68d6b57792fafa4df4fc6b16cd7c"
],
"version": "==4.5.1"
"version": "==4.5.2"
},
"redis": {
"hashes": [
@@ -2685,9 +2711,10 @@
},
"tensorflow-estimator": {
"hashes": [
"sha256:ca073f66063407a091d610ec1b22e39ea30248710198cc6f13769320bdbe3992"
"sha256:3e460f43682c7d789e5fe966630029558434d32502e632ee7f6703451258528c"
],
"version": "==1.14.0"
"index": "pypi",
"version": "==1.10.12"
},
"tensorflow-gpu": {
"hashes": [
README.md
+1 -1
View File
@@ -71,7 +71,7 @@ Supported Cars
| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
| GMC<sup>3</sup> | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Holden<sup>3</sup> | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | Bosch |
| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
| Honda | Civic Sedan/Coupe 2016-18| Honda Sensing | Yes | Yes | 0mph | 12mph | Nidec |
| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Yes | Stock | 0mph | 2mph | Bosch |
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
RELEASES.md
+12
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)
apk/ai.comma.plus.offroad.apk
BIN
View File
Binary file not shown.
cereal/car.capnp
+16
View File
@@ -326,6 +326,7 @@ struct CarParams {
lateralTuning :union {
pid @26 :LateralPIDTuning;
indi @27 :LateralINDITuning;
lqr @40 :LateralLQRTuning;
}
steerLimitAlert @28 :Bool;
@@ -341,6 +342,7 @@ struct CarParams {
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
carVin @38 :Text; # VIN number queried during fingerprinting
isPandaBlack @39: Bool;
struct LateralPIDTuning {
kpBP @0 :List(Float32);
@@ -367,6 +369,20 @@ struct CarParams {
actuatorEffectiveness @3 :Float32;
}
struct LateralLQRTuning {
scale @0 :Float32;
ki @1 :Float32;
dcGain @2 :Float32;
# State space system
a @3 :List(Float32);
b @4 :List(Float32);
c @5 :List(Float32);
k @6 :List(Float32); # LQR gain
l @7 :List(Float32); # Kalman gain
}
enum SafetyModel {
# does NOT match board setting
cereal/log.capnp
+21 -3
View File
@@ -301,7 +301,7 @@ struct HealthData {
controlsAllowed @3 :Bool;
gasInterceptorDetected @4 :Bool;
startedSignalDetectedDeprecated @5 :Bool;
isGreyPanda @6 :Bool;
hasGps @6 :Bool;
canSendErrs @7 :UInt32;
canFwdErrs @8 :UInt32;
gmlanSendErrs @9 :UInt32;
@@ -373,6 +373,8 @@ struct LiveCalibrationData {
# view_frame_from_road_frame
# ui's is inversed needs new
extrinsicMatrix @4 :List(Float32);
# the direction of travel vector in device frame
rpyCalib @7 :List(Float32);
}
struct LiveTracks {
@@ -448,9 +450,12 @@ struct ControlsState @0x97ff69c53601abf1 {
vCurvature @46 :Float32;
decelForTurn @47 :Bool;
decelForModel @54 :Bool;
lateralControlState :union {
indiState @52 :LateralINDIState;
pidState @53 :LateralPIDState;
lqrState @55 :LateralLQRState;
}
enum OpenpilotState @0xdbe58b96d2d1ac61 {
@@ -505,6 +510,13 @@ struct ControlsState @0x97ff69c53601abf1 {
saturated @8 :Bool;
}
struct LateralLQRState {
active @0 :Bool;
steerAngle @1 :Float32;
i @2 :Float32;
output @3 :Float32;
}
}
struct LiveEventData {
@@ -660,6 +672,7 @@ struct Plan {
mpc1 @1;
mpc2 @2;
mpc3 @3;
model @4;
}
}
@@ -1661,8 +1674,13 @@ struct OrbKeyFrame {
struct DriverMonitoring {
frameId @0 :UInt32;
descriptor @1 :List(Float32);
std @2 :Float32;
descriptorDEPRECATED @1 :List(Float32);
stdDEPRECATED @2 :Float32;
faceOrientation @3 :List(Float32);
facePosition @4 :List(Float32);
faceProb @5 :Float32;
leftEyeProb @6 :Float32;
rightEyeProb @7 :Float32;
}
struct Boot {
common/kalman/ekf.py
-253
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
common/kalman/tests/test_ekf.py
-116
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()
common/params.py
+19 -4
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],
@@ -82,16 +84,29 @@ keys = {
"Version": [TxType.PERSISTENT],
#dragonpilot config
"DragonEnableDashcam": [TxType.PERSISTENT],
"DragonDisableDriverSafetyCheck": [TxType.PERSISTENT],
"DragonDisableDriverSafetyCheck": [TxType.PERSISTENT], # deprecated
"DragonEnableDriverSafetyCheck": [TxType.PERSISTENT],
"DragonAutoShutdownAt": [TxType.PERSISTENT],
"DragonTempDisableSteerOnSignal": [TxType.PERSISTENT],
"DragonDisableLogger": [TxType.PERSISTENT],
"DragonDisableUploader": [TxType.PERSISTENT],
"DragonTempDisableSteerOnSignal": [TxType.PERSISTENT], # deprecated
"DragonEnableSteeringOnSignal": [TxType.PERSISTENT],
"DragonDisableLogger": [TxType.PERSISTENT], # deprecated
"DragonEnableLogger": [TxType.PERSISTENT],
"DragonDisableUploader": [TxType.PERSISTENT], # deprecated
"DragonEnableUploader": [TxType.PERSISTENT],
"DragonNoctuaMode": [TxType.PERSISTENT],
"DragonCacheCar": [TxType.PERSISTENT],
"DragonCachedModel": [TxType.PERSISTENT],
"DragonCachedFP": [TxType.PERSISTENT],
"DragonCachedVIN": [TxType.PERSISTENT],
"DragonAllowGas": [TxType.PERSISTENT],
"DragonBBUI": [TxType.PERSISTENT], # deprecated
"DragonToyotaStockDSU": [TxType.PERSISTENT],
"DragonLatCtrl": [TxType.PERSISTENT],
"DragonUIEvent": [TxType.PERSISTENT],
"DragonUIMaxSpeed": [TxType.PERSISTENT],
"DragonUIFace": [TxType.PERSISTENT],
"DragonUIDev": [TxType.PERSISTENT],
"DragonUIDevMini": [TxType.PERSISTENT],
}
common/realtime.py
+1
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()
common/vin.py
+68 -62
View File
@@ -1,61 +1,8 @@
#!/usr/bin/env python
import time
from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.boardd.boardd import can_list_to_can_capnp
def get_vin(logcan, sendcan):
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
cnts = [1, 2] # Number of messages to wait for at each iteration
vin_valid = True
dat = []
for i in range(len(query_msg)):
cnt = 0
sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan'))
got_response = False
t_start = sec_since_boot()
while sec_since_boot() - t_start < 0.05 and not got_response:
for a in messaging.drain_sock(logcan):
for can in a.can:
if can.src == 0 and can.address == 0x7e8:
vin_valid = vin_valid and is_vin_response_valid(can.dat, i, cnt)
dat += can.dat[2:] if i == 0 else can.dat[1:]
cnt += 1
if cnt == cnts[i]:
got_response = True
time.sleep(0.01)
return "".join(dat[3:]) if vin_valid else ""
"""
if 'vin' not in gctx:
print "getting vin"
gctx['vin'] = query_vin()[3:]
print "got VIN %s" % (gctx['vin'],)
cloudlog.info("got VIN %s" % (gctx['vin'],))
# *** determine platform based on VIN ****
if vin.startswith("19UDE2F36G"):
print "ACURA ILX 2016"
self.civic = False
else:
# TODO: add Honda check explicitly
print "HONDA CIVIC 2016"
self.civic = True
# *** special case VIN of Acura test platform
if vin == "19UDE2F36GA001322":
print "comma.ai test platform detected"
# it has a gas interceptor and a torque mod
self.torque_mod = True
"""
VIN_UNKNOWN = "0" * 17
# sanity checks on response messages from vin query
def is_vin_response_valid(can_dat, step, cnt):
@@ -84,12 +31,71 @@ def is_vin_response_valid(can_dat, step, cnt):
return True
if __name__ == "__main__":
import zmq
from selfdrive.services import service_list
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
time.sleep(1.) # give time to sendcan socket to start
class VinQuery():
def __init__(self, bus):
self.bus = bus
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
self.query_ext_msgs = [[0x18DB33F1, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus],
[0x18DA10f1, 0, '\x30'.ljust(8, "\x00"), bus]]
self.query_nor_msgs = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), bus]]
print get_vin(logcan, sendcan)
self.cnts = [1, 2] # number of messages to wait for at each iteration
self.step = 0
self.cnt = 0
self.responded = False
self.never_responded = True
self.dat = []
self.vin = VIN_UNKNOWN
def check_response(self, msg):
# have we got a VIN query response?
if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]:
self.never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(msg.dat, self.step, self.cnt):
self.dat += msg.dat[2:] if self.step == 0 else msg.dat[1:]
self.cnt += 1
if self.cnt == self.cnts[self.step]:
self.responded = True
self.step += 1
def send_query(self, sendcan):
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
if self.never_responded or (self.responded and self.step < len(self.cnts)):
sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan'))
sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan'))
self.responded = False
self.cnt = 0
def get_vin(self):
# only report vin if procedure is finished
if self.step == len(self.cnts) and self.cnt == self.cnts[-1]:
self.vin = "".join(self.dat[3:])
return self.vin
def get_vin(logcan, sendcan, bus, query_time=1.):
vin_query = VinQuery(bus)
frame = 0
# 1s max of VIN query time
while frame < query_time * 100:
a = messaging.recv_one(logcan)
for can in a.can:
vin_query.check_response(can)
vin_query.send_query(sendcan)
frame += 1
return vin_query.get_vin()
if __name__ == "__main__":
from selfdrive.services import service_list
logcan = messaging.sub_sock(service_list['can'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
print get_vin(logcan, sendcan, 0)
launch_chffrplus.sh
+12 -12
View File
@@ -12,18 +12,18 @@ fi
function launch {
# apply update
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
git reset --hard @{u} &&
git clean -xdf &&
# Touch all files on release2 after checkout to prevent rebuild
BRANCH=$(git rev-parse --abbrev-ref HEAD)
if [[ "$BRANCH" == "release2" ]]; then
touch **
fi
exec "${BASH_SOURCE[0]}"
fi
# if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
# git reset --hard @{u} &&
# git clean -xdf &&
#
# # Touch all files on release2 after checkout to prevent rebuild
# BRANCH=$(git rev-parse --abbrev-ref HEAD)
# if [[ "$BRANCH" == "release2" ]]; then
# touch **
# fi
#
# exec "${BASH_SOURCE[0]}"
# fi
# no cpu rationing for now
echo 0-3 > /dev/cpuset/background/cpus
models/driving_model.dlc
BIN
View File
Binary file not shown.
models/monitoring_model.dlc
BIN
View File
Binary file not shown.
opendbc/chrysler_pacifica_2017_hybrid.dbc
+2 -1
View File
@@ -262,7 +262,7 @@ BO_ 300 NEW_MSG_12C: 8 XXX
BO_ 308 ACCEL_GAS_134: 8 XXX
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
SG_ ACCEL_134 : 43|4@0+ (1,0) [0|15] "" XXX
SG_ ACCEL_134 : 46|7@0+ (1,0) [0|127] "" XXX
BO_ 532 ENERGY_RELATED_214: 8 XXX
SG_ NOISY_SLOWLY_DECREASING : 16|9@0+ (1,0) [0|255] "" XXX
@@ -413,6 +413,7 @@ CM_ SG_ 825 BEEP_339 "sent every 0.5s. 0050 is no beep. To beep send 4355 or 415
CM_ SG_ 270 ELECTRIC_MOTOR "0x7fff indicates electric motor not in use";
CM_ SG_ 291 ENERGY_GAIN_LOSS "unsure what this actually is";
CM_ SG_ 291 ENERGY_SMOOTHER_CURVE "unusre what it is, but smoother";
CM_ SG_ 308 ACCEL_134 "only set when human presses accel pedal";
CM_ SG_ 532 NOISY_SLOWLY_DECREASING "perhaps battery but do not know";
CM_ SG_ 816 TRACTION_OFF "set when traction off button is enabled";
CM_ SG_ 816 TOGGLE_PARKSENSE "sending 3000071ec0ff9000 enables or disables parksense";
opendbc/hyundai_kia_generic.dbc
+1 -1
View File
@@ -961,7 +961,7 @@ BO_ 64 DATC14: 8 DATC
SG_ DATC_ADSDisp : 28|2@1+ (1.0,0.0) [0.0|3.0] "" CLU
BO_ 832 LKAS11: 8 LDWS_LKAS
SG_ CF_Lkas_Icon : 0|2@1+ (1.0,0.0) [0.0|3.0] "" CLU,IBOX,PSB
SG_ CF_Lkas_Bca_R : 0|2@1+ (1.0,0.0) [0.0|3.0] "" CLU,IBOX,PSB
SG_ CF_Lkas_LdwsSysState : 2|4@1+ (1.0,0.0) [0.0|15.0] "" CLU,IBOX,PSB
SG_ CF_Lkas_SysWarning : 6|4@1+ (1.0,0.0) [0.0|15.0] "" BCM,CLU
SG_ CF_Lkas_LdwsLHWarning : 10|2@1+ (1.0,0.0) [0.0|3.0] "" BCM,CLU,PSB
panda/.circleci/config.yml
-7
View File
@@ -32,13 +32,6 @@ jobs:
- store_artifacts:
name: Store misra test output
path: /tmp/misra/misra_output.txt
- store_artifacts:
name: Store cppcheck safety test output
path: /tmp/misra/cppcheck_safety_output.txt
- store_artifacts:
name: Store misra safety test output
path: /tmp/misra/misra_safety_output.txt
build:
machine:
panda/VERSION
+1 -1
View File
@@ -1 +1 @@
v1.4.1
v1.4.2
panda/board/board.h
+61
View File
@@ -0,0 +1,61 @@
// ///////////////////////////////////////////////////////////// //
// Hardware abstraction layer for all different supported boards //
// ///////////////////////////////////////////////////////////// //
#include "board_declarations.h"
#include "boards/common.h"
// ///// Board definition and detection ///// //
#include "drivers/harness.h"
#ifdef PANDA
#include "boards/white.h"
#include "boards/grey.h"
#include "boards/black.h"
#else
#include "boards/pedal.h"
#endif
void detect_board_type(void) {
#ifdef PANDA
// SPI lines floating: white (TODO: is this reliable?)
if((detect_with_pull(GPIOA, 4, PULL_DOWN)) || (detect_with_pull(GPIOA, 5, PULL_DOWN)) || (detect_with_pull(GPIOA, 6, PULL_DOWN)) || (detect_with_pull(GPIOA, 7, PULL_DOWN))){
hw_type = HW_TYPE_WHITE_PANDA;
current_board = &board_white;
} else if(detect_with_pull(GPIOA, 13, PULL_DOWN)) { // Rev AB deprecated, so no pullup means black. In REV C, A13 is pulled up to 5V with a 10K
hw_type = HW_TYPE_GREY_PANDA;
current_board = &board_grey;
} else {
hw_type = HW_TYPE_BLACK_PANDA;
current_board = &board_black;
}
#else
#ifdef PEDAL
hw_type = HW_TYPE_PEDAL;
current_board = &board_pedal;
#else
hw_type = HW_TYPE_UNKNOWN;
puts("Hardware type is UNKNOWN!\n");
#endif
#endif
}
// ///// Configuration detection ///// //
bool has_external_debug_serial = 0;
bool is_entering_bootmode = 0;
void detect_configuration(void) {
// detect if external serial debugging is present
has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);
#ifdef PANDA
// check if the ESP is trying to put me in boot mode
is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
#else
is_entering_bootmode = 0;
#endif
}
// ///// Board functions ///// //
bool board_has_gps(void) {
return ((hw_type == HW_TYPE_GREY_PANDA) || (hw_type == HW_TYPE_BLACK_PANDA));
}
panda/board/board_declarations.h
+57
View File
@@ -0,0 +1,57 @@
// ******************** Prototypes ********************
typedef void (*board_init)(void);
typedef void (*board_enable_can_transciever)(uint8_t transciever, bool enabled);
typedef void (*board_enable_can_transcievers)(bool enabled);
typedef void (*board_set_led)(uint8_t color, bool enabled);
typedef void (*board_set_usb_power_mode)(uint8_t mode);
typedef void (*board_set_esp_gps_mode)(uint8_t mode);
typedef void (*board_set_can_mode)(uint8_t mode);
typedef void (*board_usb_power_mode_tick)(uint64_t tcnt);
typedef bool (*board_check_ignition)(void);
struct board {
const char *board_type;
const harness_configuration *harness_config;
board_init init;
board_enable_can_transciever enable_can_transciever;
board_enable_can_transcievers enable_can_transcievers;
board_set_led set_led;
board_set_usb_power_mode set_usb_power_mode;
board_set_esp_gps_mode set_esp_gps_mode;
board_set_can_mode set_can_mode;
board_usb_power_mode_tick usb_power_mode_tick;
board_check_ignition check_ignition;
};
// ******************* Definitions ********************
// These should match the enum in cereal/log.capnp
#define HW_TYPE_UNKNOWN 0U
#define HW_TYPE_WHITE_PANDA 1U
#define HW_TYPE_GREY_PANDA 2U
#define HW_TYPE_BLACK_PANDA 3U
#define HW_TYPE_PEDAL 4U
// LED colors
#define LED_RED 0U
#define LED_GREEN 1U
#define LED_BLUE 2U
// USB power modes
#define USB_POWER_NONE 0U
#define USB_POWER_CLIENT 1U
#define USB_POWER_CDP 2U
#define USB_POWER_DCP 3U
// ESP modes
#define ESP_GPS_DISABLED 0U
#define ESP_GPS_ENABLED 1U
#define ESP_GPS_BOOTMODE 2U
// CAN modes
#define CAN_MODE_NORMAL 0U
#define CAN_MODE_GMLAN_CAN2 1U
#define CAN_MODE_GMLAN_CAN3 2U
#define CAN_MODE_OBD_CAN2 3U
// ********************* Globals **********************
uint8_t usb_power_mode = USB_POWER_NONE;
panda/board/boards/black.h
+188
View File
@@ -0,0 +1,188 @@
// ///////////////////// //
// Black Panda + Harness //
// ///////////////////// //
void black_enable_can_transciever(uint8_t transciever, bool enabled) {
switch (transciever){
case 1U:
set_gpio_output(GPIOC, 1, !enabled);
break;
case 2U:
set_gpio_output(GPIOC, 13, !enabled);
break;
case 3U:
set_gpio_output(GPIOA, 0, !enabled);
break;
case 4U:
set_gpio_output(GPIOB, 10, !enabled);
break;
default:
puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
break;
}
}
void black_enable_can_transcievers(bool enabled) {
for(uint8_t i=1; i<=4U; i++)
black_enable_can_transciever(i, enabled);
}
void black_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
}
void black_set_usb_power_mode(uint8_t mode){
usb_power_mode = mode;
puts("Trying to set USB power mode on black panda. This is not supported.\n");
}
void black_set_esp_gps_mode(uint8_t mode) {
switch (mode) {
case ESP_GPS_DISABLED:
// ESP OFF
set_gpio_output(GPIOC, 14, 0);
set_gpio_output(GPIOC, 5, 0);
break;
case ESP_GPS_ENABLED:
// ESP ON
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
break;
case ESP_GPS_BOOTMODE:
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
default:
puts("Invalid ESP/GPS mode\n");
break;
}
}
void black_set_can_mode(uint8_t mode){
switch (mode) {
case CAN_MODE_NORMAL:
case CAN_MODE_OBD_CAN2:
if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(car_harness_status == HARNESS_STATUS_NORMAL)) {
// B12,B13: disable OBD mode
set_gpio_mode(GPIOB, 12, MODE_INPUT);
set_gpio_mode(GPIOB, 13, MODE_INPUT);
// B5,B6: normal CAN2 mode
set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
} else {
// B5,B6: disable normal CAN2 mode
set_gpio_mode(GPIOB, 5, MODE_INPUT);
set_gpio_mode(GPIOB, 6, MODE_INPUT);
// B12,B13: OBD mode
set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
}
break;
default:
puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
break;
}
}
void black_usb_power_mode_tick(uint64_t tcnt){
UNUSED(tcnt);
// Not applicable
}
bool black_check_ignition(void){
// ignition is checked through harness
return harness_check_ignition();
}
void black_init(void) {
common_init_gpio();
// A8,A15: normal CAN3 mode
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
// C0: OBD_SBU1 (orientation detection)
// C3: OBD_SBU2 (orientation detection)
set_gpio_mode(GPIOC, 0, MODE_ANALOG);
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// C10: OBD_SBU1_RELAY (harness relay driving output)
// C11: OBD_SBU2_RELAY (harness relay driving output)
set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
set_gpio_mode(GPIOC, 11, MODE_OUTPUT);
set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(GPIOC, 11, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output(GPIOC, 10, 1);
set_gpio_output(GPIOC, 11, 1);
// C8: FAN aka TIM3_CH3
set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
// C12: GPS load switch. Turn on permanently for now
set_gpio_output(GPIOC, 12, true);
//set_gpio_output(GPIOC, 12, false); //TODO: stupid inverted switch on prototype
// Initialize harness
harness_init();
// Enable CAN transcievers
black_enable_can_transcievers(true);
// Disable LEDs
black_set_led(LED_RED, false);
black_set_led(LED_GREEN, false);
black_set_led(LED_BLUE, false);
// Set normal CAN mode
black_set_can_mode(CAN_MODE_NORMAL);
// flip CAN0 and CAN2 if we are flipped
if (car_harness_status == HARNESS_STATUS_NORMAL) {
can_flip_buses(0, 2);
}
// init multiplexer
can_set_obd(car_harness_status, false);
}
const harness_configuration black_harness_config = {
.has_harness = true,
.GPIO_SBU1 = GPIOC,
.GPIO_SBU2 = GPIOC,
.GPIO_relay_normal = GPIOC,
.GPIO_relay_flipped = GPIOC,
.pin_SBU1 = 0,
.pin_SBU2 = 3,
.pin_relay_normal = 10,
.pin_relay_flipped = 11,
.adc_channel_SBU1 = 10,
.adc_channel_SBU2 = 13
};
const board board_black = {
.board_type = "Black",
.harness_config = &black_harness_config,
.init = black_init,
.enable_can_transciever = black_enable_can_transciever,
.enable_can_transcievers = black_enable_can_transcievers,
.set_led = black_set_led,
.set_usb_power_mode = black_set_usb_power_mode,
.set_esp_gps_mode = black_set_esp_gps_mode,
.set_can_mode = black_set_can_mode,
.usb_power_mode_tick = black_usb_power_mode_tick,
.check_ignition = black_check_ignition
};
panda/board/boards/common.h
+82
View File
@@ -0,0 +1,82 @@
#ifdef STM32F4
#include "stm32f4xx_hal_gpio_ex.h"
#else
#include "stm32f2xx_hal_gpio_ex.h"
#endif
// Common GPIO initialization
void common_init_gpio(void){
// TODO: Is this block actually doing something???
// pull low to hold ESP in reset??
// enable OTG out tied to ground
GPIOA->ODR = 0;
GPIOB->ODR = 0;
GPIOA->PUPDR = 0;
GPIOB->AFR[0] = 0;
GPIOB->AFR[1] = 0;
// C2: Voltage sense line
set_gpio_mode(GPIOC, 2, MODE_ANALOG);
// A11,A12: USB
set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS);
set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
// A9,A10: USART 1 for talking to the ESP / GPS
set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
// B8,B9: CAN 1
#ifdef STM32F4
set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
#else
set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
#endif
}
// Peripheral initialization
void peripherals_init(void){
// enable GPIOB, UART2, CAN, USB clock
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
#ifdef PANDA
RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
#endif
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
#ifdef CAN3
RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
#endif
RCC->APB1ENR |= RCC_APB1ENR_DACEN;
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // main counter
RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // slow loop and pedal
RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // gmlan_alt
//RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
//RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;
RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
//RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
}
// Detection with internal pullup
#define PULL_EFFECTIVE_DELAY 10
bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
set_gpio_mode(GPIO, pin, MODE_INPUT);
set_gpio_pullup(GPIO, pin, mode);
for (volatile int i=0; i<PULL_EFFECTIVE_DELAY; i++);
bool ret = get_gpio_input(GPIO, pin);
set_gpio_pullup(GPIO, pin, PULL_NONE);
return ret;
}
panda/board/boards/grey.h
+18
View File
@@ -0,0 +1,18 @@
// ////////// //
// Grey Panda //
// ////////// //
// Most hardware functionality is similar to white panda
const board board_grey = {
.board_type = "Grey",
.harness_config = &white_harness_config,
.init = white_init,
.enable_can_transciever = white_enable_can_transciever,
.enable_can_transcievers = white_enable_can_transcievers,
.set_led = white_set_led,
.set_usb_power_mode = white_set_usb_power_mode,
.set_esp_gps_mode = white_set_esp_gps_mode,
.set_can_mode = white_set_can_mode,
.usb_power_mode_tick = white_usb_power_mode_tick,
.check_ignition = white_check_ignition
};
panda/board/boards/pedal.h
+96
View File
@@ -0,0 +1,96 @@
// ///// //
// Pedal //
// ///// //
void pedal_enable_can_transciever(uint8_t transciever, bool enabled) {
switch (transciever){
case 1:
set_gpio_output(GPIOB, 3, !enabled);
break;
default:
puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
break;
}
}
void pedal_enable_can_transcievers(bool enabled) {
pedal_enable_can_transciever(1U, enabled);
}
void pedal_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOB, 10, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOB, 11, !enabled);
break;
default:
break;
}
}
void pedal_set_usb_power_mode(uint8_t mode){
usb_power_mode = mode;
puts("Trying to set USB power mode on pedal. This is not supported.\n");
}
void pedal_set_esp_gps_mode(uint8_t mode) {
UNUSED(mode);
puts("Trying to set ESP/GPS mode on pedal. This is not supported.\n");
}
void pedal_set_can_mode(uint8_t mode){
switch (mode) {
case CAN_MODE_NORMAL:
break;
default:
puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
break;
}
}
void pedal_usb_power_mode_tick(uint64_t tcnt){
UNUSED(tcnt);
// Not applicable
}
bool pedal_check_ignition(void){
// not supported on pedal
return false;
}
void pedal_init(void) {
common_init_gpio();
// C0, C1: Throttle inputs
set_gpio_mode(GPIOC, 0, MODE_ANALOG);
set_gpio_mode(GPIOC, 1, MODE_ANALOG);
// DAC outputs on A4 and A5
// apparently they don't need GPIO setup
// Enable transciever
pedal_enable_can_transcievers(true);
// Disable LEDs
pedal_set_led(LED_RED, false);
pedal_set_led(LED_GREEN, false);
}
const harness_configuration pedal_harness_config = {
.has_harness = false
};
const board board_pedal = {
.board_type = "Pedal",
.harness_config = &pedal_harness_config,
.init = pedal_init,
.enable_can_transciever = pedal_enable_can_transciever,
.enable_can_transcievers = pedal_enable_can_transcievers,
.set_led = pedal_set_led,
.set_usb_power_mode = pedal_set_usb_power_mode,
.set_esp_gps_mode = pedal_set_esp_gps_mode,
.set_can_mode = pedal_set_can_mode,
.usb_power_mode_tick = pedal_usb_power_mode_tick,
.check_ignition = pedal_check_ignition,
};
panda/board/boards/white.h
+306
View File
@@ -0,0 +1,306 @@
// /////////// //
// White Panda //
// /////////// //
void white_enable_can_transciever(uint8_t transciever, bool enabled) {
switch (transciever){
case 1U:
set_gpio_output(GPIOC, 1, !enabled);
break;
case 2U:
set_gpio_output(GPIOC, 13, !enabled);
break;
case 3U:
set_gpio_output(GPIOA, 0, !enabled);
break;
default:
puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
break;
}
}
void white_enable_can_transcievers(bool enabled) {
for(uint8_t i=1; i<=3U; i++)
white_enable_can_transciever(i, enabled);
}
void white_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
}
void white_set_usb_power_mode(uint8_t mode){
bool valid_mode = true;
switch (mode) {
case USB_POWER_CLIENT:
// B2,A13: set client mode
set_gpio_output(GPIOB, 2, 0);
set_gpio_output(GPIOA, 13, 1);
break;
case USB_POWER_CDP:
// B2,A13: set CDP mode
set_gpio_output(GPIOB, 2, 1);
set_gpio_output(GPIOA, 13, 1);
break;
case USB_POWER_DCP:
// B2,A13: set DCP mode on the charger (breaks USB!)
set_gpio_output(GPIOB, 2, 0);
set_gpio_output(GPIOA, 13, 0);
break;
default:
valid_mode = false;
puts("Invalid usb power mode\n");
break;
}
if (valid_mode) {
usb_power_mode = mode;
}
}
void white_set_esp_gps_mode(uint8_t mode) {
switch (mode) {
case ESP_GPS_DISABLED:
// ESP OFF
set_gpio_output(GPIOC, 14, 0);
set_gpio_output(GPIOC, 5, 0);
break;
case ESP_GPS_ENABLED:
// ESP ON
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
break;
case ESP_GPS_BOOTMODE:
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
default:
puts("Invalid ESP/GPS mode\n");
break;
}
}
void white_set_can_mode(uint8_t mode){
switch (mode) {
case CAN_MODE_NORMAL:
// B12,B13: disable GMLAN mode
set_gpio_mode(GPIOB, 12, MODE_INPUT);
set_gpio_mode(GPIOB, 13, MODE_INPUT);
// B3,B4: disable GMLAN mode
set_gpio_mode(GPIOB, 3, MODE_INPUT);
set_gpio_mode(GPIOB, 4, MODE_INPUT);
// B5,B6: normal CAN2 mode
set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
// A8,A15: normal CAN3 mode
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
break;
case CAN_MODE_GMLAN_CAN2:
// B5,B6: disable CAN2 mode
set_gpio_mode(GPIOB, 5, MODE_INPUT);
set_gpio_mode(GPIOB, 6, MODE_INPUT);
// B3,B4: disable GMLAN mode
set_gpio_mode(GPIOB, 3, MODE_INPUT);
set_gpio_mode(GPIOB, 4, MODE_INPUT);
// B12,B13: GMLAN mode
set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
// A8,A15: normal CAN3 mode
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
break;
case CAN_MODE_GMLAN_CAN3:
// A8,A15: disable CAN3 mode
set_gpio_mode(GPIOA, 8, MODE_INPUT);
set_gpio_mode(GPIOA, 15, MODE_INPUT);
// B12,B13: disable GMLAN mode
set_gpio_mode(GPIOB, 12, MODE_INPUT);
set_gpio_mode(GPIOB, 13, MODE_INPUT);
// B3,B4: GMLAN mode
set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3);
// B5,B6: normal CAN2 mode
set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
break;
default:
puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
break;
}
}
uint64_t marker = 0;
void white_usb_power_mode_tick(uint64_t tcnt){
#ifndef BOOTSTUB
#define CURRENT_THRESHOLD 0xF00U
#define CLICKS 5U // 5 seconds to switch modes
uint32_t current = adc_get(ADCCHAN_CURRENT);
// ~0x9a = 500 ma
// puth(current); puts("\n");
switch (usb_power_mode) {
case USB_POWER_CLIENT:
if ((tcnt - marker) >= CLICKS) {
if (!is_enumerated) {
puts("USBP: didn't enumerate, switching to CDP mode\n");
// switch to CDP
white_set_usb_power_mode(USB_POWER_CDP);
marker = tcnt;
}
}
// keep resetting the timer if it's enumerated
if (is_enumerated) {
marker = tcnt;
}
break;
case USB_POWER_CDP:
// On the EON, if we get into CDP mode we stay here. No need to go to DCP.
#ifndef EON
// been CLICKS clicks since we switched to CDP
if ((tcnt-marker) >= CLICKS) {
// measure current draw, if positive and no enumeration, switch to DCP
if (!is_enumerated && (current < CURRENT_THRESHOLD)) {
puts("USBP: no enumeration with current draw, switching to DCP mode\n");
white_set_usb_power_mode(USB_POWER_DCP);
marker = tcnt;
}
}
// keep resetting the timer if there's no current draw in CDP
if (current >= CURRENT_THRESHOLD) {
marker = tcnt;
}
#endif
break;
case USB_POWER_DCP:
// been at least CLICKS clicks since we switched to DCP
if ((tcnt-marker) >= CLICKS) {
// if no current draw, switch back to CDP
if (current >= CURRENT_THRESHOLD) {
puts("USBP: no current draw, switching back to CDP mode\n");
white_set_usb_power_mode(USB_POWER_CDP);
marker = tcnt;
}
}
// keep resetting the timer if there's current draw in DCP
if (current < CURRENT_THRESHOLD) {
marker = tcnt;
}
break;
default:
puts("USB power mode invalid\n"); // set_usb_power_mode prevents assigning invalid values
break;
}
#else
UNUSED(tcnt);
#endif
}
bool white_check_ignition(void){
// ignition is on PA1
return !get_gpio_input(GPIOA, 1);
}
void white_init(void) {
common_init_gpio();
// C3: current sense
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// A1: started_alt
set_gpio_pullup(GPIOA, 1, PULL_UP);
// A2, A3: USART 2 for debugging
set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2);
set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2);
// A4, A5, A6, A7: SPI
set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1);
set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
// Set USB power mode
white_set_usb_power_mode(USB_POWER_CLIENT);
// B12: GMLAN, ignition sense, pull up
set_gpio_pullup(GPIOB, 12, PULL_UP);
/* GMLAN mode pins:
M0(B15) M1(B14) mode
=======================
0 0 sleep
1 0 100kbit
0 1 high voltage wakeup
1 1 33kbit (normal)
*/
set_gpio_output(GPIOB, 14, 1);
set_gpio_output(GPIOB, 15, 1);
// B7: K-line enable
set_gpio_output(GPIOB, 7, 1);
// C12, D2: Setup K-line (UART5)
set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5);
set_gpio_alternate(GPIOD, 2, GPIO_AF8_UART5);
set_gpio_pullup(GPIOD, 2, PULL_UP);
// L-line enable
set_gpio_output(GPIOA, 14, 1);
// C10, C11: L-Line setup (USART3)
set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3);
set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
set_gpio_pullup(GPIOC, 11, PULL_UP);
// Enable CAN transcievers
white_enable_can_transcievers(true);
// Disable LEDs
white_set_led(LED_RED, false);
white_set_led(LED_GREEN, false);
white_set_led(LED_BLUE, false);
// Set normal CAN mode
white_set_can_mode(CAN_MODE_NORMAL);
}
const harness_configuration white_harness_config = {
.has_harness = false
};
const board board_white = {
.board_type = "White",
.harness_config = &white_harness_config,
.init = white_init,
.enable_can_transciever = white_enable_can_transciever,
.enable_can_transcievers = white_enable_can_transcievers,
.set_led = white_set_led,
.set_usb_power_mode = white_set_usb_power_mode,
.set_esp_gps_mode = white_set_esp_gps_mode,
.set_can_mode = white_set_can_mode,
.usb_power_mode_tick = white_usb_power_mode_tick,
.check_ignition = white_check_ignition
};
panda/board/bootstub.c
+19 -11
View File
@@ -12,20 +12,29 @@
#include "stm32f2xx_hal_gpio_ex.h"
#endif
// default since there's no serial
void puts(const char *a) {
UNUSED(a);
}
// ******************** Prototypes ********************
void puts(const char *a){ UNUSED(a); }
void puth(unsigned int i){ UNUSED(i); }
void puth2(unsigned int i){ UNUSED(i); }
typedef struct board board;
typedef struct harness_configuration harness_configuration;
// No CAN support on bootloader
void can_flip_buses(uint8_t bus1, uint8_t bus2){UNUSED(bus1); UNUSED(bus2);}
void can_set_obd(int harness_orientation, bool obd){UNUSED(harness_orientation); UNUSED(obd);}
void puth(unsigned int i) {
UNUSED(i);
}
// ********************* Globals **********************
int hw_type = 0;
const board *current_board;
// ********************* Includes *********************
#include "libc.h"
#include "provision.h"
#include "drivers/clock.h"
#include "drivers/llgpio.h"
#include "board.h"
#include "gpio.h"
#include "drivers/spi.h"
@@ -56,11 +65,10 @@ extern void *_app_start[];
int main(void) {
__disable_irq();
clock_init();
detect();
detect_configuration();
detect_board_type();
if (revision == PANDA_REV_C) {
set_usb_power_mode(USB_POWER_CLIENT);
}
current_board->set_usb_power_mode(USB_POWER_CLIENT);
if (enter_bootloader_mode == ENTER_SOFTLOADER_MAGIC) {
enter_bootloader_mode = 0;
panda/board/build.mk
+1 -1
View File
@@ -68,7 +68,7 @@ obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o
$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin
SETLEN=1 ../crypto/sign.py obj/code.bin $@ $(CERT)
@BINSIZE=$$(du -b "obj/$(PROJ_NAME).bin" | cut -f 1) ; \
if [ $$BINSIZE -ge 32768 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi;
if [ $$BINSIZE -ge 49152 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi;
obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o
$(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^
panda/board/drivers/can.h
+71 -32
View File
@@ -33,7 +33,6 @@ bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
// end API
#define ALL_CAN_SILENT 0xFF
#define ALL_CAN_BUT_MAIN_SILENT 0xFE
#define ALL_CAN_LIVE 0
int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_SILENT;
@@ -149,7 +148,6 @@ void can_set_speed(uint8_t can_number) {
void can_init(uint8_t can_number) {
if (can_number != 0xffU) {
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
set_can_enable(CAN, 1);
can_set_speed(can_number);
llcan_init(CAN);
@@ -165,45 +163,86 @@ void can_init_all(void) {
}
}
void can_set_gmlan(uint8_t bus) {
void can_flip_buses(uint8_t bus1, uint8_t bus2){
bus_lookup[bus1] = bus2;
bus_lookup[bus2] = bus1;
can_num_lookup[bus1] = bus2;
can_num_lookup[bus2] = bus1;
}
// first, disable GMLAN on prev bus
uint8_t prev_bus = can_num_lookup[3];
if (bus != prev_bus) {
switch (prev_bus) {
// TODO: Cleanup with new abstraction
void can_set_gmlan(uint8_t bus) {
if(hw_type != HW_TYPE_BLACK_PANDA){
// first, disable GMLAN on prev bus
uint8_t prev_bus = can_num_lookup[3];
if (bus != prev_bus) {
switch (prev_bus) {
case 1:
case 2:
puts("Disable GMLAN on CAN");
puth(prev_bus + 1U);
puts("\n");
current_board->set_can_mode(CAN_MODE_NORMAL);
bus_lookup[prev_bus] = prev_bus;
can_num_lookup[prev_bus] = prev_bus;
can_num_lookup[3] = -1;
can_init(prev_bus);
break;
default:
// GMLAN was not set on either BUS 1 or 2
break;
}
}
// now enable GMLAN on the new bus
switch (bus) {
case 1:
case 2:
puts("Disable GMLAN on CAN");
puth(prev_bus + 1U);
puts("Enable GMLAN on CAN");
puth(bus + 1U);
puts("\n");
set_can_mode(prev_bus, 0);
bus_lookup[prev_bus] = prev_bus;
can_num_lookup[prev_bus] = prev_bus;
can_num_lookup[3] = -1;
can_init(prev_bus);
current_board->set_can_mode((bus == 1U) ? CAN_MODE_GMLAN_CAN2 : CAN_MODE_GMLAN_CAN3);
bus_lookup[bus] = 3;
can_num_lookup[bus] = -1;
can_num_lookup[3] = bus;
can_init(bus);
break;
case 0xFF: //-1 unsigned
break;
default:
// GMLAN was not set on either BUS 1 or 2
puts("GMLAN can only be set on CAN2 or CAN3\n");
break;
}
} else {
puts("GMLAN not available on black panda\n");
}
}
// now enable GMLAN on the new bus
switch (bus) {
case 1:
case 2:
puts("Enable GMLAN on CAN");
puth(bus + 1U);
puts("\n");
set_can_mode(bus, 1);
bus_lookup[bus] = 3;
can_num_lookup[bus] = -1;
can_num_lookup[3] = bus;
can_init(bus);
break;
default:
puts("GMLAN can only be set on CAN2 or CAN3");
break;
// TODO: remove
void can_set_obd(uint8_t harness_orientation, bool obd){
if(obd){
puts("setting CAN2 to be OBD\n");
} else {
puts("setting CAN2 to be normal\n");
}
if(hw_type == HW_TYPE_BLACK_PANDA){
if(obd != (bool)(harness_orientation == HARNESS_STATUS_NORMAL)){
// B5,B6: disable normal mode
set_gpio_mode(GPIOB, 5, MODE_INPUT);
set_gpio_mode(GPIOB, 6, MODE_INPUT);
// B12,B13: CAN2 mode
set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
} else {
// B5,B6: CAN2 mode
set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
// B12,B13: disable normal mode
set_gpio_mode(GPIOB, 12, MODE_INPUT);
set_gpio_mode(GPIOB, 13, MODE_INPUT);
}
} else {
puts("OBD CAN not available on non-black panda\n");
}
}
@@ -326,7 +365,7 @@ void can_rx(uint8_t can_number) {
safety_rx_hook(&to_push);
set_led(LED_BLUE, 1);
current_board->set_led(LED_BLUE, true);
can_send_errs += !can_push(&can_rx_q, &to_push);
// next
panda/board/drivers/harness.h
+130
View File
@@ -0,0 +1,130 @@
uint8_t car_harness_status = 0U;
#define HARNESS_STATUS_NC 0U
#define HARNESS_STATUS_NORMAL 1U
#define HARNESS_STATUS_FLIPPED 2U
// Threshold voltage (mV) for either of the SBUs to be below before deciding harness is connected
#define HARNESS_CONNECTED_THRESHOLD 2500U
struct harness_configuration {
const bool has_harness;
GPIO_TypeDef *GPIO_SBU1;
GPIO_TypeDef *GPIO_SBU2;
GPIO_TypeDef *GPIO_relay_normal;
GPIO_TypeDef *GPIO_relay_flipped;
uint8_t pin_SBU1;
uint8_t pin_SBU2;
uint8_t pin_relay_normal;
uint8_t pin_relay_flipped;
uint8_t adc_channel_SBU1;
uint8_t adc_channel_SBU2;
};
// this function will be the API for tici
void set_intercept_relay(bool intercept) {
if (car_harness_status != HARNESS_STATUS_NC) {
if (intercept) {
puts("switching harness to intercept (relay on)\n");
} else {
puts("switching harness to passthrough (relay off)\n");
}
if(car_harness_status == HARNESS_STATUS_NORMAL){
set_gpio_output(current_board->harness_config->GPIO_relay_normal, current_board->harness_config->pin_relay_normal, !intercept);
} else {
set_gpio_output(current_board->harness_config->GPIO_relay_flipped, current_board->harness_config->pin_relay_flipped, !intercept);
}
}
}
bool harness_check_ignition(void) {
bool ret = false;
switch(car_harness_status){
case HARNESS_STATUS_NORMAL:
ret = !get_gpio_input(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2);
break;
case HARNESS_STATUS_FLIPPED:
ret = !get_gpio_input(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1);
break;
default:
break;
}
return ret;
}
// TODO: refactor to use harness config
void harness_setup_ignition_interrupts(void){
if(car_harness_status == HARNESS_STATUS_NORMAL){
SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI3_PC;
EXTI->IMR |= (1U << 3);
EXTI->RTSR |= (1U << 3);
EXTI->FTSR |= (1U << 3);
puts("setup interrupts: normal\n");
} else if(car_harness_status == HARNESS_STATUS_FLIPPED) {
SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI0_PC;
EXTI->IMR |= (1U << 0);
EXTI->RTSR |= (1U << 0);
EXTI->FTSR |= (1U << 0);
NVIC_EnableIRQ(EXTI1_IRQn);
puts("setup interrupts: flipped\n");
} else {
puts("tried to setup ignition interrupts without harness connected\n");
}
NVIC_EnableIRQ(EXTI0_IRQn);
NVIC_EnableIRQ(EXTI3_IRQn);
}
uint8_t harness_detect_orientation(void) {
uint8_t ret = HARNESS_STATUS_NC;
#ifndef BOOTSTUB
uint32_t sbu1_voltage = adc_get(current_board->harness_config->adc_channel_SBU1);
uint32_t sbu2_voltage = adc_get(current_board->harness_config->adc_channel_SBU2);
// Detect connection and orientation
if((sbu1_voltage < HARNESS_CONNECTED_THRESHOLD) || (sbu2_voltage < HARNESS_CONNECTED_THRESHOLD)){
if (sbu1_voltage < sbu2_voltage) {
// orientation normal
ret = HARNESS_STATUS_NORMAL;
} else {
// orientation flipped
ret = HARNESS_STATUS_FLIPPED;
}
}
#endif
return ret;
}
void harness_init(void) {
// delay such that the connection is fully made before trying orientation detection
current_board->set_led(LED_BLUE, true);
delay(10000000);
current_board->set_led(LED_BLUE, false);
// try to detect orientation
uint8_t ret = harness_detect_orientation();
if (ret != HARNESS_STATUS_NC) {
puts("detected car harness with orientation "); puth2(ret); puts("\n");
car_harness_status = ret;
// set the SBU lines to be inputs before using the relay. The lines are not 5V tolerant in ADC mode!
set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT);
set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT);
// now we have orientation, set pin ignition detection
if(car_harness_status == HARNESS_STATUS_NORMAL){
set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT);
} else {
set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT);
}
// keep busses connected by default
set_intercept_relay(false);
// setup ignition interrupts
harness_setup_ignition_interrupts();
} else {
puts("failed to detect car harness!\n");
}
}
panda/board/drivers/llgpio.h
+11
View File
@@ -7,6 +7,9 @@
#define PULL_UP 1
#define PULL_DOWN 2
#define OUTPUT_TYPE_PUSH_PULL 0U
#define OUTPUT_TYPE_OPEN_DRAIN 1U
void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
uint32_t tmp = GPIO->MODER;
tmp &= ~(3U << (pin * 2U));
@@ -23,6 +26,14 @@ void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) {
set_gpio_mode(GPIO, pin, MODE_OUTPUT);
}
void set_gpio_output_type(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int output_type){
if(output_type == OUTPUT_TYPE_OPEN_DRAIN) {
GPIO->OTYPER |= (1U << pin);
} else {
GPIO->OTYPER &= ~(1U << pin);
}
}
void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
uint32_t tmp = GPIO->AFR[pin >> 3U];
tmp &= ~(0xFU << ((pin & 7U) * 4U));
panda/board/gpio.h
+7 -390
View File
@@ -1,383 +1,4 @@
// this is last place with ifdef PANDA
#ifdef STM32F4
#include "stm32f4xx_hal_gpio_ex.h"
#else
#include "stm32f2xx_hal_gpio_ex.h"
#endif
// ********************* dynamic configuration detection *********************
#define PANDA_REV_AB 0
#define PANDA_REV_C 1
#define PULL_EFFECTIVE_DELAY 10
void puts(const char *a);
bool has_external_debug_serial = 0;
bool is_giant_panda = 0;
bool is_entering_bootmode = 0;
int revision = PANDA_REV_AB;
bool is_grey_panda = 0;
bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
set_gpio_mode(GPIO, pin, MODE_INPUT);
set_gpio_pullup(GPIO, pin, mode);
for (volatile int i=0; i<PULL_EFFECTIVE_DELAY; i++);
bool ret = get_gpio_input(GPIO, pin);
set_gpio_pullup(GPIO, pin, PULL_NONE);
return ret;
}
// must call again from main because BSS is zeroed
void detect(void) {
// detect has_external_debug_serial
has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);
#ifdef PANDA
// detect is_giant_panda
is_giant_panda = detect_with_pull(GPIOB, 1, PULL_DOWN);
// detect panda REV C.
// A13 floats in REV AB. In REV C, A13 is pulled up to 5V with a 10K
// resistor and attached to the USB power control chip CTRL
// line. Pulling A13 down with an internal 50k resistor in REV C
// will produce a voltage divider that results in a high logic
// level. Checking if this pin reads high with a pull down should
// differentiate REV AB from C.
revision = detect_with_pull(GPIOA, 13, PULL_DOWN) ? PANDA_REV_C : PANDA_REV_AB;
// check if the ESP is trying to put me in boot mode
is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
// check if it's a grey panda by seeing if the SPI lines are floating
// TODO: is this reliable?
is_grey_panda = !(detect_with_pull(GPIOA, 4, PULL_DOWN) | detect_with_pull(GPIOA, 5, PULL_DOWN) | detect_with_pull(GPIOA, 6, PULL_DOWN) | detect_with_pull(GPIOA, 7, PULL_DOWN));
#else
// need to do this for early detect
is_giant_panda = 0;
is_grey_panda = 0;
revision = PANDA_REV_AB;
is_entering_bootmode = 0;
#endif
}
// ********************* bringup *********************
void periph_init(void) {
// enable GPIOB, UART2, CAN, USB clock
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
#ifdef PANDA
RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
#endif
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
#ifdef CAN3
RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
#endif
RCC->APB1ENR |= RCC_APB1ENR_DACEN;
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // main counter
RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // slow loop and pedal
RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // gmlan_alt
//RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
//RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;
RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
//RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
}
// ********************* setters *********************
void set_can_enable(CAN_TypeDef *CAN_obj, bool enabled) {
// enable CAN busses
if (CAN_obj == CAN1) {
#ifdef PANDA
// CAN1_EN
set_gpio_output(GPIOC, 1, !enabled);
#else
#ifdef PEDAL
// CAN1_EN (not flipped)
set_gpio_output(GPIOB, 3, !enabled);
#else
// CAN1_EN
set_gpio_output(GPIOB, 3, enabled);
#endif
#endif
} else if (CAN_obj == CAN2) {
#ifdef PANDA
// CAN2_EN
set_gpio_output(GPIOC, 13, !enabled);
#else
// CAN2_EN
set_gpio_output(GPIOB, 4, enabled);
#endif
#ifdef CAN3
} else if (CAN_obj == CAN3) {
// CAN3_EN
set_gpio_output(GPIOA, 0, !enabled);
#endif
} else {
puts("Invalid CAN: enabling failed\n");
}
}
#ifdef PANDA
#define LED_RED 9
#define LED_GREEN 7
#define LED_BLUE 6
#else
#define LED_RED 10
#define LED_GREEN 11
#define LED_BLUE -1
#endif
void set_led(int led_num, int on) {
if (led_num != -1) {
#ifdef PANDA
set_gpio_output(GPIOC, led_num, !on);
#else
set_gpio_output(GPIOB, led_num, !on);
#endif
}
}
void set_can_mode(int can, bool use_gmlan) {
// connects to CAN2 xcvr or GMLAN xcvr
if (use_gmlan) {
if (can == 1) {
// B5,B6: disable normal mode
set_gpio_mode(GPIOB, 5, MODE_INPUT);
set_gpio_mode(GPIOB, 6, MODE_INPUT);
// B12,B13: gmlan mode
set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
#ifdef CAN3
} else if (can == 2) {
// A8,A15: disable normal mode
set_gpio_mode(GPIOA, 8, MODE_INPUT);
set_gpio_mode(GPIOA, 15, MODE_INPUT);
// B3,B4: enable gmlan mode
set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3);
#endif
} else {
puts("Invalid CAN: mode setting failed\n");
}
} else {
if (can == 1) {
// B12,B13: disable gmlan mode
set_gpio_mode(GPIOB, 12, MODE_INPUT);
set_gpio_mode(GPIOB, 13, MODE_INPUT);
// B5,B6: normal mode
set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
#ifdef CAN3
} else if (can == 2) {
// B3,B4: disable gmlan mode
set_gpio_mode(GPIOB, 3, MODE_INPUT);
set_gpio_mode(GPIOB, 4, MODE_INPUT);
// A8,A15: normal mode
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
#endif
} else {
puts("Invalid CAN: mode setting failed\n");
}
}
}
#define USB_POWER_NONE 0
#define USB_POWER_CLIENT 1
#define USB_POWER_CDP 2
#define USB_POWER_DCP 3
int usb_power_mode = USB_POWER_NONE;
void set_usb_power_mode(int mode) {
bool valid_mode = true;
switch (mode) {
case USB_POWER_CLIENT:
// B2,A13: set client mode
set_gpio_output(GPIOB, 2, 0);
set_gpio_output(GPIOA, 13, 1);
break;
case USB_POWER_CDP:
// B2,A13: set CDP mode
set_gpio_output(GPIOB, 2, 1);
set_gpio_output(GPIOA, 13, 1);
break;
case USB_POWER_DCP:
// B2,A13: set DCP mode on the charger (breaks USB!)
set_gpio_output(GPIOB, 2, 0);
set_gpio_output(GPIOA, 13, 0);
break;
default:
valid_mode = false;
puts("Invalid usb power mode\n");
break;
}
if (valid_mode) {
usb_power_mode = mode;
}
}
#define ESP_DISABLED 0
#define ESP_ENABLED 1
#define ESP_BOOTMODE 2
void set_esp_mode(int mode) {
switch (mode) {
case ESP_DISABLED:
// ESP OFF
set_gpio_output(GPIOC, 14, 0);
set_gpio_output(GPIOC, 5, 0);
break;
case ESP_ENABLED:
// ESP ON
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
break;
case ESP_BOOTMODE:
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
default:
puts("Invalid esp mode\n");
break;
}
}
// ********************* big init function *********************
// board specific
void gpio_init(void) {
// pull low to hold ESP in reset??
// enable OTG out tied to ground
GPIOA->ODR = 0;
GPIOB->ODR = 0;
GPIOA->PUPDR = 0;
//GPIOC->ODR = 0;
GPIOB->AFR[0] = 0;
GPIOB->AFR[1] = 0;
// C2,C3: analog mode, voltage and current sense
set_gpio_mode(GPIOC, 2, MODE_ANALOG);
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
#ifdef PEDAL
// comma pedal has inputs on C0 and C1
set_gpio_mode(GPIOC, 0, MODE_ANALOG);
set_gpio_mode(GPIOC, 1, MODE_ANALOG);
// DAC outputs on A4 and A5
// apparently they don't need GPIO setup
#endif
// C8: FAN aka TIM3_CH4
set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
// turn off LEDs and set mode
set_led(LED_RED, 0);
set_led(LED_GREEN, 0);
set_led(LED_BLUE, 0);
// A11,A12: USB
set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS);
set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
#ifdef PANDA
// enable started_alt on the panda
set_gpio_pullup(GPIOA, 1, PULL_UP);
// A2,A3: USART 2 for debugging
set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2);
set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2);
// A9,A10: USART 1 for talking to the ESP
set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
// B12: GMLAN, ignition sense, pull up
set_gpio_pullup(GPIOB, 12, PULL_UP);
// A4,A5,A6,A7: setup SPI
set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1);
set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
#endif
// B8,B9: CAN 1
#ifdef STM32F4
set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
#else
set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
#endif
set_can_enable(CAN1, 1);
// B5,B6: CAN 2
set_can_mode(1, 0);
set_can_enable(CAN2, 1);
// A8,A15: CAN 3
#ifdef CAN3
set_can_mode(2, 0);
set_can_enable(CAN3, 1);
#endif
/* GMLAN mode pins:
M0(B15) M1(B14) mode
=======================
0 0 sleep
1 0 100kbit
0 1 high voltage wakeup
1 1 33kbit (normal)
*/
// put gmlan transceiver in normal mode
set_gpio_output(GPIOB, 14, 1);
set_gpio_output(GPIOB, 15, 1);
#ifdef PANDA
// K-line enable moved from B4->B7 to make room for GMLAN on CAN3
set_gpio_output(GPIOB, 7, 1); // REV C
// C12,D2: K-Line setup on UART 5
set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5);
set_gpio_alternate(GPIOD, 2, GPIO_AF8_UART5);
set_gpio_pullup(GPIOD, 2, PULL_UP);
// L-line enable
set_gpio_output(GPIOA, 14, 1);
// C10,C11: L-Line setup on USART 3
set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3);
set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
set_gpio_pullup(GPIOC, 11, PULL_UP);
#endif
set_usb_power_mode(USB_POWER_CLIENT);
}
// ********************* early bringup *********************
// Early bringup
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
#define ENTER_SOFTLOADER_MAGIC 0xdeadc0de
#define BOOT_NORMAL 0xdeadb111
@@ -429,29 +50,25 @@ void early(void) {
GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0;
GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0;
detect();
detect_configuration();
detect_board_type();
#ifdef PANDA
// enable the ESP, disable ESP boot mode
// unless we are on a giant panda, then there's no ESP
// dont disable on grey panda
if (is_giant_panda) {
set_esp_mode(ESP_DISABLED);
} else {
set_esp_mode(ESP_ENABLED);
}
current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
#endif
if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
#ifdef PANDA
set_esp_mode(ESP_DISABLED);
current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
#endif
set_led(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
jump_to_bootloader();
}
if (is_entering_bootmode) {
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
}
}
}
panda/board/main.c
+199 -190
View File
@@ -1,34 +1,39 @@
//#define EON
//#define EON
//#define PANDA
// ********************* Includes *********************
#include "config.h"
#include "obj/gitversion.h"
// ********************* includes *********************
#include "libc.h"
#include "provision.h"
#include "main_declarations.h"
#include "drivers/llcan.h"
#include "drivers/llgpio.h"
#include "gpio.h"
#include "drivers/adc.h"
#include "board.h"
#include "drivers/uart.h"
#include "drivers/adc.h"
#include "drivers/usb.h"
#include "drivers/gmlan_alt.h"
#include "drivers/timer.h"
#include "drivers/clock.h"
#include "gpio.h"
#ifndef EON
#include "drivers/spi.h"
#endif
#include "power_saving.h"
#include "safety.h"
#include "drivers/can.h"
// ********************* serial debugging *********************
// ********************* Serial debugging *********************
void debug_ring_callback(uart_ring *ring) {
char rcv;
@@ -49,30 +54,23 @@ void debug_ring_callback(uart_ring *ring) {
// enable CDP mode
if (rcv == 'C') {
puts("switching USB to CDP mode\n");
set_usb_power_mode(USB_POWER_CDP);
current_board->set_usb_power_mode(USB_POWER_CDP);
}
if (rcv == 'c') {
puts("switching USB to client mode\n");
set_usb_power_mode(USB_POWER_CLIENT);
current_board->set_usb_power_mode(USB_POWER_CLIENT);
}
if (rcv == 'D') {
puts("switching USB to DCP mode\n");
set_usb_power_mode(USB_POWER_DCP);
current_board->set_usb_power_mode(USB_POWER_DCP);
}
}
}
// ***************************** started logic *****************************
bool is_gpio_started(void) {
// ignition is on PA1
return (GPIOA->IDR & (1U << 1)) == 0;
}
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void EXTI1_IRQHandler(void) {
volatile unsigned int pr = EXTI->PR & (1U << 1);
if ((pr & (1U << 1)) != 0U) {
void started_interrupt_handler(uint8_t interrupt_line) {
volatile unsigned int pr = EXTI->PR & (1U << interrupt_line);
if ((pr & (1U << interrupt_line)) != 0U) {
#ifdef DEBUG
puts("got started interrupt\n");
#endif
@@ -81,10 +79,25 @@ void EXTI1_IRQHandler(void) {
delay(100000);
// set power savings mode here
int power_save_state = is_gpio_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
int power_save_state = current_board->check_ignition() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
set_power_save_state(power_save_state);
EXTI->PR = (1U << 1);
}
EXTI->PR = (1U << interrupt_line);
}
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void EXTI0_IRQHandler(void) {
started_interrupt_handler(0);
}
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void EXTI1_IRQHandler(void) {
started_interrupt_handler(1);
}
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void EXTI3_IRQHandler(void) {
started_interrupt_handler(3);
}
void started_interrupt_init(void) {
@@ -95,18 +108,64 @@ void started_interrupt_init(void) {
NVIC_EnableIRQ(EXTI1_IRQn);
}
// ****************************** safety mode ******************************
// this is the only way to leave silent mode
void set_safety_mode(uint16_t mode, int16_t param) {
int err = safety_set_mode(mode, param);
if (err == -1) {
puts("Error: safety set mode failed\n");
} else {
if (mode == SAFETY_NOOUTPUT) {
can_silent = ALL_CAN_SILENT;
} else {
can_silent = ALL_CAN_LIVE;
}
switch (mode) {
case SAFETY_NOOUTPUT:
set_intercept_relay(false);
if(hw_type == HW_TYPE_BLACK_PANDA){
current_board->set_can_mode(CAN_MODE_NORMAL);
}
break;
case SAFETY_ELM327:
set_intercept_relay(false);
if(hw_type == HW_TYPE_BLACK_PANDA){
current_board->set_can_mode(CAN_MODE_OBD_CAN2);
}
break;
default:
set_intercept_relay(true);
if(hw_type == HW_TYPE_BLACK_PANDA){
current_board->set_can_mode(CAN_MODE_NORMAL);
}
break;
}
if (safety_ignition_hook() != -1) {
// if the ignition hook depends on something other than the started GPIO
// we have to disable power savings (fix for GM and Tesla)
set_power_save_state(POWER_SAVE_STATUS_DISABLED);
} else {
// power mode is already POWER_SAVE_STATUS_DISABLED and CAN TXs are active
}
can_init_all();
}
}
// ***************************** USB port *****************************
int get_health_pkt(void *dat) {
struct __attribute__((packed)) {
uint32_t voltage_pkt;
uint32_t current_pkt;
uint8_t started_pkt;
uint8_t controls_allowed_pkt;
uint8_t gas_interceptor_detected_pkt;
uint32_t can_send_errs_pkt;
uint32_t can_fwd_errs_pkt;
uint32_t gmlan_send_errs_pkt;
uint8_t started_pkt;
uint8_t controls_allowed_pkt;
uint8_t gas_interceptor_detected_pkt;
uint8_t car_harness_status_pkt;
} *health = dat;
//Voltage will be measured in mv. 5000 = 5V
@@ -121,11 +180,17 @@ int get_health_pkt(void *dat) {
// Avoid needing floating point math
health->voltage_pkt = (voltage * 8862U) / 1000U;
health->current_pkt = adc_get(ADCCHAN_CURRENT);
// No current sense on panda black
if(hw_type != HW_TYPE_BLACK_PANDA){
health->current_pkt = adc_get(ADCCHAN_CURRENT);
} else {
health->current_pkt = 0;
}
int safety_ignition = safety_ignition_hook();
if (safety_ignition < 0) {
//Use the GPIO pin to determine ignition
health->started_pkt = is_gpio_started();
health->started_pkt = (uint8_t)(current_board->check_ignition());
} else {
//Current safety hooks want to determine ignition (ex: GM)
health->started_pkt = safety_ignition;
@@ -136,7 +201,8 @@ int get_health_pkt(void *dat) {
health->can_send_errs_pkt = can_send_errs;
health->can_fwd_errs_pkt = can_fwd_errs;
health->gmlan_send_errs_pkt = gmlan_send_errs;
health->car_harness_status_pkt = car_harness_status;
return sizeof(*health);
}
@@ -183,8 +249,6 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
}
}
bool is_enumerated = 0;
void usb_cb_enumeration_complete() {
puts("USB enumeration complete\n");
is_enumerated = 1;
@@ -203,9 +267,9 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
puts(" err: "); puth(can_err_cnt);
puts("\n");
break;
// **** 0xc1: is grey panda
// **** 0xc1: get hardware type
case 0xc1:
resp[0] = is_grey_panda;
resp[0] = hw_type;
resp_len = 1;
break;
// **** 0xd0: fetch serial number
@@ -258,73 +322,57 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
// **** 0xd9: set ESP power
case 0xd9:
if (setup->b.wValue.w == 1U) {
set_esp_mode(ESP_ENABLED);
current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
} else if (setup->b.wValue.w == 2U) {
set_esp_mode(ESP_BOOTMODE);
current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
} else {
set_esp_mode(ESP_DISABLED);
current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
}
break;
// **** 0xda: reset ESP, with optional boot mode
case 0xda:
set_esp_mode(ESP_DISABLED);
current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
delay(1000000);
if (setup->b.wValue.w == 1U) {
set_esp_mode(ESP_BOOTMODE);
current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
} else {
set_esp_mode(ESP_ENABLED);
current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
}
delay(1000000);
set_esp_mode(ESP_ENABLED);
current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
break;
// **** 0xdb: set GMLAN multiplexing mode
// **** 0xdb: set GMLAN (white/grey) or OBD CAN (black) multiplexing mode
case 0xdb:
if (setup->b.wValue.w == 1U) {
// GMLAN ON
if (setup->b.wIndex.w == 1U) {
can_set_gmlan(1);
} else if (setup->b.wIndex.w == 2U) {
can_set_gmlan(2);
if(hw_type == HW_TYPE_BLACK_PANDA){
if (setup->b.wValue.w == 1U) {
// Enable OBD CAN
current_board->set_can_mode(CAN_MODE_OBD_CAN2);
} else {
puts("Invalid bus num for GMLAN CAN set\n");
}
// Disable OBD CAN
current_board->set_can_mode(CAN_MODE_NORMAL);
}
} else {
can_set_gmlan(-1);
if (setup->b.wValue.w == 1U) {
// GMLAN ON
if (setup->b.wIndex.w == 1U) {
can_set_gmlan(1);
} else if (setup->b.wIndex.w == 2U) {
can_set_gmlan(2);
} else {
puts("Invalid bus num for GMLAN CAN set\n");
}
} else {
can_set_gmlan(-1);
}
}
break;
// **** 0xdc: set safety mode
case 0xdc:
// this is the only way to leave silent mode
// and it's blocked over WiFi
// Allow ELM security mode to be set over wifi.
// Blocked over WiFi.
// Allow NOOUTPUT and ELM security mode to be set over wifi.
if (hardwired || (setup->b.wValue.w == SAFETY_NOOUTPUT) || (setup->b.wValue.w == SAFETY_ELM327)) {
int err = safety_set_mode(setup->b.wValue.w, (int16_t)setup->b.wIndex.w);
if (err == -1) {
puts("Error: safety set mode failed\n");
} else {
#ifndef EON
// always LIVE on EON
switch (setup->b.wValue.w) {
case SAFETY_NOOUTPUT:
can_silent = ALL_CAN_SILENT;
break;
case SAFETY_ELM327:
can_silent = ALL_CAN_BUT_MAIN_SILENT;
break;
default:
can_silent = ALL_CAN_LIVE;
break;
}
#endif
if (safety_ignition_hook() != -1) {
// if the ignition hook depends on something other than the started GPIO
// we have to disable power savings (fix for GM and Tesla)
set_power_save_state(POWER_SAVE_STATUS_DISABLED);
} else {
// power mode is already POWER_SAVE_STATUS_DISABLED and CAN TXs are active
}
can_init_all();
}
set_safety_mode(setup->b.wValue.w, (uint16_t) setup->b.wIndex.w);
}
break;
// **** 0xdd: enable can forwarding
@@ -418,13 +466,13 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
case 0xe6:
if (setup->b.wValue.w == 1U) {
puts("user setting CDP mode\n");
set_usb_power_mode(USB_POWER_CDP);
current_board->set_usb_power_mode(USB_POWER_CDP);
} else if (setup->b.wValue.w == 2U) {
puts("user setting DCP mode\n");
set_usb_power_mode(USB_POWER_DCP);
current_board->set_usb_power_mode(USB_POWER_DCP);
} else {
puts("user setting CLIENT mode\n");
set_usb_power_mode(USB_POWER_CLIENT);
current_board->set_usb_power_mode(USB_POWER_CLIENT);
}
break;
// **** 0xf0: do k-line wValue pulse on uart2 for Acura
@@ -482,6 +530,12 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
}
break;
}
// **** 0xf3: Heartbeat. Resets heartbeat counter.
case 0xf3:
{
heartbeat_counter = 0U;
break;
}
default:
puts("NO HANDLER ");
puth(setup->b.bRequest);
@@ -536,95 +590,52 @@ void __attribute__ ((noinline)) enable_fpu(void) {
}
uint64_t tcnt = 0;
uint64_t marker = 0;
// go into NOOUTPUT when the EON does not send a heartbeat for this amount of seconds.
#define EON_HEARTBEAT_THRESHOLD_IGNITION_ON 5U
#define EON_HEARTBEAT_THRESHOLD_IGNITION_OFF 2U
// called once per second
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void TIM3_IRQHandler(void) {
#define CURRENT_THRESHOLD 0xF00U
#define CLICKS 5U // 5 seconds to switch modes
if (TIM3->SR != 0) {
can_live = pending_can_live;
current_board->usb_power_mode_tick(tcnt);
//puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n");
uint32_t current = adc_get(ADCCHAN_CURRENT);
switch (usb_power_mode) {
case USB_POWER_CLIENT:
if ((tcnt - marker) >= CLICKS) {
if (!is_enumerated) {
puts("USBP: didn't enumerate, switching to CDP mode\n");
// switch to CDP
set_usb_power_mode(USB_POWER_CDP);
marker = tcnt;
}
}
// keep resetting the timer if it's enumerated
if (is_enumerated) {
marker = tcnt;
}
break;
case USB_POWER_CDP:
// On the EON, if we get into CDP mode we stay here. No need to go to DCP.
#ifndef EON
// been CLICKS clicks since we switched to CDP
if ((tcnt-marker) >= CLICKS) {
// measure current draw, if positive and no enumeration, switch to DCP
if (!is_enumerated && (current < CURRENT_THRESHOLD)) {
puts("USBP: no enumeration with current draw, switching to DCP mode\n");
set_usb_power_mode(USB_POWER_DCP);
marker = tcnt;
}
}
// keep resetting the timer if there's no current draw in CDP
if (current >= CURRENT_THRESHOLD) {
marker = tcnt;
}
#endif
break;
case USB_POWER_DCP:
// been at least CLICKS clicks since we switched to DCP
if ((tcnt-marker) >= CLICKS) {
// if no current draw, switch back to CDP
if (current >= CURRENT_THRESHOLD) {
puts("USBP: no current draw, switching back to CDP mode\n");
set_usb_power_mode(USB_POWER_CDP);
marker = tcnt;
}
}
// keep resetting the timer if there's current draw in DCP
if (current < CURRENT_THRESHOLD) {
marker = tcnt;
}
break;
default:
puts("USB power mode invalid\n"); // set_usb_power_mode prevents assigning invalid values
break;
}
// ~0x9a = 500 ma
/*puth(current);
puts("\n");*/
// reset this every 16th pass
if ((tcnt & 0xFU) == 0U) {
pending_can_live = 0;
}
#ifdef DEBUG
puts("** blink ");
puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" ");
puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" ");
puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
//TODO: re-enable
//puts("** blink ");
//puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" ");
//puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" ");
//puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
#endif
// set green LED to be controls allowed
set_led(LED_GREEN, controls_allowed);
current_board->set_led(LED_GREEN, controls_allowed);
// turn off the blue LED, turned on by CAN
// unless we are in power saving mode
set_led(LED_BLUE, (tcnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
current_board->set_led(LED_BLUE, (tcnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
// increase heartbeat counter and cap it at the uint32 limit
if (heartbeat_counter < __UINT32_MAX__) {
heartbeat_counter += 1U;
}
// check heartbeat counter if we are running EON code. If the heartbeat has been gone for a while, go to NOOUTPUT safety mode.
#ifdef EON
if (heartbeat_counter >= (current_board->check_ignition() ? EON_HEARTBEAT_THRESHOLD_IGNITION_ON : EON_HEARTBEAT_THRESHOLD_IGNITION_OFF)) {
puts("EON hasn't sent a heartbeat for 0x"); puth(heartbeat_counter); puts(" seconds. Safety is set to NOOUTPUT mode.\n");
set_safety_mode(SAFETY_NOOUTPUT, 0U);
}
#endif
// on to the next one
tcnt += 1U;
@@ -638,26 +649,27 @@ int main(void) {
// init early devices
clock_init();
periph_init();
detect();
peripherals_init();
detect_configuration();
detect_board_type();
adc_init();
// print hello
puts("\n\n\n************************ MAIN START ************************\n");
// detect the revision and init the GPIOs
puts("config:\n");
puts((revision == PANDA_REV_C) ? " panda rev c\n" : " panda rev a or b\n");
puts(has_external_debug_serial ? " real serial\n" : " USB serial\n");
puts(is_giant_panda ? " GIANTpanda detected\n" : " not GIANTpanda\n");
puts(is_grey_panda ? " gray panda detected!\n" : " white panda\n");
puts(is_entering_bootmode ? " ESP wants bootmode\n" : " no bootmode\n");
// non rev c panda are no longer supported
while (revision != PANDA_REV_C) {
// hang
// check for non-supported board types
if(hw_type == HW_TYPE_UNKNOWN){
puts("Unsupported board type\n");
while (1) { /* hang */ }
}
gpio_init();
puts("Config:\n");
puts(" Board type: "); puts(current_board->board_type); puts("\n");
puts(has_external_debug_serial ? " Real serial\n" : " USB serial\n");
puts(is_entering_bootmode ? " ESP wants bootmode\n" : " No bootmode\n");
// init board
current_board->init();
// panda has an FPU, let's use it!
enable_fpu();
@@ -669,18 +681,21 @@ int main(void) {
uart_init(USART2, 115200);
}
if (is_grey_panda) {
if (board_has_gps()) {
uart_init(USART1, 9600);
} else {
// enable ESP uart
uart_init(USART1, 115200);
}
// enable LIN
uart_init(UART5, 10400);
UART5->CR2 |= USART_CR2_LINEN;
uart_init(USART3, 10400);
USART3->CR2 |= USART_CR2_LINEN;
// there is no LIN on panda black
if(hw_type != HW_TYPE_BLACK_PANDA){
// enable LIN
uart_init(UART5, 10400);
UART5->CR2 |= USART_CR2_LINEN;
uart_init(USART3, 10400);
USART3->CR2 |= USART_CR2_LINEN;
}
// init microsecond system timer
// increments 1000000 times per second
@@ -690,9 +705,6 @@ int main(void) {
TIM2->EGR = TIM_EGR_UG;
// use TIM2->CNT to read
// enable USB
usb_init();
// default to silent mode to prevent issues with Ford
// hardcode a specific safety mode if you want to force the panda to be in a specific mode
int err = safety_set_mode(SAFETY_NOOUTPUT, 0);
@@ -702,31 +714,27 @@ int main(void) {
// if SAFETY_NOOUTPUT isn't succesfully set, we can't continue
}
}
#ifdef EON
// if we're on an EON, it's fine for CAN to be live for fingerprinting
can_silent = ALL_CAN_LIVE;
#else
can_silent = ALL_CAN_SILENT;
#endif
can_init_all();
adc_init();
#ifndef EON
spi_init();
#endif
#ifdef EON
// have to save power
if (!is_grey_panda) {
set_esp_mode(ESP_DISABLED);
if (hw_type == HW_TYPE_WHITE_PANDA) {
current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
}
// only enter power save after the first cycle
/*if (is_gpio_started()) {
/*if (current_board->check_ignition()) {
set_power_save_state(POWER_SAVE_STATUS_ENABLED);
}*/
// interrupt on started line
started_interrupt_init();
if (hw_type != HW_TYPE_BLACK_PANDA) {
// interrupt on started line
started_interrupt_init();
}
#endif
// 48mhz / 65536 ~= 732 / 732 = 1
@@ -736,6 +744,8 @@ int main(void) {
#ifdef DEBUG
puts("DEBUG ENABLED\n");
#endif
// enable USB (right before interrupts or enum can fail!)
usb_init();
puts("**** INTERRUPTS ON ****\n");
enable_interrupts();
@@ -751,9 +761,9 @@ int main(void) {
for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) {
for (int fade = 0; fade < 1024; fade += 8) {
for (int i = 0; i < (128/div_mode); i++) {
set_led(LED_RED, 1);
current_board->set_led(LED_RED, 1);
if (fade < 512) { delay(fade); } else { delay(1024-fade); }
set_led(LED_RED, 0);
current_board->set_led(LED_RED, 0);
if (fade < 512) { delay(512-fade); } else { delay(fade-512); }
}
}
@@ -765,4 +775,3 @@ int main(void) {
return 0;
}
panda/board/main_declarations.h
+14
View File
@@ -0,0 +1,14 @@
// ******************** Prototypes ********************
void puts(const char *a);
void puth(unsigned int i);
void puth2(unsigned int i);
typedef struct board board;
typedef struct harness_configuration harness_configuration;
void can_flip_buses(uint8_t bus1, uint8_t bus2);
void can_set_obd(uint8_t harness_orientation, bool obd);
// ********************* Globals **********************
uint8_t hw_type = 0;
const board *current_board;
bool is_enumerated = 0;
uint32_t heartbeat_counter = 0;
panda/board/pedal/main.c
+16 -6
View File
@@ -1,14 +1,20 @@
// ********************* Includes *********************
#include "../config.h"
#include "libc.h"
#include "main_declarations.h"
#include "drivers/llcan.h"
#include "drivers/llgpio.h"
#include "drivers/clock.h"
#include "drivers/adc.h"
#include "board.h"
#include "drivers/clock.h"
#include "drivers/dac.h"
#include "drivers/timer.h"
#include "gpio.h"
#include "libc.h"
#define CAN CAN1
@@ -25,6 +31,9 @@
void puth(unsigned int i) {
UNUSED(i);
}
void puth2(unsigned int i) {
UNUSED(i);
}
#endif
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
@@ -180,7 +189,7 @@ void CAN1_RX0_IRQHandler(void) {
if (((current_index + 1U) & COUNTER_CYCLE) == index) {
#ifdef DEBUG
puts("setting gas ");
puth(value);
puth(value_0);
puts("\n");
#endif
if (enable) {
@@ -257,7 +266,7 @@ void TIM3_IRQHandler(void) {
}
// blink the LED
set_led(LED_GREEN, led_value);
current_board->set_led(LED_GREEN, led_value);
led_value = !led_value;
TIM3->SR = 0;
@@ -294,8 +303,9 @@ int main(void) {
// init devices
clock_init();
periph_init();
gpio_init();
peripherals_init();
detect_configuration();
detect_board_type();
#ifdef PEDAL_USB
// enable USB
panda/board/pedal/main_declarations.h
+11
View File
@@ -0,0 +1,11 @@
// ******************** Prototypes ********************
void puts(const char *a);
void puth(unsigned int i);
void puth2(unsigned int i);
typedef struct board board;
typedef struct harness_configuration harness_configuration;
// ********************* Globals **********************
uint8_t hw_type = 0;
const board *current_board;
bool is_enumerated = 0;
panda/board/power_saving.h
+12 -12
View File
@@ -10,14 +10,14 @@ void set_power_save_state(int state) {
bool enable = false;
if (state == POWER_SAVE_STATUS_ENABLED) {
puts("enable power savings\n");
if (is_grey_panda) {
if (board_has_gps()) {
char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
uart_ring *ur = get_ring_by_number(1);
for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1U; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
}
} else {
puts("disable power savings\n");
if (is_grey_panda) {
if (board_has_gps()) {
char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
uart_ring *ur = get_ring_by_number(1);
for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1U; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
@@ -25,18 +25,18 @@ void set_power_save_state(int state) {
enable = true;
}
// turn on can
set_can_enable(CAN1, enable);
set_can_enable(CAN2, enable);
set_can_enable(CAN3, enable);
// Switch CAN transcievers
current_board->enable_can_transcievers(enable);
// turn on GMLAN
set_gpio_output(GPIOB, 14, enable);
set_gpio_output(GPIOB, 15, enable);
if(hw_type != HW_TYPE_BLACK_PANDA){
// turn on GMLAN
set_gpio_output(GPIOB, 14, enable);
set_gpio_output(GPIOB, 15, enable);
// turn on LIN
set_gpio_output(GPIOB, 7, enable);
set_gpio_output(GPIOA, 14, enable);
// turn on LIN
set_gpio_output(GPIOB, 7, enable);
set_gpio_output(GPIOA, 14, enable);
}
power_save_status = state;
}
panda/board/safety/safety_elm327.h
-6
View File
@@ -1,15 +1,9 @@
static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int bus = GET_BUS(to_send);
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
//All ELM traffic must appear on CAN0
if (bus != 0) {
tx = 0;
}
//All ISO 15765-4 messages must be 8 bytes long
if (len != 8) {
tx = 0;
panda/board/safety/safety_honda.h
+11 -8
View File
@@ -17,7 +17,7 @@ bool honda_alt_brake_msg = false;
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
//int len = GET_LEN(to_push);
// sample speed
if (addr == 0x158) {
@@ -60,7 +60,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if ((addr == 0x201) && (len == 6)) {
if (false) {
gas_interceptor_detected = 1;
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
@@ -72,7 +72,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press if no interceptor
if (!gas_interceptor_detected) {
if (false) {
if (addr == 0x17C) {
int gas = GET_BYTE(to_push, 0);
if (gas && !(honda_gas_prev) && long_controls_allowed) {
@@ -136,8 +136,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
int bus_pt = ((hw_type == HW_TYPE_BLACK_PANDA) && honda_bosch_hardware)? 1 : 0;
if ((addr == 0x296) && honda_bosch_hardware &&
!current_controls_allowed && (bus == 0)) {
!current_controls_allowed && (bus == bus_pt)) {
if (((GET_BYTE(to_send, 0) >> 5) & 0x7) != 2) {
tx = 0;
}
@@ -186,15 +187,17 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
int bus_rdr_cam = (hw_type == HW_TYPE_BLACK_PANDA) ? 2 : 1; // radar bus, camera side
int bus_rdr_car = (hw_type == HW_TYPE_BLACK_PANDA) ? 0 : 2; // radar bus, car side
if (bus_num == 2) {
bus_fwd = 1;
if (bus_num == bus_rdr_car) {
bus_fwd = bus_rdr_cam;
}
if (bus_num == 1) {
if (bus_num == bus_rdr_cam) {
int addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
if (!is_lkas_msg) {
bus_fwd = 2;
bus_fwd = bus_rdr_car;
}
}
return bus_fwd;
panda/board/safety/safety_toyota.h
+2 -2
View File
@@ -67,7 +67,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of interceptor gas press
if (addr == 0x201) {
if (false) {
gas_interceptor_detected = 1;
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
@@ -79,7 +79,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press
if (addr == 0x2C1) {
if (false) {
int gas = GET_BYTE(to_push, 6) & 0xFF;
if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected && long_controls_allowed) {
controls_allowed = 0;
panda/board/spi_flasher.h
+9 -9
View File
@@ -31,7 +31,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
FLASH->KEYR = 0xCDEF89AB;
resp[1] = 0xff;
}
set_led(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
unlocked = 1;
prog_ptr = (uint32_t *)0x8004000;
break;
@@ -112,7 +112,7 @@ void usb_cb_enumeration_complete(void) {
void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
set_led(LED_RED, 0);
current_board->set_led(LED_RED, 0);
for (int i = 0; i < len/4; i++) {
// program byte 1
FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
@@ -123,7 +123,7 @@ void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
//*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr;
prog_ptr++;
}
set_led(LED_RED, 1);
current_board->set_led(LED_RED, 1);
}
@@ -276,7 +276,7 @@ void soft_flasher_start(void) {
// B8,B9: CAN 1
set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
set_can_enable(CAN1, 1);
current_board->enable_can_transciever(1, true);
// init can
llcan_set_speed(CAN1, 5000, false, false);
@@ -305,7 +305,7 @@ void soft_flasher_start(void) {
usb_init();
// green LED on for flashing
set_led(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
__enable_irq();
@@ -316,13 +316,13 @@ void soft_flasher_start(void) {
// if you are connected through a hub to the phone
// you need power to be able to see the device
puts("USBP: didn't enumerate, switching to CDP mode\n");
set_usb_power_mode(USB_POWER_CDP);
set_led(LED_BLUE, 1);
current_board->set_usb_power_mode(USB_POWER_CDP);
current_board->set_led(LED_BLUE, 1);
}
// blink the green LED fast
set_led(LED_GREEN, 0);
current_board->set_led(LED_GREEN, 0);
delay(500000);
set_led(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
delay(500000);
}
}
panda/python/__init__.py
+32 -13
View File
@@ -16,7 +16,7 @@ from update import ensure_st_up_to_date
from serial import PandaSerial
from isotp import isotp_send, isotp_recv
__version__ = '0.0.8'
__version__ = '0.0.9'
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
@@ -232,10 +232,10 @@ class Panda(object):
print("flash: unlocking")
handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
# erase sectors 1 and 2
# erase sectors 1 through 3
print("flash: erasing")
handle.controlWrite(Panda.REQUEST_IN, 0xb2, 1, 0, b'')
handle.controlWrite(Panda.REQUEST_IN, 0xb2, 2, 0, b'')
for i in range(1, 4):
handle.controlWrite(Panda.REQUEST_IN, 0xb2, i, 0, b'')
# flash over EP2
STEP = 0x10
@@ -334,13 +334,19 @@ class Panda(object):
# ******************* health *******************
def health(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 13)
a = struct.unpack("IIBBBBB", dat)
return {"voltage": a[0], "current": a[1],
"started": a[2], "controls_allowed": a[3],
"gas_interceptor_detected": a[4],
"started_signal_detected": a[5],
"started_alt": a[6]}
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 24)
a = struct.unpack("IIIIIBBBB", dat)
return {
"voltage": a[0],
"current": a[1],
"can_send_errs": a[2],
"can_fwd_errs": a[3],
"gmlan_send_errs": a[4],
"started": a[5],
"controls_allowed": a[6],
"gas_interceptor_detected": a[7],
"car_harness_status": a[8]
}
# ******************* control *******************
@@ -354,9 +360,14 @@ class Panda(object):
def get_version(self):
return self._handle.controlRead(Panda.REQUEST_IN, 0xd6, 0, 0, 0x40)
def get_type(self):
return self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
def is_grey(self):
ret = self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
return ret == "\x01"
return self.get_type() == "\x02"
def is_black(self):
return self.get_type() == "\x03"
def get_serial(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
@@ -387,11 +398,16 @@ class Panda(object):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdd, from_bus, to_bus, b'')
def set_gmlan(self, bus=2):
# TODO: check panda type
if bus is None:
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 0, 0, b'')
elif bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 1, bus, b'')
def set_obd(self, obd):
# TODO: check panda type
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, int(obd), 0, b'')
def set_can_loopback(self, enable):
# set can loopback mode for all buses
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe5, int(enable), 0, b'')
@@ -559,3 +575,6 @@ class Panda(object):
msg = self.kline_ll_recv(2, bus=bus)
msg += self.kline_ll_recv(ord(msg[1])-2, bus=bus)
return msg
def send_heartbeat(self):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xf3, 0, 0, b'')
panda/tests/automated/2_usb_to_can.py
+33
View File
@@ -26,6 +26,9 @@ def test_can_loopback(serial=None):
busses = [0,1,2]
for bus in busses:
# send heartbeat
p.send_heartbeat()
# set bus 0 speed to 250
p.set_can_speed_kbps(bus, 250)
@@ -52,6 +55,9 @@ def test_safety_nooutput(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_NOOUTPUT)
# send heartbeat
p.send_heartbeat()
# enable CAN loopback mode
p.set_can_loopback(True)
@@ -76,11 +82,17 @@ def test_reliability(serial=None):
p.set_can_loopback(True)
p.set_can_speed_kbps(0, 1000)
# send heartbeat
p.send_heartbeat()
addrs = range(100, 100+MSG_COUNT)
ts = [(j, 0, "\xaa"*8, 0) for j in addrs]
# 100 loops
for i in range(LOOP_COUNT):
# send heartbeat
p.send_heartbeat()
st = time.time()
p.can_send_many(ts)
@@ -111,6 +123,9 @@ def test_throughput(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# send heartbeat
p.send_heartbeat()
# enable CAN loopback mode
p.set_can_loopback(True)
@@ -119,6 +134,9 @@ def test_throughput(serial=None):
p.set_can_speed_kbps(0, speed)
time.sleep(0.05)
# send heartbeat
p.send_heartbeat()
comp_kbps = time_many_sends(p, 0)
# bit count from https://en.wikipedia.org/wiki/CAN_bus
@@ -139,6 +157,9 @@ def test_gmlan(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# send heartbeat
p.send_heartbeat()
# enable CAN loopback mode
p.set_can_loopback(True)
@@ -148,6 +169,9 @@ def test_gmlan(serial=None):
# set gmlan on CAN2
for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
# send heartbeat
p.send_heartbeat()
p.set_gmlan(bus)
comp_kbps_gmlan = time_many_sends(p, 3)
assert_greater(comp_kbps_gmlan, 0.8 * SPEED_GMLAN)
@@ -171,11 +195,17 @@ def test_gmlan_bad_toggle(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# send heartbeat
p.send_heartbeat()
# enable CAN loopback mode
p.set_can_loopback(True)
# GMLAN_CAN2
for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
# send heartbeat
p.send_heartbeat()
p.set_gmlan(bus)
comp_kbps_gmlan = time_many_sends(p, 3)
assert_greater(comp_kbps_gmlan, 0.6 * SPEED_GMLAN)
@@ -183,6 +213,9 @@ def test_gmlan_bad_toggle(serial=None):
# normal
for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
# send heartbeat
p.send_heartbeat()
p.set_gmlan(None)
comp_kbps_normal = time_many_sends(p, bus)
assert_greater(comp_kbps_normal, 0.6 * SPEED_NORMAL)
panda/tests/automated/4_wifi_functionality.py
+13
View File
@@ -21,12 +21,18 @@ def test_throughput(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# send heartbeat
p.send_heartbeat()
# enable CAN loopback mode
p.set_can_loopback(True)
p = Panda("WIFI")
for speed in [100,250,500,750,1000]:
# send heartbeat
p.send_heartbeat()
# set bus 0 speed to speed
p.set_can_speed_kbps(0, speed)
time.sleep(0.1)
@@ -46,11 +52,18 @@ def test_recv_only(serial=None):
connect_wifi(serial)
p = Panda(serial)
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# send heartbeat
p.send_heartbeat()
p.set_can_loopback(True)
pwifi = Panda("WIFI")
# TODO: msg_count=1000 drops packets, is this fixable?
for msg_count in [10,100,200]:
# send heartbeat
p.send_heartbeat()
speed = 500
p.set_can_speed_kbps(0, speed)
comp_kbps = time_many_sends(p, 0, pwifi, msg_count)
panda/tests/automated/6_two_panda.py
+18
View File
@@ -13,6 +13,9 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
p_send.set_can_loopback(False)
# send heartbeat
p_send.send_heartbeat()
p_recv.set_can_loopback(False)
assert not p_send.legacy
@@ -27,6 +30,9 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
for bus in busses:
for speed in [100, 250, 500, 750, 1000]:
# send heartbeat
p_send.send_heartbeat()
p_send.set_can_speed_kbps(bus, speed)
p_recv.set_can_speed_kbps(bus, speed)
time.sleep(0.05)
@@ -45,6 +51,10 @@ def test_latency(serial_sender=None, serial_reciever=None):
p_send = Panda(serial_sender)
p_recv = Panda(serial_reciever)
# send heartbeat
p_send.send_heartbeat()
p_recv.send_heartbeat()
p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
p_send.set_can_loopback(False)
@@ -62,10 +72,18 @@ def test_latency(serial_sender=None, serial_reciever=None):
p_recv.can_recv()
p_send.can_recv()
# send heartbeat
p_send.send_heartbeat()
p_recv.send_heartbeat()
busses = [0,1,2]
for bus in busses:
for speed in [100, 250, 500, 750, 1000]:
# send heartbeat
p_send.send_heartbeat()
p_recv.send_heartbeat()
p_send.set_can_speed_kbps(bus, speed)
p_recv.set_can_speed_kbps(bus, speed)
time.sleep(0.1)
panda/tests/black_loopback_test.py
Executable
+139
View File
@@ -0,0 +1,139 @@
#!/usr/bin/env python
# Loopback test between black panda (+ harness and power) and white/grey panda
# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test.
# To be sure, the test should be run with both harness orientations
from __future__ import print_function
import os
import sys
import time
import random
import argparse
from hexdump import hexdump
from itertools import permutations
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
from panda import Panda
def get_test_string():
return b"test"+os.urandom(10)
def run_test(sleep_duration):
pandas = Panda.list()
print(pandas)
# make sure two pandas are connected
if len(pandas) != 2:
print("Connect white/grey and black panda to run this test!")
assert False
# connect
pandas[0] = Panda(pandas[0])
pandas[1] = Panda(pandas[1])
# find out which one is black
type0 = pandas[0].get_type()
type1 = pandas[1].get_type()
black_panda = None
other_panda = None
if type0 == "\x03" and type1 != "\x03":
black_panda = pandas[0]
other_panda = pandas[1]
elif type0 != "\x03" and type1 == "\x03":
black_panda = pandas[1]
other_panda = pandas[0]
else:
print("Connect white/grey and black panda to run this test!")
assert False
# disable safety modes
black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
# test health packet
print("black panda health", black_panda.health())
print("other panda health", other_panda.health())
# test black -> other
test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration)
test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration)
def test_buses(black_panda, other_panda, direction, test_array, sleep_duration):
if direction:
print("***************** TESTING (BLACK --> OTHER) *****************")
else:
print("***************** TESTING (OTHER --> BLACK) *****************")
for send_bus, obd, recv_buses in test_array:
black_panda.send_heartbeat()
other_panda.send_heartbeat()
print("\ntest can: ", send_bus, " OBD: ", obd)
# set OBD on black panda
black_panda.set_gmlan(True if obd else None)
# clear and flush
if direction:
black_panda.can_clear(send_bus)
else:
other_panda.can_clear(send_bus)
for recv_bus in recv_buses:
if direction:
other_panda.can_clear(recv_bus)
else:
black_panda.can_clear(recv_bus)
black_panda.can_recv()
other_panda.can_recv()
# send the characters
at = random.randint(1, 2000)
st = get_test_string()[0:8]
if direction:
black_panda.can_send(at, st, send_bus)
else:
other_panda.can_send(at, st, send_bus)
time.sleep(0.1)
# check for receive
if direction:
cans_echo = black_panda.can_recv()
cans_loop = other_panda.can_recv()
else:
cans_echo = other_panda.can_recv()
cans_loop = black_panda.can_recv()
loop_buses = []
for loop in cans_loop:
print(" Loop on bus", str(loop[3]))
loop_buses.append(loop[3])
if len(cans_loop) == 0:
print(" No loop")
# test loop buses
recv_buses.sort()
loop_buses.sort()
assert recv_buses == loop_buses
print(" TEST PASSED")
time.sleep(sleep_duration)
print("\n")
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("-n", type=int, help="Number of test iterations to run")
parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
args = parser.parse_args()
if args.n is None:
while True:
run_test(sleep_duration=args.sleep)
else:
for i in range(args.n):
run_test(sleep_duration=args.sleep)
panda/tests/safety/libpandasafety_py.py
+2
View File
@@ -37,6 +37,7 @@ bool get_long_controls_allowed(void);
void set_gas_interceptor_detected(bool c);
bool get_gas_interceptor_detetcted(void);
int get_gas_interceptor_prev(void);
int get_hw_type(void);
void set_timer(uint32_t t);
void reset_angle_control(void);
@@ -60,6 +61,7 @@ int get_honda_brake_prev(void);
int get_honda_gas_prev(void);
void set_honda_alt_brake_msg(bool);
void set_honda_bosch_hardware(bool);
int get_honda_bosch_hardware(void);
void init_tests_cadillac(void);
void set_cadillac_desired_torque_last(int t);
panda/tests/safety/test.c
+31
View File
@@ -1,5 +1,6 @@
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
typedef struct
{
@@ -51,6 +52,16 @@ TIM_TypeDef *TIM2 = &timer;
#define GET_BYTES_04(msg) ((msg)->RDLR)
#define GET_BYTES_48(msg) ((msg)->RDHR)
// from board_declarations.h
#define HW_TYPE_UNKNOWN 0U
#define HW_TYPE_WHITE_PANDA 1U
#define HW_TYPE_GREY_PANDA 2U
#define HW_TYPE_BLACK_PANDA 3U
#define HW_TYPE_PEDAL 4U
// from main_declarations.h
uint8_t hw_type = 0U;
#define UNUSED(x) (void)(x)
#define PANDA
@@ -90,6 +101,10 @@ int get_gas_interceptor_prev(void){
return gas_interceptor_prev;
}
int get_hw_type(void){
return hw_type;
}
void set_timer(uint32_t t){
timer.CNT = t;
}
@@ -228,7 +243,17 @@ void set_honda_bosch_hardware(bool c){
honda_bosch_hardware = c;
}
int get_honda_bosch_hardware(void) {
return honda_bosch_hardware;
}
void init_tests(void){
// get HW_TYPE from env variable set in test.sh
hw_type = atoi(getenv("HW_TYPE"));
}
void init_tests_toyota(void){
init_tests();
toyota_torque_meas.min = 0;
toyota_torque_meas.max = 0;
toyota_desired_torque_last = 0;
@@ -238,6 +263,7 @@ void init_tests_toyota(void){
}
void init_tests_cadillac(void){
init_tests();
cadillac_torque_driver.min = 0;
cadillac_torque_driver.max = 0;
for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = 0;
@@ -247,6 +273,7 @@ void init_tests_cadillac(void){
}
void init_tests_gm(void){
init_tests();
gm_torque_driver.min = 0;
gm_torque_driver.max = 0;
gm_desired_torque_last = 0;
@@ -256,6 +283,7 @@ void init_tests_gm(void){
}
void init_tests_hyundai(void){
init_tests();
hyundai_torque_driver.min = 0;
hyundai_torque_driver.max = 0;
hyundai_desired_torque_last = 0;
@@ -265,6 +293,7 @@ void init_tests_hyundai(void){
}
void init_tests_chrysler(void){
init_tests();
chrysler_torque_meas.min = 0;
chrysler_torque_meas.max = 0;
chrysler_desired_torque_last = 0;
@@ -274,6 +303,7 @@ void init_tests_chrysler(void){
}
void init_tests_subaru(void){
init_tests();
subaru_torque_driver.min = 0;
subaru_torque_driver.max = 0;
subaru_desired_torque_last = 0;
@@ -283,6 +313,7 @@ void init_tests_subaru(void){
}
void init_tests_honda(void){
init_tests();
honda_moving = false;
honda_brake_prev = 0;
honda_gas_prev = 0;
panda/tests/safety/test.sh
+16 -1
View File
@@ -1,2 +1,17 @@
#!/usr/bin/env sh
python -m unittest discover .
# Loop over all hardware types:
# HW_TYPE_UNKNOWN 0U
# HW_TYPE_WHITE_PANDA 1U
# HW_TYPE_GREY_PANDA 2U
# HW_TYPE_BLACK_PANDA 3U
# HW_TYPE_PEDAL 4U
# Make sure test fails if one HW_TYPE fails
set -e
for hw_type in 0 1 2 3 4
do
echo "Testing HW_TYPE: $hw_type"
HW_TYPE=$hw_type python -m unittest discover .
done
panda/tests/safety/test_honda.py
+4
View File
@@ -33,6 +33,10 @@ class TestHondaSafety(unittest.TestCase):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = msg << 21
to_send[0].RDLR = buttons << 5
is_panda_black = self.safety.get_hw_type() == 3 # black_panda
honda_bosch_hardware = self.safety.get_honda_bosch_hardware()
bus = 1 if is_panda_black and honda_bosch_hardware else 0
to_send[0].RDTR = bus << 4
return to_send
panda/tests/safety/test_honda_bosch.py
+9 -5
View File
@@ -23,16 +23,20 @@ class TestHondaSafety(unittest.TestCase):
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
is_panda_black = self.safety.get_hw_type() == 3 # black panda
bus_rdr_cam = 2 if is_panda_black else 1
bus_rdr_car = 0 if is_panda_black else 2
bus_pt = 1 if is_panda_black else 0
blocked_msgs = [0xE4, 0x33D]
for b in buss:
for m in msgs:
if b == 0:
if b == bus_pt:
fwd_bus = -1
elif b == 1:
fwd_bus = -1 if m in blocked_msgs else 2
elif b == 2:
fwd_bus = 1
elif b == bus_rdr_cam:
fwd_bus = -1 if m in blocked_msgs else bus_rdr_car
elif b == bus_rdr_car:
fwd_bus = bus_rdr_cam
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
selfdrive/boardd/boardd.cc
+51 -15
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) {
selfdrive/can/tests/test_packer_honda.py
+11 -10
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)
selfdrive/car/car_helpers.py
+66 -82
View File
@@ -1,13 +1,13 @@
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
import pickle
from common.params import Params
params = Params()
import selfdrive.crash as crash
def get_startup_alert(car_recognized, controller_available):
@@ -54,117 +54,101 @@ def _get_interface_names():
interfaces = load_interfaces(_get_interface_names())
def only_toyota_left(candidate_cars):
return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars)
return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
# **** for use live only ****
def fingerprint(logcan, sendcan):
def fingerprint(logcan, sendcan, is_panda_black):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None, "")
finger = {0: {}, 2:{}} # collect on bus 0 or 2
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
can_seen_frame = None
can_seen = False
params = Params()
car_params = params.get("CarParams")
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
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 = ""
frame = 0
if car_params is not None:
# use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode
car_params = car.CarParams.from_bytes(car_params)
vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin
elif is_panda_black:
# Vin query only reliably works thorugh OBDII
vin = get_vin(logcan, sendcan, 1)
else:
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
if params.get("DragonCacheCar") == "1" and params.get("DragonCachedFP") != "" and params.get("DragonCachedModel") != "":
candidate_cars = pickle.loads(params.get("DragonCachedModel"))
car_fingerprint = pickle.loads(params.get("DragonCachedModel"))
finger = pickle.loads(params.get("DragonCachedFP"))
vin = pickle.loads(params.get("DragonCachedVIN"))
else:
while True:
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 not done:
a = messaging.recv_one(logcan)
for can in a.can:
can_seen = True
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints.
# also exclude VIN query response on 0x7e8.
# need to independently try to fingerprint both bus 0 and 1 to work
# for the combo black_panda and honda_bosch. Ignore extended messages
# and VIN query response.
# Include bus 2 for toyotas to disambiguate cars using camera messages
# (ideally should be done for all cars but we can't for Honda Bosch)
if (can.src == 0 or (only_toyota_left(candidate_cars) and can.src == 2)) and \
can.address < 0x800 and can.address != 0x7e8:
finger[can.src][can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
for b in candidate_cars:
if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
finger[can.src][can.address] = len(can.dat)
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
if can_seen_frame is None and can_seen:
can_seen_frame = frame
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
# Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
if only_toyota_left(candidate_cars[b]):
frame_fingerprint = 100 # 1s
if len(candidate_cars[b]) == 1:
if frame > frame_fingerprint:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# if we only have one car choice and the time_fingerprint since we got our first
# message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
# broadcast immediately
if len(candidate_cars) == 1 and can_seen_frame is not None:
time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1
if (frame - can_seen_frame) > (time_fingerprint * 100):
break
# bail if no cars left or we've been waiting for more than 2s since can_seen
elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200):
return None, finger, ""
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
# TODO: VIN query temporarily disabled until we have the harness
if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan'))
vin_responded = False
vin_cnt = 0
# bail if no cars left or we've been waiting for more than 2s
failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
# only report vin if procedure is finished
if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
vin = "".join(vin_dat[3:])
if succeeded:
params.put("DragonCachedModel", pickle.dumps(car_fingerprint))
params.put("DragonCachedFP", pickle.dumps(finger))
params.put("DragonCachedVIN", pickle.dumps(vin))
params.put("DragonCachedModel", pickle.dumps(candidate_cars))
params.put("DragonCachedFP", pickle.dumps(finger))
params.put("DragonCachedVIN", pickle.dumps(vin))
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"
else:
cloudlog.warning("car does match fingerprint: %r", fingerprints)
try:
crash.capture_warning("fingerprinted %s" % candidate)
except: # fixes occasional travis errors
pass
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
selfdrive/car/chrysler/carstate.py
+2 -2
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)
selfdrive/car/chrysler/interface.py
+3 -2
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
selfdrive/car/chrysler/values.py
+1 -1
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},
],
selfdrive/car/ford/interface.py
+3 -2
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)
selfdrive/car/gm/interface.py
+3 -2
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
selfdrive/car/honda/carcontroller.py
+27 -6
View File
@@ -87,11 +87,19 @@ class CarController(object):
# dragonpilot
self.turning_signal_timer = 0
self.dragon_enable_steering_on_signal = False
self.dragon_allow_gas = False
self.dragon_lat_ctrl = True
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
# dragonpilot, don't check for param too often as it's a kernel call
if frame % 100 == 0:
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
@@ -152,29 +160,32 @@ class CarController(object):
can_sends = []
# dragonpilot
if enabled and (CS.left_blinker_on > 0 or CS.right_blinker_on > 0) and params.get("DragonTempDisableSteerOnSignal") == "1":
if enabled and (CS.left_blinker_on > 0 or CS.right_blinker_on > 0) and self.dragon_enable_steering_on_signal:
self.turning_signal_timer = 100
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
lkas_active = False
if not self.dragon_lat_ctrl:
lkas_active = False
# 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.
@@ -182,8 +193,18 @@ class CarController(object):
idx = frame // 2
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
# DragonAllowGas
# if we detect gas pedal pressed, we do not want OP to apply gas or brake
# gasPressed code from interface.py
if not CS.CP.enableGasInterceptor:
gasPressed = CS.pedal_gas > 0
else:
gasPressed = CS.user_gas_pressed
if self.dragon_allow_gas and gasPressed:
apply_brake = 0
apply_gas = 0
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:
selfdrive/car/honda/carstate.py
+4 -4
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):
selfdrive/car/honda/hondacan.py
+23 -17
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,
@@ -80,23 +86,23 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
'DASHED_LANES': hud.dashed_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)
selfdrive/car/honda/interface.py
+31 -13
View File
@@ -98,6 +98,11 @@ class CarInterface(object):
else:
self.compute_gb = compute_gb_honda
# dragonpilot
self.dragon_enable_steering_on_signal = False
self.dragon_allow_gas = False
self.ts_last_check = 0.
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
@@ -131,12 +136,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
@@ -145,7 +151,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
@@ -169,7 +175,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']
@@ -189,7 +195,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.]
@@ -215,7 +221,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.]
@@ -295,7 +301,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.]
@@ -361,6 +367,13 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# dragonpilot, don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - self.ts_last_check > 1.:
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
self.ts_last_check = ts
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
@@ -476,7 +489,7 @@ class CarInterface(object):
events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.lkMode:
events.append(create_event('manualSteeringRequired', [ET.WARNING]))
elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonTempDisableSteerOnSignal") == "1":
elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
elif self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
@@ -504,13 +517,18 @@ class CarInterface(object):
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
# DragonAllowGas
if not self.dragon_allow_gas:
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
else:
if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
selfdrive/car/honda/values.py
+9
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
@@ -143,6 +146,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),
selfdrive/car/hyundai/hyundaican.py
+1 -1
View File
@@ -8,7 +8,7 @@ def make_can_msg(addr, dat, alt):
def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False):
values = {
"CF_Lkas_Icon": 3 if enabled else 0,
"CF_Lkas_Bca_R": 3 if enabled else 0,
"CF_Lkas_LdwsSysState": 3 if steer_req else 1,
"CF_Lkas_SysWarning": hud_alert,
"CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,
selfdrive/car/hyundai/interface.py
+3 -2
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
selfdrive/car/mock/interface.py
+1 -6
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:
selfdrive/car/subaru/interface.py
+2 -1
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
selfdrive/car/toyota/carcontroller.py
+36 -1
View File
@@ -128,10 +128,18 @@ class CarController(object):
# dragonpilot
self.turning_signal_timer = 0
self.dragon_enable_steering_on_signal = False
self.dragon_allow_gas = False
self.dragon_lat_ctrl = True
def update(self, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# dragonpilot, don't check for param too often as it's a kernel call
if frame % 100 == 0:
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
# *** compute control surfaces ***
@@ -204,13 +212,27 @@ class CarController(object):
can_sends = []
# dragonpilot
if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonTempDisableSteerOnSignal") == "1":
if enabled and (CS.left_blinker_on or CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
self.turning_signal_timer = 100
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
apply_steer_req = 0
if not self.dragon_lat_ctrl:
apply_steer_req = 0
if CS.v_ego > 12.5 and not enabled:
if right_lane_depart and not CS.right_blinker_on:
apply_steer = self.last_steer + 3
apply_steer = min(apply_steer , 800)
apply_steer_req = 1
if left_lane_depart and not CS.left_blinker_on:
apply_steer = self.last_steer - 3
apply_steer = max(apply_steer , -800)
apply_steer_req = 1
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
@@ -229,6 +251,19 @@ class CarController(object):
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
# DragonAllowGas
# if we detect gas pedal pressed, we do not want OP to apply gas or brake
# gasPressed code from interface.py
if CS.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
gasPressed = CS.pedal_gas > 15
else:
gasPressed = CS.pedal_gas > 0
if self.dragon_allow_gas and gasPressed:
apply_accel = 0
apply_gas = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
selfdrive/car/toyota/carstate.py
+36 -4
View File
@@ -3,7 +3,11 @@ 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
from common.realtime import sec_since_boot
from common.params import Params
params = Params()
def parse_gear_shifter(gear, vals):
@@ -19,6 +23,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 +64,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 +97,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
@@ -108,7 +113,16 @@ class CarState(object):
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
self.dragon_toyota_stock_dsu = False
self.ts_last_check = 0.
def update(self, cp):
# dragonpilot, don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - self.ts_last_check > 1.:
self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
self.ts_last_check = ts
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
@@ -144,6 +158,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']
@@ -173,3 +195,13 @@ class CarState(object):
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on:
enable_acc = True
if not self.gear_shifter == 'drive' or not self.seatbelt or not self.door_all_closed:
enable_acc = False
self.pcm_acc_active = enable_acc
if self.standstill:
self.pcm_acc_status = 7
else:
self.pcm_acc_status = 1
selfdrive/car/toyota/interface.py
+33 -12
View File
@@ -33,6 +33,12 @@ class CarInterface(object):
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
# dragonpilot
self.dragon_toyota_stock_dsu = False
self.dragon_enable_steering_on_signal = False
self.dragon_allow_gas = False
self.ts_last_check = 0.
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@@ -42,13 +48,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
@@ -64,7 +71,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
@@ -80,7 +87,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]]
@@ -90,7 +97,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]]
@@ -215,7 +222,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
@@ -248,6 +255,14 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# dragonpilot, don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - self.ts_last_check > 1.:
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
self.ts_last_check = ts
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
@@ -349,7 +364,7 @@ class CarInterface(object):
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse' and self.CP.enableDsu:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonTempDisableSteerOnSignal") == "1":
if (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
elif self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
@@ -370,13 +385,19 @@ class CarInterface(object):
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if not self.dragon_toyota_stock_dsu:
# DragonAllowGas
if not self.dragon_allow_gas:
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
else:
if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
ret.events = events
selfdrive/common/modeldata.h
+8 -1
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
selfdrive/common/params.cc
+3
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;
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.6.1-release"
#define COMMA_VERSION "0.6.2-release"
selfdrive/controls/controlsd.py
+40 -15
View File
@@ -1,4 +1,5 @@
#!/usr/bin/env python
import os
import gc
import capnp
from cereal import car, log
@@ -48,6 +49,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):
@@ -341,7 +346,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"vEgo": CS.vEgo,
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
"curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
"curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
@@ -358,7 +363,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 +417,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,14 +431,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()
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
@@ -443,6 +451,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 +471,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,13 +484,24 @@ 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)
prof = Profiler(False) # off by default
# dragonpilot
ts_last_check = 0.
dragon_toyota_stock_dsu = False
while True:
# dragonpilot, don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - ts_last_check > 1.:
dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
ts_last_check = ts
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
@@ -498,6 +520,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:
@@ -505,9 +529,10 @@ def controlsd_thread(gctx=None):
if not CS.canValid:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not dragon_toyota_stock_dsu:
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not read_only:
# update control state
selfdrive/controls/lib/alerts.py
+14
View File
@@ -385,6 +385,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",
@@ -542,6 +549,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",
selfdrive/controls/lib/driver_monitor.py
+23 -16
View File
@@ -12,6 +12,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
@@ -25,16 +26,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)
@@ -74,6 +74,10 @@ class DriverStatus():
self.terminal_alert_cnt = 0
self._set_timers()
# dragonpilot
self.dp_last_check = 0.
self.dragon_enable_driver_safety_check = True
def _reset_filters(self):
self.driver_distraction_filter.x = 0.
self.variance_filter.x = 0.
@@ -98,24 +102,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
@@ -135,6 +137,11 @@ class DriverStatus():
def update(self, events, driver_engaged, ctrl_active, standstill):
# don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - self.dp_last_check > 1.:
self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck") == "0" else True
self.dp_last_check = ts
driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
awareness_prev = self.awareness
@@ -160,7 +167,7 @@ class DriverStatus():
elif self.awareness <= self.threshold_pre:
# pre green alert
alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
if params.get("DragonDisableDriverSafetyCheck") == "0" and alert is not None:
if alert is not None and self.dragon_enable_driver_safety_check:
events.append(create_event(alert, [ET.WARNING]))
return events
selfdrive/controls/lib/pathplanner.py
+1
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())
selfdrive/controls/lib/planner.py
+45 -41
View File
@@ -1,9 +1,9 @@
#!/usr/bin/env python
import zmq
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
@@ -16,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
@@ -38,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)
@@ -58,14 +68,12 @@ 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):
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.poller = zmq.Poller()
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
@@ -81,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:
@@ -110,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
@@ -129,51 +145,43 @@ class Planner(object):
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
if not self.model_v_kf_ready:
self.model_v_kf.x = [[v_ego],[0.0]]
self.model_v_kf_ready = True
#map_age = cur_time - rcv_times['liveMapData']
map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0
if len(sm['model'].speed):
self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
# Speed limit and curvature
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
if set_speed_limit_active and map_valid:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
if self.params.get("LimitSetSpeedNeural") == "1":
model_speed = self.model_v_kf.x[0][0]
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
if decel_for_turn:
time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# accel and jerk up limits are higher here to make model not limiting accel
# mainly done to prevent flickering of slowdown icon
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
model_speed,
2*accel_limits[1], 3*accel_limits[0],
2*jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
@@ -236,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)
selfdrive/controls/lib/radar_helpers.py
+34 -126
View File
@@ -1,48 +1,35 @@
import numpy as np
from common.numpy_fast import clip, interp
from common.realtime import DT_MDL
from common.kalman.simple_kalman import KF1D
from selfdrive.config import RADAR_TO_CENTER
# the longer lead decels, the more likely it will keep decelerating
# TODO is this a good default?
_LEAD_ACCEL_TAU = 1.5
NO_FUSION_SCORE = 100 # bad default fusion score
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
rate, ratev = 20., 20. # model and radar are both at 20Hz
ts = 1./rate
freq_v_lat = 0.2 # Hz
k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
freq_a_lead = .5 # Hz
k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
# stationary qualification parameters
v_stationary_thr = 4. # objects moving below this speed are classified as stationary
v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
v_ego_stationary = 4. # no stationary object flag below this speed
# Lead Kalman Filter params
_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]]
_VLEAD_C = [1.0, 0.0]
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]]
RDR_TO_LDR = 2.7
_VLEAD_K = [[0.1988689], [0.28555364]]
class Track(object):
def __init__(self):
self.ekf = None
self.stationary = True
self.initted = False
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
if self.initted:
# pylint: disable=access-member-before-definition
self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
self.vRelPrev = self.vRel
@@ -52,9 +39,6 @@ class Track(object):
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
# compute distance to path
self.dPath = d_path
# computed velocity and accelerations
self.vLead = self.vRel + v_ego_t_aligned
@@ -64,21 +48,8 @@ class Track(object):
self.cnt = 1
self.vision_cnt = 0
self.vision = False
self.aRel = 0. # nidec gives no information about this
self.vLat = 0.
self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
# estimate acceleration
# TODO: use Kalman filter
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
# TODO: use Kalman filter
# neglect steer override cases as dPath is too noisy
v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
self.kf.update(self.vLead)
self.cnt += 1
@@ -86,33 +57,12 @@ class Track(object):
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
if self.stationary:
# stationary objects can become non stationary, but not the other way around
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr
self.vision_score = NO_FUSION_SCORE
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau = _LEAD_ACCEL_TAU
else:
self.aLeadTau *= 0.9
def update_vision_score(self, dist_to_vision, rel_speed_diff):
# rel speed is very hard to estimate from vision
if dist_to_vision < 4.0 and rel_speed_diff < 10.:
self.vision_score = dist_to_vision + rel_speed_diff
else:
self.vision_score = NO_FUSION_SCORE
def update_vision_fusion(self):
# vision point is never stationary
# don't trust 1 or 2 fusions until model quality is much better
if self.vision_cnt >= 3:
self.vision = True
self.stationary = False
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
return [self.dRel, self.yRel*2, self.vRel]
@@ -171,89 +121,47 @@ class Cluster(object):
def aLeadTau(self):
return mean([t.aLeadTau for t in self.tracks])
@property
def vision(self):
return any([t.vision for t in self.tracks])
@property
def measured(self):
return any([t.measured for t in self.tracks])
@property
def vision_cnt(self):
return max([t.vision_cnt for t in self.tracks])
@property
def stationary(self):
return all([t.stationary for t in self.tracks])
@property
def oncoming(self):
return all([t.oncoming for t in self.tracks])
def toRadarState(self):
def get_RadarState(self, model_prob=0.0):
return {
"dRel": float(self.dRel) - RDR_TO_LDR,
"dRel": float(self.dRel),
"yRel": float(self.yRel),
"vRel": float(self.vRel),
"aRel": float(self.aRel),
"vLead": float(self.vLead),
"dPath": float(self.dPath),
"vLat": float(self.vLat),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"status": True,
"fcw": self.is_potential_fcw(),
"fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob,
"radar": True,
"aLeadTau": float(self.aLeadTau)
}
def get_RadarState_from_vision(self, lead_msg, v_ego):
return {
"dRel": float(lead_msg.dist - RADAR_TO_CENTER),
"yRel": float(lead_msg.relY),
"vRel": float(lead_msg.relVel),
"vLead": float(v_ego + lead_msg.relVel),
"vLeadK": float(v_ego + lead_msg.relVel),
"aLeadK": float(0),
"aLeadTau": _LEAD_ACCEL_TAU,
"fcw": False,
"modelProb": float(lead_msg.prob),
"radar": False,
"status": True
}
def __str__(self):
ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath)
if self.stationary:
ret += " stationary"
if self.vision:
ret += " vision"
if self.oncoming:
ret += " oncoming"
if self.vision_cnt > 0:
ret += " vision_cnt: %6.0f" % self.vision_cnt
ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
return ret
def is_potential_lead(self, v_ego):
# predict cut-ins by extrapolating lateral speed by a lookahead time
# lookahead time depends on cut-in distance. more attentive for close cut-ins
# also, above 50 meters the predicted path isn't very reliable
def potential_low_speed_lead(self, v_ego):
# stop for stuff in front of you and low speed, even without model confirmation
return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25
# the distance at which v_lat matters is higher at higher speed
lookahead_dist = 40. + v_ego/1.2 #40m at 0mph, ~70m at 80mph
t_lookahead_v = [1., 0.]
t_lookahead_bp = [10., lookahead_dist]
# average dist
d_path = self.dPath
# lat_corr used to be gated on enabled, now always running
t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact.
lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0.
# consider only cut-ins
d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path))
return abs(d_path) < 1.5 and not self.stationary and not self.oncoming
def is_potential_lead2(self, lead_clusters):
if len(lead_clusters) > 0:
lead_cluster = lead_clusters[0]
# check if the new lead is too close and roughly at the same speed of the first lead:
# it might just be the second axle of the same vehicle
return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1.
else:
return False
def is_potential_fcw(self):
# is this cluster trustrable enough for triggering fcw?
# fcw can trigger only on clusters that have been fused vision model for at least 20 frames
return self.vision_cnt >= 20
def is_potential_fcw(self, model_prob):
return model_prob > .9
selfdrive/controls/plannerd.py
+1 -5
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):
selfdrive/controls/radard.py
+72 -145
View File
@@ -6,18 +6,13 @@ from collections import defaultdict, deque
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.model_parser import ModelParser
from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
RDR_TO_LDR, NO_FUSION_SCORE
from selfdrive.controls.lib.radar_helpers import Track, Cluster
from selfdrive.config import RADAR_TO_CENTER
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
from common.params import Params
from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL
from common.kalman.ekf import EKF, SimpleSensor
DEBUG = False
@@ -26,106 +21,94 @@ DIMSV = 2
XV, SPEEDV = 0, 1
VISION_POINT = -1
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 1. / DT_MDL # model and radar are both at 20Hz
v_len = 20 # how many speed data points to remember for t alignment with rdr data
class EKFV1D(EKF):
def __init__(self):
super(EKFV1D, self).__init__(False)
self.identity = numpy.matlib.identity(DIMSV)
self.state = np.matlib.zeros((DIMSV, 1))
self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point
self.covar = self.identity * self.var_init
self.process_noise = np.matlib.diag([0.5, 1])
def laplacian_cdf(x, mu, b):
b = np.max([b, 1e-4])
return np.exp(-abs(x-mu)/b)
def match_vision_to_cluster(v_ego, lead, clusters):
# match vision point to best statistical cluster match
probs = []
offset_vision_dist = lead.dist - RADAR_TO_CENTER
for c in clusters:
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
# This is isn't exactly right, but good heuristic
combined_prob = prob_d * prob_y * prob_v
probs.append(combined_prob)
idx = np.argmax(probs)
# if no 'sane' match is found return -1
# stationary radar points can be false positives
dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2)
if dist_sane and vel_sane:
return idx
else:
return None
def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
# Determine leads, this is where the essential logic happens
if len(clusters) > 0 and ready and lead_msg.prob > .5:
lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters)
else:
lead_idx = None
lead_dict = {'status': False}
if lead_idx is not None:
lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob)
elif (lead_idx is None) and ready and (lead_msg.prob > .5):
lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
if low_speed_override:
low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
if len(low_speed_clusters) > 0:
lead_idx = np.argmin([c.dRel for c in low_speed_clusters])
if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']):
lead_dict = low_speed_clusters[lead_idx].get_RadarState()
return lead_dict
def calc_transfer_fun(self, dt):
tf = np.matlib.identity(DIMSV)
tf[XV, SPEEDV] = dt
tfj = tf
return tf, tfj
class RadarD(object):
def __init__(self, VM, mocked):
self.VM = VM
def __init__(self, mocked):
self.current_time = 0
self.mocked = mocked
self.MP = ModelParser()
self.tracks = defaultdict(dict)
self.last_md_ts = 0
self.last_controls_state_ts = 0
self.active = 0
self.steer_angle = 0.
self.steer_override = False
# Kalman filter stuff:
self.ekfv = EKFV1D()
self.speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
self.v_ego = 0.
self.v_ego_hist_t = deque([0], maxlen=v_len)
self.v_ego_hist_v = deque([0], maxlen=v_len)
self.v_ego_t_aligned = 0.
self.ready = False
def update(self, frame, delay, sm, rr):
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
if sm.updated['liveParameters']:
self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
def update(self, frame, delay, sm, rr, has_radar):
self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
if sm.updated['controlsState']:
self.active = sm['controlsState'].active
self.v_ego = sm['controlsState'].vEgo
self.steer_angle = sm['controlsState'].angleSteers
self.steer_override = sm['controlsState'].steerOverride
self.v_ego_hist_v.append(self.v_ego)
self.v_ego_hist_t.append(float(frame)/rate)
self.last_controls_state_ts = sm.logMonoTime['controlsState']
if sm.updated['model']:
self.last_md_ts = sm.logMonoTime['model']
self.MP.update(self.v_ego, sm['model'])
self.ready = True
# run kalman filter only if prob is high enough
if self.MP.lead_prob > 0.7:
reading = self.speedSensorV.read(self.MP.lead_dist, covar=np.matrix(self.MP.lead_var))
self.ekfv.update_scalar(reading)
self.ekfv.predict(DT_MDL)
# When changing lanes the distance to the lead car can suddenly change,
# which makes the Kalman filter output large relative acceleration
if self.mocked and abs(self.MP.lead_dist - self.ekfv.state[XV]) > 2.0:
self.ekfv.state[XV] = self.MP.lead_dist
self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init]))
self.ekfv.state[SPEEDV] = 0.
ar_pts[VISION_POINT] = (float(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])),
float(self.ekfv.state[SPEEDV]), False)
else:
self.ekfv.state[XV] = self.MP.lead_dist
self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init]))
self.ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if (self.active and not self.steer_override) or self.mocked:
# use path from model (always when mocking as steering is too noisy)
path_y = np.polyval(self.MP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0]
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
# *** remove missing points from meta data ***
for ids in self.tracks.keys():
@@ -134,48 +117,21 @@ class RadarD(object):
# *** compute the tracks ***
for ids in ar_pts:
# ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT and not self.mocked:
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(frame)/rate
self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v)
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in self.tracks:
self.tracks[ids] = Track()
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in self.tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - self.tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - self.tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel)
self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > self.tracks[ids].vision_score:
fused_id = ids
best_score = self.tracks[ids].vision_score
if fused_id is not None:
self.tracks[fused_id].vision_cnt += 1
self.tracks[fused_id].update_vision_fusion()
if DEBUG:
print("NEW CYCLE")
if VISION_POINT in ar_pts:
print("vision", ar_pts[VISION_POINT])
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3])
idens = list(self.tracks.keys())
track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens])
# If we have multiple points, cluster them
if len(track_pts) > 1:
cluster_idxs = cluster_points_centroid(track_pts, 2.5)
@@ -186,29 +142,12 @@ class RadarD(object):
if clusters[cluster_i] is None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(self.tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
clusters[0].add(self.tracks[idens[0]])
else:
clusters = []
if DEBUG:
for i in clusters:
print(i)
# *** extract the lead car ***
lead_clusters = [c for c in clusters
if c.is_potential_lead(self.v_ego)]
lead_clusters.sort(key=lambda x: x.dRel)
lead_len = len(lead_clusters)
# *** extract the second lead from the whole set of leads ***
lead2_clusters = [c for c in lead_clusters
if c.is_potential_lead2(lead_clusters)]
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish radarState ***
dat = messaging.new_message()
dat.init('radarState')
@@ -217,18 +156,14 @@ class RadarD(object):
dat.radarState.canMonoTimes = list(rr.canMonoTimes)
dat.radarState.radarErrors = list(rr.errors)
dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
if lead_len > 0:
dat.radarState.leadOne = lead_clusters[0].toRadarState()
if lead2_len > 0:
dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
else:
dat.radarState.leadTwo.status = False
else:
dat.radarState.leadOne.status = False
if has_radar:
dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
return dat
## fuses camera and radar data for best lead detection
# fuses camera and radar data for best lead detection
def radard_thread(gctx=None):
set_realtime_priority(2)
@@ -236,7 +171,6 @@ def radard_thread(gctx=None):
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked = CP.carName == "mock"
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
@@ -253,7 +187,9 @@ def radard_thread(gctx=None):
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(VM, mocked)
RD = RadarD(mocked)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
@@ -264,7 +200,7 @@ def radard_thread(gctx=None):
sm.update(0)
dat = RD.update(rk.frame, RI.delay, sm, rr)
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining*1000.
radarState.send(dat.to_bytes())
@@ -275,29 +211,20 @@ def radard_thread(gctx=None):
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary,
tracks[ids].measured))
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
"aRel": float(tracks[ids].aRel),
"stationary": bool(tracks[ids].stationary),
"oncoming": bool(tracks[ids].oncoming),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def main(gctx=None):
radard_thread(gctx)
if __name__ == "__main__":
main()
selfdrive/crash.py
+21 -14
View File
@@ -1,8 +1,9 @@
"""Install exception handler for process crash."""
import os
import sys
import json
#from subprocess import check_output
import threading
import capnp
from selfdrive.version import version, dirty
from selfdrive.swaglog import cloudlog
@@ -20,24 +21,32 @@ else:
from raven import Client
from raven.transport.http import HTTPTransport
with open("/data/openpilot/.git/HEAD", "r") as f:
content = f.read().split(' ')
branch = content[1]
error_tags = {'dirty': dirty, 'branch': branch}
error_tags = {'dirty': dirty, 'username': 'char_error'}
try:
with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f:
auth = json.loads(f.read())
auth = json.loads(auth['commaUser'])
error_tags['username'] = auth['username']
error_tags['email'] = auth['email']
tags = ['username', 'email']
for tag in tags:
try:
error_tags[tag] = ''.join(char for char in auth[tag].decode('utf-8', 'ignore') if char.isalnum())
except:
pass
except:
pass
logging_data = {"branch": "/data/params/d/GitBranch", "commit": "/data/params/d/GitCommit", "remote": "/data/params/d/GitRemote"}
for key in logging_data:
try:
with open(logging_data[key], "r") as f:
error_tags[key] = str(f.read())
except:
error_tags[key] = "unknown"
client = Client('https://980a0cba712a4c3593c33c78a12446e1:fecab286bcaf4dba8b04f7cff0188e2d@sentry.io/1488600',
install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags)
try:
client.user_context(error_tags['username'])
except:
pass
def capture_warning(warning_string):
client.captureMessage(warning_string, level='warning')
@@ -46,9 +55,7 @@ else:
client.captureMessage(info_string, level='info')
def capture_exception(*args, **kwargs):
exc_info = sys.exc_info()
if not exc_info[0] is capnp.lib.capnp.KjException:
client.captureException(*args, **kwargs)
client.captureException(*args, **kwargs)
cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1))
def bind_user(**kwargs):
selfdrive/dragonpilot/dragonconf/__init__.py
+54 -11
View File
@@ -1,26 +1,69 @@
#!/usr/bin/env python2.7
# import os
# import json
# file = '/data/dragonpilot.json'
from common.params import Params
default_conf = {
'DragonEnableDashcam': '1',
'DragonDisableDriverSafetyCheck': '0',
'DragonDisableDriverSafetyCheck': '0', # deprecated
'DragonEnableDriverSafetyCheck': '1',
'DragonAutoShutdownAt': '30', # in minute
'DragonTempDisableSteerOnSignal': '0',
'DragonDisableLogger': '0',
'DragonDisableUploader': '0',
'DragonTempDisableSteerOnSignal': '0', # deprecated
'DragonEnableSteeringOnSignal': '0',
'DragonDisableLogger': '0', # deprecated
'DragonEnableLogger': '1',
'DragonDisableUploader': '0', # deprecated
'DragonEnableUploader': '1',
'DragonNoctuaMode': '0',
'DragonCacheCar': '1',
'DragonCacheCar': '0',
'DragonCachedModel': '', # for cache car
'DragonCachedFP': '', # for cache car
'DragonCachedVIN': '', # for cache car
'DragonAllowGas': '0',
'DragonBBUI': '0', # deprecated
'DragonToyotaStockDSU': '0',
'DragonLatCtrl': '1',
'DragonUIEvent': '0',
'DragonUIMaxSpeed': '0',
'DragonUIFace': '0',
'DragonUIDev': '0',
'DragonUIDevMini': '1',
}
deprecated_conf = {
'DragonDisableDriverSafetyCheck': 'DragonEnableDriverSafetyCheck',
'DragonTempDisableSteerOnSignal': 'DragonEnableSteeringOnSignal',
'DragonDisableLogger': 'DragonEnableLogger',
'DragonDisableUploader': 'DragonEnableUploader',
'DragonBBUI': 'DragonUIDev',
}
deprecated_conf_invert = {
'DragonDisableDriverSafetyCheck': True,
'DragonTempDisableSteerOnSignal': False,
'DragonDisableLogger': True,
'DragonDisableUploader': True,
'DragonBBUI': False
}
def dragonpilot_set_params(params):
# remove deprecated params
for old, new in deprecated_conf.items():
if params.get(old) is not None:
if new is not None:
old_val = str(params.get(old))
new_val = old_val
# invert the value if true
if old in deprecated_conf_invert and deprecated_conf_invert[old] is True:
new_val = "1" if old_val == "0" else "0"
params.put(new, new_val)
params.delete(old)
# set params
for key, val in default_conf.items():
if params.get(key) is None:
if params.get(key) is None and key not in deprecated_conf:
params.put(key, str(val))
if __name__ == "__main__":
params = Params()
params.manager_start()
dragonpilot_set_params(params)
selfdrive/locationd/locationd_yawrate.cc
+11 -7
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) {
selfdrive/locationd/locationd_yawrate.h
+3 -1
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);
};
selfdrive/locationd/paramsd.cc
+18 -4
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 {
selfdrive/locationd/test/test_params_learner.py
+2 -2
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
selfdrive/manager.py
+9 -3
View File
@@ -137,6 +137,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',
@@ -267,6 +270,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()
@@ -374,7 +379,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
@@ -572,6 +576,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")
dragonpilot_set_params(params)
@@ -591,11 +597,11 @@ def main():
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
close_fds=True)
if params.get("DragonDisableLogger") == "1":
if params.get("DragonEnableLogger") == "0":
del managed_processes['loggerd']
del managed_processes['tombstoned']
if params.get("DragonDisableUploader") == "1":
if params.get("DragonEnableUploader") == "0":
del managed_processes['uploader']
try:
selfdrive/service_list.yaml
+2 -2
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.]
selfdrive/test/plant/maneuver.py
+25 -11
View File
@@ -1,3 +1,4 @@
from collections import defaultdict
from selfdrive.test.plant.maneuverplots import ManeuverPlot
from selfdrive.test.plant.plant import Plant
import numpy as np
@@ -16,6 +17,7 @@ class Maneuver(object):
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
self.checks = kwargs.get("checks", [])
self.duration = duration
self.title = title
@@ -28,6 +30,7 @@ class Maneuver(object):
distance_lead = self.distance_lead
)
logs = defaultdict(list)
last_controls_state = None
plot = ManeuverPlot(self.title)
@@ -42,20 +45,24 @@ class Maneuver(object):
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
if controls_state:
last_controls_state = controls_state[-1]
# distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
log = plant.step(speed_lead, current_button, grade)
d_rel = distance_lead - distance if self.lead_relevancy else 200.
v_rel = speed_lead - speed if self.lead_relevancy else 0.
if log['controls_state_msgs']:
last_controls_state = log['controls_state_msgs'][-1]
d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
log['d_rel'] = d_rel
log['v_rel'] = v_rel
if last_controls_state:
# print(last_controls_state)
#develop plots
plot.add_data(
time=plant.current_time(),
gas=gas, brake=brake, steer_torque=steer_torque,
distance=distance, speed=speed, acceleration=acceleration,
gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
uf_accel_cmd=last_controls_state.ufAccelCmd,
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
@@ -63,10 +70,17 @@ class Maneuver(object):
cruise_speed=last_controls_state.vCruise,
jerk_factor=last_controls_state.jerkFactor,
a_target=last_controls_state.aTarget,
fcw=fcw)
fcw=log['fcw'])
print("maneuver end")
return (None, plot)
for k, v in log.items():
logs[k].append(v)
valid = True
for check in self.checks:
c = check(logs)
if not c:
print check.__name__ + " not valid!"
valid = valid and c
print("maneuver end", valid)
return (plot, valid)
selfdrive/test/plant/plant.py
+33 -4
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):
selfdrive/test/tests/plant/test_longitudinal.py
+87 -50
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],
)
]
@@ -293,6 +325,7 @@ def setup_output():
class LongitudinalControl(unittest.TestCase):
@classmethod
def setUpClass(cls):
os.environ['NO_CAN_TIMEOUT'] = "1"
setup_output()
@@ -314,26 +347,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)
selfdrive/thermald.py
+7 -7
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
params = Params()
@@ -142,8 +142,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
@@ -191,13 +191,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:
@@ -268,7 +268,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),
selfdrive/ui/ui.c
+305 -50
View File
@@ -69,6 +69,7 @@ const int viz_w = vwp_w-(bdr_s*2);
const int header_h = 420;
const int footer_h = 280;
const int footer_y = vwp_h-bdr_s-footer_h;
const int bdr_is = 30;
const int UI_FREQ = 30; // Hz
@@ -127,8 +128,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;
@@ -293,6 +293,22 @@ typedef struct UIState {
model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
track_vertices_data track_vertices[2];
// dragonpilot
int dragon_ui_event_timeout;
int dragon_ui_maxspeed_timeout;
int dragon_ui_face_timeout;
int dragon_ui_dev_timeout;
int dragon_ui_dev_mini_timeout;
int dragon_enable_dashcam_timeout;
bool dragon_ui_event;
bool dragon_ui_maxspeed;
bool dragon_ui_face;
bool dragon_ui_dev;
bool dragon_ui_dev_mini;
bool dragon_enable_dashcam;
} UIState;
static int last_brightness = -1;
@@ -662,11 +678,27 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
read_param_bool(&s->is_metric, "IsMetric");
read_param_bool(&s->longitudinal_control, "LongitudinalControl");
read_param_bool(&s->limit_set_speed, "LimitSetSpeed");
// dragonpilot
read_param_bool(&s->dragon_ui_event, "DragonUIEvent");
read_param_bool(&s->dragon_ui_maxspeed, "DragonUIMaxSpeed");
read_param_bool(&s->dragon_ui_face, "DragonUIFace");
read_param_bool(&s->dragon_ui_dev, "DragonUIDev");
read_param_bool(&s->dragon_ui_dev_mini, "DragonUIDevMini");
read_param_bool(&s->dragon_enable_dashcam, "DragonEnableDashcam");
// Set offsets so params don't get read at the same time
s->longitudinal_control_timeout = UI_FREQ / 3;
s->is_metric_timeout = UI_FREQ / 2;
s->limit_set_speed_timeout = UI_FREQ;
// dragonpilot, 1 sec
s->dragon_ui_event_timeout = 100;
s->dragon_ui_maxspeed_timeout = 100;
s->dragon_ui_face_timeout = 100;
s->dragon_ui_dev_timeout = 100;
s->dragon_ui_dev_mini_timeout = 100;
s->dragon_enable_dashcam_timeout = 100;
}
static void ui_draw_transformed_box(UIState *s, uint32_t color) {
@@ -1165,17 +1197,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) {
@@ -1306,7 +1327,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);
@@ -1418,13 +1439,17 @@ static void ui_draw_vision_header(UIState *s) {
nvgRect(s->vg, ui_viz_rx, box_y, ui_viz_rw, header_h);
nvgFill(s->vg);
//ui_draw_vision_maxspeed(s);
if (s->dragon_ui_maxspeed) {
ui_draw_vision_maxspeed(s);
}
#ifdef SHOW_SPEEDLIMIT
ui_draw_vision_speedlimit(s);
#endif
ui_draw_vision_speed(s);
//ui_draw_vision_event(s);
if (s->dragon_ui_event) {
ui_draw_vision_event(s);
}
}
static void ui_draw_infobar(UIState *s) {
@@ -1445,40 +1470,54 @@ static void ui_draw_infobar(UIState *s) {
time_t t = time(NULL);
struct tm tm = *localtime(&t);
char rel_steer[9];
snprintf(rel_steer, sizeof(rel_steer), "%s%05.1f°", s->scene.angleSteers < 0? "-" : "+", fabs(s->scene.angleSteers));
if (s->dragon_ui_dev_mini) {
char rel_steer[9];
snprintf(rel_steer, sizeof(rel_steer), "%s%05.1f°", s->scene.angleSteers < 0? "-" : "+", fabs(s->scene.angleSteers));
char des_steer[9];
if (s->scene.engaged) {
snprintf(des_steer, sizeof(des_steer), "%s%05.1f°", s->scene.angleSteersDes < 0? "-" : "+", fabs(s->scene.angleSteersDes));
char des_steer[9];
if (s->scene.engaged) {
snprintf(des_steer, sizeof(des_steer), "%s%05.1f°", s->scene.angleSteersDes < 0? "-" : "+", fabs(s->scene.angleSteersDes));
} else {
snprintf(des_steer, sizeof(des_steer), "%7s", "N/A");
}
char lead_dist[8];
if (s->scene.lead_status) {
snprintf(lead_dist, sizeof(lead_dist), "%06.2fm", s->scene.lead_d_rel);
} else {
snprintf(lead_dist, sizeof(lead_dist), "%7s", "N/A");
}
snprintf(
infobar,
sizeof(infobar),
"%04d/%02d/%02d %02d:%02d:%02d | REL: %s | DES: %s | DIST: %s",
tm.tm_year + 1900,
tm.tm_mon + 1,
tm.tm_mday,
tm.tm_hour,
tm.tm_min,
tm.tm_sec,
rel_steer,
des_steer,
lead_dist
);
} else {
snprintf(des_steer, sizeof(des_steer), "%7s", "N/A");
snprintf(
infobar,
sizeof(infobar),
"%04d/%02d/%02d %02d:%02d:%02d",
tm.tm_year + 1900,
tm.tm_mon + 1,
tm.tm_mday,
tm.tm_hour,
tm.tm_min,
tm.tm_sec
);
}
char lead_dist[8];
if (s->scene.lead_status) {
snprintf(lead_dist, sizeof(lead_dist), "%06.2fm", s->scene.lead_d_rel);
} else {
snprintf(lead_dist, sizeof(lead_dist), "%7s", "N/A");
}
snprintf(
infobar,
sizeof(infobar),
"%04d/%02d/%02d %02d:%02d:%02d | REL: %s | DES: %s | DIST: %s",
tm.tm_year + 1900,
tm.tm_mon + 1,
tm.tm_mday,
tm.tm_hour,
tm.tm_min,
tm.tm_sec,
rel_steer,
des_steer,
lead_dist
);
nvgBeginPath(s->vg);
nvgRoundedRect(s->vg, rect_x + sidebar_offset, rect_y, rect_w, rect_h, 15);
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100));
@@ -1490,6 +1529,209 @@ static void ui_draw_infobar(UIState *s) {
nvgText(s->vg, rect_x + 720 + sidebar_offset, rect_y + 35, infobar, NULL);
}
//BB START: functions added for the display of various items
static int bb_ui_draw_measure(UIState *s, const char* bb_value, const char* bb_uom, const char* bb_label,
int bb_x, int bb_y, int bb_uom_dx,
NVGcolor bb_valueColor, NVGcolor bb_labelColor, NVGcolor bb_uomColor,
int bb_valueFontSize, int bb_labelFontSize, int bb_uomFontSize ) {
const UIScene *scene = &s->scene;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
int dx = 0;
if (strlen(bb_uom) > 0) {
dx = (int)(bb_uomFontSize*2.5/2);
}
//print value
nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, bb_valueFontSize*2.5);
nvgFillColor(s->vg, bb_valueColor);
nvgText(s->vg, bb_x-dx/2, bb_y+ (int)(bb_valueFontSize*2.5)+5, bb_value, NULL);
//print label
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, bb_labelFontSize*2.5);
nvgFillColor(s->vg, bb_labelColor);
nvgText(s->vg, bb_x, bb_y + (int)(bb_valueFontSize*2.5)+5 + (int)(bb_labelFontSize*2.5)+5, bb_label, NULL);
//print uom
if (strlen(bb_uom) > 0) {
nvgSave(s->vg);
int rx =bb_x + bb_uom_dx + bb_valueFontSize -3;
int ry = bb_y + (int)(bb_valueFontSize*2.5/2)+25;
nvgTranslate(s->vg,rx,ry);
nvgRotate(s->vg, -1.5708); //-90deg in radians
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, (int)(bb_uomFontSize*2.5));
nvgFillColor(s->vg, bb_uomColor);
nvgText(s->vg, 0, 0, bb_uom, NULL);
nvgRestore(s->vg);
}
return (int)((bb_valueFontSize + bb_labelFontSize)*2.5) + 5;
}
static void bb_ui_draw_measures_left(UIState *s, int bb_x, int bb_y, int bb_w ) {
const UIScene *scene = &s->scene;
int bb_rx = bb_x + (int)(bb_w/2);
int bb_ry = bb_y;
int bb_h = 5;
NVGcolor lab_color = nvgRGBA(255, 255, 255, 200);
NVGcolor uom_color = nvgRGBA(255, 255, 255, 200);
int value_fontSize=30;
int label_fontSize=15;
int uom_fontSize = 15;
int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ;
//add visual radar relative distance
if (true) {
char val_str[16];
char uom_str[6];
NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
if (scene->lead_status) {
//show RED if less than 5 meters
//show orange if less than 15 meters
if((int)(scene->lead_d_rel) < 15) {
val_color = nvgRGBA(255, 188, 3, 200);
}
if((int)(scene->lead_d_rel) < 5) {
val_color = nvgRGBA(255, 0, 0, 200);
}
// lead car relative distance is always in meters
snprintf(val_str, sizeof(val_str), "%d", (int)scene->lead_d_rel);
} else {
snprintf(val_str, sizeof(val_str), "-");
}
snprintf(uom_str, sizeof(uom_str), "m ");
bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL DIST",
bb_rx, bb_ry, bb_uom_dx,
val_color, lab_color, uom_color,
value_fontSize, label_fontSize, uom_fontSize );
bb_ry = bb_y + bb_h;
}
//add visual radar relative speed
if (true) {
char val_str[16];
char uom_str[6];
NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
if (scene->lead_status) {
//show Orange if negative speed (approaching)
//show Orange if negative speed faster than 5mph (approaching fast)
if((int)(scene->lead_v_rel) < 0) {
val_color = nvgRGBA(255, 188, 3, 200);
}
if((int)(scene->lead_v_rel) < -5) {
val_color = nvgRGBA(255, 0, 0, 200);
}
// lead car relative speed is always in meters
if (s->is_metric) {
snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 3.6 + 0.5));
} else {
snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 2.2374144 + 0.5));
}
} else {
snprintf(val_str, sizeof(val_str), "-");
}
if (s->is_metric) {
snprintf(uom_str, sizeof(uom_str), "km/h");;
} else {
snprintf(uom_str, sizeof(uom_str), "mph");
}
bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL SPEED",
bb_rx, bb_ry, bb_uom_dx,
val_color, lab_color, uom_color,
value_fontSize, label_fontSize, uom_fontSize );
bb_ry = bb_y + bb_h;
}
//finally draw the frame
bb_h += 20;
nvgBeginPath(s->vg);
nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20);
nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80));
nvgStrokeWidth(s->vg, 6);
nvgStroke(s->vg);
}
static void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) {
const UIScene *scene = &s->scene;
int bb_rx = bb_x + (int)(bb_w/2);
int bb_ry = bb_y;
int bb_h = 5;
NVGcolor lab_color = nvgRGBA(255, 255, 255, 200);
NVGcolor uom_color = nvgRGBA(255, 255, 255, 200);
int value_fontSize=30;
int label_fontSize=15;
int uom_fontSize = 15;
int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ;
//add steering angle
if (true) {
char val_str[16];
char uom_str[6];
NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
//show Orange if more than 6 degrees
//show red if more than 12 degrees
if(((int)(scene->angleSteers) < -6) || ((int)(scene->angleSteers) > 6)) {
val_color = nvgRGBA(255, 188, 3, 200);
}
if(((int)(scene->angleSteers) < -12) || ((int)(scene->angleSteers) > 12)) {
val_color = nvgRGBA(255, 0, 0, 200);
}
// steering is in degrees
snprintf(val_str, sizeof(val_str), "%.0f°",(scene->angleSteers));
snprintf(uom_str, sizeof(uom_str), "");
bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REAL STEER",
bb_rx, bb_ry, bb_uom_dx,
val_color, lab_color, uom_color,
value_fontSize, label_fontSize, uom_fontSize );
bb_ry = bb_y + bb_h;
}
//add desired steering angle
if (true) {
char val_str[16];
char uom_str[6];
NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
//show Orange if more than 6 degrees
//show red if more than 12 degrees
if(((int)(scene->angleSteersDes) < -6) || ((int)(scene->angleSteersDes) > 6)) {
val_color = nvgRGBA(255, 188, 3, 200);
}
if(((int)(scene->angleSteersDes) < -12) || ((int)(scene->angleSteersDes) > 12)) {
val_color = nvgRGBA(255, 0, 0, 200);
}
// steering is in degrees
snprintf(val_str, sizeof(val_str), "%.0f°",(scene->angleSteersDes));
snprintf(uom_str, sizeof(uom_str), "");
bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "DESIR STEER",
bb_rx, bb_ry, bb_uom_dx,
val_color, lab_color, uom_color,
value_fontSize, label_fontSize, uom_fontSize );
bb_ry = bb_y + bb_h;
}
//finally draw the frame
bb_h += 20;
nvgBeginPath(s->vg);
nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20);
nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80));
nvgStrokeWidth(s->vg, 6);
nvgStroke(s->vg);
}
static void ui_draw_bbui(UIState *s) {
const UIScene *scene = &s->scene;
const int bb_dml_w = 180;
const int bb_dml_x = (scene->ui_viz_rx + (bdr_is * 2));
const int bb_dml_y = (box_y + (bdr_is * 1.5)) + 220;
const int bb_dmr_w = 180;
const int bb_dmr_x = scene->ui_viz_rx + scene->ui_viz_rw - bb_dmr_w - (bdr_is * 2);
const int bb_dmr_y = (box_y + (bdr_is * 1.5)) + 220;
bb_ui_draw_measures_right(s, bb_dml_x, bb_dml_y, bb_dml_w);
bb_ui_draw_measures_left(s, bb_dmr_x, bb_dmr_y, bb_dmr_w);
}
static void ui_draw_vision_footer(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
@@ -1498,12 +1740,19 @@ static void ui_draw_vision_footer(UIState *s) {
nvgBeginPath(s->vg);
nvgRect(s->vg, ui_viz_rx, footer_y, ui_viz_rw, footer_h);
//ui_draw_vision_face(s);
if (s->dragon_ui_face) {
ui_draw_vision_face(s);
}
#ifdef SHOW_SPEEDLIMIT
ui_draw_vision_map(s);
#endif
ui_draw_infobar(s);
if (s->dragon_ui_dev) {
ui_draw_bbui(s);
}
if (s->dragon_ui_dev_mini || s->dragon_enable_dashcam) {
ui_draw_infobar(s);
}
}
static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
@@ -1719,8 +1968,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;
s->scene.angleSteers = datad.angleSteers;
s->scene.angleSteersDes = datad.angleSteersDes;
@@ -2404,6 +2652,13 @@ int main(int argc, char* argv[]) {
read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout);
read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout);
read_param_float_timeout(&s->speed_lim_off, "SpeedLimitOffset", &s->limit_set_speed_timeout);
// dragonpilot
read_param_bool_timeout(&s->dragon_ui_event, "DragonUIEvent", &s->dragon_ui_event_timeout);
read_param_bool_timeout(&s->dragon_ui_maxspeed, "DragonUIMaxSpeed", &s->dragon_ui_maxspeed_timeout);
read_param_bool_timeout(&s->dragon_ui_face, "DragonUIFace", &s->dragon_ui_face_timeout);
read_param_bool_timeout(&s->dragon_ui_dev, "DragonUIDev", &s->dragon_ui_dev_timeout);
read_param_bool_timeout(&s->dragon_ui_dev_mini, "DragonUIDevMini", &s->dragon_ui_dev_mini_timeout);
read_param_bool_timeout(&s->dragon_enable_dashcam, "DragonEnableDashcam", &s->dragon_enable_dashcam_timeout);
pthread_mutex_unlock(&s->lock);
selfdrive/visiond/models/commonmodel.c
+49 -16
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);
selfdrive/visiond/models/driving.cc
+61 -13
View File
@@ -22,9 +22,12 @@
#define MODEL_NAME "driving_model_dlc"
#endif
#define OUTPUT_SIZE (200 + 2*201 + 26)
#define LEAD_MDN_N 5
#define LEAD_MDN_N 5 // probs for 5 groups
#define MDN_VALS 4 // output xyva for each lead group
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
#define MDN_GROUP_SIZE 11
#define SPEED_BUCKETS 100
#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + SPEED_BUCKETS)
#ifdef TEMPORAL
#define TEMPORAL_SIZE 512
#else
@@ -60,6 +63,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
float *left_lane;
float *right_lane;
float *lead;
float *speed;
} net_outputs = {NULL};
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
@@ -71,7 +75,8 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
//fclose(f);
//sleep(1);
//printf("done \n");
//printf("%i \n",OUTPUT_SIZE);
//printf("%i \n",MDN_GROUP_SIZE);
s->m->execute(net_input_buf);
// net outputs
@@ -79,6 +84,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
ModelData model = {0};
@@ -91,9 +97,9 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
}
model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.path.prob = 1.;
model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
@@ -105,17 +111,59 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
// current lead
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*5 + 4] > net_outputs.lead[mdn_max_idx*5 + 4]) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
mdn_max_idx = i;
}
}
model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*5]);
model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist;
model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist;
model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel;
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel;
model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE]);
model.lead.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
model.lead.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
model.lead.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
model.lead.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
model.lead.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// lead in 2s
mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
mdn_max_idx = i;
}
}
model.lead_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// get speed percentiles numbers represent 5th, 15th, ... 95th percentile
for (int i=0; i < SPEED_PERCENTILES; i++) {
model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
}
float sum = 0;
for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
sum += net_outputs.speed[idx];
int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
if (idx_percentile < SPEED_PERCENTILES ){
model.speed[idx_percentile] = ((float)idx)/2.0;
}
}
// make sure no percentiles are skipped
for (int i=SPEED_PERCENTILES-1; i > 0; i--){
if (model.speed[i-1] > model.speed[i]){
model.speed[i-1] = model.speed[i];
}
}
return model;
}
selfdrive/visiond/models/monitoring.cc
+5 -3
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;
}
selfdrive/visiond/models/monitoring.h
+10 -3
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 {
selfdrive/visiond/visiond.cc
+17 -4
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;