openpilot v0.3.8.2 tweaks

old-commit-hash: 7dabcdace8

selfdrive/car/honda/interface.py

@@ -185,6 +185,9 @@ class CarInterface(object):
     ret.brakeMaxBP = [5., 20.]  # m/s
     ret.brakeMaxV = [1., 0.8]   # max brake allowed
 
+    ret.longPidDeadzoneBP = [0.]
+    ret.longPidDeadzoneV = [0.]
+
     ret.steerLimitAlert = True
 
     return ret

selfdrive/car/toyota/interface.py

@@ -65,11 +65,11 @@ class CarInterface(object):
     ret.l = 2.70
     ret.aF = ret.l * 0.44
     ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius
-    if candidate == CAR.PRIUS:
-      ret.steerKp, ret.steerKi = 0.2, 0.01
-    elif candidate == CAR.RAV4:  # rav4 control seem to be ok with integrators
-      ret.steerKp, ret.steerKi = 0.2, 0.05
-    ret.steerKf = 0.00007818594    # full torque for 10 deg at 80mph
+    ret.steerKp, ret.steerKi = 0.6, 0.05
+    ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
+
+    ret.longPidDeadzoneBP = [0., 9.]
+    ret.longPidDeadzoneV = [0., .15]
 
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.

selfdrive/controls/lib/alertmanager.py

@@ -59,9 +59,9 @@ class AlertManager(object):
         PT.MID, None, "beepSingle", .2, 0., 0.),
 
     "fcw": Alert(
-        "Brake", 
-        "Risk of Collision", 
-        PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
+        "", 
+        "", 
+        PT.LOW, None, None, .1, .1, .1),
 
     "steerSaturated": Alert(
         "Take Control", 

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -79,7 +79,7 @@ int main( )
 
   Q(3,3) = 1.0;
 
-  Q(4,4) = 0.5;
+  Q(4,4) = 2.0;
 
   // Terminal cost
   Function hN;

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f6f985c451e36d05b11e35ba61e229e28d49bcfac8874146bb500f6b0736e409
+oid sha256:f01adf6d07d2ca818d2df6a79b1c10a4806dd6a1a525438950329ff0c2791437
 size 194874

selfdrive/controls/lib/longcontrol.py

@@ -131,7 +131,8 @@ class LongControl(object):
 
       self.pid.pos_limit = gas_max
       self.pid.neg_limit = - brake_max
-      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor)
+      deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
+      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor, deadzone=deadzone)
 
     # intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:

selfdrive/controls/lib/pid.py

@@ -2,6 +2,15 @@ import numpy as np
 from common.numpy_fast import clip, interp
 import numbers
 
+def apply_deadzone(error, deadzone):
+  if error > deadzone:
+    error -= deadzone
+  elif error < - deadzone:
+    error += deadzone
+  else:
+    error = 0.
+  return error
+
 class PIController(object):
   def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
     self._k_p = k_p # proportional gain
@@ -57,11 +66,11 @@ class PIController(object):
     self.saturated = False
     self.control = 0
 
-  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0.):
+  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0., deadzone=0.):
     self.speed = speed
     self.jerk_factor = jerk_factor
 
-    error = float(setpoint - measurement)
+    error = float(apply_deadzone(setpoint - measurement, deadzone))
     self.p = error * self.k_p
     f = feedforward * self.k_f