Live indi tuning (#755)

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 {

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])
 

common/op_params.py

@@ -18,29 +18,40 @@ 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]
     self.allowed_types = allowed_types
     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,21 +114,40 @@ 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'
     self._backup_file = '/data/op_params_corrupt.json'
     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
     # Two required parameters for opEdit
     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:

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()