retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.6.4 release

Vehicle Researcher 2019-09-09 23:03:02 +00:00
parent af4f9f1f31
commit 61229779e4
112 changed files with 2408 additions and 1088 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -39,5 +39,6 @@ selfdrive/sensord/sensord
one
openpilot
notebooks
xx

124
README.md
View File

@ -58,69 +58,70 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur
Supported Cars
------
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Buick<sup>3</sup> | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
| GMC<sup>3</sup> | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Holden<sup>3</sup> | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
| Honda | Accord Hybrid 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 |
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Avalon 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | C-HR Hybrid 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-19 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Highlander 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Highlander Hybrid 2017-19| All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius Prime 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Sienna 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Make | Model (US Market Reference)| Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
| ---------------------| ---------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Buick<sup>3</sup> | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
| GMC<sup>3</sup> | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Holden<sup>3</sup> | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
| Honda | Accord Hybrid 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 |
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Lexus | IS 2017-2019 | All | Yes | Stock | 22mph | 0mph | Toyota |
| Lexus | IS Hybrid 2017 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Avalon 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | C-HR Hybrid 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-19 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Highlander 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Highlander Hybrid 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius Prime 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Sienna 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai).*** <br />
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). <br />
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/). <br />
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
<sup>4</sup>Subaru Giraffe is DIY. <br />
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed for the 2019 Sante Fe; pinout may differ for other Hyundais. <br />
@ -144,6 +145,7 @@ In Progress Cars
- All Hyundai with SmartSense.
- All Kia with SCC and LKAS.
- All Chrysler, Jeep, Fiat with Adaptive Cruise Control and LaneSense.
- All Subaru with EyeSight.
How can I add support for my car?
------

10
RELEASES.md
View File

@ -1,3 +1,13 @@
Version 0.6.4 (2019-09-08)
========================
* Forward stock AEB for Honda Nidec
* Improve lane centering on banked roads
* Always-on forward collision warning
* Always-on driver monitoring, except for right hand drive countries
* Driver monitoring learns the user's normal driving position
* Honda Fit support thanks to energee!
* Lexus IS support
Version 0.6.3 (2019-08-12)
========================
* Alert sounds from EON: requires NEOS update

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

Binary file not shown.

14
common/api/__init__.py
View File

@ -5,9 +5,10 @@ from datetime import datetime, timedelta
from selfdrive.version import version
class Api(object):
def __init__(self, dongle_id, private_key):
def __init__(self, dongle_id):
self.dongle_id = dongle_id
self.private_key = private_key
with open('/persist/comma/id_rsa') as f:
self.private_key = f.read()
def get(self, *args, **kwargs):
return self.request('GET', *args, **kwargs)
@ -19,7 +20,14 @@ class Api(object):
return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
def get_token(self):
return jwt.encode({'identity': self.dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, self.private_key, algorithm='RS256')
now = datetime.utcnow()
payload = {
'identity': self.dongle_id,
'nbf': now,
'iat': now,
'exp': now + timedelta(hours=1)
}
return jwt.encode(payload, self.private_key, algorithm='RS256')
def api_get(endpoint, method='GET', timeout=None, access_token=None, **params):
backend = "https://api.commadotai.com/"

17
common/file_helpers.py
View File

@ -24,21 +24,12 @@ def rm_tree_or_link(path):
shutil.rmtree(path)
def get_tmpdir_on_same_filesystem(path):
# TODO(mgraczyk): HACK, we should actually check for which filesystem.
normpath = os.path.normpath(path)
parts = normpath.split("/")
if len(parts) > 1:
if parts[1].startswith("raid") or parts[1].startswith("datasets"):
if len(parts) > 2 and parts[2] == "runner":
return "/{}/runner/tmp".format(parts[1])
elif len(parts) > 2 and parts[2] == "aws":
return "/{}/aws/tmp".format(parts[1])
else:
return "/{}/tmp".format(parts[1])
elif parts[1] == "aws":
return "/aws/tmp"
elif parts[1] == "scratch":
return "/scratch/tmp"
if len(parts) > 1 and parts[1] == "scratch":
return "/scratch/tmp"
elif len(parts) > 2 and parts[2] == "runner":
return "/{}/runner/tmp".format(parts[1])
return "/tmp"
class AutoMoveTempdir(object):

15
common/params.py
View File

@ -27,6 +27,7 @@ import sys
import shutil
import fcntl
import tempfile
import threading
from enum import Enum
@ -63,10 +64,9 @@ keys = {
"GitCommit": [TxType.PERSISTENT],
"GitRemote": [TxType.PERSISTENT],
"HasAcceptedTerms": [TxType.PERSISTENT],
"IsDriverMonitoringEnabled": [TxType.PERSISTENT],
"IsFcwEnabled": [TxType.PERSISTENT],
"IsGeofenceEnabled": [TxType.PERSISTENT],
"IsMetric": [TxType.PERSISTENT],
"IsRHD": [TxType.PERSISTENT],
"IsUpdateAvailable": [TxType.PERSISTENT],
"IsUploadRawEnabled": [TxType.PERSISTENT],
"IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT],
@ -346,6 +346,17 @@ class Params(object):
write_db(self.db, key, dat)
def put_nonblocking(key, val):
def f(key, val):
params = Params()
params.put(key, val)
t = threading.Thread(target=f, args=(key, val))
t.start()
return t
if __name__ == "__main__":
params = Params()
if len(sys.argv) > 2:

73
common/stat_live.py 100644
View File

@ -0,0 +1,73 @@
import numpy as np
class RunningStat():
# tracks realtime mean and standard deviation without storing any data
def __init__(self, priors=None, max_trackable=-1):
self.max_trackable = max_trackable
if priors is not None:
# initialize from history
self.M = priors[0]
self.S = priors[1]
self.n = priors[2]
self.M_last = self.M
self.S_last = self.S
else:
self.reset()
def reset(self):
self.M = 0.
self.S = 0.
self.M_last = 0.
self.S_last = 0.
self.n = 0
def push_data(self, new_data):
# short term memory hack
if self.max_trackable < 0 or self.n < self.max_trackable:
self.n += 1
if self.n == 0:
self.M_last = new_data
self.M = self.M_last
self.S_last = 0.
else:
self.M = self.M_last + (new_data - self.M_last) / self.n
self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M);
self.M_last = self.M
self.S_last = self.S
def mean(self):
return self.M
def variance(self):
if self.n >= 2:
return self.S / (self.n - 1.)
else:
return 0
def std(self):
return np.sqrt(self.variance())
def params_to_save(self):
return [self.M, self.S, self.n]
class RunningStatFilter():
def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
self.raw_stat = RunningStat(raw_priors, max_trackable)
self.filtered_stat = RunningStat(filtered_priors, max_trackable)
def reset(self):
self.raw_stat.reset()
self.filtered_stat.reset()
def push_and_update(self, new_data):
_std_last = self.raw_stat.std()
self.raw_stat.push_data(new_data)
_delta_std = self.raw_stat.std() - _std_last
if _delta_std<=0:
self.filtered_stat.push_data(new_data)
else:
pass
# self.filtered_stat.push_data(self.filtered_stat.mean())
# class SequentialBayesian():

43
common/transformations/camera.py
View File

@ -154,17 +154,40 @@ def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
W_border: size[1] - W_border]
def get_camera_frame_from_calib_frame(camera_frame_from_road_frame):
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
calib_frame_from_ground = np.dot(eon_intrinsics,
get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)]
ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground)
camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame)
return camera_frame_from_calib_frame
def pretransform_from_calib(calib):
roll, pitch, yaw, height = calib
view_frame_from_road_frame = get_view_frame_from_road_frame(roll, pitch, yaw, height)
camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame)
camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame)
return np.linalg.inv(camera_frame_from_calib_frame)
def transform_img(base_img,
augment_trans=np.array([0,0,0]),
augment_eulers=np.array([0,0,0]),
from_intr=eon_intrinsics,
to_intr=eon_intrinsics,
calib_rot_view=None,
output_size=None,
pretransform=None,
top_hacks=True):
top_hacks=False,
yuv=False,
alpha=1.0,
beta=0,
blur=0):
import cv2
if yuv:
base_img = cv2.cvtColor(base_img, cv2.COLOR_YUV2RGB_I420)
size = base_img.shape[:2]
if not output_size:
output_size = size[::-1]
@ -180,8 +203,6 @@ def transform_img(base_img,
h*np.ones(4),
h/quadrangle_norm[:,1]))
rot = orient.rot_from_euler(augment_eulers)
if calib_rot_view is not None:
rot = calib_rot_view.dot(rot)
to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
to_KE = to_intr.dot(to_extrinsics)
warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
@ -202,7 +223,19 @@ def transform_img(base_img,
M = M.dot(pretransform)
augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE)
return augmented_rgb
# brightness and contrast augment
augmented_rgb = np.clip((float(alpha)*augmented_rgb + beta), 0, 255).astype(np.uint8)
# gaussian blur
if blur > 0:
augmented_rgb = cv2.GaussianBlur(augmented_rgb,(blur*2+1,blur*2+1),cv2.BORDER_DEFAULT)
if yuv:
augmented_img = cv2.cvtColor(augmented_rgb, cv2.COLOR_RGB2YUV_I420)
else:
augmented_img = augmented_rgb
return augmented_img
def yuv_crop(frame, output_size, center=None):
# output_size in camera coordinates so u,v

6
common/transformations/model.py
View File

@ -1,8 +1,8 @@
import numpy as np
from common.transformations.camera import eon_focal_length, \
vp_from_ke, get_view_frame_from_road_frame, \
FULL_FRAME_SIZE
from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length,
get_view_frame_from_road_frame,
vp_from_ke)
# segnet

BIN
installer/updater/updater
View File

Binary file not shown.

78
installer/updater/updater.cc
View File

@ -117,11 +117,19 @@ size_t download_file_write(void *ptr, size_t size, size_t nmeb, void *up) {
return fwrite(ptr, size, nmeb, (FILE*)up);
}
bool check_battery() {
int battery_capacity() {
std::string bat_cap_s = util::read_file("/sys/class/power_supply/battery/capacity");
int bat_cap = atoi(bat_cap_s.c_str());
return atoi(bat_cap_s.c_str());
}
int battery_current() {
std::string current_now_s = util::read_file("/sys/class/power_supply/battery/current_now");
int current_now = atoi(current_now_s.c_str());
return atoi(current_now_s.c_str());
}
bool check_battery() {
int bat_cap = battery_capacity();
int current_now = battery_current();
return bat_cap > 35 || (current_now < 0 && bat_cap > 10);
}
@ -163,6 +171,7 @@ struct Updater {
// i hate state machines give me coroutines already
enum UpdateState {
CONFIRMATION,
LOW_BATTERY,
RUNNING,
ERROR,
};
@ -173,6 +182,12 @@ struct Updater {
std::string error_text;
std::string low_battery_text;
std::string low_battery_title;
std::string low_battery_context;
std::string battery_cap_text;
int min_battery_cap = 35;
// button
int b_x, b_w, b_y, b_h;
int balt_x;
@ -296,6 +311,16 @@ struct Updater {
state = ERROR;
}
void set_battery_low() {
std::lock_guard<std::mutex> guard(lock);
state = LOW_BATTERY;
}
void set_running() {
std::lock_guard<std::mutex> guard(lock);
state = RUNNING;
}
std::string stage_download(std::string url, std::string hash, std::string name) {
std::string out_fn = UPDATE_DIR "/" + util::base_name(url);
@ -323,8 +348,14 @@ struct Updater {
assert(curl);
if (!check_battery()) {
set_error("Please plug power in to your EON and wait for charge");
return;
set_battery_low();
int battery_cap = battery_capacity();
while(battery_cap < min_battery_cap) {
battery_cap = battery_capacity();
battery_cap_text = std::to_string(battery_cap);
usleep(1000000);
}
set_running();
}
if (!check_space()) {
@ -400,8 +431,14 @@ struct Updater {
}
if (!check_battery()) {
set_error("must have at least 35% battery to update");
return;
set_battery_low();
int battery_cap = battery_capacity();
while(battery_cap < min_battery_cap) {
battery_cap = battery_capacity();
battery_cap_text = std::to_string(battery_cap);
usleep(1000000);
}
set_running();
}
if (!recovery_fn.empty()) {
@ -526,6 +563,27 @@ struct Updater {
}
}
void draw_battery_screen() {
low_battery_title = "Low Battery";
low_battery_text = "Please connect EON to your charger. Update will continue once EON battery reaches 35%.";
low_battery_context = "Current battery charge: " + battery_cap_text + "%";
nvgFillColor(vg, nvgRGBA(255,255,255,255));
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
nvgFontFace(vg, "opensans_bold");
nvgFontSize(vg, 120.0f);
nvgTextBox(vg, 110, 220, fb_w-240, low_battery_title.c_str(), NULL);
nvgFontFace(vg, "opensans_regular");
nvgFontSize(vg, 86.0f);
nvgTextBox(vg, 130, 380, fb_w-260, low_battery_text.c_str(), NULL);
nvgFontFace(vg, "opensans_bold");
nvgFontSize(vg, 86.0f);
nvgTextBox(vg, 130, 700, fb_w-260, low_battery_context.c_str(), NULL);
}
void draw_progress_screen() {
// draw progress message
nvgFontSize(vg, 64.0f);
@ -584,11 +642,14 @@ struct Updater {
"Continue",
"Connect to WiFi");
break;
case LOW_BATTERY:
draw_battery_screen();
break;
case RUNNING:
draw_progress_screen();
break;
case ERROR:
draw_ack_screen("There was an error.", ("ERROR: " + error_text + "\n\nYou will need to retry").c_str(), NULL, "exit");
draw_ack_screen("There was an error", (error_text).c_str(), NULL, "Reboot");
break;
}
@ -648,6 +709,7 @@ struct Updater {
glDisable(GL_BLEND);
eglSwapBuffers(display, surface);
assert(glGetError() == GL_NO_ERROR);
// no simple way to do 30fps vsync with surfaceflinger...

BIN
models/driving_model.dlc
View File

Binary file not shown.

BIN
panda/board/obj/panda.bin.signed 100644
View File

Binary file not shown.

274
phonelibs/boringssl/LICENSE 100644
View File

@ -0,0 +1,274 @@
BoringSSL is a fork of OpenSSL. As such, large parts of it fall under OpenSSL
licensing. Files that are completely new have a Google copyright and an ISC
license. This license is reproduced at the bottom of this file.
Contributors to BoringSSL are required to follow the CLA rules for Chromium:
https://cla.developers.google.com/clas
Files in third_party/ have their own licenses, as described therein. The MIT
license, for third_party/fiat, which, unlike other third_party directories, is
compiled into non-test libraries, is included below.
The OpenSSL toolkit stays under a dual license, i.e. both the conditions of the
OpenSSL License and the original SSLeay license apply to the toolkit. See below
for the actual license texts. Actually both licenses are BSD-style Open Source
licenses. In case of any license issues related to OpenSSL please contact
openssl-core@openssl.org.
The following are Google-internal bug numbers where explicit permission from
some authors is recorded for use of their work. (This is purely for our own
record keeping.)
27287199
27287880
27287883
OpenSSL License
---------------
/* ====================================================================
* Copyright (c) 1998-2011 The OpenSSL Project. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* 3. All advertising materials mentioning features or use of this
* software must display the following acknowledgment:
* "This product includes software developed by the OpenSSL Project
* for use in the OpenSSL Toolkit. (http://www.openssl.org/)"
*
* 4. The names "OpenSSL Toolkit" and "OpenSSL Project" must not be used to
* endorse or promote products derived from this software without
* prior written permission. For written permission, please contact
* openssl-core@openssl.org.
*
* 5. Products derived from this software may not be called "OpenSSL"
* nor may "OpenSSL" appear in their names without prior written
* permission of the OpenSSL Project.
*
* 6. Redistributions of any form whatsoever must retain the following
* acknowledgment:
* "This product includes software developed by the OpenSSL Project
* for use in the OpenSSL Toolkit (http://www.openssl.org/)"
*
* THIS SOFTWARE IS PROVIDED BY THE OpenSSL PROJECT ``AS IS'' AND ANY
* EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE OpenSSL PROJECT OR
* ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
* ====================================================================
*
* This product includes cryptographic software written by Eric Young
* (eay@cryptsoft.com). This product includes software written by Tim
* Hudson (tjh@cryptsoft.com).
*
*/
Original SSLeay License
-----------------------
/* Copyright (C) 1995-1998 Eric Young (eay@cryptsoft.com)
* All rights reserved.
*
* This package is an SSL implementation written
* by Eric Young (eay@cryptsoft.com).
* The implementation was written so as to conform with Netscapes SSL.
*
* This library is free for commercial and non-commercial use as long as
* the following conditions are aheared to. The following conditions
* apply to all code found in this distribution, be it the RC4, RSA,
* lhash, DES, etc., code; not just the SSL code. The SSL documentation
* included with this distribution is covered by the same copyright terms
* except that the holder is Tim Hudson (tjh@cryptsoft.com).
*
* Copyright remains Eric Young's, and as such any Copyright notices in
* the code are not to be removed.
* If this package is used in a product, Eric Young should be given attribution
* as the author of the parts of the library used.
* This can be in the form of a textual message at program startup or
* in documentation (online or textual) provided with the package.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. All advertising materials mentioning features or use of this software
* must display the following acknowledgement:
* "This product includes cryptographic software written by
* Eric Young (eay@cryptsoft.com)"
* The word 'cryptographic' can be left out if the rouines from the library
* being used are not cryptographic related :-).
* 4. If you include any Windows specific code (or a derivative thereof) from
* the apps directory (application code) you must include an acknowledgement:
* "This product includes software written by Tim Hudson (tjh@cryptsoft.com)"
*
* THIS SOFTWARE IS PROVIDED BY ERIC YOUNG ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* The licence and distribution terms for any publically available version or
* derivative of this code cannot be changed. i.e. this code cannot simply be
* copied and put under another distribution licence
* [including the GNU Public Licence.]
*/
ISC license used for completely new code in BoringSSL:
/* Copyright (c) 2015, Google Inc.
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */
The code in third_party/fiat carries the MIT license:
Copyright (c) 2015-2016 the fiat-crypto authors (see
https://github.com/mit-plv/fiat-crypto/blob/master/AUTHORS).
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
The code in third_party/sike also carries the MIT license:
Copyright (c) Microsoft Corporation. All rights reserved.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE
Licenses for support code
-------------------------
Parts of the TLS test suite are under the Go license. This code is not included
in BoringSSL (i.e. libcrypto and libssl) when compiled, however, so
distributing code linked against BoringSSL does not trigger this license:
Copyright (c) 2009 The Go Authors. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the
distribution.
* Neither the name of Google Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
BoringSSL uses the Chromium test infrastructure to run a continuous build,
trybots etc. The scripts which manage this, and the script for generating build
metadata, are under the Chromium license. Distributing code linked against
BoringSSL does not trigger this license.
Copyright 2015 The Chromium Authors. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the
distribution.
* Neither the name of Google Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

42
phonelibs/bzip2/LICENSE 100644
View File

@ -0,0 +1,42 @@
--------------------------------------------------------------------------
This program, "bzip2", the associated library "libbzip2", and all
documentation, are copyright (C) 1996-2010 Julian R Seward. All
rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product
documentation would be appreciated but is not required.
3. Altered source versions must be plainly marked as such, and must
not be misrepresented as being the original software.
4. The name of the author may not be used to endorse or promote
products derived from this software without specific prior written
permission.
THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS
OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Julian Seward, jseward@bzip.org
bzip2/libbzip2 version 1.0.6 of 6 September 2010
--------------------------------------------------------------------------

BIN
phonelibs/hierarchy/lib/_hierarchy.so
View File

Binary file not shown.

29
phonelibs/libyuv/LICENSE 100644
View File

@ -0,0 +1,29 @@
Copyright 2011 The LibYuv Project Authors. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of Google nor the names of its contributors may
be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

1
phonelibs/openblas/libopenblas.so
View File

@ -1 +0,0 @@
libopenblas_armv8p-r0.2.19.so

BIN
phonelibs/openblas/libopenblas_armv8p-r0.2.19.so
View File

Binary file not shown.

19
phonelibs/yaml-cpp/LICENSE 100644
View File

@ -0,0 +1,19 @@
Copyright (c) 2008-2015 Jesse Beder.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

2
run_docker_tests.sh
View File

@ -13,4 +13,4 @@ docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && pyt
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd'
docker run --rm -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/tests/process_replay/ && ./test_processes.py'
docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models_openpilot.py'
docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py'

BIN
selfdrive/assets/sounds/disengaged.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/engaged.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/error.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/warning_1.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/warning_2.wav
View File

Binary file not shown.

9
selfdrive/athena/athenad.py
View File

@ -1,6 +1,5 @@
#!/usr/bin/env python2.7
import json
import jwt
import os
import random
import re
@ -13,7 +12,6 @@ import traceback
import zmq
import requests
import six.moves.queue
from datetime import datetime, timedelta
from functools import partial
from jsonrpc import JSONRPCResponseManager, dispatcher
from websocket import create_connection, WebSocketTimeoutException, ABNF
@ -104,9 +102,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
params = Params()
dongle_id = params.get("DongleId")
private_key = open("/persist/comma/id_rsa").read()
identity_token = jwt.encode({'identity':dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
identity_token = Api(dongle_id).get_token()
ws = create_connection(remote_ws_uri,
cookie="jwt=" + identity_token,
enable_multithread=True)
@ -232,8 +228,7 @@ def main(gctx=None):
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
crash.install()
private_key = open("/persist/comma/id_rsa").read()
api = Api(dongle_id, private_key)
api = Api(dongle_id)
conn_retries = 0
while 1:

4
selfdrive/boardd/boardd.cc
View File

@ -43,6 +43,7 @@
#define SAFETY_TESLA 8
#define SAFETY_CHRYSLER 9
#define SAFETY_SUBARU 10
#define SAFETY_GM_PASSIVE 11
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@ -135,6 +136,9 @@ void *safety_setter_thread(void *s) {
case cereal::CarParams::SafetyModel::GM:
safety_setting = SAFETY_GM;
break;
case cereal::CarParams::SafetyModel::GM_PASSIVE:
safety_setting = SAFETY_GM_PASSIVE;
break;
case cereal::CarParams::SafetyModel::HONDA_BOSCH:
safety_setting = SAFETY_HONDA_BOSCH;
break;

2
selfdrive/can/libdbc_py.py
View File

@ -82,7 +82,7 @@ void* can_init(int bus, const char* dbc_name,
int can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
const DBC* dbc_lookup(const char* dbc_name);

28
selfdrive/can/parser.cc
View File

@ -330,7 +330,7 @@ class CANParser {
}
}
void update_string(uint64_t sec, std::string data) {
void update_string(std::string data) {
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), data.data(), data.length());
@ -339,10 +339,12 @@ class CANParser {
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto cans = event.getCan();
UpdateCans(sec, cans);
last_sec = event.getLogMonoTime();
UpdateValid(sec);
auto cans = event.getCan();
UpdateCans(last_sec, cans);
UpdateValid(last_sec);
}
int update(uint64_t sec, bool wait) {
@ -381,17 +383,18 @@ class CANParser {
UpdateCans(sec, cans);
}
last_sec = sec;
UpdateValid(sec);
zmq_msg_close(&msg);
return result;
}
std::vector<SignalValue> query(uint64_t sec) {
std::vector<SignalValue> query_latest() {
std::vector<SignalValue> ret;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (sec != 0 && state.seen != sec) continue;
if (last_sec != 0 && state.seen != last_sec) continue;
for (int i=0; i<state.parse_sigs.size(); i++) {
const Signal &sig = state.parse_sigs[i];
@ -408,6 +411,7 @@ class CANParser {
}
bool can_valid = false;
uint64_t last_sec = 0;
private:
const int bus;
@ -451,31 +455,31 @@ int can_update(void* can, uint64_t sec, bool wait) {
return cp->update(sec, wait);
}
void can_update_string(void *can, uint64_t sec, const char* dat, int len) {
void can_update_string(void *can, const char* dat, int len) {
CANParser* cp = (CANParser*)can;
cp->update_string(sec, std::string(dat, len));
cp->update_string(std::string(dat, len));
}
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
CANParser* cp = (CANParser*)can;
if (out_can_valid) {
*out_can_valid = cp->can_valid;
}
const std::vector<SignalValue> values = cp->query(sec);
const std::vector<SignalValue> values = cp->query_latest();
if (out_values) {
std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values);
}
return values.size();
};
void can_query_vector(void* can, uint64_t sec, bool *out_can_valid, std::vector<SignalValue> &values) {
void can_query_latest_vector(void* can, bool *out_can_valid, std::vector<SignalValue> &values) {
CANParser* cp = (CANParser*)can;
if (out_can_valid) {
*out_can_valid = cp->can_valid;
}
values = cp->query(sec);
values = cp->query_latest();
};
}

10
selfdrive/can/parser_pyx.pxd
View File

@ -67,9 +67,9 @@ ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
const char* tcp_addr,
int timeout)
ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
ctypedef void (*can_update_string_func)(void* can, uint64_t sec, const char* dat, int len);
ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid, vector[SignalValue] &values)
ctypedef void (*can_update_string_func)(void* can, const char* dat, int len);
ctypedef size_t (*can_query_latest_func)(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
ctypedef void (*can_query_latest_vector_func)(void* can, bool *out_can_valid, vector[SignalValue] &values)
cdef class CANParser:
cdef:
@ -79,7 +79,7 @@ cdef class CANParser:
can_init_with_vectors_func can_init_with_vectors
can_update_func can_update
can_update_string_func can_update_string
can_query_vector_func can_query_vector
can_query_latest_vector_func can_query_latest_vector
map[string, uint32_t] msg_name_to_address
map[uint32_t, string] address_to_msg_name
vector[SignalValue] can_values
@ -91,4 +91,4 @@ cdef class CANParser:
bool can_valid
int can_invalid_cnt
cdef unordered_set[uint32_t] update_vl(self, uint64_t sec)
cdef unordered_set[uint32_t] update_vl(self)

20
selfdrive/can/parser_pyx.pyx
View File

@ -18,7 +18,7 @@ cdef class CANParser:
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
self.can_update_string = <can_update_string_func>dlsym(libdbc, 'can_update_string')
self.can_query_vector = <can_query_vector_func>dlsym(libdbc, 'can_query_vector')
self.can_query_latest_vector = <can_query_latest_vector_func>dlsym(libdbc, 'can_query_latest_vector')
if checks is None:
checks = []
@ -72,14 +72,14 @@ cdef class CANParser:
message_options_v.push_back(mpo)
self.can = self.can_init_with_vectors(bus, dbc_name, message_options_v, signal_options_v, sendcan, tcp_addr, timeout)
self.update_vl(0)
self.update_vl()
cdef unordered_set[uint32_t] update_vl(self, uint64_t sec):
cdef unordered_set[uint32_t] update_vl(self):
cdef string sig_name
cdef unordered_set[uint32_t] updated_val
cdef bool valid = False
self.can_query_vector(self.can, sec, &valid, self.can_values)
self.can_query_latest_vector(self.can, &valid, self.can_values)
# Update invalid flag
self.can_invalid_cnt += 1
@ -100,20 +100,20 @@ cdef class CANParser:
return updated_val
def update_string(self, uint64_t sec, dat):
self.can_update_string(self.can, sec, dat, len(dat))
return self.update_vl(sec)
def update_string(self, dat):
self.can_update_string(self.can, dat, len(dat))
return self.update_vl()
def update_strings(self, uint64_t sec, strings):
def update_strings(self, strings):
updated_vals = set()
for s in strings:
updated_val = self.update_string(sec, s)
updated_val = self.update_string(s)
updated_vals.update(updated_val)
return updated_vals
def update(self, uint64_t sec, bool wait):
r = (self.can_update(self.can, sec, wait) >= 0)
updated_val = self.update_vl(sec)
updated_val = self.update_vl()
return r, updated_val

4
selfdrive/can/tests/parser_old.py
View File

@ -71,12 +71,12 @@ class CANParser(object):
self.p_can_valid = ffi.new("bool*")
value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
value_count = libdbc.can_query_latest(self.can, self.p_can_valid, 0, ffi.NULL)
self.can_values = ffi.new("SignalValue[%d]" % value_count)
self.update_vl(0)
def update_vl(self, sec):
can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values)
can_values_len = libdbc.can_query_latest(self.can, self.p_can_valid, len(self.can_values), self.can_values)
assert can_values_len <= len(self.can_values)
self.can_invalid_cnt += 1

10
selfdrive/can/tests/test_packer_toyota.py
View File

@ -47,32 +47,34 @@ class TestPackerMethods(unittest.TestCase):
self.assertEqual(m_old, m)
steer = (random.randint(0, 2) % 2 == 0)
chime = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
m_old = create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
m = create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
m_old = create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
m = create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
self.assertEqual(m_old, m)
def test_performance(self):
n1 = sec_since_boot()
recursions = 100000
steer = (random.randint(0, 2) % 2 == 0)
chime = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
for _ in xrange(recursions):
create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_old = n2 - n1
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
n1 = sec_since_boot()
for _ in xrange(recursions):
create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_new = n2 - n1
# print('New API, elapsed time: {} secs'.format(elapsed_new))

5
selfdrive/can/tests/test_parser.py
View File

@ -58,16 +58,15 @@ def run_route(route):
route_ok = True
t = 0
for msg in lr:
if msg.which() == 'can':
t += DT
t = msg.logMonoTime
msg_bytes = msg.as_builder().to_bytes()
can.send(msg_bytes)
_, updated_old = parser_old.update(t, True)
_, updated_new = parser_new.update(t, True)
updated_string = parser_string.update_string(t, msg_bytes)
updated_string = parser_string.update_string(msg_bytes)
if updated_old != updated_new:
route_ok = False

13
selfdrive/car/car_helpers.py
View File

@ -1,4 +1,5 @@
import os
import zmq
from cereal import car
from common.params import Params
from common.vin import get_vin, VIN_UNKNOWN
@ -8,6 +9,16 @@ from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
def get_one_can(logcan):
while True:
try:
can = messaging.recv_one(logcan)
if len(can.can) > 0:
return can
except zmq.error.Again:
continue
def get_startup_alert(car_recognized, controller_available):
alert = 'startup'
if not car_recognized:
@ -86,7 +97,7 @@ def fingerprint(logcan, sendcan, is_panda_black):
done = False
while not done:
a = messaging.recv_one(logcan)
a = get_one_can(logcan)
for can in a.can:
# need to independently try to fingerprint both bus 0 and 1 to work

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

@ -31,7 +31,7 @@ class CarController(object):
self.packer = CANPacker(dbc_name)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert):
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:

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

@ -1,20 +1,22 @@
from cereal import car
from selfdrive.can.parser import CANParser
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
from common.kalman.simple_kalman import KF1D
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(can_gear):
if can_gear == 0x1:
return "park"
return GearShifter.park
elif can_gear == 0x2:
return "reverse"
return GearShifter.reverse
elif can_gear == 0x3:
return "neutral"
return GearShifter.neutral
elif can_gear == 0x4:
return "drive"
return GearShifter.drive
elif can_gear == 0x5:
return "low"
return "unknown"
return GearShifter.low
return GearShifter.unknown
def get_can_parser(CP):

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

@ -1,5 +1,4 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
@ -8,13 +7,14 @@ from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera
from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
GearShifter = car.CarState.GearShifter
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(object):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
@ -115,8 +115,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* 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)
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
@ -160,8 +160,6 @@ class CarInterface(object):
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = self.CS.main_on
ret.cruiseState.speedOffset = 0.
# ignore standstill in hybrid rav4, since pcm allows to restart without
# receiving any special command
ret.cruiseState.standstill = False
# TODO: button presses
@ -169,13 +167,13 @@ class CarInterface(object):
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
@ -193,7 +191,7 @@ class CarInterface(object):
# events
events = []
if not (ret.gearShifter in ('drive', 'low')):
if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -203,7 +201,7 @@ class CarInterface(object):
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
@ -236,8 +234,6 @@ class CarInterface(object):
if (self.CS.frame == -1):
return [] # if we haven't seen a frame 220, then do not update.
self.frame = self.CS.frame
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
return can_sends

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

@ -2,7 +2,6 @@
import os
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2) # c_ messages 706,...,724
RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages
@ -55,8 +54,7 @@ class RadarInterface(object):
self.trigger_msg = LAST_MSG
def update(self, can_strings):
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:

87
selfdrive/car/ford/carcontroller.py 100644
View File

@ -0,0 +1,87 @@
from cereal import car
from selfdrive.car.ford.fordcan import make_can_msg, create_steer_command, create_lkas_ui, \
spam_cancel_button
from selfdrive.can.packer import CANPacker
MAX_STEER_DELTA = 1
TOGGLE_DEBUG = False
class CarController(object):
def __init__(self, dbc_name, enable_camera, vehicle_model):
self.packer = CANPacker(dbc_name)
self.enable_camera = enable_camera
self.enabled_last = False
self.main_on_last = False
self.vehicle_model = vehicle_model
self.generic_toggle_last = 0
self.steer_alert_last = False
self.lkas_action = 0
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
can_sends = []
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
apply_steer = actuators.steer
if self.enable_camera:
if pcm_cancel:
#print "CANCELING!!!!"
can_sends.append(spam_cancel_button(self.packer))
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last)
self.lkas_action &= 0xf
else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.angle_steers, curvature, self.lkas_action))
self.generic_toggle_last = CS.generic_toggle
if (frame % 100) == 0:
can_sends.append(make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False))
#can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False))
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
(self.steer_alert_last != steer_alert):
can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert))
if (frame % 200) == 0:
can_sends.append(make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False))
if (frame % 10) == 0:
can_sends.append(make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False))
can_sends.append(make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False))
can_sends.append(make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False))
can_sends.append(make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False))
can_sends.append(make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False))
can_sends.append(make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False))
can_sends.append(make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False))
can_sends.append(make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False))
can_sends.append(make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False))
can_sends.append(make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False))
can_sends.append(make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False))
can_sends.append(make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False))
static_msgs = range(1653, 1658)
for addr in static_msgs:
cnt = (frame % 10) + 1
can_sends.append(make_can_msg(addr, chr(cnt<<4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False))
self.enabled_last = enabled
self.main_on_last = CS.main_on
self.steer_alert_last = steer_alert
return can_sends

54
selfdrive/car/ford/fordcan.py 100644
View File

@ -0,0 +1,54 @@
from common.numpy_fast import clip
from selfdrive.car.ford.values import MAX_ANGLE
def make_can_msg(addr, dat, alt, cks=False):
return [addr, 0, dat, alt]
def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action):
"""Creates a CAN message for the Ford Steer Command."""
#if enabled and lkas available:
if enabled and lkas_state in [2,3]: #and (frame % 500) >= 3:
action = lkas_action
else:
action = 0xf
angle_cmd = angle_steers/MAX_ANGLE
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE)
values = {
"Lkas_Action": action,
"Lkas_Alert": 0xf, # no alerts
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug?
#"Lane_Curvature": 0, # is it just for debug?
"Steer_Angle_Req": angle_cmd
}
return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values)
def create_lkas_ui(packer, main_on, enabled, steer_alert):
"""Creates a CAN message for the Ford Steer Ui."""
if not main_on:
lines = 0xf
elif enabled:
lines = 0x3
else:
lines = 0x6
values = {
"Set_Me_X80": 0x80,
"Set_Me_X45": 0x45,
"Set_Me_X30": 0x30,
"Lines_Hud": lines,
"Hands_Warning_W_Chime": steer_alert,
}
return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values)
def spam_cancel_button(packer):
values = {
"Cancel": 1
}
return packer.make_can_msg("Steering_Buttons", 0, values)

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

@ -1,5 +1,4 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
@ -48,6 +47,7 @@ class CarInterface(object):
ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.ford
ret.dashcamOnly = True
# pedal
ret.enableCruise = True
@ -108,7 +108,7 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
self.CS.update(self.cp)

4
selfdrive/car/ford/radar_interface.py
View File

@ -3,7 +3,6 @@ import os
import numpy as np
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
RADAR_MSGS = range(0x500, 0x540)
@ -32,8 +31,7 @@ class RadarInterface(object):
self.updated_messages = set()
def update(self, can_strings):
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:

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

@ -1,6 +1,5 @@
#!/usr/bin/env python
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -9,6 +8,7 @@ from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
ButtonType = car.CarState.ButtonEvent.Type
class CanBus(object):
def __init__(self):
@ -62,6 +62,7 @@ class CarInterface(object):
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
ret.safetyModelPassive = car.CarParams.SafetyModel.gmPassive
if candidate == CAR.VOLT:
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
@ -172,7 +173,7 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.pt_cp.update_strings(can_strings)
self.CS.update(self.pt_cp)
@ -225,19 +226,19 @@ class CarInterface(object):
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.type = ButtonType.unknown
if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
be.pressed = True
but = self.CS.cruise_buttons
@ -246,13 +247,13 @@ class CarInterface(object):
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
if not (cruiseEnabled and self.CS.standstill):
be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
be.type = ButtonType.altButton3
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
@ -302,10 +303,10 @@ class CarInterface(object):
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == "cancel" and b.pressed:
if b.type == ButtonType.cancel and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events

4
selfdrive/car/gm/radar_interface.py
View File

@ -6,7 +6,6 @@ from cereal import car
from selfdrive.can.parser import CANParser
from selfdrive.car.gm.interface import CanBus
from selfdrive.car.gm.values import DBC, CAR
from common.realtime import sec_since_boot
RADAR_HEADER_MSG = 1120
SLOT_1_MSG = RADAR_HEADER_MSG + 1
@ -60,8 +59,7 @@ class RadarInterface(object):
time.sleep(0.05) # nothing to do
return car.RadarData.new_message()
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:

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

@ -1,3 +1,4 @@
from cereal import car
from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from selfdrive.can.can_define import CANDefine
@ -5,10 +6,12 @@ from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(gear, vals):
val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
'D': 'drive', 'S': 'sport', 'L': 'low'}
val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low}
try:
return val_to_capnp[vals[gear]]
except KeyError:

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

@ -47,7 +47,7 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
"FCW": fcw << 1,
"AEB_REQ_1": 0,
"AEB_REQ_2": 0,
"AEB": 0,
"AEB_STATUS": 0,
}
bus = get_pt_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)

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

@ -3,7 +3,7 @@ import os
import numpy as np
from cereal import car
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot, DT_CTRL
from common.realtime import DT_CTRL
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
@ -15,6 +15,8 @@ from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
def compute_gb_honda(accel, speed):
creep_brake = 0.0
@ -374,8 +376,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* 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)
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
@ -438,19 +440,19 @@ class CarInterface(object):
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.type = ButtonType.unknown
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
@ -458,18 +460,18 @@ class CarInterface(object):
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = 'accelCruise'
be.type = ButtonType.accelCruise
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
be.type = ButtonType.altButton3
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.type = ButtonType.unknown
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
@ -477,7 +479,7 @@ class CarInterface(object):
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = 'altButton1'
be.type = ButtonType.altButton1
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
@ -493,7 +495,7 @@ class CarInterface(object):
events.append(create_event('steerTempUnavailable', [ET.WARNING]))
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not ret.gearShifter == 'drive':
if not ret.gearShifter == GearShifter.drive:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -503,7 +505,7 @@ class CarInterface(object):
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on or self.CS.cruise_mode:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
@ -538,7 +540,7 @@ class CarInterface(object):
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
self.last_enable_pressed = cur_time
enable_pressed = True

7
selfdrive/car/honda/radar_interface.py
View File

@ -3,7 +3,6 @@ import os
import time
from cereal import car
from selfdrive.can.parser import CANParser
from common.realtime import sec_since_boot
def _create_nidec_can_parser():
dbc_f = 'acura_ilx_2016_nidec.dbc'
@ -38,11 +37,11 @@ class RadarInterface(object):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
time.sleep(0.05)
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05)
return car.RadarData.new_message()
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:

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

@ -96,7 +96,7 @@ FINGERPRINTS = {
57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5
}],
CAR.ODYSSEY_CHN: [{
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
}],
# 2017 Pilot Touring AND 2016 Pilot EX-L w/ Added Comma Pedal Support (512L & 513L)
CAR.PILOT: [{
@ -139,7 +139,7 @@ DBC = {
CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None),
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),

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

@ -1,6 +1,5 @@
#!/usr/bin/env python
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -8,6 +7,8 @@ from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
GearShifter = car.CarState.GearShifter
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(object):
def __init__(self, CP, CarController):
@ -154,8 +155,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* 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)
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
@ -212,13 +213,13 @@ class CarInterface(object):
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
@ -236,7 +237,7 @@ class CarInterface(object):
self.low_speed_alert = False
events = []
if not ret.gearShifter == 'drive':
if not ret.gearShifter == GearShifter.drive:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -246,12 +247,11 @@ class CarInterface(object):
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:

8
selfdrive/car/hyundai/radar_interface.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python
from cereal import car
import os
import time
from cereal import car
class RadarInterface(object):
def __init__(self, CP):
@ -11,6 +11,8 @@ class RadarInterface(object):
def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05) # radard runs on RI updates
return ret

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

@ -1,6 +1,5 @@
#!/usr/bin/env python
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -8,6 +7,7 @@ from selfdrive.car.subaru.values import CAR
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(object):
def __init__(self, CP, CarController):
@ -42,6 +42,7 @@ class CarInterface(object):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.radarOffCan = True
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
@ -96,8 +97,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cam_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.cam_cp)
@ -144,18 +145,18 @@ class CarInterface(object):
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
be = car.CarState.ButtonEvent.new_message()
be.type = 'accelCruise'
be.type = ButtonType.accelCruise
buttonEvents.append(be)

9
selfdrive/car/subaru/radar_interface.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python
from cereal import car
import os
import time
from cereal import car
class RadarInterface(object):
def __init__(self, CP):
@ -10,8 +10,9 @@ class RadarInterface(object):
self.delay = 0.1
def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05) # radard runs on RI updates
return ret

16
selfdrive/car/toyota/carcontroller.py
View File

@ -5,8 +5,8 @@ from selfdrive.car import create_gas_command
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \
create_fcw_command
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSS2_CAR
create_acc_cancel_command, create_fcw_command
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR
from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -209,7 +209,11 @@ class CarController(object):
# 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
if ECU.DSU in self.fake_ecus:
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
@ -236,8 +240,12 @@ class CarController(object):
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, left_line, right_line, left_lane_depart, right_lane_depart))
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))

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

@ -1,14 +1,17 @@
import numpy as np
from cereal import car
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, NO_DSU_CAR
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(gear, vals):
val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
'D': 'drive', 'B': 'brake'}
val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'D': GearShifter.drive, 'B': GearShifter.brake}
try:
return val_to_capnp[vals[gear]]
except KeyError:
@ -37,9 +40,6 @@ def get_can_parser(CP):
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
("CRUISE_STATE", "PCM_CRUISE", 0),
("MAIN_ON", "PCM_CRUISE_2", 0),
("SET_SPEED", "PCM_CRUISE_2", 0),
("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
@ -55,11 +55,20 @@ def get_can_parser(CP):
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
("PCM_CRUISE_2", 33),
("STEER_TORQUE_SENSOR", 50),
("EPS_STATUS", 25),
]
if CP.carFingerprint == CAR.LEXUS_IS:
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
checks.append(("DSU_CRUISE", 5))
else:
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
checks.append(("PCM_CRUISE_2", 33))
if CP.carFingerprint in NO_DSU_CAR:
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
@ -158,7 +167,10 @@ class CarState(object):
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
@ -173,10 +185,14 @@ class CarState(object):
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0

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

@ -1,14 +1,16 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR, TSS2_CAR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
from selfdrive.swaglog import cloudlog
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
class CarInterface(object):
def __init__(self, CP, CarController):
self.CP = CP
@ -207,6 +209,16 @@ class CarInterface(object):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
ret.lateralTuning.pid.kf = 0.00007818594
elif candidate == CAR.LEXUS_IS:
stop_and_go = False
ret.safetyParam = 66
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
ret.lateralTuning.pid.kf = 0.00006
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
@ -236,8 +248,8 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM, candidate) or is_panda_black
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU, candidate) or (is_panda_black and candidate in TSS2_CAR)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
@ -270,11 +282,11 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
# run the cam can update for 10s as we just need to know if the camera is alive
if self.frame < 1000:
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp)
@ -335,13 +347,13 @@ class CarInterface(object):
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
@ -359,7 +371,7 @@ class CarInterface(object):
if self.cp_cam.can_valid:
self.forwarding_camera = True
if not ret.gearShifter == 'drive' and self.CP.enableDsu:
if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -369,7 +381,7 @@ class CarInterface(object):
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on and self.CP.enableDsu:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse' and self.CP.enableDsu:
if ret.gearShifter == GearShifter.reverse and self.CP.enableDsu:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))

4
selfdrive/car/toyota/radar_interface.py
View File

@ -3,7 +3,6 @@ import os
import time
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
def _create_radar_can_parser(car_fingerprint):
@ -58,8 +57,7 @@ class RadarInterface(object):
time.sleep(0.05)
return car.RadarData.new_message()
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:

16
selfdrive/car/toyota/toyotacan.py
View File

@ -95,6 +95,18 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
return packer.make_can_msg("ACC_CONTROL", 0, values)
def create_acc_cancel_command(packer):
values = {
"GAS_RELEASED": 0,
"CRUISE_ACTIVE": 0,
"STANDSTILL_ON": 0,
"ACCEL_NET": 0,
"CRUISE_STATE": 0,
"CANCEL_REQ": 1,
}
return packer.make_can_msg("PCM_CRUISE", 0, values)
def create_fcw_command(packer, fcw):
values = {
"FCW": fcw,
@ -105,7 +117,7 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("ACC_HUD", 0, values)
def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, right_lane_depart):
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart):
values = {
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
@ -117,7 +129,7 @@ def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, ri
"SET_ME_X01": 1,
"SET_ME_X01_2": 1,
"REPEATED_BEEPS": 0,
"TWO_BEEPS": 0,
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
}
return packer.make_can_msg("LKAS_HUD", 0, values)

24
selfdrive/car/toyota/values.py
View File

@ -17,6 +17,7 @@ class CAR:
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
SIENNA = "TOYOTA SIENNA XLE 2018"
LEXUS_IS = "LEXUS IS300 2018"
class ECU:
@ -77,9 +78,10 @@ ECU_FINGERPRINT = {
}
def check_ecu_msgs(fingerprint, ecu):
# return True if fingerprint contains messages normally sent by a given ecu
return ECU_FINGERPRINT[ecu] in fingerprint
def check_ecu_msgs(fingerprint, ecu, car):
# return True if the reference car fingerprint doesn't contain the ecu fingerprint msg or
# fingerprint contains messages normally sent by a given ecu
return any(ECU_FINGERPRINT[ecu] not in finger for finger in FINGERPRINTS[car]) or ECU_FINGERPRINT[ecu] in fingerprint
FINGERPRINTS = {
@ -109,8 +111,9 @@ FINGERPRINTS = {
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
},
# RX450HL
# TODO: get proper fingerprint in stock mode
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# RX540H 2019 with color hud
{
@ -128,8 +131,9 @@ FINGERPRINTS = {
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
#XSE and SE
# TODO: get proper fingerprint in stock mode
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.CAMRYH: [
#SE, LE and LE with Blindspot Monitor
@ -182,6 +186,15 @@ FINGERPRINTS = {
CAR.SIENNA: [{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.LEXUS_IS: [
# IS300 2018
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# IS300H 2017
{
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
}
STEER_THRESHOLD = 100
@ -203,6 +216,7 @@ DBC = {
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
}
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]

8
selfdrive/common/touch.c
View File

@ -69,14 +69,15 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
return -1;
}
switch (event.type) {
switch (event.type) {
case EV_ABS:
if (event.code == ABS_MT_POSITION_X) {
s->last_x = event.value;
} else if (event.code == ABS_MT_POSITION_Y) {
s->last_y = event.value;
} else if (event.code == ABS_MT_TRACKING_ID && event.value != -1) {
up = true;
}
up = true;
break;
default:
break;
@ -98,7 +99,7 @@ int touch_read(TouchState *s, int* out_x, int* out_y) {
return -1;
}
bool up = false;
switch (event.type) {
switch (event.type) {
case EV_ABS:
if (event.code == ABS_MT_POSITION_X) {
s->last_x = event.value;
@ -117,4 +118,3 @@ int touch_read(TouchState *s, int* out_x, int* out_y) {
}
return up;
}

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.6.3-release"
#define COMMA_VERSION "0.6.4-release"

3
selfdrive/config.py
View File

@ -17,7 +17,8 @@ class Conversions:
LB_TO_KG = 0.453592
RADAR_TO_CENTER = 2.7 # RADAR is ~ 2.7m ahead from center of car
RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
class UIParams:
lidar_x, lidar_y, lidar_zoom = 384, 960, 6

84
selfdrive/controls/controlsd.py
View File

@ -6,12 +6,12 @@ from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
from common.profiler import Profiler
from common.params import Params
from common.params import Params, put_nonblocking
import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_alert
from selfdrive.car.car_helpers import get_car, get_startup_alert, get_one_can
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import get_events, \
create_event, \
@ -26,6 +26,7 @@ from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.controls.lib.gps_helpers import is_rhd_region
from selfdrive.locationd.calibration_helpers import Calibration, Filter
ThermalStatus = log.ThermalData.ThermalStatus
@ -50,10 +51,6 @@ 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):
@ -85,6 +82,12 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# GPS coords RHD parsing, once every restart
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", str(int(is_rhd)))
# Handle calibration
if sm.updated['liveCalibration']:
@ -118,7 +121,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
# Driver monitoring
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
@ -283,9 +286,8 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log
def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status,
LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
@ -324,7 +326,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
if not read_only:
# send car controls over can
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = driver_status.awareness < 0.
@ -341,7 +343,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"alertType": AM.alert_type,
"alertSound": AM.audible_alert,
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
"driverMonitoringOn": bool(driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": sm.logMonoTime['plan'],
"pathPlanMonoTime": sm.logMonoTime['pathPlan'],
@ -364,7 +366,6 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(sm['plan'].jerkFactor),
"angleModelBias": 0.,
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
"decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
@ -380,7 +381,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
dat.controlsState.lateralControlState.lqrState = lac_log
elif CP.lateralTuning.which() == 'indi':
dat.controlsState.lateralControlState.indiState = lac_log
controlsstate.send(dat.to_bytes())
pm.send('controlsState', dat)
# carState
cs_send = messaging.new_message()
@ -388,7 +389,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
pm.send('carState', cs_send)
# carEvents - logged every second or on change
events_bytes = events_to_bytes(events)
@ -396,26 +397,26 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
ce_send = messaging.new_message()
ce_send.init('carEvents', len(events))
ce_send.carEvents = events
carevents.send(ce_send.to_bytes())
pm.send('carEvents', ce_send)
# carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message()
cp_send.init('carParams')
cp_send.carParams = CP
carparams.send(cp_send.to_bytes())
pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
pm.send('carControl', cc_send)
return CC, events_bytes
def controlsd_thread(gctx=None):
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
@ -423,39 +424,35 @@ 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)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
logcan = messaging.sub_sock(service_list['can'].port)
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
'gpsLocation'], ignore_alive=['gpsLocation'])
if can_sock is None:
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)
# 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)
print("Waiting for CAN messages...")
get_one_can(can_sock)
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_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
read_only = not car_recognized or not controller_available or CP.dashcamOnly
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
CP.safetyModel = CP.safetyModelPassive
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
@ -478,6 +475,9 @@ def controlsd_thread(gctx=None):
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd = bool(int(is_rhd))
state = State.disabled
soft_disable_timer = 0
@ -550,16 +550,16 @@ def controlsd_thread(gctx=None):
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams,
controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def main(gctx=None):
controlsd_thread(gctx)
def main(sm=None, pm=None, logcan=None):
controlsd_thread(sm, pm, logcan)
if __name__ == "__main__":

20
selfdrive/controls/lib/drive_helpers.py
View File

@ -59,26 +59,6 @@ def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
min_offset = -5. # deg
max_offset = 5. # deg
alpha = 1. / 36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
min_learn_speed = 1.
# learn less at low speed or when turning
slow_factor = 1. / (1. + 0.02 * abs(angle_steers) * v_ego)
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) * slow_factor
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:
angle_model_bias += c_poly[3] * alpha_v
angle_model_bias = clip(angle_model_bias, min_offset, max_offset)
return angle_model_bias
def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition

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

@ -1,14 +1,15 @@
import numpy as np
from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
from common.realtime import DT_CTRL, DT_DMON
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from common.filter_simple import FirstOrderFilter
from common.stat_live import RunningStatFilter
_AWARENESS_TIME = 90. # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
_AWARENESS_PRE_TIME_TILL_TERMINAL = 20. # a first alert is issued 20s before expiration
_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5. # a second alert is issued 5s before start decelerating the car
_DISTRACTED_TIME = 10.
_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
_AWARENESS_TIME = 100. # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status
_AWARENESS_PRE_TIME_TILL_TERMINAL = 25. # a first alert is issued 25s before expiration
_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15. # a second alert is issued 15s before start decelerating the car
_DISTRACTED_TIME = 11.
_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
_FACE_THRESHOLD = 0.4
_EYE_THRESHOLD = 0.4
@ -18,8 +19,15 @@ _METRIC_THRESHOLD = 0.4
_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch
_PITCH_NATURAL_OFFSET = 0.12 # 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
_VARIANCE_FILTER_TS = 20. # 0.008Hz
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
@ -36,7 +44,6 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
# TODO: calibrate based on position
pitch_prnet = angles_desc[0]
yaw_prnet = angles_desc[1]
roll_prnet = angles_desc[2]
@ -52,152 +59,152 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
# no calib for roll
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
return np.array([roll, pitch, yaw])
class _DriverPose():
class DriverPose():
def __init__(self):
self.yaw = 0.
self.pitch = 0.
self.roll = 0.
self.yaw_offset = 0.
self.pitch_offset = 0.
self.pitch_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
self.yaw_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
class _DriverBlink():
class DriverBlink():
def __init__(self):
self.left_blink = 0.
self.right_blink = 0.
def _monitor_hysteresis(variance_level, monitor_valid_prev):
var_thr = 0.63 if monitor_valid_prev else 0.37
return variance_level < var_thr
class DriverStatus():
def __init__(self, monitor_on=False):
self.pose = _DriverPose()
self.blink = _DriverBlink()
self.monitor_on = monitor_on
self.monitor_param_on = monitor_on
self.monitor_valid = True # variance needs to be low
def __init__(self):
self.pose = DriverPose()
self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
self.blink = DriverBlink()
self.awareness = 1.
self.awareness_active = 1.
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON)
self.variance_high = False
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
self.ts_last_check = 0.
self.face_detected = False
self.terminal_alert_cnt = 0
self.step_change = 0.
self._set_timers(self.monitor_on)
self.active_monitoring_mode = True
self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
def _reset_filters(self):
self.driver_distraction_filter.x = 0.
self.variance_filter.x = 0.
self.monitor_valid = True
self.is_rhd_region = False
self.is_rhd_region_checked = False
self._set_timers(active_monitoring=True)
def _set_timers(self, active_monitoring):
if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
if active_monitoring:
self.step_change = DT_CTRL / _DISTRACTED_TIME
else:
self.step_change = 0.
return # no exploit after orange alert
elif self.awareness <= 0.:
return
if active_monitoring:
# when falling back from passive mode to active mode, reset awareness to avoid false alert
if self.step_change == DT_CTRL / _AWARENESS_TIME:
self.awareness = 1.
if not self.active_monitoring_mode:
self.awareness = self.awareness_active
self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.step_change = DT_CTRL / _DISTRACTED_TIME
self.active_monitoring_mode = True
else:
if self.active_monitoring_mode:
self.awareness_active = self.awareness
self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.step_change = DT_CTRL / _AWARENESS_TIME
self.active_monitoring_mode = False
def _is_driver_distracted(self, pose, blink):
# TODO: natural pose calib of each driver
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
if not self.pose_calibrated:
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
else:
pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
pitch_error *= _PITCH_WEIGHT
pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
if pose_metric > _METRIC_THRESHOLD:
return DistractedType.BAD_POSE
return DistractedType.BAD_POSE
elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
return DistractedType.BAD_BLINK
else:
return DistractedType.NOT_DISTRACTED
def get_pose(self, driver_monitoring, params, cal_rpy):
def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged):
# 10 Hz
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.faceOrientation, driver_monitoring.facePosition, cal_rpy)
self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and not self.is_rhd_region
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
# first order filters
self.driver_distraction_filter.update(self.driver_distracted)
monitor_param_on_prev = self.monitor_param_on
# update offseter
# only update when driver is actively driving the car above a certain speed
if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and not op_engaged:
self.pose.pitch_offseter.push_and_update(self.pose.pitch)
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
# 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.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
self.ts_last_check = ts
self.monitor_on = self.monitor_valid and self.monitor_param_on
if monitor_param_on_prev != self.monitor_param_on:
self._reset_filters()
self._set_timers(self.monitor_on and self.face_detected)
self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
self._set_timers(self.face_detected)
def update(self, events, driver_engaged, ctrl_active, standstill):
if driver_engaged:
if (driver_engaged and self.awareness > 0) or not ctrl_active:
# reset only when on disengagement if red reached
self.awareness = 1.
self.awareness_active = 1.
return events
driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
driver_attentive = self.driver_distraction_filter.x < 0.37
awareness_prev = self.awareness
if (driver_engaged and self.awareness > 0) or not ctrl_active:
# always reset if driver is in control (unless we are in red alert state) or op isn't active
self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)
if (driver_attentive and self.face_detected and self.awareness > 0):
# only restore awareness when paying attention and alert is not red
self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
# don't display alert banner when awareness is recovering and has cleared orange
if self.awareness > self.threshold_prompt:
return events
# should always be counting if distracted unless at standstill and reaching orange
if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
if (not self.face_detected or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
self.awareness = max(self.awareness - self.step_change, -0.1)
alert = None
if self.awareness < 0.:
if self.awareness <= 0.:
# terminal red alert: disengagement required
alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
if awareness_prev >= 0.:
alert = 'driverDistracted' if self.active_monitoring_mode else 'driverUnresponsive'
if awareness_prev > 0.:
self.terminal_alert_cnt += 1
elif self.awareness <= self.threshold_prompt:
# prompt orange alert
alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
alert = 'promptDriverDistracted' if self.active_monitoring_mode else 'promptDriverUnresponsive'
elif self.awareness <= self.threshold_pre:
# pre green alert
alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
alert = 'preDriverDistracted' if self.active_monitoring_mode else 'preDriverUnresponsive'
if alert is not None:
events.append(create_event(alert, [ET.WARNING]))
return events
if __name__ == "__main__":
ds = DriverStatus(True)
ds.driver_distraction_filter.x = 0.
ds.driver_distracted = 1
for i in range(10):
ds.update([], False, True, False)
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
ds.update([], True, True, False)
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)

4
selfdrive/controls/lib/fcw.py
View File

@ -45,7 +45,6 @@ class FCWChecker(object):
def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
a_target = mpc_solution_a[1]
self.last_min_a = min(mpc_solution_a)
self.v_lead_max = max(self.v_lead_max, v_lead)
@ -66,9 +65,8 @@ class FCWChecker(object):
future_fcw_allowed = all(c >= 10 for c in self.counters.values())
future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
current_fcw = a_target < -3.0 and active
if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time):
if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
self.last_fcw_time = cur_time
self.last_fcw_a = self.last_min_a
return True

17
selfdrive/controls/lib/gps_helpers.py 100755
View File

@ -0,0 +1,17 @@
_RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \
['IN1', 6.75, 28.10, 68.17, 97.4], \
['IN2', 28.09, 35.99, 72.18, 80.87], \
['IRL', 51.42, 55.38, -10.58, -5.99], \
['JP1', 32.66, 45.52, 137.27, 146.02], \
['JP2', 32.79, 37.60, 131.41, 137.28], \
['JP3', 24.04, 34.78, 122.93, 131.42], \
['NZ', -52.61, -29.24, 166, 178.84], \
['SF', -35.14, -22.13, 16.07, 33.21], \
['UK', 49.9, 60.84, -8.62, 1.77] ]
def is_rhd_region(latitude, longitude):
for region in _RHD_REGION_MAP:
if region[1] <= latitude <= region[2] and \
region[3] <= longitude <= region[4]:
return True
return False

17
selfdrive/controls/lib/lane_planner.py
View File

@ -1,9 +1,19 @@
from common.numpy_fast import interp
import numpy as np
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv
CAMERA_OFFSET = 0.06 # m from center car to camera
def compute_path_pinv(l=50):
deg = 3
x = np.arange(l*1.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])
def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
# This will improve behaviour when lanes suddenly widen
@ -16,7 +26,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
path_from_right_lane = r_poly.copy()
path_from_right_lane[3] += lane_width / 2.0
lr_prob = l_prob + r_prob - l_prob * r_prob
lr_prob = l_prob * r_prob
d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
@ -35,7 +45,6 @@ class LanePlanner(object):
self.l_prob = 0.
self.r_prob = 0.
self.lr_prob = 0.
self._path_pinv = compute_path_pinv()
self.x_points = np.arange(50)
@ -57,8 +66,6 @@ class LanePlanner(object):
self.l_poly[3] += CAMERA_OFFSET
self.r_poly[3] += CAMERA_OFFSET
self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob
# Find current lanewidth
self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])

62
selfdrive/controls/lib/latcontrol_helpers.py
View File

@ -1,62 +0,0 @@
import numpy as np
import math
from common.numpy_fast import interp
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [3., 3.8]
# break points of speed
_LANE_WIDTH_BP = [0., 31.]
def calc_d_lookahead(v_ego, d_poly):
# this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly) > 0:
pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts)) / pts_len
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
# this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = math.radians(angle_steers - angle_offset)
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
return steer_des, curvature
def compute_path_pinv(l=50):
deg = 3
x = np.arange(l*1.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])

17
selfdrive/controls/lib/latcontrol_lqr.py
View File

@ -22,6 +22,7 @@ class LatControlLQR(object):
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.reset()
def reset(self):
@ -31,6 +32,7 @@ class LatControlLQR(object):
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, v_ego)
torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
@ -44,29 +46,32 @@ class LatControlLQR(object):
if v_ego < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if steer_override:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k)
error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
lqr_output = torque_scale * u_lqr / self.scale
self.i_lqr = clip(self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0))):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
# Clip output
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
return self.output_steer, float(self.angle_steers_des), lqr_log

11
selfdrive/controls/lib/long_mpc.py
View File

@ -12,8 +12,7 @@ LOG_MPC = os.environ.get('LOG_MPC', False)
class LongitudinalMpc(object):
def __init__(self, mpc_id, live_longitudinal_mpc):
self.live_longitudinal_mpc = live_longitudinal_mpc
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.setup_mpc()
@ -27,7 +26,7 @@ class LongitudinalMpc(object):
self.last_cloudlog_t = 0.0
def send_mpc_solution(self, qp_iterations, calculation_time):
def send_mpc_solution(self, pm, qp_iterations, calculation_time):
qp_iterations = max(0, qp_iterations)
dat = messaging.new_message()
dat.init('liveLongitudinalMpc')
@ -41,7 +40,7 @@ class LongitudinalMpc(object):
dat.liveLongitudinalMpc.qpIterations = qp_iterations
dat.liveLongitudinalMpc.mpcId = self.mpc_id
dat.liveLongitudinalMpc.calculationTime = calculation_time
self.live_longitudinal_mpc.send(dat.to_bytes())
pm.send('liveLongitudinalMpc', dat)
def setup_mpc(self):
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
@ -58,7 +57,7 @@ class LongitudinalMpc(object):
self.cur_state[0].v_ego = v
self.cur_state[0].a_ego = a
def update(self, CS, lead, v_cruise_setpoint):
def update(self, pm, CS, lead, v_cruise_setpoint):
v_ego = CS.vEgo
# Setup current mpc state
@ -97,7 +96,7 @@ class LongitudinalMpc(object):
duration = int((sec_since_boot() - t) * 1e9)
if LOG_MPC:
self.send_mpc_solution(n_its, duration)
self.send_mpc_solution(pm, n_its, duration)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]

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

@ -4,7 +4,6 @@ import numpy as np
# from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
@ -26,9 +25,6 @@ class PathPlanner(object):
self.last_cloudlog_t = 0
self.plan = messaging.pub_sock(service_list['pathPlan'].port)
self.livempc = messaging.pub_sock(service_list['liveMpc'].port)
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
self.path_offset_i = 0.0
@ -49,13 +45,12 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def update(self, sm, CP, VM):
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset_average = sm['liveParameters'].angleOffsetAverage
angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
angle_offset = sm['liveParameters'].angleOffset
self.LP.update(v_ego, sm['model'])
@ -73,7 +68,7 @@ class PathPlanner(object):
# self.path_offset_i = 0.0
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@ -85,19 +80,19 @@ class PathPlanner(object):
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers - angle_offset_bias) / VM.sR
self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
@ -121,13 +116,13 @@ class PathPlanner(object):
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(self.path_offset_i)
plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
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())
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message()
@ -137,4 +132,4 @@ class PathPlanner(object):
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
self.livempc.send(dat.to_bytes())
pm.send('liveMpc', dat)

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

@ -9,7 +9,6 @@ from cereal import car
from common.realtime import sec_since_boot, DT_PLAN
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.fcw import FCWChecker
@ -71,14 +70,11 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
class Planner(object):
def __init__(self, CP, fcw_enabled):
def __init__(self, CP):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
@ -93,7 +89,6 @@ class Planner(object):
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.path_x = np.arange(192)
self.params = Params()
@ -125,7 +120,7 @@ class Planner(object):
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):
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
@ -199,8 +194,8 @@ class Planner(object):
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
@ -251,10 +246,9 @@ class Planner(object):
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send out fcw
fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)

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

@ -26,6 +26,7 @@ class Track(object):
def __init__(self):
self.ekf = None
self.cnt = 0
self.aLeadTau = _LEAD_ACCEL_TAU
def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
# relative values, copy
@ -123,7 +124,7 @@ class Cluster(object):
@property
def measured(self):
return any([t.measured for t in self.tracks])
return any(t.measured for t in self.tracks)
def get_RadarState(self, model_prob=0.0):
return {

23
selfdrive/controls/plannerd.py
View File

@ -11,27 +11,26 @@ from selfdrive.controls.lib.pathplanner import PathPlanner
import selfdrive.messaging as messaging
def plannerd_thread():
def plannerd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(2)
params = Params()
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
PL = Planner(CP, fcw_enabled)
PL = Planner(CP)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
if sm is None:
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
if pm is None:
pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
sm['liveParameters'].valid = True
sm['liveParameters'].sensorValid = True
@ -42,13 +41,13 @@ def plannerd_thread():
sm.update()
if sm.updated['model']:
PP.update(sm, CP, VM)
PP.update(sm, pm, CP, VM)
if sm.updated['radarState']:
PL.update(sm, CP, VM, PP)
PL.update(sm, pm, CP, VM, PP)
def main(gctx=None):
plannerd_thread()
def main(sm=None, pm=None):
plannerd_thread(sm, pm)
if __name__ == "__main__":

23
selfdrive/controls/radard.py
View File

@ -173,7 +173,7 @@ class RadarD(object):
# fuses camera and radar data for best lead detection
def radard_thread(gctx=None):
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
@ -186,14 +186,17 @@ def radard_thread(gctx=None):
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
can_sock = messaging.sub_sock(service_list['can'].port)
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
if can_sock is None:
can_sock = messaging.sub_sock(service_list['can'].port)
RI = RadarInterface(CP)
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(service_list['radarState'].port)
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(mocked)
@ -212,7 +215,7 @@ def radard_thread(gctx=None):
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining*1000.
radarState.send(dat.to_bytes())
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
@ -226,13 +229,13 @@ def radard_thread(gctx=None):
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
liveTracks.send(dat.to_bytes())
pm.send('liveTracks', dat)
rk.monitor_time()
def main(gctx=None):
radard_thread(gctx)
def main(sm=None, pm=None, can_sock=None):
radard_thread(sm, pm, can_sock)
if __name__ == "__main__":

13
selfdrive/controls/tests/test_following_distance.py
View File

@ -15,9 +15,9 @@ def RW(v_ego, v_l):
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
class FakeSocket(object):
def send(self, data):
assert data
class FakePubMaster(object):
def send(self, s, data):
assert data
def run_following_distance_simulation(v_lead, t_end=200.0):
@ -32,7 +32,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
v_cruise_setpoint = v_lead + 10.
mpc = LongitudinalMpc(1, FakeSocket())
pm = FakePubMaster()
mpc = LongitudinalMpc(1)
first = True
while t < t_end:
@ -61,8 +62,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(CS.carState, lead, v_cruise_setpoint)
mpc.update(CS.carState, lead, v_cruise_setpoint)
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:

193
selfdrive/controls/tests/test_monitoring.py 100644
View File

@ -0,0 +1,193 @@
import unittest
import numpy as np
from common.realtime import DT_CTRL, DT_DMON
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \
_AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \
_AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \
_DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL
from selfdrive.controls.lib.gps_helpers import is_rhd_region
_TEST_TIMESPAN = 120 # seconds
_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1
_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
_INVISIBLE_SECONDS_TO_RED = _AWARENESS_TIME + 1
class fake_DM_msg():
def __init__(self, is_face_detected, is_distracted=False):
self.faceOrientation = [0.,0.,0.]
self.facePosition = [0.,0.]
self.faceProb = 1. * is_face_detected
self.leftEyeProb = 1.
self.rightEyeProb = 1.
self.leftBlinkProb = 1. * is_distracted
self.rightBlinkProb = 1. * is_distracted
# driver state from neural net, 10Hz
msg_NO_FACE_DETECTED = fake_DM_msg(is_face_detected=False)
msg_ATTENTIVE = fake_DM_msg(is_face_detected=True)
msg_DISTRACTED = fake_DM_msg(is_face_detected=True, is_distracted=True)
# driver interaction with car
car_interaction_DETECTED = True
car_interaction_NOT_DETECTED = False
# openpilot state
openpilot_ENGAGED = True
openpilot_NOT_ENGAGED = False
# car standstill state
car_STANDSTILL = True
car_NOT_STANDSTILL = False
# some common state vectors
always_no_face = [msg_NO_FACE_DETECTED] * int(_TEST_TIMESPAN/DT_DMON)
always_attentive = [msg_ATTENTIVE] * int(_TEST_TIMESPAN/DT_DMON)
always_distracted = [msg_DISTRACTED] * int(_TEST_TIMESPAN/DT_DMON)
always_true = [True] * int(_TEST_TIMESPAN/DT_DMON)
always_false = [False] * int(_TEST_TIMESPAN/DT_DMON)
def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status):
# inputs are all 10Hz
DS = DriverStatus()
events_from_DM = []
for idx in range(len(driver_state_msgs)):
DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx])
# cal_rpy and car_speed don't matter here
# to match frequency of controlsd (100Hz)
for _ in range(int(DT_DMON/DT_CTRL)):
event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx])
events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests
assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong'
return events_from_DM
class TestMonitoring(unittest.TestCase):
# -1. rhd parser sanity check
def test_rhd_parser(self):
cities = [[32.7, -117.1, 0],\
[51.5, 0.129, 1],\
[35.7, 139.7, 1],\
[-37.8, 144.9, 1],\
[32.1, 41.74, 0],\
[55.7, 12.69, 0]]
result = []
for city in cities:
result.append(int(is_rhd_region(city[0],city[1])))
self.assertEqual(result,[int(city[2]) for city in cities])
# 0. op engaged, driver is doing fine all the time
def test_fully_aware_driver(self):
events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)
self.assertTrue(np.sum([len(event) for event in events_output])==0)
# 1. op engaged, driver is distracted and does nothing
def test_fully_distracted_driver(self):
events_output = run_DState_seq(always_distracted, always_false, always_true, always_false)
self.assertTrue(len(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+\
((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\
((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_TIME+\
((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)][0].name, 'driverDistracted')
# 2. op engaged, no face detected the whole time, no action
def test_fully_invisible_driver(self):
events_output = run_DState_seq(always_no_face, always_false, always_true, always_false)
self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL+\
((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverUnresponsive')
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\
((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertEqual(events_output[int((_AWARENESS_TIME+\
((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)][0].name, 'driverUnresponsive')
# 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
# - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch
def test_normal_driver(self):
ds_vector = [msg_DISTRACTED] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
[msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
[msg_DISTRACTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON))
interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
[car_interaction_DETECTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0)
# 4. op engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
# driver dodges, and then touches wheel to no avail, disengages and reengages
# - orange/red alert should remain after disappearance, and only disengaging clears red
def test_biggest_comma_fan(self):
_invisible_time = 2 # seconds
ds_vector = always_distracted[:]
interaction_vector = always_false[:]
op_vector = always_true[:]
ds_vector[int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((_DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
ds_vector[int((_DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
interaction_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON)
op_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON)
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)][0].name, 'driverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)][0].name, 'driverDistracted')
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0)
# 5. op engaged, invisible driver, down to orange, driver appears; then down to orange again, driver touches wheel
# - both actions should clear the alert
def test_sometimes_transparent_commuter(self):
_visible_time = 2 # seconds
ds_vector = always_no_face[:]*2
interaction_vector = always_false[:]*2
ds_vector[int(_INVISIBLE_SECONDS_TO_ORANGE/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+_visible_time)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*1.5+_visible_time)/DT_DMON)])==0)
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+_visible_time-0.5)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+_visible_time+0.1)/DT_DMON)])==0)
# 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
# - only disengage will clear the alert
def test_last_second_responder(self):
_visible_time = 2 # seconds
ds_vector = always_no_face[:]
interaction_vector = always_false[:]
op_vector = always_true[:]
ds_vector[int(_INVISIBLE_SECONDS_TO_RED/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
op_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)][0].name, 'driverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)][0].name, 'driverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)][0].name, 'driverUnresponsive')
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0)
# 7. op not engaged, always distracted driver
# - dm should stay quiet when not engaged
def test_pure_dashcam_user(self):
events_output = run_DState_seq(always_distracted, always_false, always_false, always_false)
self.assertTrue(np.sum([len(event) for event in events_output])==0)
# 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving
# - should only reach green when stopped, but continues counting down on launch
def test_long_traffic_light_victim(self):
_redlight_time = 60 # seconds
standstill_vector = always_true[:]
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON)
events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector)
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)][0].name, 'preDriverDistracted')
self.assertEqual(events_output[int((_redlight_time-0.1)/DT_DMON)][0].name, 'preDriverDistracted')
self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted')
if __name__ == "__main__":
print 'MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS
unittest.main()

27
selfdrive/locationd/calibrationd.py
View File

@ -6,7 +6,6 @@ import numpy as np
import selfdrive.messaging as messaging
from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from common.params import Params
from common.transformations.model import model_height
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
@ -64,7 +63,7 @@ class Calibrator(object):
self.just_calibrated = True
def handle_cam_odom(self, log):
trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
trans, rot = log.trans, log.rot
if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
@ -81,7 +80,7 @@ class Calibrator(object):
else:
return None
def send_data(self, livecalibration):
def send_data(self, pm):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
@ -92,27 +91,31 @@ class Calibrator(object):
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
livecalibration.send(cal_send.to_bytes())
pm.send('liveCalibration', cal_send)
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
cameraodometry = messaging.sub_sock(service_list['cameraOdometry'].port, addr=addr, conflate=True)
livecalibration = messaging.pub_sock(service_list['liveCalibration'].port)
def calibrationd_thread(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveCalibration'])
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
co = messaging.recv_one(cameraodometry)
sm.update()
new_vp = calibrator.handle_cam_odom(co)
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
calibrator.send_data(livecalibration)
calibrator.send_data(pm)
def main(gctx=None, addr="127.0.0.1"):
calibrationd_thread(gctx, addr)
def main(sm=None, pm=None):
calibrationd_thread(sm, pm)
if __name__ == "__main__":

72
selfdrive/locationd/locationd_yawrate.cc
View File

@ -10,7 +10,7 @@
#include "locationd_yawrate.h"
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
double dt = current_time - prev_update_time;
if (dt < 0) {
@ -19,36 +19,34 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double
prev_update_time = current_time;
}
// x = A * x;
// P = A * P * A.transpose() + dt * Q;
// Simplify because A is unity
P = P + dt * Q;
x = A * x;
P = A * P * A.transpose() + dt * Q;
double y = meas - C * x;
double S = R + C * P * C.transpose();
Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
x = x + K * y;
P = (I - K * C) * P;
}
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
if (sensor_event.getType() == 4) {
if (sensor_event.getSensor() == 5) {
sensor_data_time = current_time;
double meas = -sensor_event.getGyro().getV()[0];
double meas = -sensor_event.getGyroUncalibrated().getV()[0];
update_state(C_gyro, R_gyro, current_time, meas);
}
}
}
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
double R = 100.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]);
camera_odometry_time = current_time;
}
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
@ -59,17 +57,34 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
Localizer::Localizer() {
A << 1, 0, 0, 1;
I << 1, 0, 0, 1;
// States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
A <<
1, 1, 0, 0,
0, 1, 0, 0,
0, 0, 1, 1,
0, 0, 0, 1;
I <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
Q <<
0, 0, 0, 0,
0, pow(0.1, 2.0), 0, 0,
0, 0, 0, 0,
0, 0, pow(0.0005 / 100.0, 2.0), 0;
P <<
pow(100.0, 2.0), 0, 0, 0,
0, pow(100.0, 2.0), 0, 0,
0, 0, pow(100.0, 2.0), 0,
0, 0, 0, pow(100.0, 2.0);
C_posenet << 1, 0;
C_gyro << 1, 1;
x << 0, 0;
C_posenet << 1, 0, 0, 0;
C_gyro << 1, 0, 1, 0;
x << 0, 0, 0, 0;
R_gyro = pow(0.05, 2.0);
R_gyro = pow(0.25, 2.0);
}
void Localizer::handle_log(cereal::Event::Reader event) {
@ -118,20 +133,31 @@ extern "C" {
}
double localizer_get_bias(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->x[1];
return loc->x[2];
}
void localizer_set_yaw(void * localizer, double yaw) {
double * localizer_get_state(void * localizer) {
Localizer * loc = (Localizer*) localizer;
loc->x[0] = yaw;
return loc->x.data();
}
void localizer_set_bias(void * localizer, double bias) {
void localizer_set_state(void * localizer, double * state) {
Localizer * loc = (Localizer*) localizer;
loc->x[1] = bias;
memcpy(loc->x.data(), state, 4 * sizeof(double));
}
double localizer_get_t(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->prev_update_time;
}
double * localizer_get_P(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->P.data();
}
void localizer_set_P(void * localizer, double * P) {
Localizer * loc = (Localizer*) localizer;
memcpy(loc->P.data(), P, 16 * sizeof(double));
}
}

17
selfdrive/locationd/locationd_yawrate.h
View File

@ -7,28 +7,29 @@
class Localizer
{
Eigen::Matrix2d A;
Eigen::Matrix2d I;
Eigen::Matrix2d Q;
Eigen::Matrix2d P;
Eigen::Matrix<double, 1, 2> C_posenet;
Eigen::Matrix<double, 1, 2> C_gyro;
Eigen::Matrix4d A;
Eigen::Matrix4d I;
Eigen::Matrix4d Q;
Eigen::Matrix<double, 1, 4> C_posenet;
Eigen::Matrix<double, 1, 4> C_gyro;
double R_gyro;
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
public:
Eigen::Vector2d x;
Eigen::Vector4d x;
Eigen::Matrix4d P;
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;
double camera_odometry_time = -1;
Localizer();
void handle_log(cereal::Event::Reader event);

2
selfdrive/locationd/params_learner.cc
View File

@ -45,7 +45,7 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
}
bool ParamsLearner::update(double psi, double u, double sa) {
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 15.)) {
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_ao = ao - alpha1 * ao_diff;

39
selfdrive/locationd/paramsd.cc
View File

@ -137,33 +137,32 @@ int main(int argc, char *argv[]) {
// TODO: Fix in replay
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
// Send parameters at 10 Hz
if (save_counter % 10 == 0){
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto live_params = event.initLiveParameters();
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[1]);
live_params.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
live_params.setPosenetSpeed(localizer.posenet_speed);
live_params.setPosenetValid(posenet_invalid_count < 4);
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto live_params = event.initLiveParameters();
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[2]);
live_params.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
live_params.setPosenetSpeed(localizer.posenet_speed);
live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
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
// TODO: Save in seperate thread
if (save_counter % 6000 == 0) {
json11::Json json = json11::Json::object {
{"carVin", vin},

20
selfdrive/loggerd/uploader.py
View File

@ -93,9 +93,9 @@ def is_on_hotspot():
return False
class Uploader(object):
def __init__(self, dongle_id, private_key, root):
def __init__(self, dongle_id, root):
self.dongle_id = dongle_id
self.api = Api(dongle_id, private_key)
self.api = Api(dongle_id)
self.root = root
self.upload_thread = None
@ -146,14 +146,11 @@ class Uploader(object):
return (key, fn, 0)
if with_raw:
# then upload log files
# then upload the full log files, rear and front camera files
for name, key, fn in self.gen_upload_files():
if name == "rlog.bz2":
return (key, fn, 1)
# then upload rear and front camera files
for name, key, fn in self.gen_upload_files():
if name == "fcamera.hevc":
elif name == "fcamera.hevc":
return (key, fn, 2)
elif name == "dcamera.hevc":
return (key, fn, 3)
@ -241,13 +238,12 @@ def uploader_fn(exit_event):
params = Params()
dongle_id = params.get("DongleId")
private_key = open("/persist/comma/id_rsa").read()
if dongle_id is None or private_key is None:
cloudlog.info("uploader missing dongle_id or private_key")
raise Exception("uploader can't start without dongle id and private key")
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
uploader = Uploader(dongle_id, private_key, ROOT)
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while True:

12
selfdrive/manager.py
View File

@ -47,7 +47,7 @@ if __name__ == "__main__":
if is_neos:
version = int(open("/VERSION").read()) if os.path.isfile("/VERSION") else 0
revision = int(open("/REVISION").read()) if version >= 10 else 0 # Revision only present in NEOS 10 and up
neos_update_required = version < 10 or (version == 10 and revision != 4)
neos_update_required = version < 10 or (version == 10 and revision < 4)
if neos_update_required:
# update continue.sh before updating NEOS
@ -365,6 +365,7 @@ def manager_thread():
# start frame
pm_apply_packages('enable')
system("LD_LIBRARY_PATH= appops set ai.comma.plus.offroad SU allow")
system("am start -n ai.comma.plus.frame/.MainActivity")
if os.getenv("NOBOARD") is None:
@ -498,6 +499,11 @@ def manager_update():
update_ssh()
update_apks()
uninstall = [app for app in get_installed_apks().keys() if app in ("com.spotify.music", "com.waze")]
for app in uninstall:
cloudlog.info("uninstalling %s" % app)
os.system("pm uninstall % s" % app)
def manager_prepare():
# build cereal first
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
@ -555,16 +561,12 @@ def main():
params.put("IsMetric", "0")
if params.get("RecordFront") is None:
params.put("RecordFront", "0")
if params.get("IsFcwEnabled") is None:
params.put("IsFcwEnabled", "1")
if params.get("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0")
if params.get("IsUploadRawEnabled") is None:
params.put("IsUploadRawEnabled", "1")
if params.get("IsUploadVideoOverCellularEnabled") is None:
params.put("IsUploadVideoOverCellularEnabled", "1")
if params.get("IsDriverMonitoringEnabled") is None:
params.put("IsDriverMonitoringEnabled", "1")
if params.get("IsGeofenceEnabled") is None:
params.put("IsGeofenceEnabled", "-1")
if params.get("SpeedLimitOffset") is None:

25
selfdrive/messaging.py
View File

@ -84,8 +84,8 @@ def recv_one_or_none(sock):
return None
class SubMaster():
def __init__(self, services, addr="127.0.0.1"):
class SubMaster(object):
def __init__(self, services, ignore_alive=None, addr="127.0.0.1"):
self.poller = zmq.Poller()
self.frame = -1
self.updated = {s : False for s in services}
@ -97,6 +97,12 @@ class SubMaster():
self.data = {}
self.logMonoTime = {}
self.valid = {}
if ignore_alive is not None:
self.ignore_alive = ignore_alive
else:
self.ignore_alive = []
for s in services:
# TODO: get address automatically from service_list
if addr is not None:
@ -141,7 +147,7 @@ class SubMaster():
def all_alive(self, service_list=None):
if service_list is None: # check all
service_list = self.alive.keys()
return all(self.alive[s] for s in service_list)
return all(self.alive[s] for s in service_list if s not in self.ignore_alive)
def all_valid(self, service_list=None):
if service_list is None: # check all
@ -152,3 +158,16 @@ class SubMaster():
if service_list is None: # check all
service_list = self.alive.keys()
return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list)
class PubMaster():
def __init__(self, services):
self.sock = {}
for s in services:
self.sock[s] = pub_sock(service_list[s].port)
def send(self, s, dat):
# accept either bytes or capnp builder
if not isinstance(dat, str):
dat = dat.to_bytes()
self.sock[s].send(dat)

2
selfdrive/service_list.yaml
View File

@ -16,7 +16,7 @@ thermal: [8005, true, 2., 1]
can: [8006, true, 100.]
controlsState: [8007, true, 100., 100]
#liveEvent: [8008, true, 0.]
model: [8009, true, 20.]
model: [8009, true, 20., 5]
features: [8010, true, 0.]
health: [8011, true, 2., 1]
radarState: [8012, true, 20.]

90
selfdrive/test/test_car_models_openpilot.py → selfdrive/test/test_car_models.py
View File

@ -14,6 +14,7 @@ from selfdrive.services import service_list
import selfdrive.messaging as messaging
from common.params import Params
from common.basedir import BASEDIR
from common.fingerprints import all_known_cars
from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.toyota.values import CAR as TOYOTA
from selfdrive.car.gm.values import CAR as GM
@ -54,6 +55,7 @@ def get_route_logs(route_name):
with open(log_path, "w") as f:
f.write(r.content)
else:
print "failed to download test log %s" % route_name
sys.exit(-1)
routes = {
@ -140,6 +142,10 @@ routes = {
'carFingerprint': HONDA.CRV_HYBRID,
'enableCamera': True,
},
"99e3eaed7396619e|2019-08-13--15-07-03": {
'carFingerprint': HONDA.FIT,
'enableCamera': True,
},
"2ac95059f70d76eb|2018-02-05--15-03-29": {
'carFingerprint': HONDA.ACURA_ILX,
'enableCamera': True,
@ -351,6 +357,21 @@ routes = {
'enableCamera': False,
'enableDsu': False,
},
"2e07163a1ba9a780|2019-08-25--13-15-13": {
'carFingerprint': TOYOTA.LEXUS_IS,
'enableCamera': True,
'enableDsu': False,
},
"2e07163a1ba9a780|2019-08-29--09-35-42": {
'carFingerprint': TOYOTA.LEXUS_IS,
'enableCamera': False,
'enableDsu': False,
},
"1dd19ceed0ee2b48|2018-12-22--17-36-49": {
'carFingerprint': TOYOTA.LEXUS_IS, # 300 hybrid
'enableCamera': True,
'enableDsu': False,
},
"791340bc01ed993d|2019-03-10--16-28-08": {
'carFingerprint': SUBARU.IMPREZA,
'enableCamera': True,
@ -370,33 +391,59 @@ passive_routes = [
#"bfa17080b080f3ec|2018-06-28--23-27-47",
]
public_routes = [
"f1b4c567731f4a1b|2018-06-06--14-43-46",
"f1b4c567731f4a1b|2018-04-18--11-29-37",
"f1b4c567731f4a1b|2018-04-18--11-29-37",
"7ed9cdf8d0c5f43e|2018-05-17--09-31-36",
"38bfd238edecbcd7|2018-08-22--09-45-44",
"38bfd238edecbcd7|2018-08-29--22-02-15",
"b0f5a01cf604185c|2018-01-26--00-54-32",
"b0f5a01cf604185c|2018-01-26--10-54-38",
"b0f5a01cf604185c|2018-01-26--10-59-31",
"56fb1c86a9a86404|2017-11-10--10-18-43",
"b0f5a01cf604185c|2017-12-18--20-32-32",
"b0c9d2329ad1606b|2019-04-02--13-24-43",
"791340bc01ed993d|2019-03-10--16-28-08",
# TODO: replace all these with public routes
# TODO: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
non_public_routes = [
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018
"b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018
"5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018
"265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018
"53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
"c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING
"1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
]
if __name__ == "__main__":
# TODO: add routes for untested cars and fail test if we have an untested car
tested_cars = [keys["carFingerprint"] for route, keys in routes.items()]
for car_model in all_known_cars():
if car_model not in tested_cars:
print "***** WARNING: %s not tested *****" % car_model
results = {}
for route, checks in routes.items():
if route not in public_routes:
print "route not public", route
if route not in non_public_routes:
get_route_logs(route)
elif "UNLOGGER_PATH" not in os.environ:
continue
get_route_logs(route)
for _ in range(3):
shutil.rmtree('/data/params')
manager.gctx = {}
@ -420,7 +467,10 @@ if __name__ == "__main__":
# Start unlogger
print "Start unlogger"
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
if route in non_public_routes:
unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), '%s' % route, '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
else:
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
print "Check sockets"

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

@ -332,7 +332,6 @@ class LongitudinalControl(unittest.TestCase):
shutil.rmtree('/data/params', ignore_errors=True)
params = Params()
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
params.put("IsFcwEnabled", "1")
manager.gctx = {}
manager.prepare_managed_process('radard')

9
selfdrive/test/tests/process_replay/README.md
View File

@ -13,3 +13,12 @@ Currently the following processes are tested:
* plannerd
* calibrationd
## Forks
openpilot forks can use this test with their own reference logs
To generate new logs:
`./update-refs.py --no-upload`
Then, check in the new logs using git-lfs. Make sure to also include the updated `ref_commit` file.

26
selfdrive/test/tests/process_replay/compare_logs.py
View File

@ -11,7 +11,6 @@ else:
from tools.lib.logreader import LogReader
def save_log(dest, log_msgs):
dat = ""
for msg in log_msgs:
@ -21,25 +20,42 @@ def save_log(dest, log_msgs):
with open(dest, "w") as f:
f.write(dat)
def remove_ignored_fields(msg, ignore):
msg = msg.as_builder()
for key, val in ignore:
attr = msg
keys = key.split(".")
if msg.which() not in key and len(keys) > 1:
continue
for k in keys[:-1]:
try:
attr = getattr(msg, k)
except:
break
else:
setattr(attr, keys[-1], val)
return msg.as_reader()
def compare_logs(log1, log2, ignore=[]):
assert len(log1) == len(log2), "logs are not same length"
ignore_fields = [k for k, v in ignore]
diff = []
for msg1, msg2 in tqdm(zip(log1, log2)):
assert msg1.which() == msg2.which(), "msgs not aligned between logs"
msg1_bytes = msg1.as_builder().to_bytes()
msg2_bytes = msg2.as_builder().to_bytes()
msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes()
msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes()
if msg1_bytes != msg2_bytes:
msg1_dict = msg1.to_dict(verbose=True)
msg2_dict = msg2.to_dict(verbose=True)
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore, tolerance=0)
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields, tolerance=0)
diff.extend(dd)
return diff
if __name__ == "__main__":
log1 = list(LogReader(sys.argv[1]))
log2 = list(LogReader(sys.argv[2]))
compare_logs(log1, log2, sys.argv[3:])

271
selfdrive/test/tests/process_replay/process_replay.py
View File

@ -1,14 +1,15 @@
#!/usr/bin/env python2
import gc
import os
import time
import threading
import importlib
import zmq
if "CI" in os.environ:
tqdm = lambda x: x
else:
from tqdm import tqdm
from cereal import car
from cereal import car, log
from selfdrive.car.car_helpers import get_car
import selfdrive.manager as manager
import selfdrive.messaging as messaging
@ -18,61 +19,154 @@ from collections import namedtuple
ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback'])
def fingerprint(msgs, pub_socks, sub_socks):
class FakeSocket:
def __init__(self, wait=True):
self.data = []
self.wait = wait
self.recv_called = threading.Event()
self.recv_ready = threading.Event()
def recv(self, block=None):
if block == zmq.NOBLOCK:
raise zmq.error.Again
if self.wait:
self.recv_called.set()
self.recv_ready.wait()
self.recv_ready.clear()
return self.data.pop()
def send(self, data):
if self.wait:
self.recv_called.wait()
self.recv_called.clear()
self.data.append(data)
if self.wait:
self.recv_ready.set()
def wait_for_recv(self):
self.recv_called.wait()
class DumbSocket:
def __init__(self, s=None):
if s is not None:
dat = messaging.new_message()
dat.init(s)
self.data = dat.to_bytes()
def recv(self, block=None):
return self.data
def send(self, dat):
pass
class FakeSubMaster(messaging.SubMaster):
def __init__(self, services):
super(FakeSubMaster, self).__init__(services, addr=None)
self.sock = {s: DumbSocket(s) for s in services}
self.update_called = threading.Event()
self.update_ready = threading.Event()
self.wait_on_getitem = False
def __getitem__(self, s):
# hack to know when fingerprinting is done
if self.wait_on_getitem:
self.update_called.set()
self.update_ready.wait()
self.update_ready.clear()
return self.data[s]
def update(self, timeout=-1):
self.update_called.set()
self.update_ready.wait()
self.update_ready.clear()
def update_msgs(self, cur_time, msgs):
self.update_called.wait()
self.update_called.clear()
super(FakeSubMaster, self).update_msgs(cur_time, msgs)
self.update_ready.set()
def wait_for_update(self):
self.update_called.wait()
class FakePubMaster(messaging.PubMaster):
def __init__(self, services):
self.data = {}
self.sock = {}
self.last_updated = None
for s in services:
data = messaging.new_message()
try:
data.init(s)
except:
data.init(s, 0)
self.data[s] = data.as_reader()
self.sock[s] = DumbSocket()
self.send_called = threading.Event()
self.get_called = threading.Event()
def send(self, s, dat):
self.last_updated = s
if isinstance(dat, str):
self.data[s] = log.Event.from_bytes(dat)
else:
self.data[s] = dat.as_reader()
self.send_called.set()
self.get_called.wait()
self.get_called.clear()
def wait_for_msg(self):
self.send_called.wait()
self.send_called.clear()
dat = self.data[self.last_updated]
self.get_called.set()
return dat
def fingerprint(msgs, fsm, can_sock):
print "start fingerprinting"
manager.prepare_managed_process("logmessaged")
manager.start_managed_process("logmessaged")
can = pub_socks["can"]
logMessage = messaging.sub_sock(service_list["logMessage"].port)
time.sleep(1)
messaging.drain_sock(logMessage)
# controlsd waits for a health packet before fingerprinting
msg = messaging.new_message()
msg.init("health")
pub_socks["health"].send(msg.to_bytes())
fsm.wait_on_getitem = True
# populate fake socket with data for fingerprinting
canmsgs = filter(lambda msg: msg.which() == "can", msgs)
for msg in canmsgs[:200]:
can.send(msg.as_builder().to_bytes())
can_sock.recv_called.wait()
can_sock.recv_called.clear()
can_sock.data = [msg.as_builder().to_bytes() for msg in canmsgs[:300]]
can_sock.recv_ready.set()
can_sock.wait = False
time.sleep(0.005)
log = messaging.recv_one_or_none(logMessage)
if log is not None and "fingerprinted" in log.logMessage:
break
manager.kill_managed_process("logmessaged")
# we know fingerprinting is done when controlsd sets sm['pathPlan'].sensorValid
fsm.update_called.wait()
fsm.update_called.clear()
fsm.wait_on_getitem = False
can_sock.wait = True
can_sock.data = []
fsm.update_ready.set()
print "finished fingerprinting"
def get_car_params(msgs, pub_socks, sub_socks):
sendcan = pub_socks.get("sendcan", None)
if sendcan is None:
sendcan = messaging.pub_sock(service_list["sendcan"].port)
logcan = sub_socks.get("can", None)
if logcan is None:
logcan = messaging.sub_sock(service_list["can"].port)
can = pub_socks.get("can", None)
if can is None:
can = messaging.pub_sock(service_list["can"].port)
def get_car_params(msgs, fsm, can_sock):
can = FakeSocket(wait=False)
sendcan = FakeSocket(wait=False)
time.sleep(0.5)
canmsgs = filter(lambda msg: msg.which() == "can", msgs)
for m in canmsgs[:200]:
canmsgs = filter(lambda msg: msg.which() == 'can', msgs)
for m in canmsgs[:300]:
can.send(m.as_builder().to_bytes())
_, CP = get_car(logcan, sendcan)
_, CP = get_car(can, sendcan)
Params().put("CarParams", CP.to_bytes())
time.sleep(0.5)
messaging.drain_sock(logcan)
def radar_rcv_callback(msg, CP):
if msg.which() != "can":
return []
elif CP.radarOffCan:
return ["radarState", "liveTracks"]
# hyundai and subaru don't have radar
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x475],
"hyundai": [], "chrysler": [0x2d4], "subaru": []}.get(CP.carName, None)
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
"chrysler": [0x2d4]}.get(CP.carName, None)
if radar_msgs is None:
raise NotImplementedError
@ -80,24 +174,16 @@ def radar_rcv_callback(msg, CP):
for m in msg.can:
if m.src == 1 and m.address in radar_msgs:
return ["radarState", "liveTracks"]
return []
def plannerd_rcv_callback(msg, CP):
if msg.which() in ["model", "radarState"]:
time.sleep(0.005)
else:
time.sleep(0.002)
return {"model": ["pathPlan"], "radarState": ["plan"]}.get(msg.which(), [])
CONFIGS = [
ProcessConfig(
proc_name="controlsd",
pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan"],
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": []
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [],
},
ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"],
ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)],
init_callback=fingerprint,
should_recv_callback=None,
),
@ -107,7 +193,7 @@ CONFIGS = [
"can": ["radarState", "liveTracks"],
"liveParameters": [], "controlsState": [], "model": [],
},
ignore=["logMonoTime", "radarState.cumLagMs"],
ignore=[("logMonoTime", 0), ("valid", True), ("radarState.cumLagMs", 0)],
init_callback=get_car_params,
should_recv_callback=radar_rcv_callback,
),
@ -117,69 +203,82 @@ CONFIGS = [
"model": ["pathPlan"], "radarState": ["plan"],
"carState": [], "controlsState": [], "liveParameters": [],
},
ignore=["logMonoTime", "valid", "plan.processingDelay"],
ignore=[("logMonoTime", 0), ("valid", True), ("plan.processingDelay", 0)],
init_callback=get_car_params,
should_recv_callback=plannerd_rcv_callback,
should_recv_callback=None,
),
ProcessConfig(
proc_name="calibrationd",
pub_sub={
"cameraOdometry": ["liveCalibration"]
},
ignore=["logMonoTime"],
ignore=[("logMonoTime", 0), ("valid", True)],
init_callback=get_car_params,
should_recv_callback=None,
),
]
def replay_process(cfg, lr):
gc.disable() # gc can occasionally cause canparser to timeout
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can']
pub_socks, sub_socks = {}, {}
for pub, sub in cfg.pub_sub.iteritems():
pub_socks[pub] = messaging.pub_sock(service_list[pub].port)
for s in sub:
sub_socks[s] = messaging.sub_sock(service_list[s].port)
fsm = FakeSubMaster(pub_sockets)
fpm = FakePubMaster(sub_sockets)
args = (fsm, fpm)
if 'can' in cfg.pub_sub.keys():
can_sock = FakeSocket()
args = (fsm, fpm, can_sock)
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = filter(lambda msg: msg.which() in pub_socks.keys(), all_msgs)
pub_msgs = filter(lambda msg: msg.which() in cfg.pub_sub.keys(), all_msgs)
params = Params()
params.manager_start()
params.put("Passive", "0")
manager.gctx = {}
os.environ['NO_RADAR_SLEEP'] = "1"
manager.prepare_managed_process(cfg.proc_name)
manager.start_managed_process(cfg.proc_name)
time.sleep(3) # Wait for started process to be ready
mod = importlib.import_module(manager.managed_processes[cfg.proc_name])
thread = threading.Thread(target=mod.main, args=args)
thread.daemon = True
thread.start()
if cfg.init_callback is not None:
cfg.init_callback(all_msgs, pub_socks, sub_socks)
if 'can' not in cfg.pub_sub.keys():
can_sock = None
cfg.init_callback(all_msgs, fsm, can_sock)
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
log_msgs = []
# wait for started process to be ready
if 'can' in cfg.pub_sub.keys():
can_sock.wait_for_recv()
else:
fsm.wait_for_update()
log_msgs, msg_queue = [], []
for msg in tqdm(pub_msgs):
if cfg.should_recv_callback is not None:
recv_socks = cfg.should_recv_callback(msg, CP)
else:
recv_socks = cfg.pub_sub[msg.which()]
recv_socks = [s for s in cfg.pub_sub[msg.which()] if
(fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
pub_socks[msg.which()].send(msg.as_builder().to_bytes())
should_recv = bool(len(recv_socks))
if len(recv_socks):
# TODO: add timeout
for sock in recv_socks:
m = messaging.recv_one(sub_socks[sock])
if msg.which() == 'can':
can_sock.send(msg.as_builder().to_bytes())
else:
msg_queue.append(msg.as_builder())
# make these values fixed for faster comparison
m_builder = m.as_builder()
m_builder.logMonoTime = 0
m_builder.valid = True
log_msgs.append(m_builder.as_reader())
if should_recv:
fsm.update_msgs(0, msg_queue)
msg_queue = []
gc.enable()
manager.kill_managed_process(cfg.proc_name)
recv_cnt = len(recv_socks)
while recv_cnt > 0:
m = fpm.wait_for_msg()
log_msgs.append(m)
recv_cnt -= m.which() in recv_socks
return log_msgs

2
selfdrive/test/tests/process_replay/ref_commit
View File

@ -1 +1 @@
e3388c62ffb80f4b3ca8721da56a581a93c44e79
8a11bcbc9833e154e10b59a8babb2b4545372f56

74
selfdrive/ui/spinner/Makefile 100644
View File

@ -0,0 +1,74 @@
CC = clang
CXX = clang++
PHONELIBS = ../../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
OPENGL_LIBS = -lGLESv3
FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
OBJS = spinner.o \
../../common/framebuffer.o \
$(PHONELIBS)/nanovg/nanovg.o \
../../common/spinner.o \
opensans_semibold.o \
img_spinner_track.o \
img_spinner_comma.o
DEPS := $(OBJS:.o=.d)
.PHONY: all
all: spinner
spinner: $(OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
-s \
$(FRAMEBUFFER_LIBS) \
-L/system/vendor/lib64 \
$(OPENGL_LIBS) \
-lm -llog
../../common/framebuffer.o: ../../common/framebuffer.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-I$(PHONELIBS)/android_frameworks_native/include \
-I$(PHONELIBS)/android_system_core/include \
-I$(PHONELIBS)/android_hardware_libhardware/include \
-c -o '$@' '$<'
opensans_semibold.o: ../../assets/fonts/opensans_semibold.ttf
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
img_spinner_track.o: ../../assets/img_spinner_track.png
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
img_spinner_comma.o: ../../assets/img_spinner_comma.png
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
-I../.. \
$(NANOVG_FLAGS) \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f spinner $(OBJS) $(DEPS)
-include $(DEPS)

BIN
selfdrive/ui/spinner/spinner
View File

Binary file not shown.

83
selfdrive/ui/spinner/spinner.c
View File

@ -9,92 +9,13 @@
#include <EGL/egl.h>
#include <EGL/eglext.h>
#include "nanovg.h"
#define NANOVG_GLES3_IMPLEMENTATION
#include "nanovg_gl.h"
#include "nanovg_gl_utils.h"
#include "common/framebuffer.h"
#include "common/spinner.h"
int main(int argc, char** argv) {
int err;
const char* spintext = NULL;
if (argc >= 2) {
spintext = argv[1];
}
// spinner
int fb_w, fb_h;
EGLDisplay display;
EGLSurface surface;
FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
&display, &surface, &fb_w, &fb_h);
assert(fb);
NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
assert(vg);
int font = nvgCreateFont(vg, "Bold", "../../assets/fonts/opensans_semibold.ttf");
assert(font >= 0);
int spinner_img = nvgCreateImage(vg, "../../assets/img_spinner_track.png", 0);
assert(spinner_img >= 0);
int spinner_img_s = 360;
int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
int spinner_img_y = 260;
int spinner_img_xc = (fb_w/2);
int spinner_img_yc = (fb_h/2)-100;
int spinner_comma_img = nvgCreateImage(vg, "../../assets/img_spinner_comma.png", 0);
assert(spinner_comma_img >= 0);
for (int cnt = 0; ; cnt++) {
glClearColor(0.1, 0.1, 0.1, 1.0);
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
// background
nvgBeginPath(vg);
NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
nvgFillPaint(vg, bg);
nvgRect(vg, 0, 0, fb_w, fb_h);
nvgFill(vg);
// spin track
nvgSave(vg);
nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
nvgRotate(vg, (3.75*M_PI * cnt/120.0));
nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
nvgBeginPath(vg);
nvgFillPaint(vg, spinner_imgPaint);
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
nvgFill(vg);
nvgRestore(vg);
// comma
NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
nvgBeginPath(vg);
nvgFillPaint(vg, comma_imgPaint);
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
nvgFill(vg);
// message
if (spintext) {
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
nvgFontSize(vg, 96.0f);
nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
}
nvgEndFrame(vg);
eglSwapBuffers(display, surface);
assert(glGetError() == GL_NO_ERROR);
}
spin(argc, argv);
return 0;
}

134
selfdrive/ui/ui.c
View File

@ -41,7 +41,6 @@
#define STATUS_ENGAGED 2
#define STATUS_WARNING 3
#define STATUS_ALERT 4
#define STATUS_MAX 5
#define ALERTSIZE_NONE 0
#define ALERTSIZE_SMALL 1
@ -122,7 +121,6 @@ typedef struct UIScene {
float mpc_y[50];
bool world_objects_visible;
mat3 warp_matrix; // transformed box -> frame.
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
float v_cruise;
@ -253,12 +251,15 @@ typedef struct UIState {
int awake_timeout;
int volume_timeout;
int controls_timeout;
int alert_sound_timeout;
int speed_lim_off_timeout;
int is_metric_timeout;
int longitudinal_control_timeout;
int limit_set_speed_timeout;
bool controls_seen;
int status;
bool is_metric;
bool longitudinal_control;
@ -458,6 +459,25 @@ sound_file* get_sound_file(AudibleAlert alert) {
return NULL;
}
void play_alert_sound(AudibleAlert alert) {
sound_file* sound = get_sound_file(alert);
char* error = NULL;
slplay_play(sound->uri, sound->loop, &error);
if(error) {
LOGW("error playing sound: %s", error);
}
}
void stop_alert_sound(AudibleAlert alert) {
sound_file* sound = get_sound_file(alert);
char* error = NULL;
slplay_stop_uri(sound->uri, &error);
if(error) {
LOGW("error stopping sound: %s", error);
}
}
void ui_sound_init(char **error) {
slplay_setup(error);
@ -657,43 +677,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
s->limit_set_speed_timeout = UI_FREQ;
}
static void ui_draw_transformed_box(UIState *s, uint32_t color) {
const UIScene *scene = &s->scene;
const mat3 bbt = scene->warp_matrix;
struct {
vec3 pos;
uint32_t color;
} verts[] = {
{matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{scene->transformed_width, 0.0, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{scene->transformed_width, scene->transformed_height, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{0.0, scene->transformed_height, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color},
};
for (int i=0; i<ARRAYSIZE(verts); i++) {
verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2];
verts[i].pos.v[1] = s->rgb_height - verts[i].pos.v[1] / verts[i].pos.v[2];
}
glUseProgram(s->line_program);
mat4 out_mat = matmul(device_transform,
matmul(frame_transform, s->rgb_transform));
glUniformMatrix4fv(s->line_transform_loc, 1, GL_TRUE, out_mat.v);
glEnableVertexAttribArray(s->line_pos_loc);
glVertexAttribPointer(s->line_pos_loc, 2, GL_FLOAT, GL_FALSE, sizeof(verts[0]), &verts[0].pos.v[0]);
glEnableVertexAttribArray(s->line_color_loc);
glVertexAttribPointer(s->line_color_loc, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(verts[0]), &verts[0].color);
assert(glGetError() == GL_NO_ERROR);
glDrawArrays(GL_LINE_STRIP, 0, ARRAYSIZE(verts));
}
// Projects a point in car to space to the corresponding point in full frame
// image space.
vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {
@ -1619,6 +1602,9 @@ void handle_message(UIState *s, void *which) {
struct cereal_ControlsState datad;
cereal_read_ControlsState(&datad, eventd.controlsState);
s->controls_timeout = 1 * UI_FREQ;
s->controls_seen = true;
if (datad.vCruise != s->scene.v_cruise) {
s->scene.v_cruise_update_ts = eventd.logMonoTime;
}
@ -1634,35 +1620,17 @@ void handle_message(UIState *s, void *which) {
s->scene.decel_for_model = datad.decelForModel;
s->alert_sound_timeout = 1 * UI_FREQ;
if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
char* error = NULL;
if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
sound_file* active_sound = get_sound_file(s->alert_sound);
slplay_stop_uri(active_sound->uri, &error);
if (error) {
LOGW("error stopping active sound %s", error);
}
}
sound_file* sound = get_sound_file(datad.alertSound);
slplay_play(sound->uri, sound->loop, &error);
if(error) {
LOGW("error playing sound: %s", error);
stop_alert_sound(s->alert_sound);
}
play_alert_sound(datad.alertSound);
s->alert_sound = datad.alertSound;
snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
} else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none) && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
sound_file* sound = get_sound_file(s->alert_sound);
char* error = NULL;
slplay_stop_uri(sound->uri, &error);
if(error) {
LOGW("error stopping sound: %s", error);
}
} else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none)
&& s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
stop_alert_sound(s->alert_sound);
s->alert_type[0] = '\0';
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
}
@ -1734,13 +1702,6 @@ void handle_message(UIState *s, void *which) {
struct cereal_LiveCalibrationData datad;
cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
// should we still even have this?
capn_list32 warpl = datad.warpMatrix2;
capn_resolve(&warpl.p); // is this a bug?
for (int i = 0; i < 3 * 3; i++) {
s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i));
}
capn_list32 extrinsicl = datad.extrinsicMatrix;
capn_resolve(&extrinsicl.p); // is this a bug?
for (int i = 0; i < 3 * 4; i++) {
@ -2312,18 +2273,31 @@ int main(int argc, char* argv[]) {
set_volume(s, volume);
}
// stop playing alert sounds if no controlsState msg for 1 second
if (s->alert_sound_timeout > 0) {
s->alert_sound_timeout--;
} else if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none){
sound_file* sound = get_sound_file(s->alert_sound);
char* error = NULL;
slplay_stop_uri(sound->uri, &error);
if(error) {
LOGW("error stopping sound: %s", error);
if (s->controls_timeout > 0) {
s->controls_timeout--;
} else {
// stop playing alert sound
if ((!s->vision_connected || (s->vision_connected && s->alert_sound_timeout == 0)) &&
s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
stop_alert_sound(s->alert_sound);
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
}
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
// if visiond is still running and controlsState times out, display an alert
if (s->controls_seen && s->vision_connected && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) {
s->scene.alert_size = ALERTSIZE_FULL;
update_status(s, STATUS_ALERT);
snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", "TAKE CONTROL IMMEDIATELY");
snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", "Controls Unresponsive");
ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2);
s->alert_sound_timeout = 2 * UI_FREQ;
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat;
play_alert_sound(s->alert_sound);
}
s->alert_sound_timeout--;
s->controls_seen = false;
}
read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout);

Some files were not shown because too many files have changed in this diff Show More