diff --git a/cereal/car.capnp b/cereal/car.capnp index e5dd9ccec98237..fdfb3baf11277b 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -455,10 +455,12 @@ struct CarParams { struct LateralINDITuning { outerLoopGainV @0 :List(Float32); outerLoopGainBP @1 :List(Float32); - innerLoopGain @2 :Float32; - timeConstantBP @3 :List(Float32); - timeConstantV @4:List(Float32); - actuatorEffectiveness @5 :Float32; + innerLoopGainBP @2 :List(Float32); + innerLoopGainV @3 :List(Float32); + timeConstantBP @4 :List(Float32); + timeConstantV @5:List(Float32); + actuatorEffectivenessBP @6 :List(Float32); + actuatorEffectivenessV @7 :List(Float32); } struct LateralLQRTuning { diff --git a/common/numpy_fast.py b/common/numpy_fast.py index a8361214d1ae44..9686eab4087465 100644 --- a/common/numpy_fast.py +++ b/common/numpy_fast.py @@ -6,13 +6,14 @@ def clip(x, lo, hi): def interp(x, xp, fp): N = len(xp) + T = len(fp) def get_interp(xv): hi = 0 while hi < N and xv > xp[hi]: hi += 1 low = hi - 1 - return fp[-1] if hi == N and xv > xp[low] else ( + return fp[-1] if hi >= T or (hi == N and xv > xp[low]) or (N < T and xv == xp[-1]) else ( fp[0] if hi == 0 else (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) diff --git a/common/op_params.py b/common/op_params.py index 35ed3b7c536701..16227a4b1550d5 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -18,7 +18,7 @@ class ValueTypes: class Param: - def __init__(self, default=None, allowed_types=[], description=None, live=False, hidden=False): + def __init__(self, default=None, allowed_types=[], description=None, live=False, hidden=False, depends_on=None): self.default = default if not isinstance(allowed_types, list): allowed_types = [allowed_types] @@ -26,21 +26,32 @@ def __init__(self, default=None, allowed_types=[], description=None, live=False, self.description = description self.hidden = hidden self.live = live + self.depends_on = depends_on + self.children = [] self._create_attrs() def is_valid(self, value): if not self.has_allowed_types: return True - return type(value) in self.allowed_types + if self.is_list and isinstance(value, list): + for v in value: + if type(v) not in self.allowed_types: + return False + return True + else: + return type(value) in self.allowed_types or value in self.allowed_types def _create_attrs(self): # Create attributes and check Param is valid self.has_allowed_types = isinstance(self.allowed_types, list) and len(self.allowed_types) > 0 self.has_description = self.description is not None self.is_list = list in self.allowed_types + self.is_bool = bool in self.allowed_types if self.has_allowed_types: - assert type(self.default) in self.allowed_types, 'Default value type must be in specified allowed_types!' - if self.is_list: - self.allowed_types.remove(list) + assert type(self.default) in self.allowed_types or self.default in self.allowed_types, 'Default value type must be in specified allowed_types!' + + if self.is_list and self.default: + for v in self.default: + assert type(v) in self.allowed_types, 'Default value type must be in specified allowed_types!' class opParams: @@ -103,7 +114,17 @@ def __init__(self): #'min_TR': Param(None, VT.none_or_number, 'The minimum allowed following distance in seconds. Default is 0.9 seconds.\n' #'The range is limited from 0.85 to 1.3. Set to None to disable', live=True), #'use_virtual_middle_line': Param(False, bool, 'For roads over 4m wide, hug right. For roads under 2m wide, hug left.'), - 'uniqueID': Param(None, [type(None), str], 'User\'s unique ID') + 'uniqueID': Param(None, [type(None), str], 'User\'s unique ID'), + 'enable_indi_live': Param(False, bool, live=True), + 'indi_inner_gain_bp': Param([0, 255, 255], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_inner_gain_v': Param([6.0, 6.0, 6.0], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_outer_gain_bp': Param([0, 255, 255], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_outer_gain_v': Param([15, 15, 15], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_time_constant_bp': Param([0, 255, 255], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_time_constant_v': Param([5.5, 5.5, 5.5], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_actuator_effectiveness_bp': Param([0, 255, 255], [list, float, int], live=True, depends_on='enable_indi_live'), + 'indi_actuator_effectiveness_v': Param([6, 6, 6], [list, float, int], live=True, depends_on='enable_indi_live'), + 'steer_limit_timer': Param(0.4, VT.number, live=True, depends_on='enable_indi_live') } self._params_file = '/data/op_params.json' @@ -111,6 +132,7 @@ def __init__(self): self._last_read_time = sec_since_boot() self.read_frequency = 2.5 # max frequency to read with self.get(...) (sec) self._to_delete = ['reset_integral', 'log_data'] # a list of params you want to delete (unused) + self._last_mod_time = 0. self._run_init() # restores, reads, and updates params def _run_init(self): # does first time initializing of default params @@ -118,6 +140,14 @@ def _run_init(self): # does first time initializing of default params self.fork_params['username'] = Param(None, [type(None), str, bool], 'Your identifier provided with any crash logs sent to Sentry.\nHelps the developer reach out to you if anything goes wrong') self.fork_params['op_edit_live_mode'] = Param(False, bool, 'This parameter controls which mode opEdit starts in', hidden=True) self.params = self._get_all_params(default=True) # in case file is corrupted + + for k, p in self.fork_params.items(): + d = p.depends_on + while d: + fp = self.fork_params[d] + fp.children.append(k) + d = fp.depends_on + if travis: return @@ -206,13 +236,19 @@ def _update_params(self, param_info, force_live): self._last_read_time = sec_since_boot() def _read(self): - try: - with open(self._params_file, "r") as f: - self.params = json.loads(f.read()) - return True - except Exception as e: - error(e) - return False + if os.path.isfile(self._params_file): + try: + mod_time = os.path.getmtime(self._params_file) + if mod_time > self._last_mod_time: + with open(self._params_file, "r") as f: + self.params = json.loads(f.read()) + self._last_mod_time = mod_time + return True + else: + return False + except Exception as e: + print("Unable to read file: " + str(e)) + return False def _write(self): if not travis: diff --git a/op_edit.py b/op_edit.py index 6f25b521dea5e5..7d3c7bd75d83ef 100755 --- a/op_edit.py +++ b/op_edit.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 import time -from common.op_params import opParams import ast import difflib + +from common.op_params import opParams from common.colors import COLORS +from collections import OrderedDict class opEdit: # use by running `python /data/openpilot/op_edit.py` @@ -57,9 +59,11 @@ def run_loop(self): if self.live_tuning: # only display live tunable params self.params = {k: v for k, v in self.params.items() if self.op_params.param_info(k).live} + self.params = OrderedDict(sorted(self.params.items(), key=self.sort_params)) + values_list = [] for k, v in self.params.items(): - if len(str(v)) < 20: + if len(str(v)) < 30 or len(str(v)) <= len('{} ... {}'.format(str(v)[:30], str(v)[-15:])): v_color = '' if type(v) in self.type_colors: v_color = self.type_colors[type(v)] @@ -74,14 +78,35 @@ def run_loop(self): to_print = [] blue_gradient = [33, 39, 45, 51, 87] + last_key = '' + last_info = None + shown_dots = False 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 = COLORS.OKGREEN + line - else: - _color = blue_gradient[min(round(idx / len(self.params) * len(blue_gradient)), len(blue_gradient) - 1)] - line = COLORS.BASE(_color) + line - to_print.append(line) + info = self.op_params.param_info(param) + indent = self.get_sort_key(param).count(',') + line = '' + if not info.depends_on or param in last_info.children and \ + self.op_params.get(last_key) and self.op_params.get(info.depends_on): + line = '{}. {}: {} {}'.format(idx + 1, param, values_list[idx], live[idx]) + line = indent * '.' + line + elif not shown_dots and last_info and param in last_info.children: + line = '...' + shown_dots = True + if line: + if idx == self.last_choice and self.last_choice is not None: + line = COLORS.OKGREEN + line + else: + _color = blue_gradient[min(round(idx / len(self.params) * len(blue_gradient)), len(blue_gradient) - 1)] + line = COLORS.BASE(_color) + line + if last_info and len(last_info.children) and indent == 0: + line = '\n' + line + shown_dots = False + to_print.append(line) + + if indent == 0: + last_key = param + last_info = info + extras = {'a': ('Add new parameter', COLORS.OKGREEN), 'd': ('Delete parameter', COLORS.FAIL), @@ -93,7 +118,7 @@ def run_loop(self): self.prompt('\nChoose a parameter to edit (by index or name):') choice = input('>> ').strip().lower() - parsed, choice = self.parse_choice(choice, len(to_print) - len(extras)) + parsed, choice = self.parse_choice(choice, len(self.params) + len(extras)) if parsed == 'continue': continue elif parsed == 'add': @@ -153,7 +178,7 @@ def change_parameter(self, choice): if param_info.has_description: to_print.append(COLORS.OKGREEN + '>> Description: {}'.format(param_info.description.replace('\n', '\n > ')) + COLORS.ENDC) if param_info.has_allowed_types: - to_print.append(COLORS.RED + '>> Allowed types: {}'.format(', '.join([at.__name__ for at in param_info.allowed_types])) + COLORS.ENDC) + to_print.append(COLORS.RED + '>> Allowed types: {}'.format(', '.join([at.__name__ if isinstance(at, type) else at for at in param_info.allowed_types])) + COLORS.ENDC) if param_info.live: live_msg = '>> This parameter supports live tuning!' if not self.live_tuning: @@ -180,6 +205,10 @@ def change_parameter(self, choice): return new_value = self.str_eval(new_value) + + if param_info.is_bool and type(new_value) is int: + new_value = bool(new_value) + if not param_info.is_valid(new_value): self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) continue @@ -201,31 +230,58 @@ def change_parameter(self, choice): def change_param_list(self, old_value, param_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('>> ')) + self.prompt('\nEnter index to edit (0 to {}), or -i to remove index, or +value to append value:'.format(len(old_value) - 1)) + + append_val = False + remove_idx = False + choice_idx = input('>> ') + if choice_idx == '': self.info('Exiting this parameter...', 0.5) return - if not isinstance(choice_idx, int) or choice_idx not in range(len(old_value)): + if isinstance(choice_idx, str): + if choice_idx[0] == '-': + remove_idx = True + if choice_idx[0] == '+': + append_val = True + + if append_val or remove_idx: + choice_idx = choice_idx[1::] + + choice_idx = self.str_eval(choice_idx) + is_list = isinstance(choice_idx, list) + + if not append_val and not (is_list or (isinstance(choice_idx, int) and choice_idx 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 append_val or remove_idx or is_list: + new_value = choice_idx + else: + 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() + new_value = self.str_eval(new_value) + if new_value == '': self.info('Exiting this list item...', 0.5) break - new_value = self.str_eval(new_value) if not param_info.is_valid(new_value): self.error('The type of data you entered ({}) is not allowed with this parameter!'.format(type(new_value).__name__)) - continue + break - old_value[choice_idx] = new_value + if append_val: + old_value.append(new_value) + elif remove_idx: + del old_value[choice_idx] + elif is_list: + old_value = new_value + else: + 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') @@ -302,9 +358,9 @@ def str_eval(self, dat): except: if dat.lower() == 'none': dat = None - elif dat.lower() == 'false': + elif dat.lower() in ['false', 'f']: dat = False - elif dat.lower() == 'true': # else, assume string + elif dat.lower() in ['true', 't']: # else, assume string dat = True return dat @@ -360,5 +416,19 @@ def add_parameter(self): self.info('Not saved!') return + def sort_params(self, kv): + return self.get_sort_key(kv[0]) + + def get_sort_key(self, k): + p = self.op_params.param_info(k) + + if not p.depends_on: + return f'{1 if not len(p.children) else 0}{k}' + else: + s = '' + while p.depends_on: + s = f'{p.depends_on},{s}' + p = self.op_params.param_info(p.depends_on) + return f'{0}{s}{k}' opEdit() diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 8f57f21460b592..8ca76be4171907 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -76,12 +76,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRatio = 15 # dp - indi value from donfyffe ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 3.1 - ret.lateralTuning.indi.outerLoopGainBP = [] + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [3.1] + ret.lateralTuning.indi.outerLoopGainBP = [0] ret.lateralTuning.indi.outerLoopGainV = [2.1] - ret.lateralTuning.indi.timeConstantBP = [] + ret.lateralTuning.indi.timeConstantBP = [0] ret.lateralTuning.indi.timeConstantV = [1.3] - ret.lateralTuning.indi.actuatorEffectiveness = 1.4 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.4] ret.minSteerSpeed = 32 * CV.MPH_TO_MS ret.minEnableSpeed = 10 * CV.MPH_TO_MS elif candidate == CAR.KONA: @@ -162,12 +164,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, # Genesis elif candidate == CAR.GENESIS_G70: ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 2.4 - ret.lateralTuning.indi.outerLoopGainBP = [] + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [2.4] + ret.lateralTuning.indi.outerLoopGainBP = [0] ret.lateralTuning.indi.outerLoopGainV = [3.0] - ret.lateralTuning.indi.timeConstantBP = [] + ret.lateralTuning.indi.timeConstantBP = [0] ret.lateralTuning.indi.timeConstantV = [1.0] - ret.lateralTuning.indi.actuatorEffectiveness = 2.0 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.0] ret.steerActuatorDelay = 0.4 # 0.08 stock ret.steerLimitTimer = 0.4 # down from 0.4 tire_stiffness_factor = 1.0 @@ -181,12 +185,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.wheelbase = 3.01 ret.steerRatio = 16.5 ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 3.5 - ret.lateralTuning.indi.outerLoopGainBP = [] + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [3.5] + ret.lateralTuning.indi.outerLoopGainBP = [0] ret.lateralTuning.indi.outerLoopGainV = [2.0] - ret.lateralTuning.indi.timeConstantBP = [] + ret.lateralTuning.indi.timeConstantBP = [0] ret.lateralTuning.indi.timeConstantV = [1.4] - ret.lateralTuning.indi.actuatorEffectiveness = 2.3 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] ret.minSteerSpeed = 60 * CV.KPH_TO_MS elif candidate == CAR.GENESIS_G90: ret.mass = 2200 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index eb5c3854cac4cf..1eeb6017d1ac9f 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -51,13 +51,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 4.0 - ret.lateralTuning.indi.outerLoopGainBP = [] + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [4.0] + ret.lateralTuning.indi.outerLoopGainBP = [0] ret.lateralTuning.indi.outerLoopGainV = [3.0] - ret.lateralTuning.indi.timeConstantBP = [] + ret.lateralTuning.indi.timeConstantBP = [0] ret.lateralTuning.indi.timeConstantV = [1.0] - ret.lateralTuning.indi.actuatorEffectiveness = 1.0 - ret.steerActuatorDelay = 0.5 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.0] + ret.steerActuatorDelay = 0.5 elif candidate == CAR.PRIUS_TSS2: #ret.longitudinalTuning.kpV = [0.4, 0.36, 0.325] # braking tune from rav4h @@ -78,12 +80,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.lateralTuning.pid.kf = 0.00009531750004645412 else: ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 4.0 + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [4.0] ret.lateralTuning.indi.outerLoopGainBP = [20, 21, 25, 26] ret.lateralTuning.indi.outerLoopGainV = [3.0, 4.0, 5.0, 6.0] ret.lateralTuning.indi.timeConstantBP = [12, 16, 20, 24] ret.lateralTuning.indi.timeConstantV = [1.0, 2.0, 3.0, 4.0] - ret.lateralTuning.indi.actuatorEffectiveness = 1.0 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.0] elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -219,12 +223,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerActuatorDelay = 0.12 ret.steerLimitTimer = 5.0 ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 15.0 + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [15.0] ret.lateralTuning.indi.outerLoopGainBP = [20, 21, 24, 25] ret.lateralTuning.indi.outerLoopGainV = [9.5, 16, 16, 21] ret.lateralTuning.indi.timeConstantBP = [14, 18, 29, 30] ret.lateralTuning.indi.timeConstantV = [5.0, 5.5, 6.0, 6.5] - ret.lateralTuning.indi.actuatorEffectiveness = 15.0 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [15.0] elif candidate == CAR.RAV4H_TSS2: stop_and_go = True @@ -238,12 +244,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerActuatorDelay = 0.12 ret.steerLimitTimer = 5.0 ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 15.0 + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [15.0] ret.lateralTuning.indi.outerLoopGainBP = [20, 21, 24, 25] ret.lateralTuning.indi.outerLoopGainV = [9.5, 16, 16, 21] ret.lateralTuning.indi.timeConstantBP = [14, 18, 29, 30] ret.lateralTuning.indi.timeConstantV = [5.0, 5.5, 6.0, 6.5] - ret.lateralTuning.indi.actuatorEffectiveness = 15.0 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [15.0] elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True @@ -255,12 +263,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerActuatorDelay = 0.68 ret.steerLimitTimer = 5.0 ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 15.0 + ret.lateralTuning.indi.innerLoopGainBP = [0] + ret.lateralTuning.indi.innerLoopGainV = [15.0] ret.lateralTuning.indi.outerLoopGainBP = [5.5, 20, 21, 25, 26] ret.lateralTuning.indi.outerLoopGainV = [3.75, 4.25, 8.5, 9.5, 14.99] ret.lateralTuning.indi.timeConstantBP = [4.5, 15, 20, 26] ret.lateralTuning.indi.timeConstantV = [2.8, 3.7, 4.0, 4.0] - ret.lateralTuning.indi.actuatorEffectiveness = 15.0 + ret.lateralTuning.indi.actuatorEffectivenessBP = [0] + ret.lateralTuning.indi.actuatorEffectivenessV = [15.0] elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]: stop_and_go = True diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 48c31851c76963..f5f320bba357ae 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -4,13 +4,14 @@ from cereal import log from common.realtime import DT_CTRL from common.numpy_fast import clip, interp +from common.op_params import opParams from selfdrive.car.toyota.values import SteerLimitParams from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.controls.lib.drive_helpers import get_steer_max class LatControlINDI(): - def __init__(self, CP): + def __init__(self, CP, OP=None): self.angle_steers_des = 0. A = np.array([[1.0, DT_CTRL, 0.0], @@ -34,15 +35,11 @@ def __init__(self, CP): self.enforce_rate_limit = CP.carName == "toyota" - self.RC = interp(0, CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) - self.G = CP.lateralTuning.indi.actuatorEffectiveness - self.outer_loop_gain = interp(0, CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) - self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain - self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) + if OP is None: + OP = opParams() + self.op_params = OP self.sat_count_rate = 1.0 * DT_CTRL - self.sat_limit = CP.steerLimitTimer - self.reset() def reset(self): @@ -63,9 +60,33 @@ def _check_saturation(self, control, check_saturation, limit): return self.sat_count > self.sat_limit def update(self, active, CS, CP, path_plan): - # Update params - self.outer_loop_gain = interp(CS.vEgo, CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) - self.RC = interp(CS.vEgo, CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) # Thanks to jamcar23 for the setup of timeConstantBP & V + if self.op_params.get('enable_indi_live'): + self.sat_limit = self.op_params.get('steer_limit_timer') + + act_bp = self.op_params.get('indi_actuator_effectiveness_bp') + act_v = self.op_params.get('indi_actuator_effectiveness_v') + outer_bp = self.op_params.get('indi_outer_gain_bp') + outer_v = self.op_params.get('indi_outer_gain_v') + inner_bp = self.op_params.get('indi_inner_gain_bp') + inner_v = self.op_params.get('indi_inner_gain_v') + time_bp = self.op_params.get('indi_time_constant_bp') + time_v = self.op_params.get('indi_time_constant_v') + elif CP.lateralTuning.which() == 'indi': + act_bp = CP.lateralTuning.indi.actuatorEffectivenessBP + act_v = CP.lateralTuning.indi.actuatorEffectivenessV + outer_bp = CP.lateralTuning.indi.outerLoopGainBP + outer_v = CP.lateralTuning.indi.outerLoopGainV + inner_bp = CP.lateralTuning.indi.innerLoopGainBP + inner_v = CP.lateralTuning.indi.innerLoopGainV + time_bp = CP.lateralTuning.indi.timeConstantBP + time_v = CP.lateralTuning.indi.timeConstantV + + self.sat_limit = CP.steerLimitTimer + + self.G = interp(CS.vEgo, act_bp, act_v) + self.outer_loop_gain = interp(CS.vEgo, outer_bp, outer_v) + self.inner_loop_gain = interp(CS.vEgo, inner_bp, inner_v) + self.RC = interp(CS.vEgo, time_bp, time_v) self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) # Update Kalman filter