adding live-tuning

selfdrive/car/chrysler/interface.py

@@ -78,11 +78,11 @@ def get_params(candidate, fingerprint):
     ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.7
-    ret.steerMPCReactTime = 0.0       # increase total MPC projected time by 0 ms
-    ret.steerMPCDampTime = 0.0        # dampen desired angle over 0ms (0 samples)
-    ret.steerReactTime = -0.0         # decrease total projected angle by 0 ms
-    ret.steerDampTime = 0.0           # dampen projected steer angle over 0ms (0 samples)
-    
+    ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+    ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+    ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+    ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
+
     if candidate == CAR.JEEP_CHEROKEE:
       ret.wheelbase = 2.91  # in meters
       ret.steerRatio = 12.7

selfdrive/car/ford/interface.py

@@ -77,11 +77,11 @@ def get_params(candidate, fingerprint):
     ret.steerKf = 1. / MAX_ANGLE   # MAX Steer angle to normalize FF
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
     ret.steerRateCost = 1.0
-    ret.steerMPCReactTime = 0.0       # increase total MPC projected time by 0 ms
-    ret.steerMPCDampTime = 0.0        # dampen desired angle over 0ms (0 samples)
-    ret.steerReactTime = -0.0         # decrease total projected angle by 0 ms
-    ret.steerDampTime = 0.0           # dampen projected steer angle over 0ms (0 samples)
-    
+    ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+    ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+    ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+    ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
+
     f = 1.2
     tireStiffnessFront_civic *= f
     tireStiffnessRear_civic *= f

selfdrive/car/gm/interface.py

@@ -57,11 +57,11 @@ def get_params(candidate, fingerprint):
     ret.carFingerprint = candidate
 
     ret.enableCruise = False
-    ret.steerMPCReactTime = 0.0       # increase total MPC projected time by 0 ms
-    ret.steerMPCDampTime = 0.0        # dampen desired angle over 0ms (0 samples)
-    ret.steerReactTime = -0.0         # decrease total projected angle by 0 ms
-    ret.steerDampTime = 0.0           # dampen projected steer angle over 0ms (0 samples)
-    
+    ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+    ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+    ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+    ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
+
     # Presence of a camera on the object bus is ok.
     # Have to go passive if ASCM is online (ACC-enabled cars),
     # or camera is on powertrain bus (LKA cars without ACC).

selfdrive/car/honda/interface.py

@@ -175,10 +175,10 @@ def get_params(candidate, fingerprint):
     rotationalInertia_civic = 2500
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
-    ret.steerMPCReactTime = 0.0       # increase total MPC projected time by 0 ms
-    ret.steerMPCDampTime = 0.0        # dampen desired angle over 0ms (0 samples)
-    ret.steerReactTime = -0.0         # decrease total projected angle by 0 ms
-    ret.steerDampTime = 0.0           # dampen projected steer angle over 0ms (0 samples)
+    ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+    ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+    ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+    ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
 
     # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle
     # model optimization process. Certain Hondas have an extra steering sensor at the bottom
@@ -199,6 +199,10 @@ def get_params(candidate, fingerprint):
       ret.steerRatio = 14.63  # 10.93 is end-to-end spec
       tire_stiffness_factor = 1.
       ret.syncID = 330
+      ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+      ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+      ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+      ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
       ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
@@ -221,9 +225,9 @@ def get_params(candidate, fingerprint):
       tire_stiffness_factor = 0.8467
       ret.syncID = 330
       ret.steerMPCReactTime = 0.05     # project desired angle 0 ms
-      ret.steerMPCDampTime = 0.3      # smooth desired angle over 300ms (30 samples)
+      ret.steerMPCDampTime = 0.25      # smooth desired angle over 300ms (30 samples)
       ret.steerReactTime = 0.0        # project steer angle 0 ms (using steer rate)
-      ret.steerDampTime = 0.3        # smooth projected steer angle over 300ms (30 samples)
+      ret.steerDampTime = 0.25        # smooth projected steer angle over 300ms (30 samples)
       ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]

selfdrive/car/hyundai/interface.py

@@ -68,11 +68,11 @@ def get_params(candidate, fingerprint):
     rotationalInertia_civic = 2500
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
-    ret.steerMPCReactTime = 0.0       # increase total MPC projected time by 0 ms
-    ret.steerMPCDampTime = 0.0        # dampen desired angle over 0ms (0 samples)
-    ret.steerReactTime = -0.0         # decrease total projected angle by 0 ms
-    ret.steerDampTime = 0.0           # dampen projected steer angle over 0ms (0 samples)
-    
+    ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+    ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+    ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+    ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
+
     ret.steerActuatorDelay = 0.1  # Default delay
     tire_stiffness_factor = 1.
 

selfdrive/car/toyota/interface.py

@@ -72,10 +72,10 @@ def get_params(candidate, fingerprint):
     rotationalInertia_civic = 2500
     tireStiffnessFront_civic = 192150
     tireStiffnessRear_civic = 202500
-    ret.steerMPCReactTime = 0.025      # increase total MPC projected time by 25 ms
-    ret.steerMPCDampTime = 0.10        # dampen desired angle over 100ms (10 samples)
-    ret.steerReactTime = -0.05         # decrease total projected angle by 50 ms
-    ret.steerDampTime = 0.10           # dampen projected steer angle over 100ms (10 samples)
+    ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+    ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+    ret.steerReactTime = -0.1         # decrease total projected angle by 100 ms
+    ret.steerDampTime = 0.2           # dampen projected steer angle over 200ms (20 control cycles)
 
     ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
@@ -89,10 +89,10 @@ def get_params(candidate, fingerprint):
       ret.mass = 3045 * CV.LB_TO_KG + std_cargo
       ret.steerKpV, ret.steerKiV = [[0.4], [0.01]]
       ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
-      ret.steerMPCProjectTime = 0.325    # project desired angle 32.5ms
-      ret.steerMPCSmoothTime = 0.30      # smooth desired angle over 30ms (30 samples)
-      ret.steerProjectTime = 0.30        # project steer angle 30.0 ms (using steer rate)
-      ret.steerSmoothTime = 0.01         # smooth projected steer angle over 1ms (1 sample)
+      ret.steerMPCReactTime = 0.05      # increase total MPC projected time by 50 ms
+      ret.steerMPCDampTime = 0.2        # dampen desired angle over 200ms (4 mpc cycles)
+      ret.steerReactTime = 0.15         # decrease total projected angle by 100 ms
+      ret.steerDampTime = 0.5           # dampen projected steer angle over 200ms (20 control cycles)
       # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis
       ret.steerActuatorDelay = 0.25
 

selfdrive/controls/lib/latcontrol.py

@@ -1,6 +1,8 @@
+import numpy as np
 from common.realtime import sec_since_boot
 from selfdrive.controls.lib.pid import PIController
 from common.numpy_fast import interp
+from selfdrive.kegman_conf import kegman_conf
 from cereal import car
 
 _DT = 0.01    # 100Hz
@@ -13,6 +15,36 @@ def get_steer_max(CP, v_ego):
 class LatControl(object):
   def __init__(self, CP):
 
+    kegman = kegman_conf()
+    self.write_conf = False
+    self.gernbySteer = True
+    kegman.conf['tuneGernby'] = str(1)
+    self.write_conf = True
+    if kegman.conf['tuneGernby'] == "-1":
+      kegman.conf['tuneGernby'] = str(1)
+      self.write_conf = True
+    if kegman.conf['reactSteer'] == "-1":
+      kegman.conf['reactSteer'] = str(round(CP.steerReactTime,3))
+      self.write_conf = True
+    if kegman.conf['dampSteer'] == "-1":
+      kegman.conf['dampSteer'] = str(round(CP.steerDampTime,3))
+      self.write_conf = True
+    if kegman.conf['reactMPC'] == "-1":
+      kegman.conf['reactMPC'] = str(round(CP.steerMPCReactTime,3))
+      self.write_conf = True
+    if kegman.conf['dampMPC'] == "-1":
+      kegman.conf['dampMPC'] = str(round(CP.steerMPCDampTime,3))
+      self.write_conf = True
+    if kegman.conf['Kp'] == "-1":
+      kegman.conf['Kp'] = str(round(CP.steerKpV[0],3))
+      self.write_conf = True
+    if kegman.conf['Ki'] == "-1":
+      kegman.conf['Ki'] = str(round(CP.steerKiV[0],3))
+      self.write_conf = True
+
+    if self.write_conf:
+      kegman.write_config(kegman.conf)
+
     self.mpc_frame = 0
     self.total_desired_projection = max(0.0, CP.steerMPCReactTime + CP.steerMPCDampTime)
     self.total_actual_projection = max(0.0, CP.steerReactTime + CP.steerDampTime)
@@ -32,11 +64,37 @@ def __init__(self, CP):
                             ([0.], KiV),
                             k_f=CP.steerKf, pos_limit=1.0)
 
+  def live_tune(self, CP):
+    self.mpc_frame += 1
+    if self.mpc_frame % 300 == 0:
+      # live tuning through /data/openpilot/tune.py overrides interface.py settings
+      kegman = kegman_conf()
+      if kegman.conf['tuneGernby'] == "1":
+        self.steerKpV = np.array([float(kegman.conf['Kp'])])
+        self.steerKiV = np.array([float(kegman.conf['Ki'])])
+        self.total_actual_projection = max(0.0, float(kegman.conf['dampSteer']) + float(kegman.conf['reactSteer']))
+        self.total_desired_projection = max(0.0, float(kegman.conf['dampMPC']) + float(kegman.conf['reactMPC']))
+        self.actual_smoothing = max(1.0, float(kegman.conf['dampSteer']) / _DT)
+        self.desired_smoothing = max(1.0, float(kegman.conf['dampMPC']) / _DT)
+        self.gernbySteer = (self.total_actual_projection > 0 or self.actual_smoothing > 1 or self.total_desired_projection > 0 or self.desired_smoothing > 1)
+
+        # Eliminate break-points, since they aren't needed (and would cause problems for resonance)
+        KpV = [interp(25.0, CP.steerKpBP, self.steerKpV)]
+        KiV = [interp(25.0, CP.steerKiBP, self.steerKiV)]
+        self.pid._k_i = ([0.], KiV)
+        self.pid._k_p = ([0.], KpV)
+        print(self.total_desired_projection, self.desired_smoothing, self.total_actual_projection, self.actual_smoothing, self.gernbySteer)
+      else:
+        self.gernbySteer = False
+      self.mpc_frame = 0
+
+
   def reset(self):
     self.pid.reset()
 
   def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM, path_plan):
 
+    self.live_tune(CP)
     if angle_rate == 0.0 and self.calculate_rate:
       if angle_steers != self.prev_angle_steers:
         self.steer_counter_prev = self.steer_counter
@@ -57,12 +115,18 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
       self.dampened_angle_steers = angle_steers
       self.dampened_desired_angle = angle_steers
     else:
-      projected_desired_angle = interp(sec_since_boot() + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
-      self.dampened_desired_angle = (((self.desired_smoothing - 1.) * self.dampened_desired_angle) + projected_desired_angle) / self.desired_smoothing
 
-      projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate)
-      if not steer_override:
-        self.dampened_angle_steers = (((self.actual_smoothing - 1.) * self.dampened_angle_steers) + projected_angle_steers) / self.actual_smoothing
+      if self.gernbySteer == False:
+        self.dampened_angle_steers = angle_steers
+        self.dampened_desired_angle = float(path_plan.angleSteers)
+
+      else:
+        projected_desired_angle = interp(sec_since_boot() + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
+        self.dampened_desired_angle = (((self.desired_smoothing - 1.) * self.dampened_desired_angle) + projected_desired_angle) / self.desired_smoothing
+
+        projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate)
+        if not steer_override:
+          self.dampened_angle_steers = (((self.actual_smoothing - 1.) * self.dampened_angle_steers) + projected_angle_steers) / self.actual_smoothing
 
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
 
@@ -73,8 +137,6 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
 
         feed_forward = v_ego**2 * self.dampened_desired_angle
 
-        #self.dampened_angle_steers = angle_steers
-        #self.dampened_desired_angle = path_plan.angleSteers
         output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10),
                                       override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone)