openpilot v0.3.8.2 tweaks

SAFETY.md

@@ -1,36 +1,73 @@
 openpilot Safety
 ======
 
-openpilot is an Adaptive Cruise Control and Lane Keeping Assist System. Like
-other ACC and LKAS systems, openpilot requires the driver to be alert and to pay
-attention at all times. We repeat, **driver alertness is necessary, but not
+openpilot is an Adaptive Cruise Control (ACC) and Lane Keeping Assist (LKA) system. 
+Like other ACC and LKA systems, openpilot requires the driver to be alert and to 
+pay attention at all times. We repeat, **driver alertness is necessary, but not 
 sufficient, for openpilot to be used safely**.
 
 Even with an attentive driver, we must make further efforts for the system to be
 safe. We have designed openpilot with two other safety considerations.
 
-1. The vehicle must always be controllable by the driver.
+1. The driver must always be capable to immediately retake manual control of the vehicle, 
+   by stepping on either pedal or by pressing the cancel button.
 2. The vehicle must not alter its trajectory too quickly for the driver to safely
-   react.
+   react. This means that while the system is engaged, the actuators are constrained
+   to operate within reasonable limits.
 
-To address these, we came up with two safety principles.
+Following are details of the car specific safety implementations:
 
-1. Enforced disengagements. Step on either pedal or press the cancel button to
-   retake manual control of the car immediately.
-  - These are hard enforced by the board, and soft enforced by the software. The
-    green led on the board signifies if the board is allowing control messages.
-  - Honda CAN uses both a counter and a checksum to ensure integrity and prevent
-    replay of the same message.
+Honda/Acura
+------
+
+  - While the system is engaged, gas, brake and steer limits are subject to the same limits used by
+    the stock system.
+
+  - Without an interceptor, the gas is controlled by the Powertrain Control Module (PCM). 
+    The PCM limits acceleration to what is reasonable for a cruise control system.  With an
+    interceptor, the gas is clipped to 60%.
 
-2. Actuation limits. While the system is engaged, the actuators are constrained
-   to operate within reasonable limits; the same limits used by the stock system on
-   the Honda.
-  - Without an interceptor, the gas is controlled by the PCM. The PCM limits
-    acceleration to what is reasonable for a cruise control system.  With an
-    interceptor, the gas is clipped to 60% in longcontrol.py
   - The brake is controlled by the 0x1FA CAN message. This message allows full
     braking, although the board and the software clip it to 1/4th of the max.
     This is around .3g of braking.
-  - Steering is controlled by the 0xE4 CAN message. The EPS controller in the
-    car limits the torque to a very small amount, so regardless of the message,
-    the controller cannot jerk the wheel.
+
+  - Steering is controlled by the 0xE4 CAN message. The Electronic Power Steering (EPS) 
+    controller in the car limits the torque to a very small amount, so regardless of the 
+    message, the controller cannot jerk the wheel.
+
+  - Brake and gas pedal pressed signals are contained in the 0x17C CAN message. A rising edge of
+    either signal triggers a disengagement, which is enforced by the board and in software. The
+    green led on the board signifies if the board is allowing control messages.
+
+  - Honda CAN uses both a counter and a checksum to ensure integrity and prevent
+    replay of the same message.
+
+Toyota
+------
+
+  - While the system is engaged, gas, brake and steer limits are subject to the same limits used by
+    the stock system.
+
+  - With the stock Driving Support Unit (DSU) enabled, the acceleration is controlled 
+    by the stock system and is subject to the stock adaptive cruise control limits. Without the
+    stock DSU connected, the acceleration command is controlled by the 0x343 CAN message and its
+    value is limited by the board and the software to between .3g of deceleration and .15g of
+    acceleration. The acceleration command is ignored by the Engine Control Module (ECM) while the
+    cruise control system is disengaged.
+
+  - Steering torque is controlled through the 0x2E4 CAN message and it's limited by the board and in
+    software to a value of -1500 and 1500. In addition, the vehicle EPS unit will not respond to
+    commands outside these limits.  A steering torque rate limit is enforced by the board and in
+    software so that the commanded steering torque must rise from 0 to max value no faster than
+    1.5s. Commanded steering torque is limited by the board and in software to be no more than 500
+    units above the actual EPS generated motor torque to ensure limited differences between
+    commanded and actual torques.
+
+  - Brake and gas pedal pressed signals are contained in the 0x224 and 0x1D2 CAN messages,
+    respectively. A rising edge of either signal triggers a disengagement, which is enforced by the
+    board and in software. Additionally, the cruise control system disengages on the rising edge of
+    the brake pedal pressed signal.
+
+  - The cruise control system state is contained in the 0x1D2 message. No control messages are
+    allowed if the cruise control system is not active. This is enforced by the software and the
+    board. The green led on the board signifies if the board is allowing control messages.

cereal/car.capnp

@@ -262,6 +262,9 @@ struct CarParams {
   brakeMaxBP @24 :List(Float32);
   brakeMaxV @25 :List(Float32);
 
+  longPidDeadzoneBP @33 :List(Float32);
+  longPidDeadzoneV @34 :List(Float32);
+
   enum SafetyModels {
     # does NOT match board setting
     noOutput @0;
@@ -290,5 +293,8 @@ struct CarParams {
 
   steerLimitAlert @30 :Bool;
 
+  vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state
+  directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake
+
   # TODO: Kp and Ki for long control, perhaps not needed?
 }

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

@@ -213,22 +213,22 @@ tmpQ2[0] = + tmpFx[0];
 tmpQ2[1] = + tmpFx[4];
 tmpQ2[2] = + tmpFx[8];
 tmpQ2[3] = + tmpFx[12];
-tmpQ2[4] = + tmpFx[16]*(real_t)5.0000000000000000e-01;
+tmpQ2[4] = + tmpFx[16]*(real_t)2.0000000000000000e+00;
 tmpQ2[5] = + tmpFx[1];
 tmpQ2[6] = + tmpFx[5];
 tmpQ2[7] = + tmpFx[9];
 tmpQ2[8] = + tmpFx[13];
-tmpQ2[9] = + tmpFx[17]*(real_t)5.0000000000000000e-01;
+tmpQ2[9] = + tmpFx[17]*(real_t)2.0000000000000000e+00;
 tmpQ2[10] = + tmpFx[2];
 tmpQ2[11] = + tmpFx[6];
 tmpQ2[12] = + tmpFx[10];
 tmpQ2[13] = + tmpFx[14];
-tmpQ2[14] = + tmpFx[18]*(real_t)5.0000000000000000e-01;
+tmpQ2[14] = + tmpFx[18]*(real_t)2.0000000000000000e+00;
 tmpQ2[15] = + tmpFx[3];
 tmpQ2[16] = + tmpFx[7];
 tmpQ2[17] = + tmpFx[11];
 tmpQ2[18] = + tmpFx[15];
-tmpQ2[19] = + tmpFx[19]*(real_t)5.0000000000000000e-01;
+tmpQ2[19] = + tmpFx[19]*(real_t)2.0000000000000000e+00;
 tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16];
 tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17];
 tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18];
@@ -253,7 +253,7 @@ tmpR2[0] = + tmpFu[0];
 tmpR2[1] = + tmpFu[1];
 tmpR2[2] = + tmpFu[2];
 tmpR2[3] = + tmpFu[3];
-tmpR2[4] = + tmpFu[4]*(real_t)5.0000000000000000e-01;
+tmpR2[4] = + tmpFu[4]*(real_t)2.0000000000000000e+00;
 tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4];
 }
 
@@ -1965,7 +1965,7 @@ tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5];
 tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1];
 tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2];
 tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3];
-tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)5.0000000000000000e-01;
+tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)2.0000000000000000e+00;
 objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4];
 }
 

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