diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 72e1845d7e124f..823df3a097053e 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,21 +1,2 @@ -Choose one of the templates below: - -# Fingerprint -This pull requests adds a fingerprint for . - -This is an explorer link to a drive with the stock system enabled: ... - -# Car support -This pull requests adds support for . - -This is an explorer link to a drive with the stock system enabled: ... -This is an explorer link to a drive with openpilot system enabled: ... - -# Feature -This pull requests adds feature X - -## Description -Explain what the feature does - -## Testing -Explain how the feature was tested. Either by the added unit tests, or what tests were performed while driving. +## discription +Please make a PR against the dev branch usually called something like 075-clean then remove this sentence and add a discription diff --git a/.media/op_edit.gif b/.media/op_edit.gif new file mode 100644 index 00000000000000..ac9503ce4f4aaa Binary files /dev/null and b/.media/op_edit.gif differ diff --git a/README.md b/README.md index 53d8dbd78f4dce..64e602c4183cfa 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ This README describes the custom features build by me (Arne Schwarck) on top of For a demo of this version of ArnePilot check the video below: [![demo of ArnePilot with this branch](https://img.youtube.com/vi/WKwSq8TPdpo/0.jpg)](https://www.youtube.com/playlist?list=PL3CGUyxys8DuTE1JTkdZwY93ejSfAGxyV) -Find me on Discord https://discord.gg/R5YtuVB +Find me on Discord https://discord.gg/Ebgn8Mr # Installation Put this URL in the custom URL field after uninstalling through the UI @@ -14,7 +14,6 @@ or if you want to use the command line or https://github.com/jfrux/workbench `cd /data; rm -rf openpilot; git clone https://github.com/arne182/openpilot; git checkout release4; reboot` still have trouble ?? More info about how to install this fork can be found [here](https://medium.com/@jfrux/comma-eon-installing-a-fork-of-openpilot-5c2b5c134b4b). - ## Panda flashing This is done automatically otherwise run (pkill -f boardd; cd /data/openpilot/panda/board; make; reboot) to change the following: @@ -23,6 +22,9 @@ This is done automatically otherwise run (pkill -f boardd; cd /data/openpilot/pa - adapting lane departure warning where it gives you a slight push back into the middle of the lane without needing to be engaged (not yet complete) - The Panda version is also changed and checked. +## opEdit Demo + + ## Branches `release4`: this is the default branch that is most up to date with the ArnePilot 0.7 release branch. Normally you should use this branch because it has been tested and verified that it is fully working without any issues. @@ -72,7 +74,7 @@ This is done automatically otherwise run (pkill -f boardd; cd /data/openpilot/pa - Live speedlimit_offset in op_tune.py - If the model detect's cut in it will draw two different chevron to show the user that it see's both of the car. - Control 3 gas profiles with sport eco and normal buttons on car ( only for toyota). -- [Dynamic distance profiles](https://github.com/ShaneSmiskol/ArnePilot/tree/stock_additions-devel#dynamic-follow-3-profiles) from Shane (In other word three different dynamic profiles: `close`, `normal`, `far`). Profile can be adjusted from either `python /data/ArnePilot/op_edit.py` or use live tuner to change the profile live (can take up to 4 sec to for new profile to be adjusted) `python /data/ArnePilot/op_tune.py`. +- [Dynamic distance profiles](https://github.com/ShaneSmiskol/ArnePilot/tree/stock_additions-devel#dynamic-follow-3-profiles) from Shane (In other word three different dynamic profiles: `close`, `normal`, `far`, `auto`). Profile can be adjusted from either `python /data/ArnePilot/op_edit.py` or use live tuner to change the profile live (can take up to 4 sec to for new profile to be adjusted) `python /data/ArnePilot/op_tune.py`. - Dynamic Follow Button: Now you can change the Dynamic Follow Distance just by tapping the blue button on the bottom right. - [Dynamic Gas:](https://github.com/ShaneSmiskol/ArnePilot/tree/stock_additions-devel#dynamic-gas) This aims to provide a smoother driving experience in stop and go traffic (under 20 mph) by modifying the maximum gas that can be applied based on your current velocity and the relative velocity of the lead car. It'll also of course increase the maximum gas when the lead is accelerating to help you get up to speed quicker than stock. And smoother; this eliminates the jerking you get from stock ArnePilot with comma pedal. It tries to coast if the lead is only moving slowly, it doesn't use maximum gas as soon as the lead inches forward :). When you are above 20 mph, relative velocity and the following distance is taken into consideration. @@ -80,7 +82,7 @@ This aims to provide a smoother driving experience in stop and go traffic (under - Added ability to turn on and off RSA at certain speed. `python /data/ArnePilot/op_edit.py` - Easily view the EON's IP Address.Just look at the sidebar right under wifi singal strength's. - Battery has percentage instead of the battery icon. -- [WIP] Traffic light detection from Littlemountainman, shane and brain. To get accurate result make sure your daily commute/area is mapped on [OSM](openstreetmap.org) with right direaction. [For example](https://imgur.com/purBVpd)... [still dont get it watch the video by mageymoo1](https://youtu.be/7dPaF0tDb7Y). +- [WIP] Traffic light detection from Littlemountainman, shane and brain. To get accurate result make sure your daily commute/area is mapped on [OSM](openstreetmap.org) with right direaction. [For example](https://imgur.com/purBVpd)... [still dont get it watch the video by mageymoo1](https://youtu.be/7dPaF0tDb7Y). - We also have enabled commas e2e model which will only work between 11 MPH to 29 MPH. Commas e2e model helps slows down for traffic light, stop sign, etc. e2e, traffic model and mapd all works together to help you stop at the light. All of this can be turned off via `/data/openpilot/op_edit.py`. - Loggin has been Disabled by default on this fork. If you would like to record your drive edit the [following line](https://github.com/arne182/ArnePilot/blob/4d66df96a91c9c13491a3d78b9c1c2a9e848724a/selfdrive/manager.py#L480) - Smart speed (smart speed is essentially speedlimit which eon will not go over unless you have set custom offset) can be overridden by pressing gas above the current smart speed. diff --git a/cereal/arne182.capnp b/cereal/arne182.capnp index 189ba1392f91cb..2bb509a24007d6 100644 --- a/cereal/arne182.capnp +++ b/cereal/arne182.capnp @@ -147,6 +147,11 @@ struct DynamicFollowButton { status @0 :UInt16; } +struct DynamicFollowData { + mpcTR @0 :Float32; + profilePred @1 :UInt16; +} + struct IPAddress { ipAddr @0 :Text; # dragonpilot } @@ -177,5 +182,6 @@ struct EventArne182 { ipAddress @10 :IPAddress; trafficModelRaw @11: TrafficModelRaw; trafficModelEvent @12: TrafficModelEvent; + dynamicFollowData @13 :DynamicFollowData; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index f9b7c4cf3d7194..414cac6529af9b 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -99,6 +99,7 @@ smiskolData: [8216, false, 20.] dynamicFollowButton: [8217, false, 0.] trafficModelEvent: [8218, false, 5.] trafficModelRaw: [8219, false, 5.] +dynamicFollowData: [8220, false, 20.] # 8080 is reserved for slave testing daemon # 8762 is reserved for logserver diff --git a/common/op_params.py b/common/op_params.py index b554867e53c2fd..60410f39988e8a 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -1,47 +1,36 @@ #!/usr/bin/env python3 import os import json -import time -import string -import random from common.travis_checker import travis - - -def write_params(params, params_file): - if not travis: - with open(params_file, "w") as f: - json.dump(params, f, indent=2, sort_keys=True) - os.chmod(params_file, 0o764) - - -def read_params(params_file, default_params): - try: - with open(params_file, "r") as f: - params = json.load(f) - return params, True - except Exception as e: - print(e) - params = default_params - return params, False +try: + from common.realtime import sec_since_boot +except ImportError: + import time + sec_since_boot = time.time + print("opParams WARNING: using python time.time() instead of faster sec_since_boot") class KeyInfo: + default = None + allowed_types = [] + is_list = False has_allowed_types = False live = False has_default = False has_description = False + hidden = False class opParams: def __init__(self): """ To add your own parameter to opParams in your fork, simply add a new dictionary entry with the name of your parameter and its default value to save to new users' op_params.json file. - The description, allowed_types, and live keys are no longer required but recommended to help users edit their parameters with opEdit and opTune correctly. - - The description value will be shown to users when they use opEdit or opTune to change the value of the parameter. + The description, allowed_types, and live keys are no longer required but recommended to help users edit their parameters with opEdit correctly. + - The description value will be shown to users when they use opEdit to change the value of the parameter. - The allowed_types key is used to restrict what kinds of values can be entered with opEdit so that users can't reasonably break the fork with unintended behavior. Limiting the range of floats or integers is still recommended when `.get`ting the parameter. When a None value is allowed, use `type(None)` instead of None, as opEdit checks the type against the values in the key with `isinstance()`. - - Finally, the live key tells both opParams and opTune that it's a live parameter that will change. Therefore, you must place the `op_params.get()` call in the update function so that it can update. + - Finally, the live key tells both opParams and opEdit that it's a live parameter that will change. Therefore, you must place the `op_params.get()` call in the update function so that it can update. Here's an example of the minimum required dictionary: self.default_params = {'camera_offset': {'default': 0.06}} @@ -54,17 +43,22 @@ def __init__(self): 'curvature_factor': {'default': 1.0, 'allowed_types': [float, int], 'description': 'Multiplier for the curvature slowdown. Increase for less braking.', 'live': False}, 'cloak': {'default': True, 'allowed_types': [bool], 'description': "make comma believe you are on their fork", 'live': False}, 'default_brake_distance': {'default': 250.0, 'allowed_types': [float, int], 'description': 'Distance in m to start braking for mapped speeds.', 'live': False}, - 'dynamic_follow': {'default': 'normal', 'allowed_types': [str], + 'dynamic_follow': {'default': 'auto', 'allowed_types': [str], 'description': "Can be: ('close', 'normal', 'far'): Left to right increases in following distance.\n" "All profiles support dynamic follow so you'll get your preferred distance while\n" "retaining the smoothness and safety of dynamic follow!", 'live': True}, 'force_pedal': {'default': False, 'allowed_types': [bool], 'description': "If openpilot isn't recognizing your comma pedal, set this to True", 'live': False}, + 'global_df_mod': {'default': None, 'allowed_types': [type(None), float, int], 'description': 'The modifer for the current distance used by dynamic follow. The range is limited from 0.7 to 1.1\n' + 'Smaller values will get you closer, larger will get you farther\n' + 'This is multiplied by any profile that\'s active. Set to None to disable', 'live': True}, + 'hide_auto_df_alerts': {'default': True, 'allowed_types': [bool], 'description': 'Hides the alert that shows what profile the model has chosen'}, 'keep_openpilot_engaged': {'default': True, 'allowed_types': [bool], 'description': 'True is stock behavior in this fork. False lets you use the brake and cruise control stalk to disengage as usual', 'live': False}, 'limit_rsa': {'default': False, 'allowed_types': [bool], 'description': "Switch off RSA above rsa_max_speed", 'live': False}, - 'offset_limit': {'default': 0, 'allowed_types': [float, int], 'description': 'Speed at which apk percent offset will work in m/s', 'live': False}, 'mpc_offset': {'default': 5.0, 'allowed_types': [float, int], 'description': 'Offset model braking by how many m/s. Lower numbers equals more model braking', 'live': True}, + 'offset_limit': {'default': 0, 'allowed_types': [float, int], 'description': 'Speed at which apk percent offset will work in m/s', 'live': False}, 'osm': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use OSM for drives', 'live': False}, + 'op_edit_live_mode': {'default': False, 'allowed_types': [bool], 'description': 'This parameter controls which mode opEdit starts in. It should be hidden from the user with the hide key', 'hide': True}, 'rolling_stop': {'default': False, 'allowed_types': [bool], 'description': 'If you do not want stop signs to go down to 0 kph enable this for 9kph slow down', 'live': False}, 'rsa_max_speed': {'default': 24.5, 'allowed_types': [float, int], 'description': 'Speed limit to ignore RSA in m/s', 'live': False}, 'smart_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use Smart Speed for drives above smart_speed_max_vego', 'live': False}, @@ -78,115 +72,120 @@ def __init__(self): self.params = {} self.params_file = "/data/op_params.json" - self.kegman_file = "/data/kegman.json" - self.last_read_time = time.time() - self.read_frequency = 5.0 # max frequency to read with self.get(...) (sec) + self.last_read_time = sec_since_boot() + self.read_frequency = 2.5 # max frequency to read with self.get(...) (sec) self.force_update = False # replaces values with default params if True, not just add add missing key/value pairs - self.to_delete = ['dynamic_lane_speed', 'longkiV', 'following_distance', 'static_steer_ratio'] + self.to_delete = ['reset_integral', 'log_data'] # a list of params you want to delete (unused) self.run_init() # restores, reads, and updates params - def create_id(self): # creates unique identifier to send with sentry errors. please update uniqueID with op_edit.py to your username! - need_id = False - if "uniqueID" not in self.params: - need_id = True - if "uniqueID" in self.params and self.params["uniqueID"] is None: - need_id = True - if need_id: - random_id = ''.join([random.choice(string.ascii_lowercase + string.ascii_uppercase + string.digits) for i in range(15)]) - self.params["uniqueID"] = random_id - - def add_default_params(self): - prev_params = dict(self.params) - if not travis: - self.create_id() - for key in self.default_params: - if self.force_update: - self.params[key] = self.default_params[key]['default'] - elif key not in self.params: - self.params[key] = self.default_params[key]['default'] - return prev_params == self.params - - def format_default_params(self): - return {key: self.default_params[key]['default'] for key in self.default_params} - - def run_init(self): # does first time initializing of default params, and/or restoring from kegman.json + def run_init(self): # does first time initializing of default params if travis: - self.params = self.format_default_params() + self.params = self._format_default_params() return - self.params = self.format_default_params() # in case any file is corrupted + + self.params = self._format_default_params() # in case any file is corrupted + to_write = False - no_params = False if os.path.isfile(self.params_file): - self.params, read_status = read_params(self.params_file, self.format_default_params()) - if read_status: - to_write = not self.add_default_params() # if new default data has been added - if self.delete_old(): # or if old params have been deleted - to_write = True - else: # don't overwrite corrupted params, just print to screen - print("ERROR: Can't read op_params.json file") - elif os.path.isfile(self.kegman_file): - to_write = True # write no matter what - try: - with open(self.kegman_file, "r") as f: # restore params from kegman - self.params = json.load(f) - self.add_default_params() - except: - print("ERROR: Can't read kegman.json file") + if self._read(): + to_write = not self._add_default_params() # if new default data has been added + to_write = self._delete_old or to_write # or if old params have been deleted + else: # don't overwrite corrupted params, just print + print("opParams ERROR: Can't read op_params.json file") else: - no_params = True # user's first time running a fork with kegman_conf or op_params - if to_write or no_params: - write_params(self.params, self.params_file) - - def delete_old(self): - prev_params = dict(self.params) - for i in self.to_delete: - if i in self.params: - del self.params[i] - return prev_params == self.params + to_write = True # user's first time running a fork with op_params, write default params - def put(self, key, value): - self.params.update({key: value}) - write_params(self.params, self.params_file) + if to_write: + self._write() + os.chmod(self.params_file, 0o764) def get(self, key=None, default=None, force_update=False): # can specify a default value if key doesn't exist - self.update_params(key, force_update) if key is None: - return self.params + return self._get_all() + key_info = self.key_info(key) + self._update_params(key_info, force_update) if key in self.params: - key_info = self.get_key_info(key) if key_info.has_allowed_types: value = self.params[key] - allowed_types = self.default_params[key]['allowed_types'] - valid_type = type(value) in allowed_types - if not valid_type: - if key_info.has_default: # if value in op_params.json is not correct type, use default - value = self.default_params[key]['default'] - else: # else use a standard value based on type (last resort to keep openpilot running) - value = self.value_from_types(allowed_types) + if type(value) in key_info.allowed_types: + return value # all good, returning user's value + + print('opParams WARNING: User\'s value is not valid!') + if key_info.has_default: # invalid value type, try to use default value + if type(key_info.default) in key_info.allowed_types: # actually check if the default is valid + # return default value because user's value of key is not in the allowed_types to avoid crashing openpilot + return key_info.default + + return self._value_from_types(key_info.allowed_types) # else use a standard value based on type (last resort to keep openpilot running if user's value is of invalid type) else: - value = self.params[key] - else: - value = default + return self.params[key] # no defined allowed types, returning user's value - return value + return default # not in params - def get_key_info(self, key): + def put(self, key, value): + self.params.update({key: value}) + self._write() + + def delete(self, key): + if key in self.params: + del self.params[key] + self._write() + + def key_info(self, key): key_info = KeyInfo() + if key is None or key not in self.default_params: + return key_info if key in self.default_params: if 'allowed_types' in self.default_params[key]: allowed_types = self.default_params[key]['allowed_types'] if isinstance(allowed_types, list) and len(allowed_types) > 0: key_info.has_allowed_types = True - if 'live' in self.default_params[key] and self.default_params[key]['live'] is True: - key_info.live = True + key_info.allowed_types = list(allowed_types) + if list in [type(typ) for typ in allowed_types]: + key_info.is_list = True + key_info.allowed_types.remove(list) + key_info.allowed_types = key_info.allowed_types[0] + + if 'live' in self.default_params[key]: + key_info.live = self.default_params[key]['live'] + if 'default' in self.default_params[key]: key_info.has_default = True - if 'description' in self.default_params[key]: - key_info.has_description = True + key_info.default = self.default_params[key]['default'] + + key_info.has_description = 'description' in self.default_params[key] + + if 'hide' in self.default_params[key]: + key_info.hidden = self.default_params[key]['hide'] + return key_info - def value_from_types(self, allowed_types): + def _add_default_params(self): + prev_params = dict(self.params) + for key in self.default_params: + if self.force_update: + self.params[key] = self.default_params[key]['default'] + elif key not in self.params: + self.params[key] = self.default_params[key]['default'] + return prev_params == self.params + + def _format_default_params(self): + return {key: self.default_params[key]['default'] for key in self.default_params} + + @property + def _delete_old(self): + deleted = False + for param in self.to_delete: + if param in self.params: + del self.params[param] + deleted = True + return deleted + + def _get_all(self): # returns all non-hidden params + return {k: v for k, v in self.params.items() if not self.key_info(k).hidden} + + def _value_from_types(self, allowed_types): if list in allowed_types: return [] elif float in allowed_types or int in allowed_types: @@ -197,16 +196,24 @@ def value_from_types(self, allowed_types): return '' return None # unknown type - def update_params(self, key, force_update): - if force_update or self.get_key_info(key).live: # if is a live param, we want to get updates while openpilot is running - if not travis and time.time() - self.last_read_time >= self.read_frequency: # make sure we aren't reading file too often - self.params, read_status = read_params(self.params_file, self.format_default_params()) - if not read_status: - time.sleep(1/100.) - self.params, _ = read_params(self.params_file, self.format_default_params()) # if the file was being written to, retry once - self.last_read_time = time.time() - - def delete(self, key): - if key in self.params: - del self.params[key] - write_params(self.params, self.params_file) + def _update_params(self, key_info, force_update): + if force_update or key_info.live: # if is a live param, we want to get updates while openpilot is running + if not travis and (sec_since_boot() - self.last_read_time >= self.read_frequency or force_update): # make sure we aren't reading file too often + if self._read(): + self.last_read_time = sec_since_boot() + + def _read(self): + try: + with open(self.params_file, "r") as f: + params = f.read() + self.params = json.loads(params) + return True + except Exception as e: + print('opParams ERROR: {}'.format(e)) + self.params = self._format_default_params() + return False + + def _write(self): + if not travis: + with open(self.params_file, "w") as f: + f.write(json.dumps(self.params, indent=2)) # can further speed it up by remove indentation but makes file hard to read diff --git a/op_edit.py b/op_edit.py index edbb2106b34bbc..acbfc9b6497615 100755 --- a/op_edit.py +++ b/op_edit.py @@ -1,147 +1,301 @@ #!/usr/bin/env python3 -from common.op_params import opParams import time +from common.op_params import opParams import ast +import difflib + + +class STYLES: + # HEADER = '\033[95m' + # OKBLUE = '\033[94m' + # CBLUE = '\33[44m' + # BOLD = '\033[1m' + OKGREEN = '\033[92m' + CWHITE = '\33[37m' + ENDC = '\033[0m' + CWHITE + UNDERLINE = '\033[4m' + + RED = '\033[91m' + PURPLE_BG = '\33[45m' + YELLOW = '\033[93m' + + FAIL = RED + INFO = PURPLE_BG + SUCCESS = OKGREEN + PROMPT = YELLOW + CYAN = '\033[36m' class opEdit: # use by running `python /data/openpilot/op_edit.py` def __init__(self): self.op_params = opParams() self.params = None - self.sleep_time = 1.0 - self.live_tuning = False - self.welcome() + self.sleep_time = 0.75 + self.live_tuning = self.op_params.get('op_edit_live_mode', False) + self.username = self.op_params.get('username', None) + + self.last_choice = None + + self.run_init() + + def run_init(self): + if self.username is None: + self.success('\nWelcome to the opParams command line editor!', sleep_time=0) + self.prompt('Parameter \'username\' is missing! Would you like to add your Discord username for easier crash debugging?') + + username_choice = self.input_with_options(['Y', 'n', 'don\'t ask again'], default='n')[0] + if username_choice == 0: + self.prompt('Please enter your Discord username so the developers can reach out if a crash occurs:') + username = '' + while username == '': + username = input('>> ').strip() + self.success('Thanks! Saved your Discord username\n' + 'Edit the \'username\' parameter at any time to update', sleep_time=3.0) + self.op_params.put('username', username) + self.username = username + elif username_choice == 2: + self.op_params.put('username', False) + self.info('Got it, bringing you into opEdit\n' + 'Edit the \'username\' parameter at any time to update', sleep_time=3.0) + else: + self.success('\nWelcome to the opParams command line editor, {}!'.format(self.username), sleep_time=0) - def welcome(self): - print('Welcome to the opParams command line editor!') - while True: - print('Would you like to enter opEdit\'s live-tuning mode?') - choice = input('[Y/n]: ').lower().strip() - if choice in ['y', 'ye', 'yes']: - self.live_tuning = True - self.run_loop() - break - elif choice in ['n', 'no']: - self.run_loop() - break - elif choice in ['e', 'exit']: - break + self.run_loop() def run_loop(self): - if not self.live_tuning: - print('Here are your parameters:\n') - else: - print('Here are your live parameters:\n') while True: + if not self.live_tuning: + self.info('Here are your parameters:', end='\n', sleep_time=0) + else: + self.info('Here are your live parameters:', end='\n', sleep_time=0) self.params = self.op_params.get(force_update=True) if self.live_tuning: # only display live tunable params - self.params = {k: v for k, v in self.params.items() if self.op_params.get_key_info(k).live} + self.params = {k: v for k, v in self.params.items() if self.op_params.key_info(k).live} values_list = [self.params[i] if len(str(self.params[i])) < 20 else '{} ... {}'.format(str(self.params[i])[:30], str(self.params[i])[-15:]) for i in self.params] - live = ['(live!)' if self.op_params.get_key_info(i).live else '' for i in self.params] + live = ['(live!)' if self.op_params.key_info(i).live else '' for i in self.params] + + to_print = [] + for idx, param in enumerate(self.params): + line = '{}. {}: {} {}'.format(idx + 1, param, values_list[idx], live[idx]) + if idx == self.last_choice and self.last_choice is not None: + line = STYLES.OKGREEN + line + else: + line = STYLES.CYAN + line + to_print.append(line) - to_print = ['{}. {}: {} {}'.format(idx + 1, i, values_list[idx], live[idx]) for idx, i in enumerate(self.params)] - to_print.append('\n{}. Add new parameter!'.format(len(self.params) + 1)) - to_print.append('{}. Delete parameter!'.format(len(self.params) + 2)) + extras = {'a': 'Add new parameter', + 'd': 'Delete parameter', + 'l': 'Toggle live tuning', + 'e': 'Exit opEdit'} + to_print += ['---'] + ['{}. {}'.format(e, extras[e]) for e in extras] print('\n'.join(to_print)) - print('\nChoose a parameter to explore (by integer index): ') + self.prompt('\nChoose a parameter to edit (by index or name):') - choice = input('>> ').strip() - parsed, choice = self.parse_choice(choice) + choice = input('>> ').strip().lower() + parsed, choice = self.parse_choice(choice, len(to_print) - len(extras)) if parsed == 'continue': continue elif parsed == 'add': self.add_parameter() elif parsed == 'change': + self.last_choice = choice self.change_parameter(choice) elif parsed == 'delete': self.delete_parameter() - elif parsed == 'error': + elif parsed == 'live': + self.last_choice = None + self.live_tuning = not self.live_tuning + self.op_params.put('op_edit_live_mode', self.live_tuning) # for next opEdit startup + elif parsed == 'exit': return - def parse_choice(self, choice): + def parse_choice(self, choice, opt_len): if choice.isdigit(): choice = int(choice) choice -= 1 - elif choice == '': - print('Exiting opEdit!') - return 'error', choice - else: - self.message('Not an integer!') - return 'retry', choice - if choice not in range(0, len(self.params) + 2): # three for add/delete parameter - self.message('Not in range!') - return 'continue', choice + if choice not in range(opt_len): # number of options to choose from + self.error('Not in range!') + return 'continue', choice + return 'change', choice - if choice == len(self.params): # add new parameter + if choice in ['a', 'add']: # add new parameter return 'add', choice - - if choice == len(self.params) + 1: # delete parameter + elif choice in ['d', 'delete', 'del']: # delete parameter return 'delete', choice - - return 'change', choice + elif choice in ['l', 'live']: # live tuning mode + return 'live', choice + elif choice in ['exit', 'e', '']: + self.error('Exiting opEdit!', sleep_time=0) + return 'exit', choice + else: # find most similar param to user's input + param_sims = [(idx, self.str_sim(choice, param.lower())) for idx, param in enumerate(self.params)] + param_sims = [param for param in param_sims if param[1] > 0.5] + if len(param_sims) > 0: + chosen_param = sorted(param_sims, key=lambda param: param[1], reverse=True)[0] + return 'change', chosen_param[0] # return idx + + self.error('Invalid choice!') + return 'continue', choice + + def str_sim(self, a, b): + return difflib.SequenceMatcher(a=a, b=b).ratio() def change_parameter(self, choice): while True: chosen_key = list(self.params)[choice] - key_info = self.op_params.get_key_info(chosen_key) + key_info = self.op_params.key_info(chosen_key) old_value = self.params[chosen_key] - print('Chosen parameter: {}'.format(chosen_key)) + self.info('Chosen parameter: {}'.format(chosen_key), sleep_time=0) to_print = [] if key_info.has_description: - to_print.append('>> Description: {}'.format(self.op_params.default_params[chosen_key]['description'].replace('\n', '\n > '))) + to_print.append(STYLES.OKGREEN + '>> Description: {}'.format(self.op_params.default_params[chosen_key]['description'].replace('\n', '\n > ')) + STYLES.ENDC) if key_info.has_allowed_types: - allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] - to_print.append('>> Allowed types: {}'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) + to_print.append(STYLES.RED + '>> Allowed types: {}'.format(', '.join([i.__name__ for i in key_info.allowed_types])) + STYLES.ENDC) if key_info.live: - to_print.append('>> This parameter supports live tuning! Updates should take affect within 5 seconds.\n') + to_print.append(STYLES.YELLOW + '>> This parameter supports live tuning! Updates should take affect within 5 seconds' + STYLES.ENDC) if to_print: print('\n{}\n'.format('\n'.join(to_print))) - print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) - if key_info.live or self.live_tuning: # similar to opTune - while True: - print('Enter your new value:') - new_value = input('>> ').strip() - if new_value == '': - self.message('Exiting this parameter...') - return + if key_info.is_list: + self.change_param_list(old_value, key_info, chosen_key) # TODO: need to merge the code in this function with the below to reduce redundant code + return - new_value = self.parse_input(new_value) - if key_info.has_allowed_types and type(new_value) not in allowed_types: - self.message('The type of data you entered ({}) is not allowed with this parameter!'.format(str(type(new_value)).split("'")[1])) - continue + self.info('Current value: {} (type: {})'.format(old_value, type(old_value).__name__), sleep_time=0) - self.op_params.put(chosen_key, new_value) - print('Saved {} with value: {}! (type: {})\n'.format(chosen_key, new_value, str(type(new_value)).split("'")[1])) - else: - print('Enter your new value:') + while True: + self.prompt('\nEnter your new value:') new_value = input('>> ').strip() if new_value == '': - self.message('Exiting this parameter...') + self.info('Exiting this parameter...', 0.5) return - new_value = self.parse_input(new_value) - if key_info.has_allowed_types and type(new_value) not in allowed_types: - self.message('The type of data you entered ({}) is not allowed with this parameter!'.format(str(type(new_value)).split("'")[1])) + new_value = self.str_eval(new_value) + if key_info.has_allowed_types and type(new_value) not in key_info.allowed_types: + self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) continue - print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) - print('New value: {} (type: {})'.format(new_value, str(type(new_value)).split("'")[1])) - print('Do you want to save this?') - choice = input('[Y/n]: ').lower().strip() - if choice == 'y': + if key_info.live: # stay in live tuning interface self.op_params.put(chosen_key, new_value) - self.message('Saved!') - else: - self.message('Not saved!') + self.success('Saved {} with value: {}! (type: {})'.format(chosen_key, new_value, type(new_value).__name__)) + else: # else ask to save and break + print('\nOld value: {} (type: {})'.format(old_value, type(old_value).__name__)) + print('New value: {} (type: {})'.format(new_value, type(new_value).__name__)) + self.prompt('\nDo you want to save this?') + if self.input_with_options(['Y', 'n'], 'n')[0] == 0: + self.op_params.put(chosen_key, new_value) + self.success('Saved!') + else: + self.info('Not saved!', sleep_time=0) + return + + def change_param_list(self, old_value, key_info, chosen_key): + while True: + self.info('Current value: {} (type: {})'.format(old_value, type(old_value).__name__), sleep_time=0) + self.prompt('\nEnter index to edit (0 to {}):'.format(len(old_value) - 1)) + choice_idx = self.str_eval(input('>> ')) + if choice_idx == '': + self.info('Exiting this parameter...', 0.5) return - def parse_input(self, dat): + if not isinstance(choice_idx, int) or choice_idx not in range(len(old_value)): + self.error('Must be an integar within list range!') + continue + + while True: + self.info('Chosen index: {}'.format(choice_idx), sleep_time=0) + self.info('Value: {} (type: {})'.format(old_value[choice_idx], type(old_value[choice_idx]).__name__), sleep_time=0) + self.prompt('\nEnter your new value:') + new_value = input('>> ').strip() + if new_value == '': + self.info('Exiting this list item...', 0.5) + break + + new_value = self.str_eval(new_value) + if key_info.has_allowed_types and type(new_value) not in key_info.allowed_types: + self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) + continue + + old_value[choice_idx] = new_value + + self.op_params.put(chosen_key, old_value) + self.success('Saved {} with value: {}! (type: {})'.format(chosen_key, new_value, type(new_value).__name__), end='\n') + break + + def cyan(self, msg, end=''): + msg = self.str_color(msg, style='cyan') + # print(msg, flush=True, end='\n' + end) + return msg + + def prompt(self, msg, end=''): + msg = self.str_color(msg, style='prompt') + print(msg, flush=True, end='\n' + end) + + def info(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='info') + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def error(self, msg, sleep_time=None, end='', surround=True): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='fail', surround=surround) + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def success(self, msg, sleep_time=None, end=''): + if sleep_time is None: + sleep_time = self.sleep_time + msg = self.str_color(msg, style='success', surround=False, underline=False) + + print(msg, flush=True, end='\n' + end) + time.sleep(sleep_time) + + def str_color(self, msg, style, surround=False, underline=False): + if style == 'success': + style = STYLES.SUCCESS + elif style == 'fail': + style = STYLES.FAIL + elif style == 'prompt': + style = STYLES.PROMPT + elif style == 'info': + style = STYLES.INFO + elif style == 'cyan': + style = STYLES.CYAN + + if underline: + underline = STYLES.UNDERLINE + else: + underline = '' + + if surround: + msg = '{}--------\n{}{}\n{}--------{}'.format(style, underline, msg, STYLES.ENDC + style, STYLES.ENDC) + else: + msg = '{}{}{}'.format(style + underline, msg, STYLES.ENDC) + + return msg + + def input_with_options(self, options, default=None): + """ + Takes in a list of options and asks user to make a choice. + The most similar option list index is returned along with the similarity percentage from 0 to 1 + """ + user_input = input('[{}]: '.format('/'.join(options))).lower().strip() + if not user_input: + return default, 0.0 + sims = [self.str_sim(i.lower().strip(), user_input) for i in options] + argmax = sims.index(max(sims)) + return argmax, sims[argmax] + + def str_eval(self, dat): dat = dat.strip() try: dat = ast.literal_eval(dat) @@ -156,65 +310,55 @@ def parse_input(self, dat): def delete_parameter(self): while True: - print('Enter the name of the parameter to delete:') - key = input('>> ').lower() - key = self.parse_input(key) + self.prompt('Enter the name of the parameter to delete:') + key = self.str_eval(input('>> ')) if key == '': return if not isinstance(key, str): - self.message('Input must be a string!') + self.error('Input must be a string!') continue if key not in self.params: - self.message("Parameter doesn't exist!") + self.error("Parameter doesn't exist!") continue value = self.params.get(key) print('Parameter name: {}'.format(key)) - print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) - print('Do you want to delete this?') + print('Parameter value: {} (type: {})'.format(value, type(value).__name__)) + self.prompt('Do you want to delete this?') - choice = input('[Y/n]: ').lower().strip() - if choice == 'y': + if self.input_with_options(['Y', 'n'], default='n')[0] == 0: self.op_params.delete(key) - self.message('Deleted!') + self.success('Deleted!') else: - self.message('Not saved!') + self.info('Not deleted!') return def add_parameter(self): while True: - print('Type the name of your new parameter:') - key = input('>> ').strip() + self.prompt('Type the name of your new parameter:') + key = self.str_eval(input('>> ')) + if key == '': return - - key = self.parse_input(key) - if not isinstance(key, str): - self.message('Input must be a string!') + self.error('Input must be a string!') continue - print("Enter the data you'd like to save with this parameter:") + self.prompt("Enter the data you'd like to save with this parameter:") value = input('>> ').strip() - value = self.parse_input(value) + value = self.str_eval(value) print('Parameter name: {}'.format(key)) - print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) - print('Do you want to save this?') + print('Parameter value: {} (type: {})'.format(value, type(value).__name__)) + self.prompt('Do you want to save this?') - choice = input('[Y/n]: ').lower().strip() - if choice == 'y': + if self.input_with_options(['Y', 'n'], default='n')[0] == 0: self.op_params.put(key, value) - self.message('Saved!') + self.success('Saved!') else: - self.message('Not saved!') + self.info('Not saved!') return - def message(self, msg): - print('--------\n{}\n--------'.format(msg), flush=True) - time.sleep(self.sleep_time) - print() - opEdit() diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index adfd1d44ca1f5d..ee65da3f5badf9 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -88,7 +88,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of brake press if (addr == 916) { bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { + if (brake_pressed && (!brake_pressed_prev || vehicle_moving) && !unsafe_mode) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 5cd47a29f2ba71..14e934563f26da 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -9,7 +9,7 @@ class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - self.pcm_acc_active = False + #self.pcm_acc_active = False can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["GEAR"]['PRNDL'] @@ -46,9 +46,10 @@ def update(self, cp, cp_cam): ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE'] ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE'] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None)) - if cp.vl["ACC_2"]['ACC_STATUS_2'] == 7: # ACC is green. - self.pcm_acc_active = True - ret.cruiseState.enabled = self.pcm_acc_active + ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green. + #if cp.vl["ACC_2"]['ACC_STATUS_2'] == 7: # ACC is green. + # self.pcm_acc_active = True + #ret.cruiseState.enabled = self.pcm_acc_active ret.cruiseState.available = ret.cruiseState.enabled # FIXME: for now same as enabled ret.cruiseState.speed = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH'] * CV.KPH_TO_MS diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index dd6443b4b161e3..adcf5166f3beb5 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -196,7 +196,10 @@ def update(self, cp, cp_cam, frame): else: self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] - self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2 + if self.CP.carFingerprint == CAR.COROLLAH_TSS2: + self.low_speed_lockout = False + else: + self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2 ret.cruiseState.available = self.main_on v_cruise_pcm_max = ret.cruiseState.speed if self.CP.carFingerprint in TSS2_CAR: @@ -383,7 +386,7 @@ def get_can_parser_init(CP): signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) checks.append(("PCM_CRUISE_2", 33)) - if CP.carFingerprint in [CAR.COROLLAH_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_UXH_TSS2]: + if CP.carFingerprint in [CAR.COROLLAH_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_UXH_TSS2, CAR.CHRH]: signals.append(("SPORT_ON", "GEAR_PACKET2", 0)) signals.append(("ECON_ON", "GEAR_PACKET2", 0)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 42e1563c30089a..547a34a193bed5 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -27,18 +27,20 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 - #if ret.enableGasInterceptor: - # ret.gasMaxBP = [0., 9., 55] - # ret.gasMaxV = [0.2, 0.5, 0.7] - # ret.longitudinalTuning.kpV = [0.5, 0.4, 0.3] # braking tune - # ret.longitudinalTuning.kiV = [0.135, 0.1] - #else: - ret.gasMaxBP = [0., 9., 55] - ret.gasMaxV = [0.2, 0.5, 0.7] - ret.longitudinalTuning.kpV = [0.7, 0.5, 0.325] # braking tune from rav4h - ret.longitudinalTuning.kiV = [0.15, 0.10] - - if candidate not in [CAR.PRIUS_2019, CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI + if ret.enableGasInterceptor: + ret.gasMaxBP = [0., 9., 55] + ret.gasMaxV = [0.2, 0.5, 0.7] + # ret.longitudinalTuning.kpV = [0.5, 0.4, 0.3] # braking tune, todo: test me vs. stock below + # ret.longitudinalTuning.kiV = [0.135, 0.1] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiV = [0.18, 0.12] + else: + ret.gasMaxBP = [0., 9., 55] + ret.gasMaxV = [0.2, 0.5, 0.7] + ret.longitudinalTuning.kpV = [0.39, 0.35, 0.325] # braking tune from rav4h + ret.longitudinalTuning.kiV = [0.19, 0.10] + + if candidate not in [CAR.PRIUS_2019, CAR.PRIUS, CAR.RAV4, CAR.RAV4H, CAR.COROLLA]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -103,8 +105,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] - ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.init('lqr') + + ret.lateralTuning.lqr.scale = 1500.0 + ret.lateralTuning.lqr.ki = 0.06 + + ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] + ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] + ret.lateralTuning.lqr.c = [1., 0.] + ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] + ret.lateralTuning.lqr.l = [0.3233671, 0.3185757] + ret.lateralTuning.lqr.dcGain = 0.002237852961363602 elif candidate == CAR.LEXUS_RX: stop_and_go = True @@ -192,7 +203,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]] + ret.longitudinalTuning.kpV = [0.2, 0.25, 0.325] + ret.longitudinalTuning.kiV = [0.10, 0.10] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13], [0.05]] ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00004 @@ -204,7 +217,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.7933 ret.longitudinalTuning.kpV = [0.2, 0.25, 0.325] ret.longitudinalTuning.kiV = [0.10, 0.10] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.12], [0.04]] ret.mass = 3800. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kf = 0.00004 @@ -215,7 +228,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.5], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 720a501e3c9824..e10124ac779b49 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -126,7 +126,7 @@ class ECU: {36: 8, 37: 8, 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, 614: 8, 643: 7, 658: 8, 705: 8, 740: 5, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 913: 8, 918: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8} ], CAR.CHRH: [ - {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 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: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 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, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 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: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2030: 8} ], CAR.CAMRY: [ {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, 513: 6, 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, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 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, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1767: 4, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1956: 8, 1961: 8, 1964: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} @@ -177,7 +177,7 @@ class ECU: {36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1116: 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, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} ], CAR.RAV4H_TSS2: [ - {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1800: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} + {36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1800: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1952: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} ], CAR.LEXUS_NXH: [ {36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 822: 8, 824: 8, 835: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 889: 8, 891: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 987: 8, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 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, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 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, 1656: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} @@ -419,6 +419,7 @@ class ECU: b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B12361\x00\x00\x00\x00\x00\x00', @@ -428,6 +429,7 @@ class ECU: b'\x018965B12470\x00\x00\x00\x00\x00\x00', b'\x018965B12500\x00\x00\x00\x00\x00\x00', b'\x018965B12530\x00\x00\x00\x00\x00\x00', + b'\x018965B12490\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152612590\x00\x00\x00\x00\x00\x00', @@ -438,6 +440,7 @@ class ECU: b'F152612840\x00\x00\x00\x00\x00\x00', b'F152612A10\x00\x00\x00\x00\x00\x00', b'F152642540\x00\x00\x00\x00\x00\x00', + b'F152612820\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -736,6 +739,7 @@ class ECU: b'\x018966342X6000\x00\x00\x00\x00', b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642541\x00\x00\x00\x00\x00\x00', @@ -749,6 +753,7 @@ class ECU: b'8965B42171\x00\x00\x00\x00\x00\x00', b'8965B42181\x00\x00\x00\x00\x00\x00', b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', + b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301200\x00\x00\x00\x00', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 937dba56bae716..b191eeec2dc5de 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -29,10 +29,12 @@ from selfdrive.locationd.calibration_helpers import Calibration, Filter #from common.travis_checker import travis from common.op_params import opParams -from selfdrive.controls.df_alert_manager import DfAlertManager +from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager op_params = opParams() +df_manager = dfManager(op_params) +hide_auto_df_alerts = op_params.get('hide_auto_df_alerts', False) traffic_light_alerts = op_params.get('traffic_light_alerts', True) #LANE_DEPARTURE_THRESHOLD = 0.1 @@ -129,7 +131,7 @@ def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter else: events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if CS.vEgo > 92 * CV.MPH_TO_MS: + if CS.vEgo > 160 * CV.KPH_TO_MS: events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # When the panda and controlsd do not agree on controls_allowed @@ -149,7 +151,7 @@ def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter return CS, events, cal_perc, mismatch_counter, can_error_counter, events_arne182 -def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182, arne_sm, df_alert_manager): +def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182, arne_sm): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) @@ -164,6 +166,17 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # decrease the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) + + df_out = df_manager.update() + if df_out.changed: + df_alert = 'dfButtonAlert' + if df_out.is_auto and df_out.last_is_auto: + if CS.cruiseState.enabled and not hide_auto_df_alerts: + df_alert += 'NoSound' + AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)', extra_text_2='Dynamic follow: {} profile active'.format(df_out.model_profile_text)) + else: + AM.add(frame, df_alert, enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) + if traffic_light_alerts: traffic_status = arne_sm['trafficModelEvent'].status traffic_confidence = round(arne_sm['trafficModelEvent'].confidence * 100, 2) @@ -175,10 +188,6 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ elif traffic_status == 'DEAD': # confidence will be 100 AM.add(frame, 'trafficDead', enabled) - df_alert = df_alert_manager.update(arne_sm) - if df_alert is not None: - AM.add(frame, 'dfButtonAlert', enabled, extra_text_1=df_alert, extra_text_2='Dynamic follow: {} profile active'.format(df_alert)) - # DISABLED if state == State.disabled: if get_events(events, [ET.ENABLE]): @@ -669,7 +678,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None, arne_sm=None): prof = Profiler(False) # off by default - df_alert_manager = DfAlertManager(op_params) while True: start_time = sec_since_boot() @@ -718,7 +726,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None, arne_sm=None): if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ - state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182, arne_sm, df_alert_manager) + state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182, arne_sm) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 8c90069b7385fa..83abd1641ac9b1 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -227,6 +227,18 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Alert("dfButtonAlert", + "Using profile: ", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), + + Alert("dfButtonAlertNoSound", + "Using profile: ", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), + Alert( "ethicalDilemma", "TAKE CONTROL IMMEDIATELY", diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 5c24eaa1aa16da..f4389a5a451926 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -17,7 +17,7 @@ class MPC_COST_LAT: class MPC_COST_LONG: - TTC = 15.0 + TTC = 10.0 DISTANCE = 0.1 ACCELERATION = 10.0 JERK = 20.0 diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py new file mode 100644 index 00000000000000..77c7f9235db7c9 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -0,0 +1,307 @@ +import math +import numpy as np +import cereal.messaging_arne as messaging_arne +from common.realtime import sec_since_boot +from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG +from common.op_params import opParams +from common.numpy_fast import interp, clip +from selfdrive.config import Conversions as CV +from common.travis_checker import travis + +from selfdrive.controls.lib.dynamic_follow.auto_df import load_weights, predict +from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager +from selfdrive.controls.lib.dynamic_follow.support import LeadData, CarData, dfData, dfProfiles + + +class DynamicFollow: + def __init__(self, mpc_id): + self.mpc_id = mpc_id + self.op_params = opParams() + self.df_profiles = dfProfiles() + self.df_manager = dfManager(self.op_params) + load_weights() + + if not travis and mpc_id == 1: + self.pm = messaging_arne.PubMaster(['dynamicFollowData']) + else: + self.pm = None + + # Model variables + mpc_rate = 1 / 20. + self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 139.52000427246094]} + self.predict_rate = 1 / 4. + self.skip_every = round(0.2 / mpc_rate) + self.model_input_len = round(35 / mpc_rate) # int: model input time + + # Dynamic follow variables + self.default_TR = 1.8 + self.TR = 1.8 + # self.v_lead_retention = 2.0 # keep only last x seconds + self.v_ego_retention = 2.5 + self.v_rel_retention = 1.5 + + self._setup_changing_variables() + + def _setup_changing_variables(self): + self.TR = self.default_TR + self.user_profile = self.df_profiles.normal # just a starting point + self.model_profile = self.df_profiles.normal + + self.sng = False + self.car_data = CarData() + self.lead_data = LeadData() + self.df_data = dfData() # dynamic follow data + + self.last_cost = 0.0 + self.last_predict_time = 0.0 + self.auto_df_model_data = [] + + def update(self, CS, libmpc): + self._get_live_params() + self._update_car(CS) + self._get_profiles() + + if not self.lead_data.status: + self.TR = self.default_TR + else: + self._store_df_data() + self.TR = self._get_TR() + + if not travis: + self._change_cost(libmpc) + self._send_cur_state() + + return self.TR + + def _get_profiles(self): + """This receives profile change updates from dfManager and runs the auto-df prediction if auto mode""" + df_out = self.df_manager.update() + self.user_profile = df_out.user_profile + if df_out.is_auto: # todo: find some way to share prediction between the two mpcs to reduce processing overhead + self._get_pred() # sets self.model_profile, all other checks are inside function + + def _norm(self, x, name): + self.x = x + return np.interp(x, self.model_scales[name], [0, 1]) + + def _send_cur_state(self): + if self.mpc_id == 1 and self.pm is not None: + dat = messaging_arne.new_message() + dat.init('dynamicFollowData') + dat.dynamicFollowData.mpcTR = 1.8 # self.TR # FIX THIS! sometimes nonetype + dat.dynamicFollowData.profilePred = self.model_profile + self.pm.send('dynamicFollowData', dat) + + def _change_cost(self, libmpc): + TRs = [0.9, 1.8, 2.7] + costs = [1.0, 0.115, 0.05] + cost = interp(self.TR, TRs, costs) + if self.last_cost != cost: + libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.last_cost = cost + + def _store_df_data(self): + cur_time = sec_since_boot() + # Store custom relative accel over time + if self.lead_data.status: + if self.lead_data.new_lead: + self.df_data.v_rels = [] # reset when new lead + else: + self.df_data.v_rels = self._remove_old_entries(self.df_data.v_rels, cur_time, self.v_rel_retention) + self.df_data.v_rels.append({'v_ego': self.car_data.v_ego, 'v_lead': self.lead_data.v_lead, 'time': cur_time}) + + # Store our velocity for better sng + self.df_data.v_egos = self._remove_old_entries(self.df_data.v_egos, cur_time, self.v_ego_retention) + self.df_data.v_egos.append({'v_ego': self.car_data.v_ego, 'time': cur_time}) + + # Store data for auto-df model + self.auto_df_model_data.append([self._norm(self.car_data.v_ego, 'v_ego'), + self._norm(self.lead_data.v_lead, 'v_lead'), + self._norm(self.lead_data.x_lead, 'x_lead')]) + while len(self.auto_df_model_data) > self.model_input_len: + del self.auto_df_model_data[0] + + def _get_pred(self): + cur_time = sec_since_boot() + if self.car_data.cruise_enabled and self.lead_data.status and not travis: + if cur_time - self.last_predict_time > self.predict_rate: + if len(self.auto_df_model_data) == self.model_input_len: + pred = predict(np.array(self.auto_df_model_data[::self.skip_every], dtype=np.float32).flatten()) + self.last_predict_time = cur_time + self.model_profile = int(np.argmax(pred)) + + def _remove_old_entries(self, lst, cur_time, retention): + return [sample for sample in lst if cur_time - sample['time'] <= retention] + + def _calculate_relative_accel_new(self): + # """ + # Moving window returning the following: (final relative velocity - initial relative velocity) / dT with a few extra mods + # Output properties: + # When the lead is starting to decelerate, and our car remains the same speed, the output decreases (and vice versa) + # However when our car finally starts to decelerate at the same rate as the lead car, the output will move to near 0 + # >>> a = [(15 - 18), (14 - 17)] + # >>> (a[-1] - a[0]) / 1 + # > 0.0 + # """ + min_consider_time = 0.5 # minimum amount of time required to consider calculation + if len(self.df_data.v_rels) > 0: # if not empty + elapsed_time = self.df_data.v_rels[-1]['time'] - self.df_data.v_rels[0]['time'] + if elapsed_time > min_consider_time: + x = [-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447, 0.8941, 1.7882, 2.6822] + y = [0.3245, 0.277, 0.11075, 0.08106, 0.06325, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35] + + v_lead_start = self.df_data.v_rels[0]['v_lead'] # setup common variables + v_ego_start = self.df_data.v_rels[0]['v_ego'] + v_lead_end = self.df_data.v_rels[-1]['v_lead'] + v_ego_end = self.df_data.v_rels[-1]['v_ego'] + + v_ego_change = v_ego_end - v_ego_start + v_lead_change = v_lead_end - v_lead_start + + if v_lead_change - v_ego_change == 0 or v_lead_change + v_ego_change == 0: + return None + + initial_v_rel = v_lead_start - v_ego_start + cur_v_rel = v_lead_end - v_ego_end + delta_v_rel = (cur_v_rel - initial_v_rel) / elapsed_time + + neg_pos = False + if v_ego_change == 0 or v_lead_change == 0: # FIXME: this all is a mess, but works. need to simplify + lead_factor = v_lead_change / (v_lead_change - v_ego_change) + + elif (v_ego_change < 0) != (v_lead_change < 0): # one is negative and one is positive, or ^ = XOR + lead_factor = v_lead_change / (v_lead_change - v_ego_change) + if v_ego_change > 0 > v_lead_change: + delta_v_rel = -delta_v_rel # switch when appropriate + neg_pos = True + + elif v_ego_change * v_lead_change > 0: # both are negative or both are positive + lead_factor = v_lead_change / (v_lead_change + v_ego_change) + if v_ego_change > 0 and v_lead_change > 0: # both are positive + if v_ego_change < v_lead_change: + delta_v_rel = -delta_v_rel # switch when appropriate + elif v_ego_change > v_lead_change: # both are negative and v_ego_change > v_lead_change + delta_v_rel = -delta_v_rel + + else: + raise Exception('Uncovered case! Should be impossible to be be here') + + if not neg_pos: # negative and positive require different mod code to be correct + rel_vel_mod = (-delta_v_rel * abs(lead_factor)) + (delta_v_rel * (1 - abs(lead_factor))) + else: + rel_vel_mod = math.copysign(delta_v_rel, v_lead_change - v_ego_change) * lead_factor + + calc_mod = np.interp(rel_vel_mod, x, y) + if v_lead_end > v_ego_end and calc_mod >= 0: + # if we're accelerating quicker than lead but lead is still faster, reduce mod + # todo: could remove this since we restrict this mod where called + x = np.array([0, 2, 4, 8]) * CV.MPH_TO_MS + y = [1.0, -0.25, -0.65, -0.95] + v_rel_mod = np.interp(v_lead_end - v_ego_end, x, y) + calc_mod *= v_rel_mod + return calc_mod + return None + + def global_profile_mod(self, TR, profile_mod_pos, profile_mod_neg): + if self.global_df_mod is not None: # only apply when not in sng + TR *= self.global_df_mod + profile_mod_pos *= (1 - self.global_df_mod) + 1 + profile_mod_neg *= self.global_df_mod + return TR, profile_mod_pos, profile_mod_neg + + def _get_TR(self): + x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] + + if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this + df_profile = self.model_profile + else: + df_profile = self.user_profile + + if df_profile == self.df_profiles.far: + y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs + profile_mod_pos = [0.92, 0.7, 0.25, 0.15] + profile_mod_neg = [1.1, 1.3, 2.0, 2.3] + elif df_profile == self.df_profiles.close: # for in congested traffic + x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] + # y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original + # y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph + y_dist = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156] + profile_mod_pos = [1.05, 1.55, 2.6, 3.75] + profile_mod_neg = [0.84, .275, 0.1, 0.05] + elif df_profile == self.df_profiles.normal: # default to relaxed/stock + y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] + profile_mod_pos = [1.0] * 4 + profile_mod_neg = [1.0] * 4 + else: + raise Exception('Unknown profile type: {}'.format(df_profile)) + + # Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics + profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x, profile_mod_pos) + profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x, profile_mod_neg) + + sng_TR = 1.8 # reacceleration stop and go TR + sng_speed = 18.0 * CV.MPH_TO_MS + + if self.car_data.v_ego > sng_speed: # keep sng distance until we're above sng speed again + self.sng = False + + if (self.car_data.v_ego >= sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng: + # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease + TR = interp(self.car_data.v_ego, x_vel, y_dist) + TR, profile_mod_pos, profile_mod_neg = self.global_profile_mod(TR, profile_mod_pos, profile_mod_neg) # only within normal driving conditions + else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating + self.sng = True + x = [sng_speed * 0.7, sng_speed] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating + y = [sng_TR, interp(sng_speed, x_vel, y_dist)] + TR = interp(self.car_data.v_ego, x, y) + + TR_mods = [] + # Dynamic follow modifications (the secret sauce) + x = [-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541, -2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898, 2.7316, 4.4704] # relative velocity values + y = [.76, 0.62323, 0.49488, 0.40656, 0.32227, 0.23914, 0.12269, 0.10483, 0.08074, 0.04886, 0.0072, 0.0, -0.05648, -0.0792, -0.15675, -0.23289, -0.315] # modification values + TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y)) + + x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values + y = [0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042, -0.053, -0.059] # modification values + TR_mods.append(interp(self.lead_data.a_lead, x, y)) + + rel_accel_mod = self._calculate_relative_accel_new() + if rel_accel_mod is not None: # if available + deadzone = 2 * CV.MPH_TO_MS + if self.lead_data.v_lead - deadzone > self.car_data.v_ego: + TR_mods.append(rel_accel_mod) + + x = [sng_speed / 5.0, sng_speed] # as we approach 0, apply x% more distance + y = [1.05, 1.0] + profile_mod_pos *= interp(self.car_data.v_ego, x, y) # but only for currently positive mods + + TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mods]) # alter TR modification according to profile + TR += TR_mod + + if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.close: + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, .75, .65] # reduce TR when changing lanes + TR *= interp(self.car_data.v_ego, x, y) + return clip(TR, 0.9, 2.7) + + def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False): + self.lead_data.v_lead = v_lead + self.lead_data.a_lead = a_lead + self.lead_data.x_lead = x_lead + + self.lead_data.status = status + self.lead_data.new_lead = new_lead + + def _update_car(self, CS): + self.car_data.v_ego = CS.vEgo + self.car_data.a_ego = CS.aEgo + + self.car_data.left_blinker = CS.leftBlinker + self.car_data.right_blinker = CS.rightBlinker + self.car_data.cruise_enabled = CS.cruiseState.enabled + + def _get_live_params(self): + self.global_df_mod = self.op_params.get('global_df_mod', None) + if self.global_df_mod is not None: + self.global_df_mod = np.clip(self.global_df_mod, 0.7, 1.1) diff --git a/selfdrive/controls/lib/dynamic_follow/auto-df-best-use-me.h5 b/selfdrive/controls/lib/dynamic_follow/auto-df-best-use-me.h5 new file mode 100644 index 00000000000000..6cf53bc51c93a5 Binary files /dev/null and b/selfdrive/controls/lib/dynamic_follow/auto-df-best-use-me.h5 differ diff --git a/selfdrive/controls/lib/dynamic_follow/auto_df.py b/selfdrive/controls/lib/dynamic_follow/auto_df.py new file mode 100644 index 00000000000000..eb7214e687c0a8 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/auto_df.py @@ -0,0 +1,25 @@ +""" + Generated using Konverter: https://github.com/ShaneSmiskol/Konverter +""" + +import numpy as np + + +def load_weights(): + global w, b + wb = np.load('/data/openpilot/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz', allow_pickle=True) + w, b = wb['wb'] + +def softmax(x): + return np.exp(x) / np.sum(np.exp(x), axis=0) + +def predict(x): + l0 = np.dot(x, w[0]) + b[0] + l0 = np.maximum(0, l0) + l1 = np.dot(l0, w[1]) + b[1] + l1 = np.maximum(0, l1) + l2 = np.dot(l1, w[2]) + b[2] + l2 = np.maximum(0, l2) + l3 = np.dot(l2, w[3]) + b[3] + l3 = softmax(l3) + return l3 diff --git a/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz b/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz new file mode 100644 index 00000000000000..865f3cdc610285 Binary files /dev/null and b/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz differ diff --git a/selfdrive/controls/lib/dynamic_follow/df_manager.py b/selfdrive/controls/lib/dynamic_follow/df_manager.py new file mode 100644 index 00000000000000..7fe15864b6360a --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/df_manager.py @@ -0,0 +1,74 @@ +import cereal.messaging_arne as messaging_arne +from selfdrive.controls.lib.dynamic_follow.support import dfProfiles +from common.realtime import sec_since_boot + + +class dfReturn: + user_profile = None # stays at user selected profile + user_profile_text = None # same as user_profile, but is its text representation + model_profile = None # only changes if user selects auto, is model output + model_profile_text = None # same as model_profile, but is its text representation + changed = False # true if either profile from model or user changes profile + is_auto = False # true if auto + last_is_auto = False + + +class dfManager: + def __init__(self, op_params, is_df=False): + self.op_params = op_params + self.is_df = is_df + self.df_profiles = dfProfiles() + self.sm = messaging_arne.SubMaster(['dynamicFollowButton', 'dynamicFollowData']) + + self.cur_user_profile = self.op_params.get('dynamic_follow', default='auto').strip().lower() + if not isinstance(self.cur_user_profile, str) or self.cur_user_profile not in self.df_profiles.to_idx: + self.cur_user_profile = self.df_profiles.normal # relaxed + else: + self.cur_user_profile = self.df_profiles.to_idx[self.cur_user_profile] + + self.cur_model_profile = 0 + self.alert_duration = 2.0 + + self.offset = self.cur_user_profile + self.profile_pred = None + self.change_time = sec_since_boot() + self.first_run = True + self.last_is_auto = False + + @property + def is_auto(self): + return self.cur_user_profile == self.df_profiles.auto + + @property + def can_show_alert(self): + return sec_since_boot() - self.change_time > self.alert_duration + + def update(self): + self.sm.update(0) + df_out = dfReturn() + if self.first_run: + #df_out.changed = True # to show alert on start + self.first_run = False + + button_status = self.sm['dynamicFollowButton'].status + df_out.user_profile = (button_status + self.offset) % len(self.df_profiles.to_profile) + df_out.user_profile_text = self.df_profiles.to_profile[df_out.user_profile] + + if self.cur_user_profile != df_out.user_profile: + self.change_time = sec_since_boot() + self.last_is_auto = False + df_out.changed = True + self.op_params.put('dynamic_follow', self.df_profiles.to_profile[df_out.user_profile]) # save current profile for next drive + self.cur_user_profile = df_out.user_profile + + if self.is_auto: + df_out.model_profile = self.sm['dynamicFollowData'].profilePred + df_out.model_profile_text = self.df_profiles.to_profile[df_out.model_profile] + df_out.is_auto = True + df_out.last_is_auto = self.last_is_auto + self.last_is_auto = True + if self.cur_model_profile != df_out.model_profile and self.can_show_alert: + df_out.changed = True # to hide pred alerts until user-selected auto alert has finished + self.cur_model_profile = df_out.model_profile + + return df_out diff --git a/selfdrive/controls/lib/dynamic_follow/support.py b/selfdrive/controls/lib/dynamic_follow/support.py new file mode 100644 index 00000000000000..d74bde76fae648 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/support.py @@ -0,0 +1,29 @@ +class LeadData: + v_lead = None + x_lead = None + a_lead = None + status = False + new_lead = False + + +class CarData: + v_ego = 0.0 + a_ego = 0.0 + + left_blinker = False + right_blinker = False + cruise_enabled = True + + +class dfData: + v_egos = [] + v_rels = [] + + +class dfProfiles: + close = 0 + normal = 1 + far = 2 + auto = 3 + to_profile = {0: 'close', 1: 'normal', 2: 'far', 3: 'auto'} + to_idx = {v: k for k, v in to_profile.items()} diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 4ecb5303a312a4..2ef51b29b9dde8 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -2,15 +2,14 @@ import math import cereal.messaging as messaging -import cereal.messaging_arne as messaging_arne from selfdrive.swaglog import cloudlog from common.realtime import sec_since_boot +from common.travis_checker import travis from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG -from common.op_params import opParams -from common.numpy_fast import interp, clip -from common.travis_checker import travis +if not travis: + from selfdrive.controls.lib.dynamic_follow import DynamicFollow LOG_MPC = os.environ.get('LOG_MPC', False) @@ -18,8 +17,8 @@ class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id - self.op_params = opParams() - + if not travis: + self.dynamic_follow = DynamicFollow(mpc_id) self.setup_mpc() self.v_mpc = 0.0 self.v_mpc_future = 0.0 @@ -28,16 +27,8 @@ def __init__(self, mpc_id): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False - self.TR_Mod = 0 - self.last_cloudlog_t = 0.0 - if not travis and mpc_id == 1: - self.pm = messaging_arne.PubMaster(['smiskolData']) - else: - self.pm = None - self.last_cost = 0.0 - self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() - self.sng = False + self.last_cloudlog_t = 0.0 def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) @@ -69,61 +60,9 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a - def get_TR(self, CS, lead): - if not lead.status or travis: - TR = 1.8 - elif CS.vEgo < 5.0: - TR = 1.8 - else: - TR = self.dynamic_follow(CS, lead) - - if not travis: - self.change_cost(TR,CS.vEgo) - self.send_cur_TR(TR) - return TR - - def send_cur_TR(self, TR): - if self.mpc_id == 1 and self.pm is not None: - dat = messaging_arne.new_message('smiskolData') - dat.smiskolData.mpcTR = TR - self.pm.send('smiskolData', dat) - - def change_cost(self, TR, vEgo): - TRs = [0.9, 1.8, 2.7] - costs = [1.0, 0.11, 0.05] - cost = interp(TR, TRs, costs) - if self.last_cost != cost: - self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.last_cost = cost - - def dynamic_follow(self, CS, lead): - self.df_profile = self.op_params.get('dynamic_follow', 'normal').strip().lower() - x_vel = [5.0, 15.0] # velocities - if self.df_profile == 'far': - y_dist = [1.8, 2.7] # TRs - elif self.df_profile == 'close': # for in congested traffic - x_vel = [5.0, 15.0] - y_dist = [1.8, 0.9] - else: # default to normal - y_dist = [1.8, 1.8] - TR = interp(CS.vEgo, x_vel, y_dist) - - # Dynamic follow modifications (the secret sauce) - x = [-5.0, 0.0, 5.0] # relative velocity values - y = [0.3, 0.0, -0.3] # modification values - - self.TR_Mod = interp(lead.vRel, x, y) - TR += self.TR_Mod - - if CS.leftBlinker or CS.rightBlinker: - x = [9.0, 55.0] # - y = [1.0, 0.65] # reduce TR when changing lanes - TR *= interp(CS.vEgo, x, y) - - return clip(TR, 0.9, 2.7) - def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo + # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -135,17 +74,21 @@ def update(self, pm, CS, lead, v_cruise_setpoint): if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 + self.a_lead_tau = lead.aLeadTau self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) self.new_lead = True - + if not travis: + self.dynamic_follow.update_lead(v_lead, a_lead, x_lead, lead.status, self.new_lead) self.prev_lead_status = True self.prev_lead_x = x_lead self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: + if not travis: + self.dynamic_follow.update_lead(new_lead=self.new_lead) self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 @@ -155,7 +98,11 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS, lead)) + if not travis: + TR = self.dynamic_follow.update(CS, self.libmpc) # update dynamic follow + else: + TR = 1.8 + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: diff --git a/selfdrive/crash.py b/selfdrive/crash.py index a0c05e5e21590c..8a79616c24fa96 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -8,7 +8,6 @@ from selfdrive.version import version, dirty, origin, branch from common.op_params import opParams op_params = opParams() -uniqueID = op_params.get('uniqueID', None) from selfdrive.swaglog import cloudlog from common.android import ANDROID @@ -34,8 +33,18 @@ def install(): ipaddress = requests.get('https://checkip.amazonaws.com/').text.strip() except: ipaddress = "255.255.255.255" - error_tags = {'dirty': dirty, 'username': uniqueID, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin} - + error_tags = {'dirty': dirty, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin} + username = op_params.get('username', None) + uniqueID = op_params.get('uniqueID', None) + + u_tag = [] + if isinstance(username, str): + u_tag.append(username) + if isinstance(uniqueID, str): + u_tag.append(uniqueID) + if len(u_tag) > 0: + error_tags['username'] = ''.join(u_tag) + client = Client('https://137e8e621f114f858f4c392c52e18c6d:8aba82f49af040c8aac45e95a8484970@sentry.io/1404547', install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags) diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index 9b88130e62f19b..449d7c00bcf09c 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -550,6 +550,11 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead, tra speed_ahead_dist = way_pts[count, 0] loop_must_break = True break + elif n.tags['traffic_calming']=='yes': + speed_ahead = 40/3.6 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break count += 1 if loop_must_break: break except (KeyError, IndexError, ValueError): diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a015655c5321de..5746d2972a4ad9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -142,7 +142,7 @@ static bool handle_df_touch(UIState *s, int touch_x, int touch_y) { if (touch_x >= 1212 && touch_x <= 1310 && touch_y >= 902 && touch_y <= 1013) { s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping df button s->scene.dfButtonStatus++; - if (s->scene.dfButtonStatus > 2) { + if (s->scene.dfButtonStatus > 3) { s->scene.dfButtonStatus = 0; } send_df(s, s->scene.dfButtonStatus); @@ -1183,7 +1183,7 @@ int main(int argc, char* argv[]) { if (touched == 1) { set_awake(s, true); handle_sidebar_touch(s, touch_x, touch_y); - if (!handle_df_touch(s, touch_x, touch_y)){ // disables sidebar from popping out when tapping df bu + if (!handle_df_touch(s, touch_x, touch_y)){ // disables sidebar from popping out when tapping df button handle_vision_touch(s, touch_x, touch_y); } }