diff --git a/README.md b/README.md index 87257f000d897b..5bf64dc97ab78e 100644 --- a/README.md +++ b/README.md @@ -1,35 +1,79 @@ -Shane's Stock Additions (0.7) +Shane's Stock Additions 0.7 (version 0.1) ===== -This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla. +This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla /w comma pedal. Highlight Features -==== - -1. **Dynamic gas**: This aims to provide a smoother driving experience in stop and go traffic 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 openpilot with comma pedal. Better tuning will be next. -2. **Dynamic follow**: This is my dynamic follow from 0.5, where it changes your TR (following distance) dynamically based on multiple vehicle factors, as well as data from the lead vehicle. [Here's an old write up from a while ago explaining how it works exactly. Some of it might be out of date, but how it functions is the same.](https://github.com/ShaneSmiskol/openpilot/blob/dynamic-follow/README.md) -3. **(NOT YET ADDED) Two PID loops to control gas and brakes independently (new!)**: If you have a Toyota Corolla with a comma pedal, you'll love this addition. Two longitudinal PID loops are set up in `longcontrol.py` so that one is running with comma pedal tuning to control the gas, and the other is running stock non-pedal tuning for better braking control. In the car, this feels miles better than stock openpilot, and nearly as good as your stock Toyota cruise control before you pulled out your DSU! It won't accelerate up to stopped cars and brake at the last moment anymore. -3. **Custom wheel offset to reduce lane hugging**: Stock openpilot doesn't seem to be able to identify your car's true angle offset. With the `LaneHugging` module you can specify a custom angle offset to be added to your desired steering angle. Simply find the angle your wheel is at when you're driving on a straight highway. By default, this is disabled, to enable you can: - - Use the `opEdit` class in the root directory of openpilot. To use it, simply open an `ssh` shell and enter the commands below: - ```python - cd /data/openpilot - python op_edit.py - ``` - You'll be greeted with a list of your parameters you can explore, enter the number corresponding to `lane_hug_direction`. Your options are to enter `'left'` or `'right'` for whichever direction your car has a tendency to hug toward. `None` will disable the feature. - Finally you'll need to enter your absolute angle offset (negative will be converted to positive) with the `opParams` parameter: `lane_hug_angle_offset`. -4. **Custom following distance**: Using the `following_distance` parameter in `opParams`, you can specify a custom TR value to always be used. Afraid of technology and want to give yourself the highest following distance out there? Try out 2.7s! Are you daredevil and don't care about pissing off the car you're tailgating ahead? Try 0.9s! - - Again, you can use `opEdit` to change this: - ```python - cd /data/openpilot - python op_edit.py - ``` - Then enter the number for the `following_distance` parameter and set to a float or integer between `0.9` and `2.7`. `None` will use dynamic follow! -5. **Customize this branch (opEdit Parameter class)**: This is a handy tool to change your `opParams` parameters without diving into any json files or code. You can specify parameters to be used in any fork's operation that supports `opParams`. First, ssh in to your EON and make sure you're in `/data/openpilot`, then start `opEdit`: +===== + +* [**Dynamic gas**](#dynamic-gas) +* [**Dynamic follow**](#dynamic-follow) +* [**Dynamic lane speed (new!)**](#dynamic-lane-speed) +* [**(NOT YET ADDED) Two PID loops to control gas and brakes independently**](#Two-PID-loops-to-control-gas-and-brakes-independently) +* [**Custom wheel offset to reduce lane hugging**](#Custom-wheel-offset-to-reduce-lane-hugging) +* [**Custom following distance**](#Custom-following-distance) +* [**Customize this branch (opEdit Parameter class)**](#Customize-this-branch-opEdit-Parameter-class) +* [**Live tuning support**](#Live-tuning-support) + +----- + +Dynamic gas +----- +This aims to provide a smoother driving experience in stop and go traffic 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 openpilot 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 :). Better tuning for distance will be next. + +Dynamic follow +----- +This is my dynamic follow from 0.5, where it changes your TR (following distance) dynamically based on multiple vehicle factors, as well as data from the lead vehicle. [Here's an old write up from a while ago explaining how it works exactly. Some of it might be out of date, but how it functions is the same.](https://github.com/ShaneSmiskol/openpilot/blob/dynamic-follow/README.md) The goal is to essentially smoothen the driving experience and increase safety, braking sooner. + +Dynamic lane speed +----- +This is a new feature that reduces your cruising speed if many vehicles around you are significantly slower than you. This works with and without an openpilot-identified lead. Ex.: It will slow you down if traveling in an open lane with cars in adjacent lanes that are slower than you. Or if the lead in front of the lead is slowing down, as well as cars in other lanes far ahead. The most it will slow you down is some average of: (the set speed and the average of the surrounding cars) The more the radar points, the more weight goes to the speeds of surrounding vehicles. + +~~Two PID loops to control gas and brakes independently~~ +----- +***Update**: Probably going to remove this addition, as tuning the current pedal parameters will be a more robust solution in the long run.* + +If you have a Toyota Corolla with a comma pedal, you'll love this addition. Two longitudinal PID loops are set up in `longcontrol.py` so that one is running with comma pedal tuning to control the gas, and the other is running stock non-pedal tuning for better braking control. In the car, this feels miles better than stock openpilot, and nearly as good as your stock Toyota cruise control before you pulled out your DSU! It won't accelerate up to stopped cars and brake at the last moment anymore. + +~~Custom wheel offset to reduce lane hugging~~ +----- +***Update**: This also may be removed, I was able to get good results live tuning camera offset. Perhaps angle offset isn't needed?* + +Stock openpilot doesn't seem to be able to identify your car's true angle offset. With the `LaneHugging` module you can specify a custom angle offset to be added to your desired steering angle. Simply find the angle your wheel is at when you're driving on a straight highway. By default, this is disabled, to enable you can: +- Use the `opEdit` class in the root directory of openpilot. To use it, simply open an `ssh` shell and enter the commands below: ```python cd /data/openpilot python op_edit.py ``` - A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions-07/common/op_params.py#L29). + You'll be greeted with a list of your parameters you can explore, enter the number corresponding to `lane_hug_direction`. Your options are to enter `'left'` or `'right'` for whichever direction your car has a tendency to hug toward. `None` will disable the feature. + Finally you'll need to enter your absolute angle offset (negative will be converted to positive) with the `opParams` parameter: `lane_hug_angle_offset`. + +Custom following distance +----- +Using the `following_distance` parameter in `opParams`, you can specify a custom TR value to always be used. Afraid of technology and want to give yourself the highest following distance out there? Try out 2.7s! Are you daredevil and don't care about pissing off the car you're tailgating ahead? Try 0.9s! Please note dynamic follow modifications will be disabled if you set this parameter. +- Again, you can use `opEdit` to change this: + ```python + cd /data/openpilot + python op_edit.py + ``` + Then enter the number for the `following_distance` parameter and set to a float or integer between `0.9` and `2.7`. `None` will use dynamic follow! + +Customize this branch (opEdit Parameter class) +----- +This is a handy tool to change your `opParams` parameters without diving into any json files or code. You can specify parameters to be used in any fork's operation that supports `opParams`. First, ssh in to your EON and make sure you're in `/data/openpilot`, then start `opEdit`: +```python +cd /data/openpilot +python op_edit.py +``` +A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions/common/op_params.py#L29). + +Parameters are stored at `/data/op_params.json` + +Live tuning support +----- +This has just been added and currently only the `camera_offset` parameter is officially supported. +- Just start opEdit with the instructions above and pick a parameter. It will let you know if it supports live tuning, if so, updates will take affect within 5 seconds! +- Alternatively, you can use the new opTune module to live tune quicker and easier! It stays in the parameter edit view so you can more easily experiment with values. opTune show below: - Parameters are stored at `/data/op_params.json` + diff --git a/RELEASES.md b/RELEASES.md index 2e99a28bc06feb..aff144251b8317 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,14 @@ +Stock Additions 0.7 (version 0.1) +======================== + * Dynamic lane speed is a new feature that reduces your cruising speed if many vehicles around you are significantly slower than you. This works with and without an openpilot-identified lead. + * Dynamic gas tuning. Above 20 mph we take lead velocity and the following distance into account. Possibility of different tuning for different cars in the future. (DYNAMIC GAS NOW ONLY WORKS ON TOYOTA COROLLA AND RAV4 PEDAL) + * Dynamic follow tuning, don't get as close when lead is accelerating. + * Added static_steer_ratio parameter, if True openpilot will use the steer ratio in your interface file. Default is true, false uses the openpilot learned value which can vary through your drives. + * Added ability to live tune parameters with `op_tune.py`. Currently only the camera offset (`camera_offset`) is supported. + * Some Corolla tuning. + * Reduce max acceleration. + * TO NOTE: Dynamic Lane Speed will not work with stopped cars, at any speed. There is also a margin that cars must be traveling within in order to affect your speed. Don't expect anything magical, just minor quality of drive improvements. + Version 0.7 (2019-12-13) ======================== * Move to SCons build system! diff --git a/cereal/log.capnp b/cereal/log.capnp index b059218feba89c..dc2080bc5874df 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1814,6 +1814,10 @@ struct KalmanOdometry { rotStd @3 :List(Float32); # std rad/s in device frame } +struct SmiskolData { + mpcTR @0 :Float32; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -1889,5 +1893,6 @@ struct Event { carEvents @68: List(Car.CarEvent); carParams @69: Car.CarParams; frontFrame @70: FrameData; + smiskolData @71 :SmiskolData; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 884bfe9a5bf991..b09eb2b9fa7754 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -75,6 +75,7 @@ thumbnail: [8069, true, 0.2, 1] carEvents: [8070, true, 1., 1] carParams: [8071, true, 0.02, 1] frontFrame: [8072, true, 10.] +smiskolData: [8073, true, 20.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] diff --git a/common/op_params.py b/common/op_params.py index 4f2dab81db155b..c04ef9854c2cf6 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -5,7 +5,6 @@ import random from common.travis_checker import travis - def write_params(params, params_file): if not travis: with open(params_file, "w") as f: @@ -26,25 +25,29 @@ def read_params(params_file, default_params): class opParams: def __init__(self): - self.default_params = {'camera_offset': {'default': 0.06, 'allowed_types': [float, int], 'description': 'Your camera offset to use in lane_planner.py'}, - 'awareness_factor': {'default': 2.0, 'allowed_types': [float, int], 'description': 'Multiplier for the awareness times'}, - 'lane_hug_direction': {'default': None, 'allowed_types': [type(None), str], 'description': "(NoneType, 'left', 'right'): Direction of your lane hugging, if present. None will disable this modification"}, + self.default_params = {'camera_offset': {'default': 0.06, 'allowed_types': [float, int], 'description': 'Your camera offset to use in lane_planner.py', 'live': True}, + 'awareness_factor': {'default': 2.0, 'allowed_types': [float, int], 'description': 'Multiplier for the awareness times', 'live': False}, + 'lane_hug_direction': {'default': None, 'allowed_types': [type(None), str], 'description': "(NoneType, 'left', 'right'): Direction of your lane hugging, if present. None will disable this modification", 'live': False}, 'lane_hug_angle_offset': {'default': 0.0, 'allowed_types': [float, int], 'description': ('This is the angle your wheel reads when driving straight at highway speeds. ' 'Used to offset desired angle_steers in latcontrol to help fix lane hugging. ' - 'Enter absolute value here, direction is determined by parameter \'lane_hug_direction\'')}, - 'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching'}, - 'force_pedal': {'default': False, 'allowed_types': [bool], 'description': "If openpilot isn't recognizing your comma pedal, set this to True"}, - 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'}, + 'Enter absolute value here, direction is determined by parameter \'lane_hug_direction\''), 'live': False}, + 'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching', 'live': False}, + 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR', 'live': False}, 'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. ' - 'If False, lane change will occur IMMEDIATELY after signaling')}, - 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}} + 'If False, lane change will occur IMMEDIATELY after signaling'), 'live': False}, + 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)', 'live': False}, + 'static_steer_ratio': {'default': False, 'allowed_types': [bool], 'description': 'Whether you want openpilot to use the steering ratio in interface.py, or the automatically learned steering ratio. If True, it will use the static value in interface.py', 'live': False}, + 'use_dynamic_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to adjust your speed based on surrounding vehicles', 'live': False}, + 'min_dynamic_lane_speed': {'default': 15.0, 'allowed_types': [float, int], 'description': 'The minimum speed to allow dynamic lane speed to operate (in MPH)', 'live': False}, + 'longkiV': {'default': 0.0, 'allowed_types': [float, int], 'description': 'This is a temp parameter', 'live': True}} self.params = {} self.params_file = "/data/op_params.json" self.kegman_file = "/data/kegman.json" self.last_read_time = time.time() - self.read_frequency = 10.0 # max frequency to read with self.get(...) (sec) + self.read_frequency = 5.0 # 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'] 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! @@ -82,6 +85,8 @@ def run_init(self): # does first time initializing of default params, and/or re 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): @@ -97,18 +102,29 @@ def run_init(self): # does first time initializing of default params, and/or re if to_write or no_params: write_params(self.params, self.params_file) + def delete_old(self): + prev_params = self.params + for i in self.to_delete: + if i in self.params: + del self.params[i] + return prev_params == self.params + def put(self, key, value): self.params.update({key: value}) write_params(self.params, self.params_file) def get(self, key=None, default=None): # can specify a default value if key doesn't exist - if (time.time() - self.last_read_time) >= self.read_frequency and not travis: # make sure we aren't reading file too often - self.params, read_status = read_params(self.params_file, self.format_default_params()) - self.last_read_time = time.time() - if key is None: # get all + if key is None: return self.params - else: - return self.params[key] if key in self.params else default + if not travis and key in self.default_params and self.default_params[key]['live']: # if is a live param, we want to get updates while openpilot is running + if 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(0.01) + self.params, read_status = read_params(self.params_file, self.format_default_params()) # if the file was being written to, retry once + self.last_read_time = time.time() + + return self.params[key] if key in self.params else default def delete(self, key): if key in self.params: diff --git a/gifs/op_tune.gif b/gifs/op_tune.gif new file mode 100644 index 00000000000000..ecf971b1ea5a6c Binary files /dev/null and b/gifs/op_tune.gif differ diff --git a/op_edit.py b/op_edit.py index 5921287a9444cc..d43f26a870f274 100644 --- a/op_edit.py +++ b/op_edit.py @@ -8,8 +8,6 @@ def __init__(self): self.op_params = opParams() self.params = None self.sleep_time = 1.0 - print('Welcome to the opParams command line editor!') - print('Here are your parameters:\n') self.run_loop() def run_loop(self): @@ -17,91 +15,103 @@ def run_loop(self): print('Here are your parameters:\n') while True: self.params = self.op_params.get() + 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] - to_print = ['{}. {}: {} (type: {})'.format(idx + 1, i, values_list[idx], str(type(self.params[i])).split("'")[1]) for idx, i in enumerate(self.params)] - to_print.append('{}. Add new parameter!'.format(len(self.params) + 1)) + live = [' (live!)' if i in self.op_params.default_params and self.op_params.default_params[i]['live'] else '' for i in self.params] + + 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)) print('\n'.join(to_print)) print('\nChoose a parameter to explore (by integer index): ') - choice = input('>> ') + choice = input('>> ').strip() parsed, choice = self.parse_choice(choice) if parsed == 'continue': continue elif parsed == 'add': - if self.add_parameter() == 'error': - return + self.add_parameter() elif parsed == 'change': - if self.change_parameter(choice) == 'error': - return + self.change_parameter(choice) elif parsed == 'delete': - if self.delete_parameter() == 'error': - return + self.delete_parameter() elif parsed == 'error': return def parse_choice(self, choice): if choice.isdigit(): choice = int(choice) + choice -= 1 elif choice == '': - print('Exiting...') + print('Exiting opEdit!') return 'error', choice else: print('\nNot an integer!\n', flush=True) time.sleep(self.sleep_time) return 'retry', choice - if choice not in range(1, len(self.params) + 3): # three for add/delete parameter + if choice not in range(0, len(self.params) + 2): # three for add/delete parameter print('Not in range!\n', flush=True) time.sleep(self.sleep_time) return 'continue', choice - if choice == len(self.params) + 1: # add new parameter + if choice == len(self.params): # add new parameter return 'add', choice - if choice == len(self.params) + 2: # delete parameter + if choice == len(self.params) + 1: # delete parameter return 'delete', choice return 'change', choice def change_parameter(self, choice): - chosen_key = list(self.params)[choice - 1] - extra_info = False - if chosen_key in self.op_params.default_params: - extra_info = True - param_allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] - param_description = self.op_params.default_params[chosen_key]['description'] - - old_value = self.params[chosen_key] - print('Chosen parameter: {}'.format(chosen_key)) - print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) - if extra_info: - print('\nDescription: {}'.format(param_description)) - print('Allowed types: {}\n'.format(', '.join([str(i).split("'")[1] for i in param_allowed_types]))) - print('Enter your new value:') - new_value = input('>> ') - if len(new_value) == 0: - print('Entered value cannot be empty!') - return 'error' - status, new_value = self.parse_input(new_value) - if not status: - print('Cannot parse input, exiting!') - return 'error' - - if extra_info and not any([isinstance(new_value, typ) for typ in param_allowed_types]): - print('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(new_value)).split("'")[1])) + while True: + chosen_key = list(self.params)[choice] + extra_info = False + live = False + if chosen_key in self.op_params.default_params: + extra_info = True + allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] + description = self.op_params.default_params[chosen_key]['description'] + live = self.op_params.default_params[chosen_key]['live'] + + old_value = self.params[chosen_key] + print('Chosen parameter: {}'.format(chosen_key)) + print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + if extra_info: + print('\n- Description: {}'.format(description)) + print('- Allowed types: {}'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) + if live: + print('- This parameter supports live tuning! Updates should take affect within 5 seconds.\n') + print('It\'s recommended to use the new opTune module! It\'s been streamlined to make live tuning easier and quicker.') + print('Just exit out of this and type:') + print('python op_tune.py') + print('In the directory /data/openpilot\n') + else: + print() + print('Enter your new value:') + new_value = input('>> ').strip() + if new_value == '': + return + + status, new_value = self.parse_input(new_value) + + if not status: + continue + + if extra_info and not any([isinstance(new_value, typ) for typ in allowed_types]): + self.message('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(new_value)).split("'")[1])) + 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': + self.op_params.put(chosen_key, new_value) + print('\nSaved!\n', flush=True) + else: + print('\nNot saved!\n', flush=True) time.sleep(self.sleep_time) return - 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() - if choice == 'y': - self.op_params.put(chosen_key, new_value) - print('\nSaved!\n') - else: - print('\nNot saved!\n', flush=True) - time.sleep(self.sleep_time) - def parse_input(self, dat): try: dat = ast.literal_eval(dat) @@ -109,68 +119,78 @@ def parse_input(self, dat): try: dat = ast.literal_eval('"{}"'.format(dat)) except ValueError: + self.message('Cannot parse input, please try again!') return False, dat return True, dat def delete_parameter(self): - print('Enter the name of the parameter to delete:') - key = input('>> ') - status, key = self.parse_input(key) - if not status: - print('Cannot parse input, exiting!') - return 'error' - if not isinstance(key, str): - print('Input must be a string!') - return 'error' - if key not in self.params: - print("Parameter doesn't exist!") - return 'error' - - 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?') - - choice = input('[Y/n]: ').lower() - if choice == 'y': - self.op_params.delete(key) - print('\nDeleted!\n') - else: - print('\nNot saved!\n', flush=True) - time.sleep(self.sleep_time) + while True: + print('Enter the name of the parameter to delete:') + key = input('>> ').lower() + status, key = self.parse_input(key) + if key == '': + return + if not status: + continue + if not isinstance(key, str): + self.message('Input must be a string!') + continue + if key not in self.params: + self.message("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?') + + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.delete(key) + print('\nDeleted!\n') + else: + print('\nNot saved!\n', flush=True) + time.sleep(self.sleep_time) + return def add_parameter(self): - print('Type the name of your new parameter:') - key = input('>> ') - if len(key) == 0: - print('Entered key cannot be empty!') - return 'error' - status, key = self.parse_input(key) - if not status: - print('Cannot parse input, exiting!') - return 'error' - if not isinstance(key, str): - print('Input must be a string!') - return 'error' - - print("Enter the data you'd like to save with this parameter:") - value = input('>> ') - status, value = self.parse_input(value) - if not status: - print('Cannot parse input, exiting!') - return 'error' - - print('Parameter name: {}'.format(key)) - print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) - print('Do you want to save this?') - - choice = input('[Y/n]: ').lower() - if choice == 'y': - self.op_params.put(key, value) - print('\nSaved!\n') - else: - print('\nNot saved!\n', flush=True) + while True: + print('Type the name of your new parameter:') + key = input('>> ').strip() + if key == '': + return + + status, key = self.parse_input(key) + + if not status: + continue + if not isinstance(key, str): + self.message('Input must be a string!') + continue + + print("Enter the data you'd like to save with this parameter:") + value = input('>> ').strip() + status, value = self.parse_input(value) + if not status: + continue + + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to save this?') + + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.put(key, value) + print('\nSaved!\n', flush=True) + else: + print('\nNot saved!\n', flush=True) + time.sleep(self.sleep_time) + return + + def message(self, msg): + print('--------\n{}\n--------'.format(msg), flush=True) time.sleep(self.sleep_time) + print() opEdit() diff --git a/op_tune.py b/op_tune.py new file mode 100644 index 00000000000000..e9a3ee22acb7f3 --- /dev/null +++ b/op_tune.py @@ -0,0 +1,68 @@ +from common.op_params import opParams +import ast +import time + + +class opTune: + def __init__(self): + self.op_params = opParams() + self.sleep_time = 1.0 + self.start() + + def start(self): + print('Welcome to the opParams command line live tuner!') + editable = [p for p in self.op_params.get() if p in self.op_params.default_params and self.op_params.default_params[p]['live']] + while True: + print('Choose a parameter to tune:') + print('\n'.join(['{}. {}'.format(idx + 1, p) for idx, p in enumerate(editable)])) + choice = input('>> ') + if not choice: + print('Exiting opTune!') + break + choice = ast.literal_eval(choice) - 1 + if choice not in range(len(editable)): + self.message('Error, not in range!') + continue + self.chosen(editable[choice]) + + def chosen(self, param): + allowed_types = self.op_params.default_params[param]['allowed_types'] + print('\nChosen parameter: {}'.format(param)) + print('Current value: {}'.format(self.op_params.get(param))) + print('\n- Description: {}'.format(self.op_params.default_params[param]['description'])) + print('- Allowed types: {}\n'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) + while True: + value = input('Enter value: ') + if value == '': + self.message('Exiting this parameter...') + break + + status, value = self.parse_input(value) + if not status: + self.message('Cannot parse input!') + continue + + if not any([isinstance(value, typ) for typ in allowed_types]): + self.message('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(value)).split("'")[1])) + continue + self.op_params.put(param, value) + print('Saved {} with value: {}! (type: {})\n'.format(param, value, str(type(value)).split("'")[1])) + + def message(self, msg): + print('--------\n{}\n--------'.format(msg), flush=True) + time.sleep(self.sleep_time) + print() + + def parse_input(self, dat): + dat = dat.replace("'", '"') + try: + dat = ast.literal_eval(dat) + except: + try: + dat = ast.literal_eval('"{}"'.format(dat)) + except ValueError: + return False, dat + return True, dat + + +opTune() diff --git a/panda/VERSION b/panda/VERSION index 538a65cc825174..b7c8e167db982f 100644 --- a/panda/VERSION +++ b/panda/VERSION @@ -1 +1 @@ -v1.6.9 \ No newline at end of file +v1.7.0 \ No newline at end of file diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index d7e011d8fb7ecc..8526a5b993edb2 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 4cef17f79d0ed5..ab8ceb52905206 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -6,7 +6,7 @@ // accel rising edge // brake rising edge // brake > 0mph -const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}, {0x39F, 0}}; +const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}}; const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file @@ -216,7 +216,6 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { // fwd from car to camera. also fwd certain msgs from camera to car // 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX, // 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud, - // 0x39f is radar hud int bus_fwd = -1; if (!relay_malfunction) { @@ -227,7 +226,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { // block stock lkas messages and stock acc messages (if OP is doing ACC) int addr = GET_ADDR(to_fwd); bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D); - bool is_acc_hud_msg = (addr == 0x30C) || (addr == 0x39F); + bool is_acc_hud_msg = addr == 0x30C; bool is_brake_msg = addr == 0x1FA; bool block_fwd = is_lkas_msg || (is_acc_hud_msg && long_controls_allowed) || diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py index 6ab247835af7e4..f89e7a593f4e85 100755 --- a/panda/tests/safety/test_honda.py +++ b/panda/tests/safety/test_honda.py @@ -8,7 +8,7 @@ MAX_BRAKE = 255 INTERCEPTOR_THRESHOLD = 328 -TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0], [0x39F, 0]] +TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]] class TestHondaSafety(unittest.TestCase): @classmethod @@ -253,7 +253,7 @@ def test_fwd_hook(self): self.safety.set_long_controls_allowed(l) blocked_msgs = [0xE4, 0x194, 0x33D] if l: - blocked_msgs += [0x30C, 0x39F] + blocked_msgs += [0x30C] if not f: blocked_msgs += [0x1FA] for b in buss: diff --git a/panda/tests/safety_replay/test_safety_replay.py b/panda/tests/safety_replay/test_safety_replay.py index b4278351fee0b3..4b2f5372df07c5 100755 --- a/panda/tests/safety_replay/test_safety_replay.py +++ b/panda/tests/safety_replay/test_safety_replay.py @@ -11,7 +11,7 @@ # (route, safety mode, param) logs = [ - ("b0c9d2329ad1606b|2019-05-30--20-23-57.bz2", Panda.SAFETY_HONDA, 0), # HONDA.CIVIC + ("2425568437959f9d|2019-12-22--16-24-37.bz2", Panda.SAFETY_HONDA_NIDEC, 0), # HONDA.CIVIC (fcw presents: 0x1FA blocked as expected) ("38bfd238edecbcd7|2019-06-07--10-15-25.bz2", Panda.SAFETY_TOYOTA, 66), # TOYOTA.PRIUS ("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", Panda.SAFETY_GM, 0), # GM.VOLT ("0375fdf7b1ce594d|2019-05-21--20-10-33.bz2", Panda.SAFETY_HONDA_BOSCH, 1), # HONDA.ACCORD diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 855a0e7dce8ca9..411d0c881fdbea 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -1,5 +1,5 @@ from selfdrive.config import Conversions as CV -from selfdrive.car.honda.values import CAR, HONDA_BOSCH +from selfdrive.car.honda.values import HONDA_BOSCH def get_pt_bus(car_fingerprint, has_relay): @@ -25,7 +25,7 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ "COMPUTER_BRAKE_REQUEST": brake_rq, "SET_ME_1": 1, "BRAKE_LIGHTS": brakelights, - "CHIME": stock_brake["CHIME"], # chime issued when disabling FCM + "CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw "FCW": fcw << 1, # TODO: Why are there two bits for fcw? "AEB_REQ_1": 0, "AEB_REQ_2": 0, @@ -76,14 +76,6 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, } commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx)) - if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY): - radar_hud_values = { - 'ACC_ALERTS': hud.acc_alert, - 'LEAD_SPEED': 0x1fe, # What are these magic values - 'LEAD_STATE': 0x7, - 'LEAD_DISTANCE': 0x1e, - } - commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx)) return commands diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 94b73917f55d93..d2d45bf088ddb9 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -55,6 +55,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay ret.steerLimitTimer = 0.4 ret.enableGasInterceptor = 0x201 in fingerprint[0] + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kiBP = [0., 35.] + if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] ret.gasMaxV = [0.2, 0.5, 0.7] @@ -116,19 +121,22 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] ret.lateralTuning.lqr.l = [0.3233671, 0.3185757] ret.lateralTuning.lqr.dcGain = 0.002237852961363602 + if ret.enableGasInterceptor: + ret.longitudinalTuning.kpV = [1.2 * 0.925, 0.8 * 0.9125, 0.5 * 0.9] + ret.longitudinalTuning.kiV = [0.18 * 1.05, 0.12 * 1.15] elif candidate == CAR.COROLLA: stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 - ret.steerRatio = 18.27 + ret.steerRatio = 14.0 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 * 0.639 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 if ret.enableGasInterceptor: - ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] - # ret.longitudinalTuning.kiV = [0.135, 0.09] + ret.longitudinalTuning.kpV = [1.2 * 0.925, 0.8 * 0.9125, 0.5 * 0.9] + ret.longitudinalTuning.kiV = [0.18 * 1.05, 0.12 * 1.15] elif candidate == CAR.LEXUS_RXH: stop_and_go = True @@ -279,10 +287,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay # removing the DSU disables AEB and it's considered a community maintained feature ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kiBP = [0., 35.] ret.stoppingControl = False ret.startAccel = 0.0 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6379dd768908f6..ab8676c3b4783f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,7 +11,7 @@ from selfdrive.config import Conversions as CV from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_alert -from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET +# from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import get_events, \ create_event, \ EventTypes as ET, \ @@ -28,6 +28,7 @@ from selfdrive.controls.lib.gps_helpers import is_rhd_region from selfdrive.locationd.calibration_helpers import Calibration, Filter from common.travis_checker import travis +from common.op_params import opParams LANE_DEPARTURE_THRESHOLD = 0.1 @@ -239,7 +240,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm_smiskol): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -258,10 +259,14 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) - # send FCW alert if triggered by planner - if plan.fcw or CS.stockFcw: + if plan.fcw: + # send FCW alert if triggered by planner AM.add(frame, "fcw", enabled) + elif CS.stockFcw: + # send a silent alert when stock fcw triggers, since the car is already beeping + AM.add(frame, "fcwStock", enabled) + # State specific actions if state in [State.preEnabled, State.disabled]: @@ -286,7 +291,13 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop - passable_loc = {'lead_one': passable_state_control['lead_one'], 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead} + passable_loc = {} + if not travis: + passable_loc['lead_one'] = sm_smiskol['radarState'].leadOne + passable_loc['mpc_TR'] = sm_smiskol['smiskolData'].mpcTR + passable_loc['live_tracks'] = sm_smiskol['liveTracks'] + passable_loc['has_lead'] = plan.hasLead + passable_loc['gas_pressed'] = CS.gasPressed actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable_loc) # Steering PID loop and lateral MPC @@ -317,7 +328,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr 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, - last_blinker_frame, is_ldw_enabled): + last_blinker_frame, is_ldw_enabled, op_params): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() @@ -350,6 +361,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + CAMERA_OFFSET = op_params.get('camera_offset', 0.06) l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) @@ -479,7 +491,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ - 'model', 'gpsLocation', 'radarState'], ignore_alive=['gpsLocation']) + 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) + sm_smiskol = messaging.SubMaster(['radarState', 'smiskolData', 'liveTracks']) if can_sock is None: @@ -553,8 +566,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None prof = Profiler(False) # off by default + op_params = opParams() while True: + sm_smiskol.update(0) start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) @@ -597,19 +612,15 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - if not travis: - passable_state_control = {'lead_one': sm['radarState'].leadOne} - else: - passable_state_control = {'lead_one': None} actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm_smiskol) prof.checkpoint("State Control") # Publish data 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, last_blinker_frame, is_ldw_enabled) + LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled, op_params) prof.checkpoint("Sent") rk.monitor_time() diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index a7d85058aba4cc..e9295bfef7a110 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -80,6 +80,13 @@ def __gt__(self, alert2): AlertStatus.critical, AlertSize.full, Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeat, 1., 2., 2.), + Alert( + "fcwStock", + "BRAKE!", + "Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.), # no EON chime for stock FCW + Alert( "steerSaturated", "TAKE CONTROL", diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index ed838d42871f5a..0f6ab12b6621a2 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -3,7 +3,8 @@ import numpy as np from cereal import log -CAMERA_OFFSET = opParams().get('camera_offset', 0.06) # m from center car to camera +op_params = opParams() +# CAMERA_OFFSET = opParams().get('camera_offset', 0.06) # m from center car to camera def compute_path_pinv(l=50): deg = 3 @@ -72,6 +73,7 @@ def parse_model(self, md): def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense + CAMERA_OFFSET = op_params.get('camera_offset', 0.06) self.l_poly[3] += CAMERA_OFFSET self.r_poly[3] += CAMERA_OFFSET diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index e90064a2d50229..8ede06afdde78c 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -31,12 +31,16 @@ def __init__(self, mpc_id): self.new_lead = False self.last_cloudlog_t = 0.0 + self.pm = None self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 self.customTR = self.op_params.get('following_distance', None) + def set_pm(self, pm): + self.pm = pm + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) dat = messaging.new_message() @@ -79,8 +83,16 @@ def get_TR(self, CS): if not travis: self.change_cost(TR) + 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.new_message() + dat.init('smiskolData') + dat.smiskolData.mpcTR = TR + self.pm.send('smiskolData', dat) + def change_cost(self, TR): TRs = [0.9, 1.8, 2.7] costs = [1.0, 0.1, 0.05] @@ -117,7 +129,9 @@ def lead_accel_over_time(self): def dynamic_follow(self, CS): # 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 # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs - x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469] + # 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 + # y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs #todo: average this and + x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469] # todo: this! y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881] sng_TR = 1.8 # stop and go parameters @@ -131,13 +145,13 @@ def dynamic_follow(self, CS): TR = interp(self.car_data['v_ego'], x, y) # Dynamic follow modifications (the secret sauce) - x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909] # relative velocity values - y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28] # modification values + x = [-20.0, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0.0, 1.3411, 1.89, 2.6909] # relative velocity values + y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.06, -0.144, -0.224] # modification values TR_mod = interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y) - x = [-4.4704, -1.77, -0.3145, 0, 0.446, 1.3411] # lead acceleration values - y = [0.237, 0.12, 0.027, 0, -0.105, -0.195] # modification values - TR_mod += interp(self.lead_accel_over_time(), x, y) # todo: test if these modifications are too much + x = [-4.4704, -1.77, -0.3145, 0.0, 0.1495, 0.5104, 0.7037, 0.9357] # lead acceleration values + y = [0.237, 0.12, 0.027, 0, -0.006, -0.036, -0.042, -0.045] # modification values + TR_mod += interp(self.lead_accel_over_time(), x, y) TR += TR_mod @@ -178,6 +192,10 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Setup current mpc state self.cur_state[0].x_ego = 0.0 + + if not travis and CS.cruiseState.enabled and CS.gearShifter == 'drive': + with open("/data/set_speed", "a") as f: + f.write("{}".format([v_ego, CS.cruiseState.speed, time.time()])) if lead is not None and lead.status: x_lead = lead.dRel diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ed4ba308c7d51a..d5a5f7cbfcaa12 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,8 +1,14 @@ from cereal import log from common.numpy_fast import clip, interp -from selfdrive.controls.lib.pid import PIController from common.travis_checker import travis -from selfdrive.car.toyota.values import CAR +if not travis: + from selfdrive.controls.lib.pid import PIController +else: + from selfdrive.controls.lib.pid import PIController +from selfdrive.car.toyota.values import CAR as CAR_TOYOTA +from selfdrive.config import Conversions as CV +from common.op_params import opParams +import numpy as np LongCtrlState = log.ControlsState.LongControlState @@ -67,11 +73,20 @@ def __init__(self, CP, compute_gb, candidate): convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 + + self.op_params = opParams() + self.use_dynamic_lane_speed = self.op_params.get('use_dynamic_lane_speed', default=True) + self.min_dynamic_lane_speed = max(self.op_params.get('min_dynamic_lane_speed', default=15.), 15.) * CV.MPH_TO_MS + self.candidate = candidate + self.toyota_candidates = [attr for attr in dir(CAR_TOYOTA) if not attr.startswith("__")] + + self.gas_pressed = False self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False} + self.track_data = [] + self.mpc_TR = 1.8 self.v_ego = 0.0 - self.gas_pressed = False - self.candidate = candidate - self.toyota_candidates = [attr for attr in dir(CAR) if not attr.startswith("__")] + self.k_i_last = 0.0 + def reset(self, v_pid): """Reset PID controller and change setpoint""" @@ -80,53 +95,106 @@ def reset(self, v_pid): def dynamic_gas(self, CP): x, y = [], [] - if CP.enableGasInterceptor: # if pedal, todo: make different profiles for different vehicles - if self.candidate in [CAR.COROLLA]: + if CP.enableGasInterceptor: # todo: make different profiles for different vehicles + if self.candidate == CAR_TOYOTA.COROLLA: x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] - y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) - elif self.candidate in self.toyota_candidates: - x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] - y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] + y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] + elif self.candidate == CAR_TOYOTA.RAV4: + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] + y = [0.234, 0.237, 0.246, 0.26, 0.279, 0.297, 0.332, 0.354, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] + + # elif self.candidate in self.toyota_candidates: + # x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] + # y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] if not x: - x, y = CP.gasMaxBP, CP.gasMaxV # if unsupported car, use stock. todo: think about disallowing dynamic follow for unsupported cars + # x, y = CP.gasMaxBP, CP.gasMaxV # if unsupported car, use stock. todo: think about disallowing dynamic follow for unsupported cars + return interp(self.v_ego, CP.gasMaxBP, CP.gasMaxV) gas = interp(self.v_ego, x, y) if self.lead_data['status']: # if lead if self.v_ego <= 8.9408: # if under 20 mph - # TR = 1.8 # desired TR, might need to switch this to hardcoded distance values - # current_TR = self.lead_data['x_lead'] / self.v_ego if self.v_ego > 0 else TR - x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481] # relative velocity mod y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0] gas_mod = -interp(self.lead_data['v_rel'], x, y) - # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684] # lead accel mod # todo: this - # y = [0.0, (gas * 0.08), (gas * 0.20), (gas * 0.4), (gas * 0.52), (gas * 0.6)] - # gas_mod += interp(a_lead, x, y) + x = [0.44704, 1.78816] # lead accel mod + y = [0.0, gas_mod * .4] # maximum we can reduce gas_mod is 40 percent of it + gas_mod -= interp(self.lead_data['a_lead'], x, y) # reduce the reduction of the above mod (the max this will ouput is the original gas value, it never increases it) # x = [TR * 0.5, TR, TR * 1.5] # as lead gets further from car, lessen gas mod # todo: this # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5] # gas_mod += (interp(current_TR, x, y)) - new_gas = gas + gas_mod # (interp(current_TR, x, y)) + new_gas = gas + gas_mod x = [1.78816, 6.0, 8.9408] # slowly ramp mods down as we approach 20 mph y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas] gas = interp(self.v_ego, x, y) else: - x = [-0.89408, 0, 2.0] # need to tune this - y = [-.17, -.08, .01] - gas += interp(self.lead_data['v_rel'], x, y) + current_TR = self.lead_data['x_lead'] / self.v_ego + x = [-1.78816, -0.89408, 0, 1.78816, 2.68224] # relative velocity mod + y = [-gas * 0.35, -gas * 0.25, -gas * 0.075, gas * 0.1575, gas * 0.2025] + gas_mod = interp(self.lead_data['v_rel'], x, y) + + x = [self.mpc_TR - 0.22, self.mpc_TR, self.mpc_TR + 0.2, self.mpc_TR + 0.4] + y = [-gas_mod * 0.36, 0.0, gas_mod * 0.15, gas_mod * 0.4] + gas_mod -= interp(current_TR, x, y) + + gas += gas_mod return clip(gas, 0.0, 1.0) + def handle_live_tracks(self, live_tracks): + self.track_data = [] + for track in live_tracks: + self.track_data.append({'v_lead': self.v_ego + track.vRel, 'y_rel': track.yRel, 'x_lead': track.dRel}) + + def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target): + v_cruise *= CV.KPH_TO_MS # convert to m/s + MPC_TIME_STEP = 1 / 20. + track_tolerance_v = 0.05 * CV.MPH_TO_MS + + vels = [i * CV.MPH_TO_MS for i in [5, 40, 70]] + margins = [0.4, 0.55, 0.6] + track_speed_margin = interp(self.v_ego, vels, margins) # tracks must be within this times v_ego + + max_TR = 2.0 # the maximum TR we'll allow for each track + + if self.v_ego > self.min_dynamic_lane_speed and len(self.track_data) > 0: + tracks = [] + for track in self.track_data: + valid = all([True if abs(trk['v_lead'] - track['v_lead']) >= track_tolerance_v else False for trk in tracks]) # radar sometimes reports multiple points for one vehicle, especially semis + if valid: + tracks.append(track) + tracks = [trk for trk in tracks if (self.v_ego * track_speed_margin) <= trk['v_lead'] <= v_cruise] + tracks = [trk['v_lead'] for trk in tracks if trk['v_lead'] > 0.0 and (trk['x_lead'] / trk['v_lead']) <= max_TR] + average_track_speed = np.mean(tracks) + if average_track_speed < v_target and average_track_speed < v_target_future: + x = [0, 3, 6, 19] + y = [.05, 0.2, .4, 0.5] + # todo: give less weight to further away tracks, but increase the above! + track_speed_weight = interp(len(tracks), x, y) + if self.lead_data['status']: # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...) + track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y)) + # v_ego_v_cruise = (self.v_ego + v_cruise) / 2.0 + v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight) # average set speed and average of tracks + if v_target_slow < v_target and v_target_slow < v_target_future: # just a sanity check, don't want to run into any leads if we somehow predict faster velocity + a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0) # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that? + a_target = a_target_slow + v_target = v_target_slow + v_target_future = v_target_slow + + return v_target, v_target_future, a_target + def handle_passable(self, passable): self.gas_pressed = passable['gas_pressed'] self.lead_data['v_rel'] = passable['lead_one'].vRel self.lead_data['a_lead'] = passable['lead_one'].aLeadK self.lead_data['x_lead'] = passable['lead_one'].dRel self.lead_data['status'] = passable['has_lead'] # this fixes radarstate always reporting a lead, thanks to arne + self.mpc_TR = passable['mpc_TR'] + self.handle_live_tracks(passable['live_tracks']) def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable): """Update longitudinal control. This updates the state machine and runs a PID loop""" @@ -134,8 +202,16 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ # Actuation limits if not travis: + # k_i = self.op_params.get('longkiV', default=0.0) + # if k_i != self.k_i_last: + # self.v_pid = v_ego + # self.pid.reset() + # self.k_i_last = k_i + # self.pid.k_i = k_i self.handle_passable(passable) gas_max = self.dynamic_gas(CP) + if self.use_dynamic_lane_speed: + v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target) else: gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 74f68454fcc4a0..22d9de943c5901 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -10,6 +10,7 @@ from cereal import log from common.op_params import opParams from selfdrive.controls.lane_hugging import LaneHugging +from common.travis_checker import travis LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection @@ -49,6 +50,7 @@ def __init__(self, CP): self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost + self.steer_ratio = CP.steerRatio self.setup_mpc() self.solution_invalid_cnt = 0 diff --git a/selfdrive/controls/lib/pid_long.py b/selfdrive/controls/lib/pid_long.py new file mode 100644 index 00000000000000..046cfcef14cb91 --- /dev/null +++ b/selfdrive/controls/lib/pid_long.py @@ -0,0 +1,85 @@ +import numpy as np +from common.numpy_fast import clip, interp + +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + +class PIController(): + def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p # proportional gain + self._k_i = k_i # integral gain + self.k_f = k_f # feedforward gain + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.sat_limit = sat_limit + self.convert = convert + + self.reset() + self.k_i = 0.0 # modify this when tuning integral + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + def _check_saturation(self, control, check_saturation, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and check_saturation and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.f = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + self.speed = speed + + error = float(apply_deadzone(setpoint - measurement, deadzone)) + self.p = error * self.k_p + self.f = feedforward * self.k_f + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + self.f + i + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + + control = self.p + self.f + self.i + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + self.saturated = self._check_saturation(control, check_saturation, error) + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e2e07409a03b3c..a2901d08625408 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -29,7 +29,7 @@ # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits if not travis: - _A_CRUISE_MAX_V = [2.0, 1.9, 0.8, .4] + _A_CRUISE_MAX_V = [1.8, 1.8, 0.7, .4] else: _A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4] _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] @@ -67,6 +67,9 @@ def __init__(self, CP): self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) + if not travis: + pm = messaging.PubMaster(['smiskolData']) + self.mpc1.set_pm(pm) self.v_acc_start = 0.0 self.a_acc_start = 0.0 diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 1559a893e8cedd..39d03c796daa8b 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import numpy as np from numpy.linalg import solve +from common.op_params import opParams +from common.travis_checker import travis """ Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani" @@ -106,13 +108,16 @@ def __init__(self, CP): self.cF_orig = CP.tireStiffnessFront self.cR_orig = CP.tireStiffnessRear + self.static_steer_ratio = opParams().get('static_steer_ratio', default=False) + self.sR = CP.steerRatio self.update_params(1.0, CP.steerRatio) def update_params(self, stiffness_factor, steer_ratio): """Update the vehicle model with a new stiffness factor and steer ratio""" self.cF = stiffness_factor * self.cF_orig self.cR = stiffness_factor * self.cR_orig - self.sR = steer_ratio + if travis or not self.static_steer_ratio: # leave sR at CP.steerRatio if static_steer_ratio is True + self.sR = steer_ratio def steady_state_sol(self, sa, u): """Returns the steady state solution. diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index bea384b5431756..fc17e7c85f85a0 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -134,23 +134,24 @@ def gen_upload_files(self): yield (name, key, fn) - def next_file_to_upload(self, with_raw): - upload_files = list(self.gen_upload_files()) - # try to upload qlog files first - for name, key, fn in upload_files: - if name in self.immediate_priority: - return (key, fn) - - if with_raw: - # then upload the full log files, rear and front camera files + def next_file_to_upload(self, with_raw, should_upload): + if should_upload: + upload_files = list(self.gen_upload_files()) + # try to upload qlog files first for name, key, fn in upload_files: - if name in self.high_priority: + if name in self.immediate_priority: return (key, fn) - # then upload other files - for name, key, fn in upload_files: - if not name.endswith('.lock') and not name.endswith(".tmp"): - return (key, fn) + if with_raw: + # then upload the full log files, rear and front camera files + for name, key, fn in upload_files: + if name in self.high_priority: + return (key, fn) + + # then upload other files + for name, key, fn in upload_files: + if not name.endswith('.lock') and not name.endswith(".tmp"): + return (key, fn) return None @@ -244,7 +245,7 @@ def uploader_fn(exit_event): if exit_event.is_set(): return - d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload) + d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload, should_upload=should_upload) if d is None: time.sleep(5) continue diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f603a2498603c3..0c0be19b61858f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b2364d6239bca0a8caaf11f0433bf766c66e15a4 \ No newline at end of file +89304bdcab73fa43a8dd39cab93bc4ea4c9cbbdb \ No newline at end of file