Clean up and update Live Tuner (#3)

selfdrive/car/gm/interface.py

@@ -116,6 +116,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
     ret.lateralTuning.pid.kf = 0.00004   # full torque for 20 deg at 80mph means 0.00007818594
     ret.steerRateCost = 1.0
+    ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
 
     if candidate == CAR.VOLT:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -126,8 +127,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       ret.steerRatio = 15.7
       ret.steerRatioRear = 0.
       ret.centerToFront = ret.wheelbase * 0.4 # wild guess
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.12], [0.05]]
-      ret.steerRateCost = 0.7
 
     elif candidate == CAR.MALIBU:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)

selfdrive/car/honda/carcontroller.py

@@ -5,12 +5,12 @@
 from selfdrive.car import create_gas_command
 from selfdrive.car.honda import hondacan
 from selfdrive.car.honda.values import AH, CruiseButtons, CAR
-from selfdrive.kegman_conf import kegman_conf
-
-kegman = kegman_conf()
+from selfdrive.kegman_conf import KegmanConf
 from opendbc.can.packer import CANPacker
 
 
+kegman = KegmanConf()
+
 def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
   # hyst params
   brake_hyst_on = 0.02     # to activate brakes exceed this value

selfdrive/car/honda/carstate.py

@@ -6,7 +6,7 @@
 from opendbc.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 
 GearShifter = car.CarState.GearShifter
 
@@ -202,7 +202,7 @@ def get_cam_can_parser(CP):
 
 class CarState():
   def __init__(self, CP):
-    self.kegman = kegman_conf()
+    self.kegman = KegmanConf()
     self.trMode = int(self.kegman.conf['lastTrMode'])     # default to last distance interval on startup
     #self.trMode = 1
     self.lkMode = True
@@ -392,7 +392,7 @@ def update(self, cp, cp_cam):
     if self.cruise_setting == 3:
       if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
         self.trMode = (self.trMode + 1 ) % 4
-        self.kegman = kegman_conf()
+        self.kegman = KegmanConf()
         self.kegman.conf['lastTrMode'] = str(self.trMode)   # write last distance bar setting to file
         self.kegman.write_config(self.kegman.conf) 
 

selfdrive/controls/lib/drive_helpers.py

@@ -1,10 +1,10 @@
 from cereal import car
 from common.numpy_fast import clip, interp
 from selfdrive.config import Conversions as CV
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 
 # kph
-kegman = kegman_conf()
+kegman = KegmanConf()
 V_CRUISE_MAX = 144
 V_CRUISE_MIN = 8
 V_CRUISE_DELTA = int(kegman.conf['CruiseDelta'])

selfdrive/controls/lib/driver_monitor.py

@@ -3,8 +3,10 @@
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from common.filter_simple import FirstOrderFilter
 from common.stat_live import RunningStatFilter
-from selfdrive.kegman_conf import kegman_conf
-kegman = kegman_conf()
+from selfdrive.kegman_conf import KegmanConf
+
+
+kegman = KegmanConf()
 
 
 _AWARENESS_TIME = min(int(kegman.conf['wheelTouchSeconds']), 600)    # x minutes limit without user touching steering wheels make the car enter a terminal status

selfdrive/controls/lib/lane_planner.py

@@ -1,9 +1,10 @@
 from common.numpy_fast import interp
 import numpy as np
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 from cereal import log
 
-kegman = kegman_conf()
+
+kegman = KegmanConf()
 CAMERA_OFFSET = float(kegman.conf['cameraOffset'])  # m from center car to camera
 
 #zorrobyte

selfdrive/controls/lib/latcontrol_pid.py

@@ -2,12 +2,12 @@
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from cereal import car
 from cereal import log
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 
 
 class LatControlPID():
   def __init__(self, CP):
-    self.kegman = kegman_conf(CP)
+    self.kegman = KegmanConf(CP)
     self.deadzone = float(self.kegman.conf['deadzone'])
     self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
@@ -22,7 +22,7 @@ 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
-      self.kegman = kegman_conf()
+      self.kegman = KegmanConf(CP)
       if self.kegman.conf['tuneGernby'] == "1":
         self.steerKpV = [float(self.kegman.conf['Kp'])]
         self.steerKiV = [float(self.kegman.conf['Ki'])]

selfdrive/controls/lib/long_mpc.py

@@ -8,10 +8,10 @@
 from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 
 # One, two and three bar distances (in s)
-kegman = kegman_conf()
+kegman = KegmanConf()
 if "ONE_BAR_DISTANCE" in kegman.conf:
     ONE_BAR_DISTANCE = float(kegman.conf['ONE_BAR_DISTANCE'])
 else:
@@ -88,7 +88,7 @@ def __init__(self, mpc_id):
 
     self.bp_counter = 0  
 
-    kegman = kegman_conf()
+    kegman = KegmanConf()
     self.oneBarBP = [float(kegman.conf['1barBP0']), float(kegman.conf['1barBP1'])]
     self.twoBarBP = [float(kegman.conf['2barBP0']), float(kegman.conf['2barBP1'])]
     self.threeBarBP = [float(kegman.conf['3barBP0']), float(kegman.conf['3barBP1'])]
@@ -178,7 +178,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
     # Live Tuning of breakpoints for braking profile change
     self.bp_counter += 1
     if self.bp_counter % 500 == 0:
-      kegman = kegman_conf()
+      kegman = KegmanConf()
       self.oneBarBP = [float(kegman.conf['1barBP0']), float(kegman.conf['1barBP1'])]
       self.twoBarBP = [float(kegman.conf['2barBP0']), float(kegman.conf['2barBP1'])]
       self.threeBarBP = [float(kegman.conf['3barBP0']), float(kegman.conf['3barBP1'])]

selfdrive/controls/lib/longcontrol.py

@@ -1,9 +1,9 @@
 from cereal import log
 from common.numpy_fast import clip, interp
 from selfdrive.controls.lib.pid import PIController
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 
-kegman = kegman_conf()
+kegman = KegmanConf()
 LongCtrlState = log.ControlsState.LongControlState
 
 STOPPING_EGO_SPEED = 0.5

selfdrive/controls/lib/pathplanner.py

@@ -5,7 +5,7 @@
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
 from selfdrive.controls.lib.lane_planner import LanePlanner
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 from common.numpy_fast import interp
 import cereal.messaging as messaging
 from cereal import log
@@ -58,7 +58,7 @@ def __init__(self, CP):
     self.steerRatio_new = 0.0
     self.sR_time = 1
 
-    kegman = kegman_conf(CP)
+    kegman = KegmanConf(CP)
     if kegman.conf['steerRatio'] == "-1":
       self.steerRatio = CP.steerRatio
     else:
@@ -68,6 +68,11 @@ def __init__(self, CP):
       self.steerRateCost = CP.steerRateCost
     else:
       self.steerRateCost = float(kegman.conf['steerRateCost'])
+
+    if kegman.conf['steerActuatorDelay'] == "-1":
+      self.steerActuatorDelay = CP.steerActuatorDelay
+    else:
+      self.steerActuatorDelay = float(kegman.conf['steerActuatorDelay'])
 
     self.sR = [float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost']))]
     self.sRBP = [float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1'])]
@@ -116,16 +121,18 @@ def update(self, sm, pm, CP, VM):
     VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
     curvature_factor = VM.curvature_factor(v_ego)
 
-    # Get steerRatio and steerRateCost from kegman.json every x seconds
+    # Get steerRatio, steerRateCost, steerActuatorDelay from kegman.json every x seconds
     self.mpc_frame += 1
     if self.mpc_frame % 500 == 0:
       # live tuning through /data/openpilot/tune.py overrides interface.py settings
-      kegman = kegman_conf()
+      kegman = KegmanConf()
       if kegman.conf['tuneGernby'] == "1":
         self.steerRateCost = float(kegman.conf['steerRateCost'])
         if self.steerRateCost != self.steerRateCost_prev:
           self.setup_mpc()
           self.steerRateCost_prev = self.steerRateCost
+
+        self.steerActuatorDelay = float(kegman.conf['steerActuatorDelay'])
 
         self.sR = [float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost']))]
         self.sRBP = [float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1'])]
@@ -146,8 +153,6 @@ def update(self, sm, pm, CP, VM):
         self.sR_delay_counter = 0
     else:
       self.steerRatio = self.sR[0]
-
-    print("steerRatio = ", self.steerRatio)
 
     self.LP.parse_model(sm['model'])
 
@@ -229,7 +234,7 @@ def update(self, sm, pm, CP, VM):
     #   self.path_offset_i = 0.0
 
     # account for actuation delay
-    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, self.steerRatio, CP.steerActuatorDelay)
+    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, self.steerRatio, self.steerActuatorDelay)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,

selfdrive/controls/lib/planner.py

@@ -13,7 +13,7 @@
 from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
 from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
-from selfdrive.kegman_conf import kegman_conf
+from selfdrive.kegman_conf import KegmanConf
 
 
 MAX_SPEED = 255.0
@@ -82,7 +82,7 @@ def __init__(self, CP):
     self.path_x = np.arange(192)
 
     self.params = Params()
-    self.kegman = kegman_conf()
+    self.kegman = KegmanConf()
     self.mpc_frame = 0
 
   def choose_solution(self, v_cruise_setpoint, enabled):
@@ -128,7 +128,7 @@ def update(self, sm, pm, CP, VM, PP):
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
 
     if self.mpc_frame % 1000 == 0:
-      self.kegman = kegman_conf()
+      self.kegman = KegmanConf()
       self.mpc_frame = 0
 
     self.mpc_frame += 1