Added simplified tuning parameters

icmma · Jan 1, 2019 · bf6c44d · bf6c44d
1 parent b307f70
commit bf6c44d
Showing 1 changed file with 17 additions and 8 deletions.
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
@@ -33,16 +33,25 @@ def apply_deadzone(angle, deadzone):
 
 class LatControl(object):
   def __init__(self, CP):
-    self.pid = PIController((CP.steerKpBP, CP.steerKpV),
-                            (CP.steerKiBP, CP.steerKiV),
-                            k_f=CP.steerKf, pos_limit=1.0)
+
+    _ADJUST_REACTANCE = 1.5
+    _ADJUST_INDUCTANCE = 1.5
+    _ADJUST_RESISTANCE = 0.5
+
+    KpV = [np.interp(25.0, CP.steerKpBP, CP.steerKpV) * _ADJUST_REACTANCE]
+    KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _ADJUST_REACTANCE]
+    Kf = CP.steerKf * _ADJUST_INDUCTANCE
+    print(KpV, KiV, Kf)
+    self.pid = PIController(([0.], KpV),
+                            ([0.], KiV),
+                            k_f=Kf, pos_limit=1.0)
     self.last_cloudlog_t = 0.0
     self.setup_mpc(CP.steerRateCost)
-    self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT      # Multiplier for inductive component (feed forward)
-    self.projection_factor = 5.0 * _DT      #  Mutiplier for reactive component (PI)
-    self.accel_limit = 2.0                                 # Desired acceleration limit to prevent "whip steer" (resistive component)
-    self.ff_angle_factor = 0.5         # Kf multiplier for angle-based feed forward
-    self.ff_rate_factor = 5.0         # Kf multiplier for rate-based feed forward
+    self.smooth_factor = _ADJUST_INDUCTANCE * 2.0 * CP.steerActuatorDelay / _DT    # Multiplier for inductive component (feed forward)
+    self.projection_factor = _ADJUST_REACTANCE * 5.0 * _DT                         #  Mutiplier for reactive component (PI)
+    self.accel_limit = 2.0 / _ADJUST_RESISTANCE                                    # Desired acceleration limit to prevent "whip steer" (resistive component)
+    self.ff_angle_factor = 0.5                                                     # Kf multiplier for angle-based feed forward
+    self.ff_rate_factor = 5.0                                                      # Kf multiplier for rate-based feed forward
     self.prev_angle_rate = 0
     self.feed_forward = 0.0
     self.steerActuatorDelay = CP.steerActuatorDelay