diff --git a/.gitignore b/.gitignore index 9bdfbc7163..44294dde72 100644 --- a/.gitignore +++ b/.gitignore @@ -39,5 +39,6 @@ selfdrive/sensord/sensord one openpilot +notebooks xx diff --git a/README.md b/README.md index ba97ded04f..23c81e4303 100644 --- a/README.md +++ b/README.md @@ -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 | 25mph1| 25mph | Nidec | -| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec | -| Buick3 | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| 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 | -| GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| 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 | 25mph1| 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 | 25mph1| 12mph | Inverted Nidec | -| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph1| 0mph | Inverted Nidec | -| Honda | Passport 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | -| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | -| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | -| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | -| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6| -| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| -| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6| -| 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 | Custom6| -| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| -| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| -| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | -| Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4| -| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4| -| Toyota | Avalon 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | -| Toyota | Avalon 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | -| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph5 | 0mph | Toyota | -| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph5 | 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 | Yes2| 20mph1| 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 | Yes2| 0mph | 0mph | Toyota | -| Toyota | Highlander Hybrid 2017-19| All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius Prime 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | -| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | -| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | -| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Sienna 2018 | All | Yes | Yes2| 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 | 25mph1| 25mph | Nidec | +| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec | +| Buick3 | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| 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 | +| GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| +| 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 | 25mph1| 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 | 25mph1| 0mph | Inverted Nidec | +| Honda | Passport 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | +| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | +| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | +| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | +| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6| +| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| +| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6| +| 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 | Custom6| +| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| +| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| +| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | +| Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 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 | Custom4| +| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4| +| Toyota | Avalon 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Avalon 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph5 | 0mph | Toyota | +| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph5 | 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 | Yes2| 20mph1| 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 | Yes2| 0mph | 0mph | Toyota | +| Toyota | Highlander Hybrid 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius Prime 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | +| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | +| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Sienna 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | 1[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).***
-2When 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).
-3[GM installation guide](https://zoneos.com/volt/).
+2When 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).***
+3[GM installation guide](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
4Subaru Giraffe is DIY.
528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
6Open 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.
@@ -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? ------ diff --git a/RELEASES.md b/RELEASES.md index d847bb3f05..14ddf78b6b 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 5dd8b8f204..682761bbc4 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/common/api/__init__.py b/common/api/__init__.py index e457272a12..8eb9817399 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -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/" diff --git a/common/file_helpers.py b/common/file_helpers.py index 3300ae595a..beeee89088 100644 --- a/common/file_helpers.py +++ b/common/file_helpers.py @@ -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): diff --git a/common/params.py b/common/params.py index 7f1b3ab98b..9af435361f 100755 --- a/common/params.py +++ b/common/params.py @@ -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: diff --git a/common/stat_live.py b/common/stat_live.py new file mode 100644 index 0000000000..06b4f993be --- /dev/null +++ b/common/stat_live.py @@ -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(): \ No newline at end of file diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 2e04c5ac22..a5c14bb4bf 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -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 diff --git a/common/transformations/model.py b/common/transformations/model.py index e832cb7ee9..c13cc7e387 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -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 diff --git a/installer/updater/updater b/installer/updater/updater index 51886bd334..15858eabb4 100755 Binary files a/installer/updater/updater and b/installer/updater/updater differ diff --git a/installer/updater/updater.cc b/installer/updater/updater.cc index 89ac7f6359..e2d3e7dae0 100644 --- a/installer/updater/updater.cc +++ b/installer/updater/updater.cc @@ -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 guard(lock); + state = LOW_BATTERY; + } + + void set_running() { + std::lock_guard 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... diff --git a/models/driving_model.dlc b/models/driving_model.dlc index 79181185f7..3dc890079f 100644 Binary files a/models/driving_model.dlc and b/models/driving_model.dlc differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed new file mode 100644 index 0000000000..a1e01931c6 Binary files /dev/null and b/panda/board/obj/panda.bin.signed differ diff --git a/phonelibs/boringssl/LICENSE b/phonelibs/boringssl/LICENSE new file mode 100644 index 0000000000..2f4dfcdb04 --- /dev/null +++ b/phonelibs/boringssl/LICENSE @@ -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. diff --git a/phonelibs/bzip2/LICENSE b/phonelibs/bzip2/LICENSE new file mode 100644 index 0000000000..cc614178cf --- /dev/null +++ b/phonelibs/bzip2/LICENSE @@ -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 + +-------------------------------------------------------------------------- diff --git a/phonelibs/hierarchy/lib/_hierarchy.so b/phonelibs/hierarchy/lib/_hierarchy.so deleted file mode 100755 index 367b717758..0000000000 Binary files a/phonelibs/hierarchy/lib/_hierarchy.so and /dev/null differ diff --git a/phonelibs/libyuv/LICENSE b/phonelibs/libyuv/LICENSE new file mode 100644 index 0000000000..c911747a6b --- /dev/null +++ b/phonelibs/libyuv/LICENSE @@ -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. diff --git a/phonelibs/openblas/libopenblas.so b/phonelibs/openblas/libopenblas.so deleted file mode 120000 index 7a792bc907..0000000000 --- a/phonelibs/openblas/libopenblas.so +++ /dev/null @@ -1 +0,0 @@ -libopenblas_armv8p-r0.2.19.so \ No newline at end of file diff --git a/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so b/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so deleted file mode 100755 index ace58c8a13..0000000000 Binary files a/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so and /dev/null differ diff --git a/phonelibs/yaml-cpp/LICENSE b/phonelibs/yaml-cpp/LICENSE new file mode 100644 index 0000000000..991fdbbe7d --- /dev/null +++ b/phonelibs/yaml-cpp/LICENSE @@ -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. diff --git a/run_docker_tests.sh b/run_docker_tests.sh index dfee5f8669..21549ab089 100755 --- a/run_docker_tests.sh +++ b/run_docker_tests.sh @@ -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' diff --git a/selfdrive/assets/sounds/disengaged.wav b/selfdrive/assets/sounds/disengaged.wav index 655aa31261..3aa8e51e69 100644 Binary files a/selfdrive/assets/sounds/disengaged.wav and b/selfdrive/assets/sounds/disengaged.wav differ diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav index b33e8181fa..1451f937f3 100644 Binary files a/selfdrive/assets/sounds/engaged.wav and b/selfdrive/assets/sounds/engaged.wav differ diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav index 309eaef8ae..e805181aeb 100644 Binary files a/selfdrive/assets/sounds/error.wav and b/selfdrive/assets/sounds/error.wav differ diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav index 920c11846a..43ca74cc5c 100644 Binary files a/selfdrive/assets/sounds/warning_1.wav and b/selfdrive/assets/sounds/warning_1.wav differ diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav index f5ed8521dd..e9709d9fdb 100644 Binary files a/selfdrive/assets/sounds/warning_2.wav and b/selfdrive/assets/sounds/warning_2.wav differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 6aa84d106e..4f1059930c 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -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: diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index f4ca033f5b..8468238f31 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -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; diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py index 61cf8cd057..fbf36a3d4d 100644 --- a/selfdrive/can/libdbc_py.py +++ b/selfdrive/can/libdbc_py.py @@ -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); diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 69b30fb511..830e8b14c0 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -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((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(); + last_sec = event.getLogMonoTime(); + auto cans = event.getCan(); - UpdateCans(sec, cans); + UpdateCans(last_sec, cans); - UpdateValid(sec); + 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 query(uint64_t sec) { + std::vector query_latest() { std::vector 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; iupdate(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 values = cp->query(sec); + const std::vector 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 &values) { +void can_query_latest_vector(void* can, bool *out_can_valid, std::vector &values) { CANParser* cp = (CANParser*)can; if (out_can_valid) { *out_can_valid = cp->can_valid; } - values = cp->query(sec); + values = cp->query_latest(); }; } diff --git a/selfdrive/can/parser_pyx.pxd b/selfdrive/can/parser_pyx.pxd index ac619707ac..6b1e50ce6d 100644 --- a/selfdrive/can/parser_pyx.pxd +++ b/selfdrive/can/parser_pyx.pxd @@ -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) diff --git a/selfdrive/can/parser_pyx.pyx b/selfdrive/can/parser_pyx.pyx index c6f1f58e03..4f92970960 100644 --- a/selfdrive/can/parser_pyx.pyx +++ b/selfdrive/can/parser_pyx.pyx @@ -18,7 +18,7 @@ cdef class CANParser: self.dbc_lookup = dlsym(libdbc, 'dbc_lookup') self.can_update = dlsym(libdbc, 'can_update') self.can_update_string = dlsym(libdbc, 'can_update_string') - self.can_query_vector = dlsym(libdbc, 'can_query_vector') + self.can_query_latest_vector = 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 diff --git a/selfdrive/can/tests/parser_old.py b/selfdrive/can/tests/parser_old.py index 5a899efa50..ccf613ab3c 100644 --- a/selfdrive/can/tests/parser_old.py +++ b/selfdrive/can/tests/parser_old.py @@ -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 diff --git a/selfdrive/can/tests/test_packer_toyota.py b/selfdrive/can/tests/test_packer_toyota.py index f5f0e8a7f4..fc53399ee5 100644 --- a/selfdrive/can/tests/test_packer_toyota.py +++ b/selfdrive/can/tests/test_packer_toyota.py @@ -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)) diff --git a/selfdrive/can/tests/test_parser.py b/selfdrive/can/tests/test_parser.py index 53c95ce912..ff3cbddf7e 100755 --- a/selfdrive/can/tests/test_parser.py +++ b/selfdrive/can/tests/test_parser.py @@ -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 diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 45b9f94e12..a90f8d43e0 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -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 diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index dac43ca95d..2ddc4bdaf3 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -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: diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 6368847c8f..13ea25b59c 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -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): diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index c43c6c08ab..e470b3c1d8 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -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 diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 43f5c6105e..815d2c3e4c 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -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: diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py new file mode 100644 index 0000000000..3566fd5e75 --- /dev/null +++ b/selfdrive/car/ford/carcontroller.py @@ -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 diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py new file mode 100644 index 0000000000..a55f298964 --- /dev/null +++ b/selfdrive/car/ford/fordcan.py @@ -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) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 59acc84324..efc7301f36 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -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) diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 04cab9c66d..be408da03f 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -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: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f5f641836f..b3e82e5e61 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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 diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 6788e1ce74..4f54c82756 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -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: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 0ec7aff052..87ecdb34ef 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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: diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 723368a92d..b66b6c8c53 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -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) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index be7da4aa04..13f399310a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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 diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index f8cecd6de4..6943d35634 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -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: diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index a7c5a92063..5e704515fa 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -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'), diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index be92371f1d..b39f07d6f1 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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: diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 1d7772fd3b..04c1005947 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -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 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index c5fe0062a7..5c91c95463 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -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) diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py index 0f81087711..6c4e7a16a1 100644 --- a/selfdrive/car/subaru/radar_interface.py +++ b/selfdrive/car/subaru/radar_interface.py @@ -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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 28ea8cf6ec..0cfd67b9ea 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 920a8ab5d3..f36738f8ee 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 1e3b61624d..457169c4d6 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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])) diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 4e0a0e809b..0a1ce07ea2 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -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: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 35ba674528..b886b1c675 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 48b6e11ba7..1cb32ccf18 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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] diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index 9117154b0c..4527cb5320 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -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; } - diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index d17343827e..10517c6799 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.3-release" +#define COMMA_VERSION "0.6.4-release" diff --git a/selfdrive/config.py b/selfdrive/config.py index 751a84e285..6c2296273a 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9a463bacae..558c1ba15b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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']) + + if sm is None: + sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ + 'gpsLocation'], ignore_alive=['gpsLocation']) - logcan = messaging.sub_sock(service_list['can'].port) + 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) - - CI, CP = get_car(logcan, sendcan, is_panda_black) - logcan.close() + print("Waiting for CAN messages...") + get_one_can(can_sock) - # 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__": diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9c1144b966..a3580a8a91 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 3b45e5a063..f76102ee9f 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -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) diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index 8180fadeba..c080fecb4a 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -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 diff --git a/selfdrive/controls/lib/gps_helpers.py b/selfdrive/controls/lib/gps_helpers.py new file mode 100755 index 0000000000..6ac27202d8 --- /dev/null +++ b/selfdrive/controls/lib/gps_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 60a24b6d9b..9363631e45 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -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]) diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py deleted file mode 100644 index ff5fa478ac..0000000000 --- a/selfdrive/controls/lib/latcontrol_helpers.py +++ /dev/null @@ -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]) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 8c0e3ec31f..23d91cae43 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -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 diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 9d43e97f26..cb1ab94dc6 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -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] diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index faa812ac6f..b62d9f1107 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -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) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 52712d15a7..26230843a8 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -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) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 02a38ca2a8..d246d5d8ca 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -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 { diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 9286ea0588..7a5742b3e5 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -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__": diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 84410fa8e1..d516316332 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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__": diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 994ea7ea5d..bcc67c42a3 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -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: diff --git a/selfdrive/controls/tests/test_monitoring.py b/selfdrive/controls/tests/test_monitoring.py new file mode 100644 index 0000000000..eccc974026 --- /dev/null +++ b/selfdrive/controls/tests/test_monitoring.py @@ -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() diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 2890b0f61d..db4b0c2266 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -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__": diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index 93e706499a..de6ff9ff4e 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -10,7 +10,7 @@ #include "locationd_yawrate.h" -void Localizer::update_state(const Eigen::Matrix &C, const double R, double current_time, double meas) { +void Localizer::update_state(const Eigen::Matrix &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 &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::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; - - Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0); - P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0); - - C_posenet << 1, 0; - C_gyro << 1, 1; - x << 0, 0; - - R_gyro = pow(0.05, 2.0); + // 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 << + 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, 0, 0; + C_gyro << 1, 0, 1, 0; + x << 0, 0, 0, 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)); + } } diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h index d5db91e790..9c6885241b 100644 --- a/selfdrive/locationd/locationd_yawrate.h +++ b/selfdrive/locationd/locationd_yawrate.h @@ -7,28 +7,29 @@ class Localizer { - Eigen::Matrix2d A; - Eigen::Matrix2d I; - Eigen::Matrix2d Q; - Eigen::Matrix2d P; - Eigen::Matrix C_posenet; - Eigen::Matrix C_gyro; + Eigen::Matrix4d A; + Eigen::Matrix4d I; + Eigen::Matrix4d Q; + Eigen::Matrix C_posenet; + Eigen::Matrix C_gyro; double R_gyro; - void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas); + void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas); void handle_sensor_events(capnp::List::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); diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc index 912abde357..66f378a1da 100644 --- a/selfdrive/locationd/params_learner.cc +++ b/selfdrive/locationd/params_learner.cc @@ -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; diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index 2d9f11b3eb..b9b4a9c1ad 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -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(); - 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); - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); - } + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + 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); // Save parameters every minute + // TODO: Save in seperate thread if (save_counter % 6000 == 0) { json11::Json json = json11::Json::object { {"carVin", vin}, diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 59765d13ed..38887f4ad2 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -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: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index c5990f4fdf..62bf834b68 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -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: diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py index cad7e2e8bf..13fb41efeb 100644 --- a/selfdrive/messaging.py +++ b/selfdrive/messaging.py @@ -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) diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 0c82a3187e..ec8b0b9d44 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -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.] diff --git a/selfdrive/test/test_car_models_openpilot.py b/selfdrive/test/test_car_models.py similarity index 77% rename from selfdrive/test/test_car_models_openpilot.py rename to selfdrive/test/test_car_models.py index 53184653c6..317f325918 100755 --- a/selfdrive/test/test_car_models_openpilot.py +++ b/selfdrive/test/test_car_models.py @@ -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" diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index 09e81b2a11..ca05924200 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -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') diff --git a/selfdrive/test/tests/process_replay/README.md b/selfdrive/test/tests/process_replay/README.md index 639ca9051d..7e92717f9d 100644 --- a/selfdrive/test/tests/process_replay/README.md +++ b/selfdrive/test/tests/process_replay/README.md @@ -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. diff --git a/selfdrive/test/tests/process_replay/compare_logs.py b/selfdrive/test/tests/process_replay/compare_logs.py index 260e06c771..5a1bd3fd73 100755 --- a/selfdrive/test/tests/process_replay/compare_logs.py +++ b/selfdrive/test/tests/process_replay/compare_logs.py @@ -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:]) diff --git a/selfdrive/test/tests/process_replay/process_replay.py b/selfdrive/test/tests/process_replay/process_replay.py index 66067b405d..b360aed972 100755 --- a/selfdrive/test/tests/process_replay/process_replay.py +++ b/selfdrive/test/tests/process_replay/process_replay.py @@ -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") + fsm.wait_on_getitem = True - can = pub_socks["can"] - logMessage = messaging.sub_sock(service_list["logMessage"].port) + # populate fake socket with data for fingerprinting + canmsgs = filter(lambda msg: msg.which() == "can", msgs) + 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(1) - messaging.drain_sock(logMessage) + # we know fingerprinting is done when controlsd sets sm['pathPlan'].sensorValid + fsm.update_called.wait() + fsm.update_called.clear() - # 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 = False + can_sock.wait = True + can_sock.data = [] - canmsgs = filter(lambda msg: msg.which() == "can", msgs) - for msg in canmsgs[:200]: - can.send(msg.as_builder().to_bytes()) - - 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") + 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) - - time.sleep(0.5) +def get_car_params(msgs, fsm, can_sock): + can = FakeSocket(wait=False) + sendcan = FakeSocket(wait=False) - 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 - - pub_socks, sub_socks = {}, {} - for pub, sub in cfg.pub_sub.iteritems(): - pub_socks[pub] = messaging.pub_sock(service_list[pub].port) + 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'] - 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) - return log_msgs + 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 diff --git a/selfdrive/test/tests/process_replay/ref_commit b/selfdrive/test/tests/process_replay/ref_commit index 30a1a28538..3744386a82 100644 --- a/selfdrive/test/tests/process_replay/ref_commit +++ b/selfdrive/test/tests/process_replay/ref_commit @@ -1 +1 @@ -e3388c62ffb80f4b3ca8721da56a581a93c44e79 \ No newline at end of file +8a11bcbc9833e154e10b59a8babb2b4545372f56 \ No newline at end of file diff --git a/selfdrive/ui/spinner/Makefile b/selfdrive/ui/spinner/Makefile new file mode 100644 index 0000000000..93871bb009 --- /dev/null +++ b/selfdrive/ui/spinner/Makefile @@ -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) diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/spinner/spinner index 65c198aabd..2b9e634fea 100755 Binary files a/selfdrive/ui/spinner/spinner and b/selfdrive/ui/spinner/spinner differ diff --git a/selfdrive/ui/spinner/spinner.c b/selfdrive/ui/spinner/spinner.c index 3ec36e7403..14b452a49b 100644 --- a/selfdrive/ui/spinner/spinner.c +++ b/selfdrive/ui/spinner/spinner.c @@ -9,92 +9,13 @@ #include #include -#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; } diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 3eedb809c6..1c88d21923 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -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; irgb_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; + 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; + } + + // 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; - slplay_stop_uri(sound->uri, &error); - if(error) { - LOGW("error stopping sound: %s", error); + s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat; + play_alert_sound(s->alert_sound); } - s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none; + s->alert_sound_timeout--; + s->controls_seen = false; } read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout); diff --git a/selfdrive/visiond/cameras/camera_frame_stream.cc b/selfdrive/visiond/cameras/camera_frame_stream.cc new file mode 100644 index 0000000000..81b2bd4008 --- /dev/null +++ b/selfdrive/visiond/cameras/camera_frame_stream.cc @@ -0,0 +1,172 @@ +#include "camera_frame_stream.h" + +#include +#include +#include +#include + +#include +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/util.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "buffering.h" + +extern "C" { +#include +} + +extern volatile int do_exit; + +#define FRAME_WIDTH 1164 +#define FRAME_HEIGHT 874 + +namespace { +void camera_open(CameraState *s, VisionBuf *camera_bufs, bool rear) { + assert(camera_bufs); + s->camera_bufs = camera_bufs; +} + +void camera_close(CameraState *s) { + tbuffer_stop(&s->camera_tb); +} + +void camera_release_buffer(void *cookie, int buf_idx) { + CameraState *s = static_cast(cookie); +} + +void camera_init(CameraState *s, int camera_id, unsigned int fps) { + assert(camera_id < ARRAYSIZE(cameras_supported)); + s->ci = cameras_supported[camera_id]; + assert(s->ci.frame_width != 0); + + s->frame_size = s->ci.frame_height * s->ci.frame_stride; + s->fps = fps; + + tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s); +} + +void run_frame_stream(DualCameraState *s) { + int err; + zsock_t *recorder_sock = zsock_new_sub(">tcp://127.0.0.1:8002", ""); + assert(recorder_sock); + void *recorder_sock_raw = zsock_resolve(recorder_sock); + + CameraState *const rear_camera = &s->rear; + auto *tb = &rear_camera->camera_tb; + + while (!do_exit) { + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + + err = zmq_msg_recv(&msg, recorder_sock_raw, 0); + if(err == -1) + break; + + // make copy due to alignment issues, will be freed on out of scope + size_t len = zmq_msg_size(&msg); + auto amsg = kj::heapArray((len / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), (const uint8_t*)zmq_msg_data(&msg), len); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + auto frame = event.getFrame(); + + const int buf_idx = tbuffer_select(tb); + rear_camera->camera_bufs_metadata[buf_idx] = { + .frame_id = frame.getFrameId(), + .timestamp_eof = frame.getTimestampEof(), + .frame_length = static_cast(frame.getFrameLength()), + .integ_lines = static_cast(frame.getIntegLines()), + .global_gain = static_cast(frame.getGlobalGain()), + }; + + cl_command_queue q = rear_camera->camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = rear_camera->camera_bufs[buf_idx].buf_cl; + cl_event map_event; + void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, + CL_MAP_WRITE, 0, frame.getImage().size(), + 0, NULL, &map_event, &err); + assert(err == 0); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + memcpy(yuv_buf, frame.getImage().begin(), frame.getImage().size()); + + clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + tbuffer_dispatch(tb, buf_idx); + + err = zmq_msg_close(&msg); + assert(err == 0); + } + zsock_destroy(&recorder_sock); +} + +} // namespace + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_IMX298] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_WIDTH*3, + .bayer = false, + .bayer_flip = false, + }, + [CAMERA_ID_OV8865] = { + .frame_width = 1632, + .frame_height = 1224, + .frame_stride = 2040, // seems right + .bayer = false, + .bayer_flip = 3, + .hdr = false + }, +}; + +void cameras_init(DualCameraState *s) { + memset(s, 0, sizeof(*s)); + + camera_init(&s->rear, CAMERA_ID_IMX298, 20); + s->rear.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; + + camera_init(&s->front, CAMERA_ID_OV8865, 10); + s->front.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; +} + +void camera_autoexposure(CameraState *s, float grey_frac) {} + +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, + VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, + VisionBuf *camera_bufs_front) { + assert(camera_bufs_rear); + assert(camera_bufs_front); + int err; + + // LOG("*** open front ***"); + camera_open(&s->front, camera_bufs_front, false); + + // LOG("*** open rear ***"); + camera_open(&s->rear, camera_bufs_rear, true); +} + +void cameras_close(DualCameraState *s) { + camera_close(&s->rear); +} + +void cameras_run(DualCameraState *s) { + set_thread_name("frame_streaming"); + run_frame_stream(s); + cameras_close(s); +} diff --git a/selfdrive/visiond/cameras/camera_frame_stream.h b/selfdrive/visiond/cameras/camera_frame_stream.h new file mode 100644 index 0000000000..6351a183d3 --- /dev/null +++ b/selfdrive/visiond/cameras/camera_frame_stream.h @@ -0,0 +1,56 @@ +#ifndef CAMERA_FRAME_STREAM_H +#define CAMERA_FRAME_STREAM_H + +#include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include "common/mat.h" + +#include "buffering.h" +#include "common/visionbuf.h" +#include "camera_common.h" + +#define FRAME_BUF_COUNT 16 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CameraState { + int camera_id; + CameraInfo ci; + int frame_size; + + VisionBuf *camera_bufs; + FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; + TBuffer camera_tb; + + int fps; + float digital_gain; + + mat3 transform; +} CameraState; + + +typedef struct DualCameraState { + int ispif_fd; + + CameraState rear; + CameraState front; +} DualCameraState; + +void cameras_init(DualCameraState *s); +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_run(DualCameraState *s); +void cameras_close(DualCameraState *s); +void camera_autoexposure(CameraState *s, float grey_frac); +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/selfdrive/visiond/models/commonmodel.c b/selfdrive/visiond/models/commonmodel.c index 33aabd1cee..0369d16d5d 100644 --- a/selfdrive/visiond/models/commonmodel.c +++ b/selfdrive/visiond/models/commonmodel.c @@ -66,134 +66,3 @@ float sigmoid(float input) { float softplus(float input) { return log1p(expf(input)); } - -void softmax(const float* input, float* output, size_t len) { - float max_val = -FLT_MAX; - for(int i = 0; i < len; i++) { - const float v = input[i]; - if( v > max_val ) { - max_val = v; - } - } - - float denominator = 0; - for(int i = 0; i < len; i++) { - float const v = input[i]; - float const v_exp = expf(v - max_val); - denominator += v_exp; - output[i] = v_exp; - } - - const float inv_denominator = 1. / denominator; - for(int i = 0; i < len; i++) { - output[i] *= inv_denominator; - } - -} - - -static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, const PathData data) { - capn_list32 poly_ptr = capn_new_list32(cs, POLYFIT_DEGREE); - for (int i=0; im->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); #endif +#ifdef DESIRE + s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); + for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; + s->m->addDesire(s->desire, DESIRE_SIZE); +#endif // Build Vandermonde matrix for(int i = 0; i < MODEL_PATH_DISTANCE; i++) { @@ -53,7 +58,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t ModelData model_eval_frame(ModelState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock) { + mat3 transform, void* sock, float *desire_in) { struct { float *path; float *left_lane; @@ -62,6 +67,12 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, float *speed; } net_outputs = {NULL}; +#ifdef DESIRE + if (desire_in != NULL) { + for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i]; + } +#endif + //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n"); float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform); @@ -201,3 +212,57 @@ void poly_fit(float *in_pts, float *in_stds, float *out) { // Apply scale to output p = p.transpose() * scale.asDiagonal(); } + + +void fill_path(cereal::ModelData::PathData::Builder path, const PathData path_data) { + kj::ArrayPtr poly(&path_data.poly[0], ARRAYSIZE(path_data.poly)); + path.setPoly(poly); + path.setProb(path_data.prob); + path.setStd(path_data.std); +} + +void fill_lead(cereal::ModelData::LeadData::Builder lead, const LeadData lead_data) { + lead.setDist(lead_data.dist); + lead.setProb(lead_data.prob); + lead.setStd(lead_data.std); + lead.setRelY(lead_data.rel_y); + lead.setRelYStd(lead_data.rel_y_std); + lead.setRelVel(lead_data.rel_v); + lead.setRelVelStd(lead_data.rel_v_std); + lead.setRelA(lead_data.rel_a); + lead.setRelAStd(lead_data.rel_a_std); +} + +void model_publish(void* sock, uint32_t frame_id, + const ModelData data, uint64_t timestamp_eof) { + // make msg + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initModel(); + framed.setFrameId(frame_id); + framed.setTimestampEof(timestamp_eof); + + kj::ArrayPtr speed(&data.speed[0], ARRAYSIZE(data.speed)); + framed.setSpeed(speed); + + + auto lpath = framed.initPath(); + fill_path(lpath, data.path); + auto left_lane = framed.initLeftLane(); + fill_path(left_lane, data.left_lane); + auto right_lane = framed.initRightLane(); + fill_path(right_lane, data.right_lane); + + auto lead = framed.initLead(); + fill_lead(lead, data.lead); + auto lead_future = framed.initLeadFuture(); + fill_lead(lead_future, data.lead_future); + + + // send message + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + zmq_send(sock, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); + } diff --git a/selfdrive/visiond/models/driving.h b/selfdrive/visiond/models/driving.h index 966cf69473..c384d6ef4a 100644 --- a/selfdrive/visiond/models/driving.h +++ b/selfdrive/visiond/models/driving.h @@ -3,25 +3,41 @@ // gate this here #define TEMPORAL +#define DESIRE + +#ifdef DESIRE + #define DESIRE_SIZE 8 +#endif #include "common/mat.h" #include "common/modeldata.h" +#include "common/util.h" #include "commonmodel.h" #include "runners/run.h" +#include "cereal/gen/cpp/log.capnp.h" +#include +#include + + typedef struct ModelState { ModelInput in; float *output; RunModel *m; +#ifdef DESIRE + float *desire; +#endif } ModelState; void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal); ModelData model_eval_frame(ModelState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock); + mat3 transform, void* sock, float *desire_in); void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); +void model_publish(void* sock, uint32_t frame_id, + const ModelData data, uint64_t timestamp_eof); #endif diff --git a/selfdrive/visiond/models/monitoring.cc b/selfdrive/visiond/models/monitoring.cc index f6e47880fd..959902b985 100644 --- a/selfdrive/visiond/models/monitoring.cc +++ b/selfdrive/visiond/models/monitoring.cc @@ -1,6 +1,7 @@ #include #include "monitoring.h" #include "common/mat.h" +#include "common/timing.h" #define MODEL_WIDTH 320 #define MODEL_HEIGHT 160 @@ -47,9 +48,32 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, return ret; } +void monitoring_publish(void* sock, uint32_t frame_id, const MonitoringResult res) { + // make msg + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initDriverMonitoring(); + framed.setFrameId(frame_id); + + kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); + kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); + framed.setFaceOrientation(face_orientation); + framed.setFacePosition(face_position); + framed.setFaceProb(res.face_prob); + framed.setLeftEyeProb(res.left_eye_prob); + framed.setRightEyeProb(res.right_eye_prob); + framed.setLeftBlinkProb(res.left_blink_prob); + framed.setRightBlinkProb(res.right_blink_prob); + + // send message + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + zmq_send(sock, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); + } void monitoring_free(MonitoringState* s) { model_input_free(&s->in); delete s->m; } - diff --git a/selfdrive/visiond/models/monitoring.h b/selfdrive/visiond/models/monitoring.h index 2be96825f8..49b910ef5c 100644 --- a/selfdrive/visiond/models/monitoring.h +++ b/selfdrive/visiond/models/monitoring.h @@ -1,9 +1,14 @@ #ifndef MONITORING_H #define MONITORING_H +#include "common/util.h" #include "commonmodel.h" #include "runners/run.h" +#include "cereal/gen/cpp/log.capnp.h" +#include +#include + #ifdef __cplusplus extern "C" { #endif @@ -32,6 +37,7 @@ typedef struct MonitoringState { void monitoring_init(MonitoringState* s, cl_device_id device_id, cl_context context); MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height); +void monitoring_publish(void* sock, uint32_t frame_id, const MonitoringResult res); void monitoring_free(MonitoringState* s); #ifdef __cplusplus diff --git a/selfdrive/visiond/runners/runmodel.h b/selfdrive/visiond/runners/runmodel.h index 245f4c8df2..d1563a61a5 100644 --- a/selfdrive/visiond/runners/runmodel.h +++ b/selfdrive/visiond/runners/runmodel.h @@ -4,6 +4,7 @@ class RunModel { public: virtual void addRecurrent(float *state, int state_size) {} + virtual void addDesire(float *state, int state_size) {} virtual void execute(float *net_input_buf) {} }; diff --git a/selfdrive/visiond/runners/snpemodel.cc b/selfdrive/visiond/runners/snpemodel.cc index 15b3bcc317..ae69f76d66 100644 --- a/selfdrive/visiond/runners/snpemodel.cc +++ b/selfdrive/visiond/runners/snpemodel.cc @@ -86,18 +86,27 @@ SNPEModel::SNPEModel(const char *path, float *output, size_t output_size) { } void SNPEModel::addRecurrent(float *state, int state_size) { + recurrentBuffer = this->addExtra(state, state_size, 2); +} + +void SNPEModel::addDesire(float *state, int state_size) { + desireBuffer = this->addExtra(state, state_size, 1); +} + +std::unique_ptr SNPEModel::addExtra(float *state, int state_size, int idx) { // get input and output names const auto &strListi_opt = snpe->getInputTensorNames(); if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names"); const auto &strListi = *strListi_opt; - const char *input_tensor_name = strListi.at(1); - printf("adding recurrent: %s\n", input_tensor_name); + const char *input_tensor_name = strListi.at(idx); + printf("adding index %d: %s\n", idx, input_tensor_name); zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); - std::vector recurrentStrides = {state_size * sizeof(float), sizeof(float)}; - recurrentBuffer = ubFactory.createUserBuffer(state, state_size * sizeof(float), recurrentStrides, &userBufferEncodingFloat); - inputMap.add(input_tensor_name, recurrentBuffer.get()); + std::vector retStrides = {state_size * sizeof(float), sizeof(float)}; + auto ret = ubFactory.createUserBuffer(state, state_size * sizeof(float), retStrides, &userBufferEncodingFloat); + inputMap.add(input_tensor_name, ret.get()); + return ret; } void SNPEModel::execute(float *net_input_buf) { diff --git a/selfdrive/visiond/runners/snpemodel.h b/selfdrive/visiond/runners/snpemodel.h index 0a6e030f47..cd47a3ade1 100644 --- a/selfdrive/visiond/runners/snpemodel.h +++ b/selfdrive/visiond/runners/snpemodel.h @@ -20,6 +20,7 @@ public: if (model_data) free(model_data); } void addRecurrent(float *state, int state_size); + void addDesire(float *state, int state_size); void execute(float *net_input_buf); private: uint8_t *model_data = NULL; @@ -36,8 +37,10 @@ private: std::unique_ptr outputBuffer; float *output; - // recurrent + // recurrent and desire + std::unique_ptr addExtra(float *state, int state_size, int idx); std::unique_ptr recurrentBuffer; + std::unique_ptr desireBuffer; }; #endif diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index a0cce70ab8..53d1f82777 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -155,6 +155,8 @@ struct VisionState { int rgb_front_width, rgb_front_height, rgb_front_stride; VisionBuf rgb_front_bufs[UI_BUF_COUNT]; cl_mem rgb_front_bufs_cl[UI_BUF_COUNT]; + int front_meteringbox_xmin, front_meteringbox_xmax; + int front_meteringbox_ymin, front_meteringbox_ymax; ModelState model; ModelData model_bufs[UI_BUF_COUNT]; @@ -725,40 +727,27 @@ void* monitoring_thread(void *arg) { double t2 = millis_since_boot(); - // send driver monitoring packet + // set front camera metering target + if (res.face_prob > 0.4) { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto framed = event.initDriverMonitoring(); - framed.setFrameId(frame_data.frame_id); - - // junk 0s from legacy model - //kj::ArrayPtr descriptor_DEPRECATED(&res.descriptor_DEPRECATED[0], ARRAYSIZE(res.descriptor_DEPRECATED)); - //framed.setDescriptor(descriptor_DEPRECATED); - //framed.setStd(res.std_DEPRECATED); - // why not use this junk space for reporting inference time instead - // framed.setStdDEPRECATED(static_cast(t2-t1)); - - kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); - kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); - framed.setFaceOrientation(face_orientation); - framed.setFacePosition(face_position); - framed.setFaceProb(res.face_prob); - framed.setLeftEyeProb(res.left_eye_prob); - framed.setRightEyeProb(res.right_eye_prob); - framed.setLeftBlinkProb(res.left_blink_prob); - framed.setRightBlinkProb(res.right_blink_prob); - - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(s->monitoring_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); + int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height; + s->front_meteringbox_xmin = x_offset + (res.face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72; + s->front_meteringbox_xmax = x_offset + (res.face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72; + s->front_meteringbox_ymin = (res.face_position[1] + 0.5) * (s->rgb_front_height) - 72; + s->front_meteringbox_ymax = (res.face_position[1] + 0.5) * (s->rgb_front_height) + 72; + } + else // use default setting if no face + { + s->front_meteringbox_ymin = s->rgb_front_height * 0; + s->front_meteringbox_ymax = s->rgb_front_height * 2 / 3; + s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; + s->front_meteringbox_xmax = s->rgb_front_width; } - t2 = millis_since_boot(); + // send dm packet + monitoring_publish(s->monitoring_sock_raw, frame_data.frame_id, res); + //t2 = millis_since_boot(); //LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1; } @@ -816,11 +805,26 @@ void* frontview_thread(void *arg) { if (cnt % 3 == 0) #endif { - // for driver autoexposure, use bottom right corner - const int y_start = s->rgb_front_height / 3; - const int y_end = s->rgb_front_height; - const int x_start = s->rgb_front_width * 2 / 3; - const int x_end = s->rgb_front_width; + // use driver face crop for AE + int x_start; + int x_end; + int y_start; + int y_end; + + if (s->front_meteringbox_xmax > 0) + { + x_start = s->front_meteringbox_xmin<0 ? 0:s->front_meteringbox_xmin; + x_end = s->front_meteringbox_xmax>=s->rgb_front_width ? s->rgb_front_width-1:s->front_meteringbox_xmax; + y_start = s->front_meteringbox_ymin<0 ? 0:s->front_meteringbox_ymin; + y_end = s->front_meteringbox_ymax>=s->rgb_front_height ? s->rgb_front_height-1:s->front_meteringbox_ymax; + } + else + { + y_start = s->rgb_front_height * 0; + y_end = s->rgb_front_height * 2 / 3; + x_start = s->rgb_front_width * 3 / 5; + x_end = s->rgb_front_width; + } uint32_t lum_binning[256] = {0,}; for (int y = y_start; y < y_end; ++y) { @@ -1007,10 +1011,10 @@ void* processing_thread(void *arg) { mt1 = millis_since_boot(); s->model_bufs[ui_idx] = model_eval_frame(&s->model, q, yuv_cl, s->yuv_width, s->yuv_height, - model_transform, img_sock_raw); + model_transform, img_sock_raw, NULL); mt2 = millis_since_boot(); - model_publish(model_sock_raw, frame_id, model_transform, s->model_bufs[ui_idx]); + model_publish(model_sock_raw, frame_id, s->model_bufs[ui_idx], frame_data.timestamp_eof); } @@ -1073,6 +1077,8 @@ void* processing_thread(void *arg) { posenetd.setTransStd(trans_std_vs); kj::ArrayPtr rot_std_vs(&s->posenet.output[9], 3); posenetd.setRotStd(rot_std_vs); + posenetd.setTimestampEof(frame_data.timestamp_eof); + posenetd.setFrameId(frame_id); auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes();