Merge pull request #154 from commaai/devel

openpilot v0.3.9

README.md

@@ -30,21 +30,25 @@ Supported Cars
   - Can only be enabled above 25 mph
 
 - Toyota RAV-4 2016+ with TSS-P (alpha!)
-  - Can only be enabled above 20 mph
+  - By default it uses stock Toyota ACC for longitudinal control
+  - 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](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29)
 
 In Progress Cars
 ------
+- Probably all TSS-P Toyota with Steering Assist.
+  - 'Full Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the Prius, Camry and C-HR have this option.
+  - Even though the Tundra, Sequoia and the Land Cruiser have TSS-P, they don't have Steering Assist and are not supported.
 
-- Toyota Prius 2017
-
-- Probably all TSS-P Toyota
-
-Community Ported Cars
+Community WIP Cars
 ------
 
-- Chevy Volt 2016-2018 Premier with Driver Confidence II
+- [Chevy Volt 2016-2018 Premier with Driver Confidence II](https://github.com/commaai/openpilot/pull/104)
 
-- Classic Tesla Model S (pre-AP)
+- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145)
 
 Directory structure
 ------
@@ -98,7 +102,7 @@ User Data / chffr Account / Crash Reporting
 
 By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
 
-It's open source software, so you are free to disable it if you wish. 
+It's open source software, so you are free to disable it if you wish.
 
 It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
 It does not log the user facing camera or the microphone.

RELEASES.md

@@ -1,3 +1,10 @@
+Version 0.3.9 (2017-11-21)
+==========================
+ * Add alpha support for 2017 Toyota Prius
+ * Improved longitudinal control using model predictive control
+ * Enable Forward Collision Warning
+ * Acura ILX now maintains openpilot engaged at standstill when brakes are applied
+
 Version 0.3.8.2 (2017-10-30)
 ==========================
  * Add alpha support for 2017 Toyota RAV4
@@ -17,9 +24,9 @@ Version 0.3.7 (2017-09-30)
  * Fixed sporadic longitudinal pulsing in Civic
  * Cleanups to vehicle interface
 
-Version 0.3.6.1 (2017-08-15)   
-============================    
- * Mitigate low speed steering oscillations on some vehicles    
+Version 0.3.6.1 (2017-08-15)
+============================
+ * Mitigate low speed steering oscillations on some vehicles
  * Include board steering check for CR-V
 
 Version 0.3.6 (2017-08-08)
@@ -139,4 +146,3 @@ Version 0.1  (2016-11-29)
   * Lane keep assist is working
   * Support for Acura ILX 2016 with AcuraWatch Plus
   * Support for Honda Civic 2016 Touring Edition
-

cereal/car.capnp

@@ -77,6 +77,7 @@ struct CarState {
   # brake pedal, 0.0-1.0
   brake @5 :Float32;      # this is user pedal only
   brakePressed @6 :Bool;  # this is user pedal only
+  brakeLights @19 :Bool;
 
   # steering wheel
   steeringAngle @7 :Float32;   # deg
@@ -92,6 +93,8 @@ struct CarState {
 
   # button presses
   buttonEvents @11 :List(ButtonEvent);
+  leftBlinker @20 :Bool;
+  rightBlinker @21 :Bool;
 
   # which packets this state came from
   canMonoTimes @12: List(UInt64);
@@ -109,6 +112,7 @@ struct CarState {
     speed @1 :Float32;
     available @2 :Bool;
     speedOffset @3 :Float32;
+    standstill @4 :Bool;
   }
 
   enum GearShifter {
@@ -169,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;
   }
 }
 
@@ -248,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
@@ -274,27 +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;
-
-  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?
+  steerKp @15 :Float32;
+  steerKi @16 :Float32;
+  steerKf @25 :Float32;
+
+  # Kp and Ki for the longitudinal control
+  longitudinalKpBP @36 :List(Float32);
+  longitudinalKpV @37 :List(Float32);
+  longitudinalKiBP @38 :List(Float32);
+  longitudinalKiV @39 :List(Float32);
+
+  steerLimitAlert @29 :Bool;
+
+  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;
@@ -337,19 +337,23 @@ struct Live100Data {
   vTargetLead @3 :Float32;
   upAccelCmd @4 :Float32;
   uiAccelCmd @5 :Float32;
+  ufAccelCmd @33 :Float32;
   yActualDEPRECATED @6 :Float32;
-  yDes @7 :Float32;
+  yDesDEPRECATED @7 :Float32;
   upSteer @8 :Float32;
   uiSteer @9 :Float32;
-  aTargetMin @10 :Float32;
-  aTargetMax @11 :Float32;
+  ufSteer @34 :Float32;
+  aTargetMinDEPRECATED @10 :Float32;
+  aTargetMaxDEPRECATED @11 :Float32;
+  aTarget @35 :Float32;
   jerkFactor @12 :Float32;
   angleSteers @13 :Float32;     # Steering angle in degrees.
   angleSteersDes @29 :Float32;
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
 
   enabled @19 :Bool;
+  active @36 :Bool;
   steerOverride @20 :Bool;
 
   vCruise @22 :Float32;
@@ -388,7 +392,7 @@ struct ModelData {
   leftLane @2 :PathData;
   rightLane @3 :PathData;
   lead @4 :LeadData;
-  leadNew @6 :List(Float32);
+  freePath @6 :List(Float32);
 
   settings @5 :ModelSettings;
 
@@ -472,10 +476,13 @@ struct Plan {
 
   # longitudinal
   longitudinalValid @2 :Bool;
+  vCruise @16 :Float32;
+  aCruise @17 :Float32;
   vTarget @3 :Float32;
   vTargetFuture @14 :Float32;
-  aTargetMin @4 :Float32;
-  aTargetMax @5 :Float32;
+  aTargetMinDEPRECATED @4 :Float32;
+  aTargetMaxDEPRECATED @5 :Float32;
+  aTarget @18 :Float32;
   jerkFactor @6 :Float32;
   hasLead @7 :Bool;
   fcw @8 :Bool;

common/dbc.py

@@ -143,7 +143,7 @@ def decode(self, x, arr=None, debug=False):
     msg = self.msgs.get(x[0])
     if msg is None:
       if x[0] not in self._warned_addresses:
-        print("WARNING: Unknown message address {}".format(x[0]))
+        #print("WARNING: Unknown message address {}".format(x[0]))
         self._warned_addresses.add(x[0])
       return None, None
 

common/fingerprints.py

@@ -19,6 +19,9 @@
   "TOYOTA RAV4 2017": {
     36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
   },
+  "TOYOTA PRIUS 2017": {
+    36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
+  },
 }
 
 # support additional internal only fingerprints

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

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

selfdrive/car/__init__.py

@@ -85,4 +85,4 @@ def get_car(logcan, sendcan=None, timeout=None):
   interface_cls = interfaces[candidate]
   params = interface_cls.get_params(candidate, fingerprints)
 
-  return interface_cls(params, logcan, sendcan), params
+  return interface_cls(params, sendcan), params