openpilot v0.3.9 tweaks

README.md

@@ -31,11 +31,11 @@ Supported Cars
 
 - Toyota RAV-4 2016+ with TSS-P (alpha!)
   - By default it uses stock Toyota ACC for longitudinal control
-  - openpilot longitudinal control available after unplugging the Driving Support ECU and can be enabled above 20 mph
+  - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) and can be enabled above 20 mph
 
 - Toyota Prius 2017 (alpha!)
   - By default it uses stock Toyota ACC for longitudinal control
-  - openpilot longitudinal control available after unplugging the Driving Support ECU
+  - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29)
 
 In Progress Cars
 ------

cereal/car.capnp

@@ -173,6 +173,9 @@ struct RadarState {
     # these are optional and valid if they are not NaN
     aRel @4 :Float32; # m/s^2
     yvRel @5 :Float32; # m/s
+
+    # some radars flag measurements VS estimates
+    measured @6 :Bool;
   }
 }
 
@@ -252,22 +255,22 @@ struct CarParams {
   enableGas @4 :Bool;
   enableBrake @5 :Bool;
   enableCruise @6 :Bool;
-  enableCamera @27 :Bool;
-  enableDsu @28 :Bool; # driving support unit
-  enableApgs @29 :Bool; # advanced parking guidance system
+  enableCamera @26 :Bool;
+  enableDsu @27 :Bool; # driving support unit
+  enableApgs @28 :Bool; # advanced parking guidance system
 
-  minEnableSpeed @18 :Float32;
-  safetyModel @19 :Int16;
+  minEnableSpeed @17 :Float32;
+  safetyModel @18 :Int16;
 
-  steerMaxBP @20 :List(Float32);
-  steerMaxV @21 :List(Float32);
-  gasMaxBP @22 :List(Float32);
-  gasMaxV @23 :List(Float32);
-  brakeMaxBP @24 :List(Float32);
-  brakeMaxV @25 :List(Float32);
+  steerMaxBP @19 :List(Float32);
+  steerMaxV @20 :List(Float32);
+  gasMaxBP @21 :List(Float32);
+  gasMaxV @22 :List(Float32);
+  brakeMaxBP @23 :List(Float32);
+  brakeMaxV @24 :List(Float32);
 
-  longPidDeadzoneBP @33 :List(Float32);
-  longPidDeadzoneV @34 :List(Float32);
+  longPidDeadzoneBP @32 :List(Float32);
+  longPidDeadzoneV @33 :List(Float32);
 
   enum SafetyModels {
     # does NOT match board setting
@@ -278,33 +281,32 @@ struct CarParams {
   }
 
   # things about the car in the manual
-  m @7 :Float32;     # [kg] running weight
-  l @8 :Float32;     # [m] wheelbase
-  sR @9 :Float32;    # [] steering ratio
-  aF @10 :Float32;   # [m] GC distance to front axle
-  aR @11 :Float32;   # [m] GC distance to rear axle
-  chi @12 :Float32;  # [] rear steering ratio wrt front steering (usually 0)
+  mass @7 :Float32;             # [kg] running weight
+  wheelbase @8 :Float32;        # [m] distance from rear to front axle
+  centerToFront @9 :Float32;   # [m] GC distance to front axle
+  steerRatio @10 :Float32;       # [] ratio between front wheels and steering wheel angles
+  steerRatioRear @11 :Float32;  # [] rear steering ratio wrt front steering (usually 0)
 
   # things we can derive
-  j @13 :Float32;    # [kg*m2] body rotational inertia
-  cF @14 :Float32;   # [N/rad] front tire coeff of stiff
-  cR @15 :Float32;   # [N/rad] rear tire coeff of stiff
+  rotationalInertia @12 :Float32;    # [kg*m2] body rotational inertia
+  tireStiffnessFront @13 :Float32;   # [N/rad] front tire coeff of stiff
+  tireStiffnessRear @14 :Float32;    # [N/rad] rear tire coeff of stiff
 
   # Kp and Ki for the lateral control
-  steerKp @16 :Float32;
-  steerKi @17 :Float32;
-  steerKf @26 :Float32;
+  steerKp @15 :Float32;
+  steerKi @16 :Float32;
+  steerKf @25 :Float32;
 
   # Kp and Ki for the longitudinal control
-  longitudinalKpBP @37 :List(Float32);
-  longitudinalKpV @38 :List(Float32);
-  longitudinalKiBP @39 :List(Float32);
-  longitudinalKiV @40 :List(Float32);
+  longitudinalKpBP @36 :List(Float32);
+  longitudinalKpV @37 :List(Float32);
+  longitudinalKiBP @38 :List(Float32);
+  longitudinalKiV @39 :List(Float32);
 
-  steerLimitAlert @30 :Bool;
+  steerLimitAlert @29 :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
-  stoppingControl @35 :Bool; # Does the car allows full control even at lows speeds when stopping
-  startAccel @36 :Float32; # Required acceleraton to overcome creep braking
+  vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
+  directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake
+  stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
+  startAccel @35 :Float32; # Required acceleraton to overcome creep braking
 }

cereal/log.capnp

@@ -287,7 +287,7 @@ struct Live20Data {
     vRel @2 :Float32;
     aRel @3 :Float32;
     vLead @4 :Float32;
-    aLead @5 :Float32;
+    aLeadDEPRECATED @5 :Float32;
     dPath @6 :Float32;
     vLat @7 :Float32;
     vLeadK @8 :Float32;

common/kalman/ekf.py

@@ -19,6 +19,7 @@
 # update() should be called once per sensor, and can be called multiple times between predict steps.
 # Access and set the state of the filter directly with ekf.state and ekf.covar.
 
+
 class SensorReading:
   # Given a perfect model and no noise, data = obs_model * state
   def __init__(self, data, covar, obs_model):
@@ -33,7 +34,7 @@ def __repr__(self):
 
 # A generic sensor class that does no pre-processing of data
 class SimpleSensor:
-  # obs_model can be 
+  # obs_model can be
   #   a full obesrvation model matrix, or
   #   an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
   # covar can be
@@ -131,11 +132,11 @@ def update_scalar(self, reading):
     # like update but knowing that measurment is a scalar
     # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
 
-    # innovation = reading.data - np.matmul(reading.obs_model, self.state)   
-    # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar    
-    # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar   
-    # self.state += np.matmul(kalman_gain, innovation)   
-    # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)    
+    # innovation = reading.data - np.matmul(reading.obs_model, self.state)
+    # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
+    # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
+    # self.state += np.matmul(kalman_gain, innovation)
+    # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
     # self.covar =  np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
 
     # written without np.matmul
@@ -174,7 +175,7 @@ def predict(self, dt):
 
     #! Clip covariance to avoid explosions
     self.covar = np.clip(self.covar,-1e10,1e10)
-    
+
   @abc.abstractmethod
   def calc_transfer_fun(self, dt):
     """Return a tuple with the transfer function and transfer function jacobian
@@ -190,6 +191,7 @@ def calc_transfer_fun(self, dt):
       and using it during calcualtion of A and J
     """
 
+
 class FastEKF1D(EKF):
   """Fast version of EKF for 1D problems with scalar readings."""
 

common/kalman/simple_kalman.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+
+class KF1D:
+  # this EKF assumes constant covariance matrix, so calculations are much simpler
+  # the Kalman gain also needs to be precomputed using the control module
+
+  def __init__(self, x0, A, C, K):
+    self.x = x0
+    self.A = A
+    self.C = C
+    self.K = K
+
+    self.A_K = self.A - np.dot(self.K, self.C)
+
+    # K matrix needs to  be pre-computed as follow:
+    # import control
+    # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
+    # self.K = np.transpose(K)
+
+  def update(self, meas):
+    self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
+    return self.x

common/params.py

@@ -57,6 +57,7 @@ class UnknownKeyName(Exception):
   "IsMetric": TxType.PERSISTANT,
   "IsRearViewMirror": TxType.PERSISTANT,
   "IsFcwEnabled": TxType.PERSISTANT,
+  "HasAcceptedTerms": TxType.PERSISTANT,
 # written: visiond
 # read:    visiond, controlsd
   "CalibrationParams": TxType.PERSISTANT,

selfdrive/car/honda/carcontroller.py

@@ -11,10 +11,6 @@
 from . import hondacan
 from .values import AH
 
-# msgs sent for steering controller by camera module on can 0.
-# those messages are mutually exclusive on non-rav4 and rav4 cars
-CAMERA_MSGS = [0xe4, 0x194]
-
 
 def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
   # hyst params... TODO: move these to VehicleParams
@@ -126,7 +122,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
     GAS_MAX = 1004
     BRAKE_MAX = 1024/4
     if CS.civic:
-      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
+      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
       STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
     elif CS.crv:
       STEER_MAX = 0x300  # CR-V only uses 12-bits and requires a lower value

selfdrive/car/honda/interface.py

@@ -10,7 +10,6 @@
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 from selfdrive.car.honda.carstate import CarState, get_can_parser
-from selfdrive.car.honda.carcontroller import CAMERA_MSGS
 from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
 from selfdrive.controls.lib.planner import A_ACC_MAX
 
@@ -20,6 +19,11 @@
   CarController = None
 
 
+# msgs sent for steering controller by camera module on can 0.
+# those messages are mutually exclusive on CRV and non-CRV cars
+CAMERA_MSGS = [0xe4, 0x194]
+
+
 def compute_gb_honda(accel, speed):
   creep_brake = 0.0
   creep_speed = 2.3
@@ -108,7 +112,7 @@ def __init__(self, CP, sendcan=None):
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
     eA = a_ego - a_target
     valuesA = [1.0, 0.1]
-    bpA = [0.0, 0.5]
+    bpA = [0.3, 1.1]
 
     eV = v_ego - v_target
     valuesV = [1.0, 0.1]
@@ -144,22 +148,22 @@ def get_params(candidate, fingerprint):
 
     # FIXME: hardcoding honda civic 2016 touring params so they can be used to
     # scale unknown params for other cars
-    m_civic = 2923./2.205 + std_cargo
-    l_civic = 2.70
-    aF_civic = l_civic * 0.4
-    aR_civic = l_civic - aF_civic
-    j_civic = 2500
-    cF_civic = 85400
-    cR_civic = 90000
+    mass_civic = 2923./2.205 + std_cargo
+    wheelbase_civic = 2.70
+    centerToFront_civic = wheelbase_civic * 0.4
+    centerToRear_civic = wheelbase_civic - centerToFront_civic
+    rotationalInertia_civic = 2500
+    tireStiffnessFront_civic = 85400
+    tireStiffnessRear_civic = 90000
 
     if candidate == "HONDA CIVIC 2016 TOURING":
       stop_and_go = True
-      ret.m = m_civic
-      ret.l = l_civic
-      ret.aF = aF_civic
-      ret.sR = 13.0
+      ret.mass = mass_civic
+      ret.wheelbase = wheelbase_civic
+      ret.centerToFront = centerToFront_civic
+      ret.steerRatio = 13.0
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
-      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
+      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
       ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
 
       ret.longitudinalKpBP = [0., 5., 35.]
@@ -168,10 +172,10 @@ def get_params(candidate, fingerprint):
       ret.longitudinalKiV = [0.54, 0.36]
     elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
       stop_and_go = False
-      ret.m = 3095./2.205 + std_cargo
-      ret.l = 2.67
-      ret.aF = ret.l * 0.37
-      ret.sR = 15.3
+      ret.mass = 3095./2.205 + std_cargo
+      ret.wheelbase = 2.67
+      ret.centerToFront = ret.wheelbase * 0.37
+      ret.steerRatio = 15.3
       # Acura at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
       ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
@@ -182,10 +186,10 @@ def get_params(candidate, fingerprint):
       ret.longitudinalKiV = [0.18, 0.12]
     elif candidate == "HONDA ACCORD 2016 TOURING":
       stop_and_go = False
-      ret.m = 3580./2.205 + std_cargo
-      ret.l = 2.74
-      ret.aF = ret.l * 0.38
-      ret.sR = 15.3
+      ret.mass = 3580./2.205 + std_cargo
+      ret.wheelbase = 2.74
+      ret.centerToFront = ret.wheelbase * 0.38
+      ret.steerRatio = 15.3
       ret.steerKp, ret.steerKi = 0.8, 0.24
 
       ret.longitudinalKpBP = [0., 5., 35.]
@@ -194,10 +198,10 @@ def get_params(candidate, fingerprint):
       ret.longitudinalKiV = [0.18, 0.12]
     elif candidate == "HONDA CR-V 2016 TOURING":
       stop_and_go = False
-      ret.m = 3572./2.205 + std_cargo
-      ret.l = 2.62
-      ret.aF = ret.l * 0.41
-      ret.sR = 15.3
+      ret.mass = 3572./2.205 + std_cargo
+      ret.wheelbase = 2.62
+      ret.centerToFront = ret.wheelbase * 0.41
+      ret.steerRatio = 15.3
       ret.steerKp, ret.steerKi = 0.8, 0.24
 
       ret.longitudinalKpBP = [0., 5., 35.]
@@ -214,18 +218,23 @@ def get_params(candidate, fingerprint):
     # conflict with PCM acc
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS
 
-    ret.aR = ret.l - ret.aF
+    centerToRear = ret.wheelbase - ret.centerToFront
     # TODO: get actual value, for now starting with reasonable value for
     # civic and scaling by mass and wheelbase
-    ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
+    ret.rotationalInertia = rotationalInertia_civic * \
+                            ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
 
     # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
     # mass and CG position, so all cars will have approximately similar dyn behaviors
-    ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
-    ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
+    ret.tireStiffnessFront = tireStiffnessFront_civic * \
+                             ret.mass / mass_civic * \
+                             (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
+    ret.tireStiffnessRear = tireStiffnessRear_civic * \
+                            ret.mass / mass_civic * \
+                            (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
 
     # no rear steering, at least on the listed cars above
-    ret.chi = 0.
+    ret.steerRatioRear = 0.
 
     # no max steer limit VS speed
     ret.steerMaxBP = [0.]  # m/s