openpilot v0.3.7 release

old-commit-hash: daf54ad54d

.travis.yml

@@ -8,5 +8,5 @@ install:
 
 script:
   - docker run --rm
-      -v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out
+      -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out
-      tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'
+      tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'

Dockerfile.openpilot

@@ -1,8 +1,7 @@
 FROM ubuntu:16.04
 ENV PYTHONUNBUFFERED 1
 
-RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev
+RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev libusb-1.0-0
-
 RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib
 
 COPY requirements_openpilot.txt /tmp/
@@ -10,4 +9,11 @@ RUN pip install -r /tmp/requirements_openpilot.txt
 
 ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH
 
-COPY . /tmp/openpilot
+COPY ./common /tmp/openpilot/common
+COPY ./cereal /tmp/openpilot/cereal
+COPY ./opendbc /tmp/openpilot/opendbc
+COPY ./selfdrive /tmp/openpilot/selfdrive
+COPY ./phonelibs /tmp/openpilot/phonelibs
+COPY ./pyextra /tmp/openpilot/pyextra
+
+RUN mkdir /tmp/openpilot/selfdrive/test/out

README.md

@@ -22,12 +22,20 @@ Supported Cars
 - Acura ILX 2016 with AcuraWatch Plus
   - Due to use of the cruise control for gas, it can only be enabled above 25 mph
 
-- Honda Civic 2016 with Honda Sensing
+- Honda Civic 2016-2017 with Honda Sensing
   - Due to limitations in steering firmware, steering is disabled below 12 mph
+  - Note that the hatchback model is not supported
 
 - Honda CR-V Touring 2015-2016 (very alpha!)
   - Can only be enabled above 25 mph
 
+In Progress Cars
+------
+
+- Chevy Volt 2016-2018 Premier with Driver Confidence II
+
+- All 2017 Toyota Prius, Corolla, and RAV4
+
 Directory structure
 ------
 

RELEASES.md

@@ -1,6 +1,18 @@
-Version 0.3.6.1 (2017-08-15)
+Version 0.3.7 (2017-09-30)
-============================
+==========================
- * Mitigate low speed steering oscillations on some vehicles
+ * Improved lateral control using model predictive control
+ * Improved lane centering
+ * Improved GPS
+ * Reduced tendency of path deviation near right side exits
+ * Enable engagement while the accelerator pedal is pressed
+ * Enable engagement while the brake pedal is pressed, when stationary and with lead vehicle within 5m
+ * Disable engagement when park brake or brake hold are active
+ * 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    
  * Include board steering check for CR-V
 
 Version 0.3.6 (2017-08-08)

cereal/Makefile

@@ -1,4 +1,45 @@
--include build_from_src.mk
+SRCS := log.capnp car.capnp
+
+GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
+
+
+UNAME_M ?= $(shell uname -m)
+
+# only generate C++ for docker tests
+ifneq ($(OPTEST),1)
+	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
+
+# Dont build java on the phone...
+ifeq ($(UNAME_M),x86_64)
+	GENS += gen/java/Car.java gen/java/Log.java
+endif
+
+endif
+
+.PHONY: all
+all: $(GENS)
+
+.PHONY: clean
+clean:
+	rm -rf gen
+
+gen/c/%.capnp.c: %.capnp
+	@echo "[ CAPNPC C ] $@"
+	mkdir -p gen/c/
+	capnpc '$<' -o c:gen/c/
+
+gen/cpp/%.capnp.c++: %.capnp
+	@echo "[ CAPNPC C++ ] $@"
+	mkdir -p gen/cpp/
+	capnpc '$<' -o c++:gen/cpp/
+
+gen/java/Car.java gen/java/Log.java: $(SRCS)
+	@echo "[ CAPNPC java ] $@"
+	mkdir -p gen/java/
+	capnpc $^ -o java:gen/java
+
+# c-capnproto needs some empty headers
+gen/c/c++.capnp.h gen/c/java.capnp.h:
+	mkdir -p gen/c/
+	touch '$@'
 
-release:
-	@echo "cereal: this is a release"

cereal/build_from_src.mk

@@ -1,45 +0,0 @@
-SRCS := log.capnp car.capnp
-
-GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
-
-
-UNAME_M ?= $(shell uname -m)
-
-# only generate C++ for docker tests
-ifneq ($(OPTEST),1)
-	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
-
-# Dont build java on the phone...
-ifeq ($(UNAME_M),x86_64)
-	GENS += gen/java/Car.java gen/java/Log.java
-endif
-
-endif
-
-.PHONY: all
-all: $(GENS)
-
-.PHONY: clean
-clean:
-	rm -rf gen
-
-gen/c/%.capnp.c: %.capnp
-	@echo "[ CAPNPC C ] $@"
-	mkdir -p gen/c/
-	capnpc '$<' -o c:gen/c/
-
-gen/cpp/%.capnp.c++: %.capnp
-	@echo "[ CAPNPC C++ ] $@"
-	mkdir -p gen/cpp/
-	capnpc '$<' -o c++:gen/cpp/
-
-gen/java/Car.java gen/java/Log.java: $(SRCS)
-	@echo "[ CAPNPC java ] $@"
-	mkdir -p gen/java/
-	capnpc $^ -o java:gen/java
-
-# c-capnproto needs some empty headers
-gen/c/c++.capnp.h gen/c/java.capnp.h:
-	mkdir -p gen/c/
-	touch '$@'
-

cereal/car.capnp

@@ -7,11 +7,59 @@ $Java.outerClassname("Car");
 
 @0x8e2af1e708af8b8d;
 
+# ******* events causing controls state machine transition *******
+
+struct CarEvent @0x9b1657f34caf3ad3 {
+  name @0 :EventName;
+  enable @1 :Bool;
+  preEnable @7 :Bool;
+  noEntry @2 :Bool;
+  warning @3 :Bool;
+  userDisable @4 :Bool;
+  softDisable @5 :Bool;
+  immediateDisable @6 :Bool;
+
+  enum EventName @0xbaa8c5d505f727de {
+    # TODO: copy from error list
+    commIssue @0;
+    steerUnavailable @1;
+    brakeUnavailable @2;
+    gasUnavailable @3;
+    wrongGear @4;
+    doorOpen @5;
+    seatbeltNotLatched @6;
+    espDisabled @7;
+    wrongCarMode @8;
+    steerTempUnavailable @9;
+    reverseGear @10;
+    buttonCancel @11;
+    buttonEnable @12;
+    pedalPressed @13;
+    cruiseDisabled @14;
+    radarCommIssue @15;
+    dataNeeded @16;
+    speedTooLow @17;
+    outOfSpace @18;
+    overheat @19;
+    calibrationInProgress @20;
+    calibrationInvalid @21;
+    controlsMismatch @22;
+    pcmEnable @23;
+    pcmDisable @24;
+    noTarget @25;
+    radarFault @26;
+    modelCommIssue @27;
+    brakeHold @28;
+    parkBrake @29;
+  }
+}
+
 # ******* main car state @ 100hz *******
 # all speeds in m/s
 
 struct CarState {
-  errors @0: List(Error);
+  errorsDEPRECATED @0 :List(CarEvent.EventName);
+  events @13 :List(CarEvent);
 
   # car speed
   vEgo @1 :Float32;       # best estimate of speed
@@ -33,6 +81,9 @@ struct CarState {
   # cruise state
   cruiseState @10 :CruiseState;
 
+  # gear
+  gearShifter @14 :GearShifter;
+
   # button presses
   buttonEvents @11 :List(ButtonEvent);
 
@@ -48,31 +99,28 @@ struct CarState {
   }
 
   struct CruiseState {
-    enabled @0: Bool;
+    enabled @0 :Bool;
-    speed @1: Float32;
+    speed @1 :Float32;
-    available @2: Bool;
+    available @2 :Bool;
+    speedOffset @3 :Float32;
   }
 
-  enum Error {
+  enum GearShifter {
-    # TODO: copy from error list
+    unknown @0;
-    commIssue @0;
+    park @1;
-    steerUnavailable @1;
+    drive @2;
-    brakeUnavailable @2;
+    neutral @3;
-    gasUnavailable @3;
+    reverse @4;
-    wrongGear @4;
+    sport @5;
-    doorOpen @5;
+    low @6;
-    seatbeltNotLatched @6;
+    brake @7;
-    espDisabled @7;
-    wrongCarMode @8;
-    steerTempUnavailable @9;
-    reverseGear @10;
-    # ...
   }
 
+
   # send on change
   struct ButtonEvent {
-    pressed @0: Bool;
+    pressed @0 :Bool;
-    type @1: Type;
+    type @1 :Type;
 
     enum Type {
       unknown @0;
@@ -91,29 +139,30 @@ struct CarState {
 # ******* radar state @ 20hz *******
 
 struct RadarState {
-  errors @0: List(Error);
+  errors @0 :List(Error);
-  points @1: List(RadarPoint);
+  points @1 :List(RadarPoint);
 
   # which packets this state came from
-  canMonoTimes @2: List(UInt64);
+  canMonoTimes @2 :List(UInt64);
 
   enum Error {
-    notValid @0;
+    commIssue @0;
+    fault @1;
   }
 
   # similar to LiveTracks
   # is one timestamp valid for all? I think so
   struct RadarPoint {
-    trackId @0: UInt64;  # no trackId reuse
+    trackId @0 :UInt64;  # no trackId reuse
 
     # these 3 are the minimum required
-    dRel @1: Float32; # m from the front bumper of the car
+    dRel @1 :Float32; # m from the front bumper of the car
-    yRel @2: Float32; # m
+    yRel @2 :Float32; # m
-    vRel @3: Float32; # m/s
+    vRel @3 :Float32; # m/s
 
     # these are optional and valid if they are not NaN
-    aRel @4: Float32; # m/s^2
+    aRel @4 :Float32; # m/s^2
-    yvRel @5: Float32; # m/s
+    yvRel @5 :Float32; # m/s
   }
 }
 
@@ -121,17 +170,24 @@ struct RadarState {
 
 struct CarControl {
   # must be true for any actuator commands to work
-  enabled @0: Bool;
+  enabled @0 :Bool;
 
-  # range from 0.0 - 1.0
+  gasDEPRECATED @1 :Float32;
-  gas @1: Float32;
+  brakeDEPRECATED @2 :Float32;
-  brake @2: Float32;
+  steeringTorqueDEPRECATED @3 :Float32;
 
-  # range from -1.0 - 1.0
+  actuators @6 :Actuators;
-  steeringTorque @3 :Float32;
 
-  cruiseControl @4: CruiseControl;
+  cruiseControl @4 :CruiseControl;
-  hudControl @5: HUDControl;
+  hudControl @5 :HUDControl;
+
+  struct Actuators {
+    # range from 0.0 - 1.0
+    gas @0: Float32;
+    brake @1: Float32;
+    # range from -1.0 - 1.0
+    steer @2: Float32;
+  }
 
   struct CruiseControl {
     cancel @0: Bool;
@@ -178,31 +234,48 @@ struct CarControl {
 # ****** car param ******
 
 struct CarParams {
-  carName @0: Text;
+  carName @0 :Text;
-  radarName @1: Text;
+  radarName @1 :Text;
-  carFingerprint @2: Text;
+  carFingerprint @2 :Text;
-
+
-  enableSteer @3: Bool;
+  enableSteer @3 :Bool;
-  enableGas @4: Bool;
+  enableGas @4 :Bool;
-  enableBrake @5: Bool;
+  enableBrake @5 :Bool;
-  enableCruise @6: Bool;
+  enableCruise @6 :Bool;
+
+  minEnableSpeed @18 :Float32;
+  safetyModel @19 :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);
+
+  enum SafetyModels {
+    # from board
+    default @0;
+    honda @1;
+    toyota @2;
+  }
 
   # things about the car in the manual
-  m @7: Float32;     # [kg] running weight
+  m @7 :Float32;     # [kg] running weight
-  l @8: Float32;     # [m] wheelbase
+  l @8 :Float32;     # [m] wheelbase
-  sR @9: Float32;    # [] steering ratio
+  sR @9 :Float32;    # [] steering ratio
-  aF @10: Float32;   # [m] GC distance to front axle
+  aF @10 :Float32;   # [m] GC distance to front axle
-  aR @11: Float32;   # [m] GC distance to rear axle
+  aR @11 :Float32;   # [m] GC distance to rear axle
-  chi @12: Float32;  # [] rear steering ratio wrt front steering (usually 0)
+  chi @12 :Float32;  # [] rear steering ratio wrt front steering (usually 0)
 
   # things we can derive
-  j @13: Float32;    # [kg*m2] body rot inertia
+  j @13 :Float32;    # [kg*m2] body rotational inertia
-  cF @14: Float32;   # [N/rad] front tire coeff of stiff
+  cF @14 :Float32;   # [N/rad] front tire coeff of stiff
-  cR @15: Float32;   # [N/rad] rear tire coeff of stiff
+  cR @15 :Float32;   # [N/rad] rear tire coeff of stiff
 
   # Kp and Ki for the lateral control
-  steerKp @16: Float32;
+  steerKp @16 :Float32;
-  steerKi @17: Float32;
+  steerKi @17 :Float32;
 
   # TODO: Kp and Ki for long control, perhaps not needed?
 }

cereal/log.capnp

@@ -92,10 +92,10 @@ struct InitData {
   }
 
   struct PandaInfo {
-    hasPanda @0: Bool;
+    hasPanda @0 :Bool;
-    dongleId @1: Text;
+    dongleId @1 :Text;
-    stVersion @2: Text;
+    stVersion @2 :Text;
-    espVersion @3: Text;
+    espVersion @3 :Text;
   }
 }
 
@@ -110,9 +110,10 @@ struct FrameData {
 
   frameType @7 :FrameType;
   timestampSof @8 :UInt64;
+  transform @10 :List(Float32);
 
   androidCaptureResult @9 :AndroidCaptureResult;
-  
+
   enum FrameType {
     unknown @0;
     neo @1;
@@ -196,10 +197,10 @@ struct GpsLocationData {
   timestamp @7 :Int64;
 
   source @8 :SensorSource;
-  
+
   # Represents NED velocity in m/s.
   vNED @9 :List(Float32);
-  
+
   # Represents expected vertical accuracy in meters. (presumably 1 sigma?)
   verticalAccuracy @10 :Float32;
 
@@ -239,7 +240,9 @@ struct ThermalData {
   # not thermal
   freeSpace @7 :Float32;
   batteryPercent @8 :Int16;
-  batteryStatus @9: Text;
+  batteryStatus @9 :Text;
+
+  fanSpeed @10 :UInt16;
 }
 
 struct HealthData {
@@ -254,8 +257,8 @@ struct HealthData {
 
 struct LiveUI {
   rearViewCam @0 :Bool;
-  alertText1 @1 :Text; 
+  alertText1 @1 :Text;
-  alertText2 @2 :Text; 
+  alertText2 @2 :Text;
   awarenessStatus @3 :Float32;
 }
 
@@ -264,6 +267,7 @@ struct Live20Data {
   mdMonoTime @6 :UInt64;
   ftMonoTimeDEPRECATED @7 :UInt64;
   l100MonoTime @11 :UInt64;
+  radarErrors @12 :List(Car.RadarState.Error);
 
   # all deprecated
   warpMatrixDEPRECATED @0 :List(Float32);
@@ -294,6 +298,7 @@ struct Live20Data {
 
 struct LiveCalibrationData {
   warpMatrix @0 :List(Float32);
+  warpMatrix2 @5 :List(Float32);
   calStatus @1 :Int8;
   calCycle @2 :Int32;
   calPerc @3 :Int8;
@@ -340,14 +345,14 @@ struct Live100Data {
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
 
-  enabled @19: Bool;
+  enabled @19 :Bool;
-  steerOverride @20: Bool;
+  steerOverride @20 :Bool;
 
-  vCruise @22: Float32;
+  vCruise @22 :Float32;
 
   rearViewCam @23 :Bool;
-  alertText1 @24 :Text; 
+  alertText1 @24 :Text;
-  alertText2 @25 :Text; 
+  alertText2 @25 :Text;
   awarenessStatus @26 :Float32;
 
   angleOffset @27 :Float32;
@@ -387,6 +392,7 @@ struct ModelData {
     bigBoxHeight @3 :UInt16;
     boxProjection @4 :List(Float32);
     yuvCorrection @5 :List(Float32);
+    inputTransform @6 :List(Float32);
   }
 }
 
@@ -399,7 +405,7 @@ struct CalibrationFeatures {
 }
 
 struct EncodeIndex {
-  # picture from camera 
+  # picture from camera
   frameId @0 :UInt32;
   type @1 :Type;
   # index of encoder from start of route
@@ -438,69 +444,79 @@ struct LogRotate {
 struct Plan {
   mdMonoTime @9 :UInt64;
   l20MonoTime @10 :UInt64;
+  events @13 :List(Car.CarEvent);
 
   # lateral, 3rd order polynomial
-  lateralValid @0: Bool;
+  lateralValid @0 :Bool;
   dPoly @1 :List(Float32);
+  laneWidth @11 :Float32;
 
   # longitudinal
-  longitudinalValid @2: Bool;
+  longitudinalValid @2 :Bool;
   vTarget @3 :Float32;
   aTargetMin @4 :Float32;
   aTargetMax @5 :Float32;
   jerkFactor @6 :Float32;
   hasLead @7 :Bool;
   fcw @8 :Bool;
+
+  # gps trajectory in car frame
+  gpsTrajectory @12 :GpsTrajectory;
+
+  struct GpsTrajectory {
+    x @0 :List(Float32);
+    y @1 :List(Float32);
+  }
 }
 
 struct LiveLocationData {
-  status @0: UInt8;
+  status @0 :UInt8;
 
-  # 3D fix 
+  # 3D fix
-  lat @1: Float64;
+  lat @1 :Float64;
-  lon @2: Float64;
+  lon @2 :Float64;
-  alt @3: Float32;     # m
+  alt @3 :Float32;     # m
 
   # speed
-  speed @4: Float32;   # m/s
+  speed @4 :Float32;   # m/s
 
   # NED velocity components
-  vNED @5: List(Float32);
+  vNED @5 :List(Float32);
 
   # roll, pitch, heading (x,y,z)
-  roll @6: Float32;     # WRT to center of earth?
+  roll @6 :Float32;     # WRT to center of earth?
-  pitch @7: Float32;    # WRT to center of earth?
+  pitch @7 :Float32;    # WRT to center of earth?
-  heading @8: Float32;  # WRT to north?
+  heading @8 :Float32;  # WRT to north?
 
   # what are these?
-  wanderAngle @9: Float32;
+  wanderAngle @9 :Float32;
-  trackAngle @10: Float32;
+  trackAngle @10 :Float32;
 
   # car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png
 
   # gyro, in car frame, deg/s
-  gyro @11: List(Float32);
+  gyro @11 :List(Float32);
 
   # accel, in car frame, m/s^2
-  accel @12: List(Float32);
+  accel @12 :List(Float32);
 
-  accuracy @13: Accuracy;
+  accuracy @13 :Accuracy;
 
   struct Accuracy {
-    pNEDError @0: List(Float32);
+    pNEDError @0 :List(Float32);
-    vNEDError @1: List(Float32);
+    vNEDError @1 :List(Float32);
-    rollError @2: Float32;
+    rollError @2 :Float32;
-    pitchError @3: Float32;
+    pitchError @3 :Float32;
-    headingError @4: Float32;
+    headingError @4 :Float32;
-    ellipsoidSemiMajorError @5: Float32;
+    ellipsoidSemiMajorError @5 :Float32;
-    ellipsoidSemiMinorError @6: Float32;
+    ellipsoidSemiMinorError @6 :Float32;
-    ellipsoidOrientationError @7: Float32;
+    ellipsoidOrientationError @7 :Float32;
   }
 }
 
 struct EthernetPacket {
   pkt @0 :Data;
-  ts @1: Float32;
+  ts @1 :Float32;
 }
 
 struct NavUpdate {
@@ -602,7 +618,7 @@ struct AndroidGnss {
 
       hasLeapSecond @4 :Bool;
       leapSecond @5 :Int32;
-      
+
       hasFullBiasNanos @6 :Bool;
       fullBiasNanos @7 :Int64;
 
@@ -611,10 +627,10 @@ struct AndroidGnss {
 
       hasBiasUncertaintyNanos @10 :Bool;
       biasUncertaintyNanos @11 :Float64;
-      
+
       hasDriftNanosPerSecond @12 :Bool;
       driftNanosPerSecond @13 :Float64;
-      
+
       hasDriftUncertaintyNanosPerSecond @14 :Bool;
       driftUncertaintyNanosPerSecond @15 :Float64;
     }
@@ -633,7 +649,7 @@ struct AndroidGnss {
       accumulatedDeltaRangeState @9 :Int32;
       accumulatedDeltaRangeMeters @10 :Float64;
       accumulatedDeltaRangeUncertaintyMeters @11 :Float64;
-      
+
       hasCarrierFrequencyHz @12 :Bool;
       carrierFrequencyHz @13 :Float32;
       hasCarrierCycles @14 :Bool;
@@ -705,6 +721,8 @@ struct QcomGnss {
     measurementReport @1 :MeasurementReport;
     clockReport @2 :ClockReport;
     drMeasurementReport @3 :DrMeasurementReport;
+    drSvPoly @4 :DrSvPolyReport;
+    rawLog @5 :Data;
   }
 
   enum MeasurementSource @0xd71a12b6faada7ee {
@@ -742,18 +760,18 @@ struct QcomGnss {
     measurementNotUsable @12 :Bool;
     sirCheckIsNeeded @13 :Bool;
     probationMode @14 :Bool;
-    
+
     glonassMeanderBitEdgeValid @15 :Bool;
     glonassTimeMarkValid @16 :Bool;
-    
+
     gpsRoundRobinRxDiversity @17 :Bool;
     gpsRxDiversity @18 :Bool;
     gpsLowBandwidthRxDiversityCombined @19 :Bool;
     gpsHighBandwidthNu4 @20 :Bool;
     gpsHighBandwidthNu8 @21 :Bool;
     gpsHighBandwidthUniform @22 :Bool;
-    gpsMultipathIndicator @23 :Bool;
+    multipathIndicator @23 :Bool;
-    
+
     imdJammingIndicator @24 :Bool;
     lteB13TxJammingIndicator @25 :Bool;
     freshMeasurementIndicator @26 :Bool;
@@ -953,6 +971,37 @@ struct QcomGnss {
       goodParity @32 :Bool;
     }
   }
+
+  struct DrSvPolyReport {
+    svId @0 :UInt16;
+    frequencyIndex @1 :Int8;
+
+    hasPosition @2 :Bool;
+    hasIono @3 :Bool;
+    hasTropo @4 :Bool;
+    hasElevation @5 :Bool;
+    polyFromXtra @6 :Bool;
+    hasSbasIono @7 :Bool;
+
+    iode @8 :UInt16;
+    t0 @9 :Float64;
+    xyz0 @10 :List(Float64);
+    xyzN @11 :List(Float64);
+    other @12 :List(Float32);
+
+    positionUncertainty @13 :Float32;
+    ionoDelay @14 :Float32;
+    ionoDot @15 :Float32;
+    sbasIonoDelay @16 :Float32;
+    sbasIonoDot @17 :Float32;
+    tropoDelay @18 :Float32;
+    elevation @19 :Float32;
+    elevationDot @20 :Float32;
+    elevationUncertainty @21 :Float32;
+
+    velocityCoeff @22 :List(Float64);
+
+  }
 }
 
 struct LidarPts {
@@ -1037,12 +1086,12 @@ struct UbloxGnss {
     # num of measurements to follow
     numMeas @4 :UInt8;
     measurements @5 :List(Measurement);
-        
+
     struct ReceiverStatus {
-      # leap seconds have been determined 
+      # leap seconds have been determined
       leapSecValid @0 :Bool;
       # Clock reset applied
-      clkReset @1 : Bool;
+      clkReset @1 :Bool;
     }
 
     struct Measurement {
@@ -1060,7 +1109,7 @@ struct UbloxGnss {
       # carrier phase locktime counter in ms
       locktime @7 :UInt16;
       # Carrier-to-noise density ratio (signal strength) in dBHz
-      cno @8 : UInt8;
+      cno @8 :UInt8;
       # pseudorange standard deviation in meters
       pseudorangeStdev @9 :Float32;
       # carrier phase standard deviation in cycles
@@ -1103,31 +1152,47 @@ struct UbloxGnss {
     ecc @15 :Float64;
     cus @16 :Float64;
     a @17 :Float64; # note that this is not the root!!
-    
+
     toe @18 :Float64;
     cic @19 :Float64;
     omega0 @20 :Float64;
     cis @21 :Float64;
-    
+
     i0 @22 :Float64;
     crc @23 :Float64;
     omega @24 :Float64;
     omegaDot @25 :Float64;
-    
+
     iDot @26 :Float64;
     codesL2 @27 :Float64;
     gpsWeek @28 :Float64;
     l2 @29 :Float64;
-    
+
     svAcc @30 :Float64;
     svHealth @31 :Float64;
     tgd @32 :Float64;
     iodc @33 :Float64;
-    
+
     transmissionTime @34 :Float64;
     fitInterval @35 :Float64;
   }
 }
+
+struct Clocks {
+  bootTimeNanos @0 :UInt64;
+  monotonicNanos @1 :UInt64;
+  monotonicRawNanos @2 :UInt64;
+  wallTimeNanos @3 :UInt64;
+  modemUptimeMillis @4 :UInt64;
+}
+
+struct LiveMpcData {
+  x @0 :List(Float32);
+  y @1 :List(Float32);
+  psi @2 :List(Float32);
+  delta @3 :List(Float32);
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -1144,7 +1209,7 @@ struct Event {
     model @9 :ModelData;
     features @10 :CalibrationFeatures;
     sensorEvents @11 :List(SensorEventData);
-    health @12 : HealthData;
+    health @12 :HealthData;
     live20 @13 :Live20Data;
     liveUIDEPRECATED @14 :LiveUI;
     encodeIdx @15 :EncodeIndex;
@@ -1167,5 +1232,7 @@ struct Event {
     lidarPts @32 :LidarPts;
     procLog @33 :ProcLog;
     ubloxGnss @34 :UbloxGnss;
+    clocks @35 :Clocks;
+    liveMpc @36 :LiveMpcData;
   }
 }

common/basedir.py

@@ -0,0 +1,4 @@
+import os
+BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
+
+

common/crash.py

@@ -2,7 +2,7 @@
 import os
 import sys
 
-if os.getenv("NOLOG"):
+if os.getenv("NOLOG") or os.getenv("NOCRASH"):
   def capture_exception(*exc_info):
     pass
   def bind_user(**kwargs):

common/fingerprints.py

@@ -18,6 +18,9 @@ _FINGERPRINTS = {
     57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8,
     # sent messages
     0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5,
+  },
+  "TOYOTA PRIUS 2017": {
+    896L: 8, 832L: 8, 898L: 8, 899L: 8, 577L: 8, 528L: 8, 529L: 8, 530L: 8, 531L: 8, 532L: 8, 533L: 8, 534L: 8, 535L: 8, 536L: 8, 537L: 8, 538L: 8, 539L: 8, 540L: 8, 541L: 8, 542L: 8, 543L: 8, 544L: 8, 545L: 8, 546L: 8, 547L: 8, 548L: 8, 549L: 8, 550L: 8, 551L: 8, 296L: 6, 553L: 6, 554L: 6, 555L: 6, 556L: 6, 557L: 6, 558L: 6, 559L: 6, 560L: 7, 561L: 8, 562L: 8, 883L: 8, 837L: 8, 833L: 8, 576L: 8, 321L: 4, 834L: 8, 835L: 8, 580L: 8, 581L: 8, 897L: 8, 584L: 8, 1136L: 4, 976L: 8, 977L: 8, 978L: 8, 291L: 7, 881L: 8, 352L: 8, 353L: 7, 867L: 8, 868L: 8, 869L: 8, 1126L: 3, 304L: 7, 880L: 8, 552L: 6, 882L: 8, 979L: 8, 884L: 8, 885L: 8, 836L: 8
   }
 }
 

common/params.py

@@ -66,6 +66,13 @@ keys = {
 # read:    radard
   "CarParams": TxType.CLEAR_ON_CAR_START}
 
+def fsync_dir(path):
+  fd = os.open(path, os.O_RDONLY)
+  try:
+    os.fsync(fd)
+  finally:
+    os.close(fd)
+
 
 class FileLock(object):
   def __init__(self, path, create):
@@ -182,15 +189,32 @@ class DBWriter(DBAccessor):
     self._check_entered()
 
     try:
+      # data_path refers to the externally used path to the params. It is a symlink.
+      # old_data_path is the path currently pointed to by data_path.
+      # tempdir_path is a path where the new params will go, which the new data path will point to.
+      # new_data_path is a temporary symlink that will atomically overwrite data_path.
+      #
+      # The current situation is:
+      #   data_path -> old_data_path
+      # We're going to write params data to tempdir_path
+      #   tempdir_path -> params data
+      # Then point new_data_path to tempdir_path
+      #   new_data_path -> tempdir_path
+      # Then atomically overwrite data_path with new_data_path
+      #   data_path -> tempdir_path
       old_data_path = None
       new_data_path = None
       tempdir_path = tempfile.mkdtemp(prefix=".tmp", dir=self._path)
+
       try:
         # Write back all keys.
         os.chmod(tempdir_path, 0o777)
         for k, v in self._vals.items():
           with open(os.path.join(tempdir_path, k), "wb") as f:
             f.write(v)
+            f.flush()
+            os.fsync(f.fileno())
+        fsync_dir(tempdir_path)
 
         data_path = self._data_path()
         try:
@@ -203,16 +227,21 @@ class DBWriter(DBAccessor):
         new_data_path = "{}.link".format(tempdir_path)
         os.symlink(os.path.basename(tempdir_path), new_data_path)
         os.rename(new_data_path, data_path)
-      # TODO(mgraczyk): raise useful error when values are bad.
+        fsync_dir(self._path)
-      except:
+      finally:
-        shutil.rmtree(tempdir_path)
+        # If the rename worked, we can delete the old data. Otherwise delete the new one.
-        if new_data_path is not None:
+        success = new_data_path is not None and os.path.exists(data_path) and (
+          os.readlink(data_path) == os.path.basename(tempdir_path))
+
+        if success:
+          if old_data_path is not None:
+            shutil.rmtree(old_data_path)
+        else:
+          shutil.rmtree(tempdir_path)
+
+        # Regardless of what happened above, there should be no link at new_data_path.
+        if new_data_path is not None and os.path.islink(new_data_path):
           os.remove(new_data_path)
-        raise
-
-      # Keep holding the lock while we clean up the old data.
-      if old_data_path is not None:
-        shutil.rmtree(old_data_path)
     finally:
       os.umask(self._prev_umask)
       self._prev_umask = None
@@ -249,6 +278,10 @@ class Params(object):
   def car_start(self):
     self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START)
 
+  def delete(self, key):
+    with self.env.begin(write=True) as txn:
+      txn.delete(key)
+
   def get(self, key, block=False):
     if key not in keys:
       raise UnknownKeyName(key)

common/profiler.py

@@ -14,6 +14,12 @@ class Profiler(object):
     self.cp.append((name, tt - self.last_time))
     self.last_time = tt
 
+  def reset(self, enabled=False):
+    self.enabled = enabled
+    self.cp = []
+    self.start_time = sec_since_boot()
+    self.last_time = self.start_time
+
   def display(self):
     if not self.enabled:
       return

opendbc

@@ -1 +1 @@
-Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba
+Subproject commit 063032ff2b9b878c2cc10301504bad9db54f655f

panda

@@ -1 +1 @@
-Subproject commit 4901d52104e369f2444b5d56846199fdfc3fd695
+Subproject commit 92a1c773e763297b7478b5bca44e25fb8cd8bdf2

phonelibs/qpoases/EXAMPLES/example1.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:018b8429d7eafb7dbc55145bf29b6ee033fab367764d5994c0d2d3876d1a771e
+size 1949

phonelibs/qpoases/EXAMPLES/example1b.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c5a9064ef08e632ea9f9aa31c7f4d6013756b57200c5791d8fc8bb1b5d1583be
+size 1776

phonelibs/qpoases/INCLUDE/Bounds.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d77876b6d9bbb9968f95cc7908d2e7a7706b71cc1d79d44c9f269b7e5078278
+size 5743

phonelibs/qpoases/INCLUDE/Constants.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5b515339eee590e3af6ae78679e58338fe5e164d51550ba93d3de2b8bb0f2e09
+size 3501

phonelibs/qpoases/INCLUDE/Constraints.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a394fb4f49deb8598ee72920e1b03a682c457b42765b8aac0f8a2ac43bdf118
+size 5864

phonelibs/qpoases/INCLUDE/CyclingManager.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:325c722b58ad39b7860137ea1ecfb1bd48fc44251717d16574dcd3ad28e11089
+size 3861

phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:60dc80b14e5a7fac2257648a004987d0706d4d32602e3657d2c97d24363406bb
+size 3435

phonelibs/qpoases/INCLUDE/Indexlist.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:53cf87460a46afce93acc7d5d76011366a387a93d67401bec25350e05bb783a5
+size 4928

phonelibs/qpoases/INCLUDE/MessageHandling.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:67c24d5d24e53580b5af982cfa36b115e88314b1306e2cea6b3c1e9ea746a369
+size 20367

phonelibs/qpoases/INCLUDE/QProblem.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01213e5cb12bf937d65ffd05c7a9ba70a7ab4e043753449edda98fd08f733930
+size 33063

phonelibs/qpoases/INCLUDE/QProblemB.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3c9f053c8eed7d0b8aa72dec81ecdb7cb59bc87dee0108398bc34b79a76c7bd3
+size 27333

phonelibs/qpoases/INCLUDE/SubjectTo.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fdbee52661ba442d31f697828ff283505bf48e78f7740aac59f39aa62a18cae2
+size 5572

phonelibs/qpoases/INCLUDE/Types.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ff13c48ec416a3c30f7582d38e4ca43b30e8867b28bcebdab3a6cced6e141258
+size 4467

phonelibs/qpoases/INCLUDE/Utils.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2f37d5507c5288bce904e776045b68702240d2d51896262f645084cba7e971cc
+size 6713

phonelibs/qpoases/LICENSE.txt

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:319daefe38ebeead3df178e5937898a52d67c0c795f54f1c0bb10ac3b9cffb63
+size 26940

phonelibs/qpoases/README.txt

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ae75304482e680a0c6bc6f81eafcf29c3a77b407a6344181eb6f67cd3b2fbf69
+size 3177

phonelibs/qpoases/SRC/Bounds.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dedf2112735d4beae4682878dc0a97cc2bcda648459d917e0049f4997654ac52
+size 5901

phonelibs/qpoases/SRC/Bounds.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:099752cb994e787b7d6c9a5dbde908eb19d4d3a4e0bec63e36c3907704732e57
+size 2545

phonelibs/qpoases/SRC/Constraints.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3028ce5088b5690e774267d4c1f6de8c330b3ecf0add682fc2350d87cee68030
+size 6102

phonelibs/qpoases/SRC/Constraints.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f13088527d6bdd946bf66581edb71d93ea7659261cd797fbd2cc2a3ed836a903
+size 2641

phonelibs/qpoases/SRC/CyclingManager.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:67bab79e60c64e87a7977957cd55726912837629115d6246cd089dbb9aae4fc7
+size 3949

phonelibs/qpoases/SRC/CyclingManager.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:85df62984743813f8f9e92ff299b74c790d9e174906a5966b916ebb21f068165
+size 1643

phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1cc39dede19249ae695a72203ee643e65b29a0738316b1b2e77cc5f9d72821fe
+size 12434

phonelibs/qpoases/SRC/Indexlist.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d40beb3c8639d8c9bb3d529d98a5c51d7b1a9239febda97ef136a2d27a785bf
+size 6642

phonelibs/qpoases/SRC/Indexlist.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9bf145bff7575704135bd3f2933a9af5bf37cace1ad56c7ed887128f2bfc459f
+size 2163

phonelibs/qpoases/SRC/MessageHandling.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2b2de17f7e28f36f108afc05ea257b82fb112ad7afb0aef965e857484e951f06
+size 19449

phonelibs/qpoases/SRC/MessageHandling.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:247726b5943a872b367d5eb948078428f570e3684f9c0e541a13890729020ecb
+size 3215

phonelibs/qpoases/SRC/QProblem.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e50488623df439ee0539898819839afd7c064be68709c32eea19db552377b81a
+size 96202

phonelibs/qpoases/SRC/QProblem.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:279519a38bb964e574752c75a9be6ce6e2720477183397a85cd505859329654d
+size 5667

phonelibs/qpoases/SRC/QProblemB.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a4dc609ba60648cf2307536b2c5088fe082caff7785d31a5686ef6b8bee6db7a
+size 51089

phonelibs/qpoases/SRC/QProblemB.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:789884d3683b59533bc5e84ee44e616c3a48b414285266f0155846f7ee73cad5
+size 7273

phonelibs/qpoases/SRC/SubjectTo.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6e850a791144172ed5bfec0fca3b32abd0347885f0e739ae3aa306f9fb4889bb
+size 4244

phonelibs/qpoases/SRC/SubjectTo.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6872db4cea8f779994636a27c100724b2a7ca7ff1451aa2bffcc768798c9bb80
+size 2847

phonelibs/qpoases/SRC/Utils.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:695b8e4e30ab4ec7e522446463716fbbb1be92c840117830fa1bd389d969ddfa
+size 10228

phonelibs/qpoases/SRC/Utils.ipp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:abdbf8757354226ba1723eaa4d8ace186471c6a193812dca3a9a06f4b30b9fce
+size 1315

phonelibs/qpoases/VERSIONS.txt

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f02a5ff04907df394d7b905216cfcece2a7a54cdbed25a4d892bb08f2f760d77
+size 3061

pyextra

@@ -1 +1 @@
-Subproject commit e0738376db27d208603d7e63dd465e003ca06325
+Subproject commit 4eda4dd765c2bc719da9064774de6b2c14c322d1

run_docker_tests.sh

@@ -2,6 +2,7 @@
 set -e
 
 docker build -t tmppilot -f Dockerfile.openpilot .
+
 docker run --rm \
-  -v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out \
+  -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out \
-  tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'
+  tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'

selfdrive/boardd/Makefile

@@ -4,6 +4,7 @@ CXX = clang++
 ARCH := $(shell uname -m)
 OS := $(shell uname -o)
 
+BASEDIR = ../..
 PHONELIBS = ../../phonelibs
 
 WARN_FLAGS = -Werror=implicit-function-declaration \
@@ -12,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
              -Werror=return-type \
              -Werror=format-extra-args
 
-CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
-CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
 
 ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
 ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
@@ -37,6 +38,8 @@ ZMQ_LIBS = -L$(HOME)/one/external/zmq/lib/ \
 CEREAL_LIBS = -L$(HOME)/one/external/capnp/lib/ \
               -l:libcapnp.a -l:libcapnp_c.a -l:libkj.a
 EXTRA_LIBS = -lusb-1.0 -lpthread
+CXXFLAGS += -I/usr/include/libusb-1.0
+CFLAGS += -I/usr/include/libusb-1.0
 endif
 
 .PHONY: all
@@ -46,6 +49,8 @@ include ../common/cereal.mk
 
 OBJS = boardd.o \
        ../common/swaglog.o \
+       ../common/params.o \
+       ../common/util.o \
        $(PHONELIBS)/json/src/json.o \
        $(CEREAL_OBJS)
 

selfdrive/boardd/boardd.cc

@@ -19,7 +19,9 @@
 
 #include <capnp/serialize.h>
 #include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/gen/cpp/car.capnp.h"
 
+#include "common/params.h"
 #include "common/swaglog.h"
 #include "common/timing.h"
 
@@ -38,20 +40,55 @@ bool loopback_can = false;
 #define TIMEOUT 0
 
 #define SAFETY_NOOUTPUT  0x0000
-#define SAFETY_HONDA     0x0001
-#define SAFETY_ALLOUTPUT 0x1337
 
+pthread_t safety_setter_thread_handle = -1;
+
+void *safety_setter_thread(void *s) {
+  char *value;
+  size_t value_sz = 0;
+
+  LOGW("waiting for params to set safety model");
+  while (1) {
+    const int result = read_db_value(NULL, "CarParams", &value, &value_sz);
+    if (value_sz > 0) break;
+    usleep(100*1000);
+  }
+  LOGW("got %d bytes CarParams", value_sz);
+
+  // format for board, make copy due to alignment issues, will be freed on out of scope
+  auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
+  memcpy(amsg.begin(), value, value_sz);
+
+  capnp::FlatArrayMessageReader cmsg(amsg);
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+  int safety_model = car_params.getSafetyModel();
+  LOGW("setting safety model: %d", safety_model);
+
+  pthread_mutex_lock(&usb_lock);
+
+  // set in the mutex to avoid race
+  safety_setter_thread_handle = -1;
+
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, 0, NULL, 0, TIMEOUT);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  return NULL;
+}
+
+// must be called before threads or with mutex
 bool usb_connect() {
   int err;
 
   dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
-  if (dev_handle == NULL) { return false; }
+  if (dev_handle == NULL) { goto fail; }
 
   err = libusb_set_configuration(dev_handle, 1);
-  if (err != 0) { return false; }
+  if (err != 0) { goto fail; }
 
   err = libusb_claim_interface(dev_handle, 0);
-  if (err != 0) { return false; }
+  if (err != 0) { goto fail; }
 
   if (loopback_can) {
     libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
@@ -60,30 +97,23 @@ bool usb_connect() {
   // power off ESP
   libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
 
-  // forward CAN1 to CAN3...soon
+  // power on charging (may trigger a reconnection, should be okay)
-  //libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT);
+  #ifndef __x86_64__
-
+    libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT);
-  // set UART modes for Honda Accord
+  #else
-  /*for (int uart = 2; uart <= 3; uart++) {
+    LOGW("not enabling charging on x86_64");
-    // 9600 baud
+  #endif
-    libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT);
-    // even parity
-    libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT);
-    // callback 1
-    libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT);
-  }
 
-  // TODO: Boardd should be able to set the baud rate
+  // no output is the default
-  int baud = 500000;
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
-  libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0,
-                          (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1
-  libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0,
-                          (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/
 
-  // TODO: Boardd should be able to be told which safety model to use
+  if (safety_setter_thread_handle == -1) {
-  libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT);
+    err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
+  }
 
   return true;
+fail:
+  return false;
 }
 
 void usb_retry_connect() {
@@ -93,7 +123,7 @@ void usb_retry_connect() {
 }
 
 void handle_usb_issue(int err, const char func[]) {
-  LOGE("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
+  LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
   if (err == -4) {
     LOGE("lost connection");
     usb_retry_connect();
@@ -113,7 +143,7 @@ void can_recv(void *s) {
   do {
     err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
     if (err != 0) { handle_usb_issue(err, __func__); }
-    if (err == -8) { LOGE("overflow got 0x%x", recv); };
+    if (err == -8) { LOGE_100("overflow got 0x%x", recv); };
 
     // timeout is okay to exit, recv still happened
     if (err == -7) { break; }
@@ -261,6 +291,46 @@ void can_send(void *s) {
 
 // **** threads ****
 
+void *thermal_thread(void *crap) {
+  int err;
+  LOGD("start thermal thread");
+
+  // thermal = 8005
+  void *context = zmq_ctx_new();
+  void *subscriber = zmq_socket(context, ZMQ_SUB);
+  zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
+  zmq_connect(subscriber, "tcp://127.0.0.1:8005");
+
+  // run as fast as messages come in
+  while (!do_exit) {
+    // recv from thermal
+    zmq_msg_t msg;
+    zmq_msg_init(&msg);
+    err = zmq_msg_recv(&msg, subscriber, 0);
+    assert(err >= 0);
+
+    // format for board, make copy due to alignment issues, will be freed on out of scope
+    // copied from send thread...
+    auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
+    memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
+
+    capnp::FlatArrayMessageReader cmsg(amsg);
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    uint16_t target_fan_speed = event.getThermal().getFanSpeed();
+    //LOGW("setting fan speed %d", target_fan_speed);
+
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+  }
+
+  // turn the fan off when we exit
+  libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, NULL, 0, TIMEOUT);
+
+  return NULL;
+}
+
 void *can_send_thread(void *crap) {
   LOGD("start send thread");
 
@@ -364,8 +434,16 @@ int main() {
                        can_recv_thread, NULL);
   assert(err == 0);
 
+  pthread_t thermal_thread_handle;
+  err = pthread_create(&thermal_thread_handle, NULL,
+                       thermal_thread, NULL);
+  assert(err == 0);
+
   // join threads
 
+  err = pthread_join(thermal_thread_handle, NULL);
+  assert(err == 0);
+
   err = pthread_join(can_recv_thread_handle, NULL);
   assert(err == 0);
 

selfdrive/boardd/boardd.py

@@ -20,6 +20,8 @@ except Exception:
 
 SAFETY_NOOUTPUT = 0
 SAFETY_HONDA = 1
+SAFETY_TOYOTA = 2
+SAFETY_TOYOTA_NOLIMITS = 0x1336
 SAFETY_ALLOUTPUT = 0x1337
 
 # *** serialization functions ***
@@ -33,7 +35,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
       cc = dat.can[i]
     cc.address = can_msg[0]
     cc.busTime = can_msg[1]
-    cc.dat = can_msg[2]
+    cc.dat = str(can_msg[2])
     cc.src = can_msg[3]
   return dat
 
@@ -111,6 +113,7 @@ def can_init():
 def boardd_mock_loop():
   context = zmq.Context()
   can_init()
+  handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
 
   logcan = messaging.sub_sock(context, service_list['can'].port)
 

selfdrive/can/Makefile

@@ -1,6 +1,7 @@
 CC = clang
 CXX = clang++
 
+BASEDIR = ../..
 PHONELIBS := ../../phonelibs
 
 UNAME_S := $(shell uname -s)
@@ -53,7 +54,7 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h
 ../../cereal/gen/cpp/log.capnp.h:
 	cd ../../cereal && make
 
-libdbc.so:: parser.cc $(DBC_CCS)
+libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS)
 	$(CXX) -fPIC -shared -o '$@' $^ \
 	  -I. \
 	  -I../.. \
@@ -63,7 +64,7 @@ libdbc.so:: parser.cc $(DBC_CCS)
     $(CEREAL_CXXFLAGS) \
     $(CEREAL_LIBS)
 
-dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc
+dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc
 	PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
 
 .PHONY: clean

selfdrive/can/common.h

@@ -1,13 +1,20 @@
-#ifndef PARSER_COMMON_H
+#ifndef SELFDRIVE_CAN_COMMON_H
-#define PARSER_COMMON_H
+#define SELFDRIVE_CAN_COMMON_H
 
 #include <cstddef>
 #include <cstdint>
+#include <string>
 
 #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
 
 
 
+struct SignalPackValue {
+  const char* name;
+  double value;
+};
+
+
 struct SignalParseOptions {
   uint32_t address;
   const char* name;
@@ -54,6 +61,8 @@ struct DBC {
   const Msg *msgs;
 };
 
+const DBC* dbc_lookup(const std::string& dbc_name);
+
 void dbc_register(const DBC* dbc);
 
 #define dbc_init(dbc) \

selfdrive/can/dbc.cc

@@ -0,0 +1,26 @@
+#include <string>
+#include <vector>
+
+#include "common.h"
+
+namespace {
+
+std::vector<const DBC*>& get_dbcs() {
+  static std::vector<const DBC*> vec;
+  return vec;
+}
+
+}
+
+const DBC* dbc_lookup(const std::string& dbc_name) {
+  for (const auto& dbci : get_dbcs()) {
+    if (dbc_name == dbci->name) {
+      return dbci;
+    }
+  }
+  return NULL;
+}
+
+void dbc_register(const DBC* dbc) {
+  get_dbcs().push_back(dbc);
+}

selfdrive/can/dbc_template.cc

@@ -1,6 +1,6 @@
 #include <cstdint>
 
-#include "parser_common.h"
+#include "common.h"
 
 namespace {
 

selfdrive/can/libdbc_py.py

@@ -0,0 +1,51 @@
+import os
+import subprocess
+
+from cffi import FFI
+
+can_dir = os.path.dirname(os.path.abspath(__file__))
+libdbc_fn = os.path.join(can_dir, "libdbc.so")
+subprocess.check_output(["make"], cwd=can_dir)
+
+ffi = FFI()
+ffi.cdef("""
+
+typedef struct SignalParseOptions {
+  uint32_t address;
+  const char* name;
+  double default_value;
+} SignalParseOptions;
+
+typedef struct MessageParseOptions {
+  uint32_t address;
+  int check_frequency;
+} MessageParseOptions;
+
+typedef struct SignalValue {
+  uint32_t address;
+  uint16_t ts;
+  const char* name;
+  double value;
+} SignalValue;
+
+void* can_init(int bus, const char* dbc_name,
+              size_t num_message_options, const MessageParseOptions* message_options,
+              size_t num_signal_options, const SignalParseOptions* signal_options);
+
+void can_update(void* can, uint64_t sec, bool wait);
+
+size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
+
+
+typedef struct SignalPackValue {
+  const char* name;
+  double value;
+} SignalPackValue;
+
+void* canpack_init(const char* dbc_name);
+
+uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals);
+
+""")
+
+libdbc = ffi.dlopen(libdbc_fn)

selfdrive/can/packer.cc

@@ -0,0 +1,81 @@
+#include <cassert>
+
+#include <string>
+#include <vector>
+#include <utility>
+#include <algorithm>
+#include <map>
+
+#include "common.h"
+
+#define WARN printf
+
+namespace {
+
+class CANPacker {
+public:
+  CANPacker(const std::string& dbc_name) {
+    dbc = dbc_lookup(dbc_name);
+    assert(dbc);
+
+    for (int i=0; i<dbc->num_msgs; i++) {
+      const Msg* msg = &dbc->msgs[i];
+      for (int j=0; j<msg->num_sigs; j++) {
+        const Signal* sig = &msg->sigs[j];
+        signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
+      }
+    }
+  }
+
+  uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals) {
+    uint64_t ret = 0;
+    for (const auto& sigval : signals) {
+      std::string name = std::string(sigval.name);
+      double value = sigval.value;
+
+      auto sig_it = signal_lookup.find(make_pair(address, name));
+      if (sig_it == signal_lookup.end()) {
+        WARN("undefined signal %s", name.c_str());
+        continue;
+      }
+      auto sig = sig_it->second;
+
+      int64_t ival = (int64_t)((value - sig.offset) / sig.factor);
+      if (ival < 0) {
+        WARN("signed pack unsupported right now");
+        continue;
+      }
+
+      uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo;
+      uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo;
+      ret &= ~mask;
+      ret |= dat;
+    }
+
+    return ret;
+  }
+
+
+
+private:
+  const DBC *dbc = NULL;
+  std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
+};
+
+}
+
+extern "C" {
+
+void* canpack_init(const char* dbc_name) {
+  CANPacker *ret = new CANPacker(std::string(dbc_name));
+  return (void*)ret;
+}
+
+uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals) {
+  CANPacker *cp = (CANPacker*)inst;
+
+  return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals));
+}
+
+}
+

selfdrive/can/packer.py

@@ -0,0 +1,37 @@
+import struct
+
+from selfdrive.can.libdbc_py import libdbc, ffi
+
+class CANPacker(object):
+  def __init__(self, dbc_name):
+    self.packer = libdbc.canpack_init(dbc_name)
+    self.sig_names = {}
+
+  def pack(self, addr, values):
+    # values: [(signal_name, signal_value)]
+
+    values_thing = []
+    for name, value in values:
+      if name not in self.sig_names:
+        self.sig_names[name] = ffi.new("char[]", name)
+
+      values_thing.append({
+        'name': self.sig_names[name],
+        'value': value
+      })
+
+    values_c = ffi.new("SignalPackValue[]", values_thing)
+
+    return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c)
+
+  def pack_bytes(self, addr, values):
+    return struct.pack(">Q", self.pack(addr, values))
+
+
+if __name__ == "__main__":
+  cp = CANPacker("honda_civic_touring_2016_can")
+  s = cp.pack_bytes(0x30c, [
+    ("PCM_SPEED", 123),
+    ("PCM_GAS", 10),
+  ])
+  print s.encode("hex")

selfdrive/can/parser.cc

@@ -18,7 +18,7 @@
 #include <capnp/serialize.h>
 #include "cereal/gen/cpp/log.capnp.h"
 
-#include "parser_common.h"
+#include "common.h"
 
 #define DEBUG(...)
 // #define DEBUG printf
@@ -40,8 +40,6 @@ uint64_t read_u64_be(const uint8_t* v) {
           | (uint64_t)v[7]);
 }
 
-std::vector<const DBC*> g_dbc;
-
 bool honda_checksum(int address, uint64_t d, int l) {
   int target = (d >> l) & 0xF;
 
@@ -77,9 +75,9 @@ struct MessageState {
     for (int i=0; i < parse_sigs.size(); i++) {
       auto& sig = parse_sigs[i];
 
-      int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1);
+      int64_t tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1);
       if (sig.is_signed) {
-        tmp -= (tmp >> (sig.b2-1)) ? (1<<sig.b2) : 0; //signed
+        tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
       }
 
       DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp);
@@ -123,6 +121,7 @@ struct MessageState {
 
 };
 
+
 class CANParser {
  public:
   CANParser(int abus, const std::string& dbc_name,
@@ -135,15 +134,10 @@ class CANParser {
     zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
     zmq_connect(subscriber, "tcp://127.0.0.1:8006");
 
-    for (auto dbci : g_dbc) {
+    dbc = dbc_lookup(dbc_name);
-      if (dbci->name == dbc_name) {
-        dbc = dbci;
-        break;
-      }
-    }
     assert(dbc);
 
-    for (auto &op : options) {
+    for (const auto& op : options) {
       MessageState state = {
         .address = op.address,
         // .check_frequency = op.check_frequency,
@@ -178,7 +172,7 @@ class CANParser {
       }
 
       // track requested signals for this message
-      for (auto &sigop : sigoptions) {
+      for (const auto& sigop : sigoptions) {
         if (sigop.address != op.address) continue;
 
         for (int i=0; i<msg->num_sigs; i++) {
@@ -230,8 +224,8 @@ class CANParser {
 
   void UpdateValid(uint64_t sec) {
     can_valid = true;
-    for (auto &kv : message_states) {
+    for (const auto& kv : message_states) {
-      auto &state = kv.second;
+      const auto& state = kv.second;
       if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
         if (state.seen > 0) {
           INFO("%X TIMEOUT\n", state.address);
@@ -279,8 +273,8 @@ class CANParser {
   std::vector<SignalValue> query(uint64_t sec) {
     std::vector<SignalValue> ret;
 
-    for (auto &kv : message_states) {
+    for (const auto& kv : message_states) {
-      auto &state = kv.second;
+      const auto& state = kv.second;
       if (sec != 0 && state.seen != sec) continue;
       
       for (int i=0; i<state.parse_sigs.size(); i++) {
@@ -311,10 +305,6 @@ class CANParser {
 
 }
 
-void dbc_register(const DBC* dbc) {
-  g_dbc.push_back(dbc);
-}
-
 extern "C" {
 
 void* can_init(int bus, const char* dbc_name,

selfdrive/can/parser.py

@@ -1,46 +1,8 @@
 import os
 import time
-import subprocess
 from collections import defaultdict
 
-from cffi import FFI
+from selfdrive.can.libdbc_py import libdbc, ffi
-
-can_dir = os.path.dirname(os.path.abspath(__file__))
-libdbc_fn = os.path.join(can_dir, "libdbc.so")
-subprocess.check_output(["make"], cwd=can_dir)
-
-ffi = FFI()
-ffi.cdef("""
-
-typedef struct SignalParseOptions {
-  uint32_t address;
-  const char* name;
-  double default_value;
-} SignalParseOptions;
-
-typedef struct MessageParseOptions {
-  uint32_t address;
-  int check_frequency;
-} MessageParseOptions;
-
-typedef struct SignalValue {
-  uint32_t address;
-  uint16_t ts;
-  const char* name;
-  double value;
-} SignalValue;
-
-void* can_init(int bus, const char* dbc_name,
-              size_t num_message_options, const MessageParseOptions* message_options,
-              size_t num_signal_options, const SignalParseOptions* signal_options);
-
-void can_update(void* can, uint64_t sec, bool wait);
-
-size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
-
-""")
-
-libdbc = ffi.dlopen(libdbc_fn)
 
 class CANParser(object):
   def __init__(self, dbc_name, signals, checks=[], bus=0):

selfdrive/car/__init__.py

@@ -2,6 +2,11 @@ from common.fingerprints import fingerprint
 
 from .honda.interface import CarInterface as HondaInterface
 
+try:
+  from .toyota.interface import CarInterface as ToyotaInterface
+except ImportError:
+  ToyotaInterface = None
+
 try:
   from .simulator.interface import CarInterface as SimInterface
 except ImportError:
@@ -17,6 +22,7 @@ interfaces = {
   "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
   "HONDA ACCORD 2016 TOURING": HondaInterface,
   "HONDA CR-V 2016 TOURING": HondaInterface,
+  "TOYOTA PRIUS 2017": ToyotaInterface,
 
   "simulator": SimInterface,
   "simulator2": Sim2Interface

selfdrive/car/honda/carcontroller.py

@@ -1,5 +1,5 @@
 from collections import namedtuple
-
+import os
 import common.numpy_fast as np
 from common.numpy_fast import clip, interp
 from common.realtime import sec_since_boot
@@ -10,25 +10,25 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
 from . import hondacan
 
 
-def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
+def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
   # hyst params... TODO: move these to VehicleParams
   brake_hyst_on = 0.055 if civic else 0.1    # to activate brakes exceed this value
   brake_hyst_off = 0.005                     # to deactivate brakes below this value
   brake_hyst_gap = 0.01                      # don't change brake command for small ocilalitons within this value
 
   #*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
-  if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
+  if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
-    final_brake = 0.
+    brake = 0.
-  braking = final_brake > 0.
+  braking = brake > 0.
 
   # for small brake oscillations within brake_hyst_gap, don't change the brake command
-  if final_brake == 0.:
+  if brake == 0.:
     brake_steady = 0.
-  elif final_brake > brake_steady + brake_hyst_gap:
+  elif brake > brake_steady + brake_hyst_gap:
-    brake_steady = final_brake - brake_hyst_gap
+    brake_steady = brake - brake_hyst_gap
-  elif final_brake < brake_steady - brake_hyst_gap:
+  elif brake < brake_steady - brake_hyst_gap:
-    brake_steady = final_brake + brake_hyst_gap
+    brake_steady = brake + brake_hyst_gap
-  final_brake = brake_steady
+  brake = brake_steady
 
   if not civic:
     brake_on_offset_v  = [.25, .15]   # min brake command on brake activation. below this no decel is perceived
@@ -36,10 +36,10 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
     # offset the brake command for threshold in the brake system. no brake torque perceived below it
     brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
     brake_offset = brake_on_offset - brake_hyst_on
-    if final_brake > 0.0:
+    if brake > 0.0:
-      final_brake += brake_offset
+      brake += brake_offset
 
-  return final_brake, braking, brake_steady
+  return brake, braking, brake_steady
 
 class AH:
   #[alert_idx, value]
@@ -78,12 +78,9 @@ class CarController(object):
   def __init__(self):
     self.braking = False
     self.brake_steady = 0.
-    self.final_brake_last = 0.
+    self.brake_last = 0.
-
-    # redundant safety check with the board
-    self.controls_allowed = False
 
-  def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
+  def update(self, sendcan, enabled, CS, frame, actuators, \
              pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
              hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
              snd_beep, snd_chime):
@@ -94,20 +91,16 @@ class CarController(object):
       return
 
     # *** apply brake hysteresis ***
-    final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
+    brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
 
     # *** no output if not enabled ***
-    if not enabled:
+    if not enabled and CS.pcm_acc_status:
-      final_gas = 0.
-      final_brake = 0.
-      final_steer = 0.
       # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
-      if CS.pcm_acc_status:
+      pcm_cancel_cmd = True
-        pcm_cancel_cmd = True
 
     # *** rate limit after the enable check ***
-    final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
+    brake = rate_limit(brake, self.brake_last, -2., 1./100)
-    self.final_brake_last = final_brake
+    self.brake_last = brake
 
     # vehicle hud display, wait for one update from 10Hz 0x304 msg
     #TODO: use enum!!
@@ -143,7 +136,8 @@ class CarController(object):
     GAS_MAX = 1004
     BRAKE_MAX = 1024/4
     if CS.civic:
-      STEER_MAX = 0x1000
+      is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
+      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
     else:
@@ -151,50 +145,22 @@ class CarController(object):
     GAS_OFFSET = 328
 
     # steer torque is converted back to CAN reference (positive when steering right)
-    apply_gas = int(clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
+    apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
-    apply_brake = int(clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
+    apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1))
-    apply_steer = int(clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
+    apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
 
     # no gas if you are hitting the brake or the user is
     if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
-      print "CANCELLING GAS", apply_brake
       apply_gas = 0
 
     # no computer brake if the gas is being pressed
     if CS.car_gas > 0 and apply_brake != 0:
-      print "CANCELLING BRAKE"
       apply_brake = 0
 
     # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
     if CS.steer_not_allowed:
-      print "STEER ALERT, TORQUE INHIBITED"
       apply_steer = 0
 
-    # *** entry into controls state ***
-    if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
-        CS.cruise_buttons == 0 and not self.controls_allowed:
-      print "CONTROLS ARE LIVE"
-      self.controls_allowed = True
-
-    # *** exit from controls state on cancel, gas, or brake ***
-    if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
-        CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
-      print "CONTROLS ARE DEAD"
-      self.controls_allowed = False
-
-    # *** controls fail on steer error, brake error, or invalid can ***
-    if CS.steer_error:
-      print "STEER ERROR"
-      self.controls_allowed = False
-
-    if CS.brake_error:
-      print "BRAKE ERROR"
-      self.controls_allowed = False
-
-    if not CS.can_valid and self.controls_allowed:   # 200 ms
-      print "CAN INVALID"
-      self.controls_allowed = False
-
     # Send CAN commands.
     can_sends = []
 

selfdrive/car/honda/carstate.py

@@ -1,18 +1,54 @@
 import os
 import time
-
+from cereal import car
 import common.numpy_fast as np
 from common.realtime import sec_since_boot
-
 import selfdrive.messaging as messaging
+from selfdrive.can.parser import CANParser
+from selfdrive.config import Conversions as CV
 
-from selfdrive.car.honda.can_parser import CANParser
-from selfdrive.can.parser import CANParser as CANParserC
 
-NEW_CAN = os.getenv("OLD_CAN") is None
+def parse_gear_shifter(can_gear_shifter, is_acura):
 
-def get_can_parser(CP):
+  if can_gear_shifter == 0x1:
-  # this function generates lists for signal, messages and initial values
+    return "park"
+  elif can_gear_shifter == 0x2:
+    return "reverse"
+
+  if is_acura:
+    if can_gear_shifter == 0x3:
+      return "neutral"
+    elif can_gear_shifter == 0x4:
+      return "drive"
+    elif can_gear_shifter == 0xa:
+      return "sport"
+
+  else:
+    if can_gear_shifter == 0x4:
+      return "neutral"
+    elif can_gear_shifter == 0x8:
+      return "drive"
+    elif can_gear_shifter == 0x10:
+      return "sport"
+    elif can_gear_shifter == 0x20:
+      return "low"
+
+  return "unknown"
+
+_K0 = -0.3
+_K1 = -0.01879
+_K2 = 0.01013
+
+def calc_cruise_offset(offset, speed):
+  # euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
+  # constraints to solve for _K0, _K1, _K2 are:
+  # - speed = 0m/s, out = -0.3
+  # - speed = 34m/s, offset = 20, out = -0.25
+  # - speed = 34m/s, offset = -2.5, out = -1.8
+  return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
+
+def get_can_signals(CP):
+# this function generates lists for signal, messages and initial values
   if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
     dbc_f = 'honda_civic_touring_2016_can.dbc'
     signals = [
@@ -50,8 +86,9 @@ def get_can_parser(CP):
       ("CRUISE_SETTING", 0x296, 0),
       ("LEFT_BLINKER", 0x326, 0),
       ("RIGHT_BLINKER", 0x326, 0),
-      ("COUNTER", 0x324, 0),
+      ("CRUISE_SPEED_OFFSET", 0x37c, 0),
-      ("ENGINE_RPM", 0x17C, 0)
+      ("EPB_STATE", 0x1c2, 0),
+      ("BRAKE_HOLD_ACTIVE", 0x1A4, 0),
     ]
     checks = [
       # address, frequency
@@ -65,6 +102,7 @@ def get_can_parser(CP):
       (0x1d0, 50),
       (0x305, 10),
       (0x324, 10),
+      (0x37c, 10),
       (0x405, 3),
     ]
 
@@ -104,8 +142,7 @@ def get_can_parser(CP):
       ("CRUISE_SETTING", 0x1a6, 0),
       ("LEFT_BLINKER", 0x294, 0),
       ("RIGHT_BLINKER", 0x294, 0),
-      ("COUNTER", 0x324, 0),
+      ("CRUISE_SPEED_OFFSET", 0x37c, 0)
-      ("ENGINE_RPM", 0x17C, 0)
     ]
     checks = [
       (0x156, 100),
@@ -118,6 +155,7 @@ def get_can_parser(CP):
       (0x1d0, 50),
       (0x305, 10),
       (0x324, 10),
+      (0x37c, 10),
       (0x405, 3),
     ]
   elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
@@ -157,8 +195,6 @@ def get_can_parser(CP):
       ("CRUISE_SETTING", 0x1a6, 0),
       ("LEFT_BLINKER", 0x294, 0),
       ("RIGHT_BLINKER", 0x294, 0),
-      ("COUNTER", 0x324, 0),
-      ("ENGINE_RPM", 0x17C, 0)
     ]
     checks = [
       (0x156, 100),
@@ -209,8 +245,6 @@ def get_can_parser(CP):
       ("CRUISE_SETTING", 0x1a6, 0),
       ("LEFT_BLINKER", 0x294, 0),
       ("RIGHT_BLINKER", 0x294, 0),
-      ("COUNTER", 0x324, 0),
-      ("ENGINE_RPM", 0x17C, 0)
     ]
     checks = [
       (0x156, 100),
@@ -230,22 +264,23 @@ def get_can_parser(CP):
     signals.append(("INTERCEPTOR_GAS", 0x201, 0))
     checks.append((0x201, 50))
 
-  if NEW_CAN:
+  return dbc_f, signals, checks
-    return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0)
+
-  else:
+def get_can_parser(CP):
-    return CANParser(dbc_f, signals, checks)
+  dbc_f, signals, checks = get_can_signals(CP)
+  return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
 
 class CarState(object):
   def __init__(self, CP, logcan):
+    self.acura = False
     self.civic = False
     self.accord = False
     self.crv = False
     if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
       self.civic = True
     elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS":
-      self.civic = False
+      self.acura = True
     elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
-      # fake civic
       self.accord = True
     elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
       self.crv = True
@@ -274,10 +309,7 @@ class CarState(object):
 
   def update(self, can_pub_main=None):
     cp = self.cp
-    if NEW_CAN:
+    cp.update(int(sec_since_boot() * 1e9), False)
-      cp.update(int(sec_since_boot() * 1e9), False)
-    else:
-      cp.update_can(can_pub_main)
 
     # copy can_valid
     self.can_valid = cp.can_valid
@@ -294,8 +326,6 @@ class CarState(object):
     self.prev_left_blinker_on = self.left_blinker_on
     self.prev_right_blinker_on = self.right_blinker_on
 
-    self.rpm = cp.vl[0x17C]['ENGINE_RPM']
-
     # ******************* parse out can *******************
     self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
                                cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
@@ -316,9 +346,11 @@ class CarState(object):
     self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
     self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
     # calc best v_ego estimate, by averaging two opposite corners
-    self.v_wheel = (
+    self.v_wheel_fl = cp.vl[0x1D0]['WHEEL_SPEED_FL']
-      cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
+    self.v_wheel_fr = cp.vl[0x1D0]['WHEEL_SPEED_FR']
-      cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
+    self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL']
+    self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR']
+    self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
     # blend in transmission speed at low speed, since it has more low speed accuracy
     self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
     self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
@@ -328,49 +360,59 @@ class CarState(object):
       self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
       #print self.user_gas, self.user_gas_pressed
     if self.civic:
-      self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
+      can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
       self.gear = 0  # TODO: civic has CVT... needs rev engineering
       self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
       self.main_on = cp.vl[0x326]['MAIN_ON']
-      self.gear_shifter_valid = self.gear_shifter in [1,8]  # TODO: 1/P allowed for debug
       self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
       self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
       self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
+      self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
+      self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0
+      self.brake_hold = cp.vl[0x1A4]['BRAKE_HOLD_ACTIVE']
     elif self.accord:
-      self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
+      can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
       self.gear = 0  # TODO: accord has CVT... needs rev engineering
       self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
       self.main_on = cp.vl[0x1A6]['MAIN_ON']
-      self.gear_shifter_valid = self.gear_shifter in [1,8]  # TODO: 1/P allowed for debug
       self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
       self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
       self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
+      self.cruise_speed_offset = -0.3
+      self.park_brake = 0  # TODO
+      self.brake_hold = 0  # TODO
     elif self.crv:
-      self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
+      can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
       self.gear = cp.vl[0x191]['GEAR']
       self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
       self.main_on = cp.vl[0x1A6]['MAIN_ON']
-      self.gear_shifter_valid = self.gear_shifter in [1,8]  # TODO: 1/P allowed for debug
       self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
       self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
       self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
-    else:
+      self.cruise_speed_offset = -0.3
-      self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
+      self.park_brake = 0  # TODO
+      self.brake_hold = 0  # TODO
+    elif self.acura:
+      can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
       self.gear = cp.vl[0x1A3]['GEAR']
       self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
       self.main_on = cp.vl[0x1A6]['MAIN_ON']
-      self.gear_shifter_valid = self.gear_shifter in [1,4]  # TODO: 1/P allowed for debug
       self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
       self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
       self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
+      self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
+      self.park_brake = 0  # TODO
+      self.brake_hold = 0
+
+    self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.acura)
 
     if self.accord:
       # on the accord, this doesn't seem to include cruise control
@@ -398,4 +440,25 @@ class CarState(object):
     self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
     self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
     self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
-    self.counter_pcm = cp.vl[0x324]['COUNTER']
+
+
+# carstate standalone tester
+if __name__ == '__main__':
+  import zmq
+  import time
+  from selfdrive.services import service_list
+  context = zmq.Context()
+  logcan = messaging.sub_sock(context, service_list['can'].port) 
+
+  class CarParams(object):
+    def __init__(self):
+      self.carFingerprint = "HONDA CIVIC 2016 TOURING"
+      self.enableGas = 0
+      self.enableCruise = 0
+  CP = CarParams()
+  CS = CarState(CP, logcan)
+
+  while 1:
+    CS.update()
+    time.sleep(0.01)
+

selfdrive/car/honda/interface.py

@@ -1,19 +1,65 @@
 #!/usr/bin/env python
 import os
 import time
-import common.numpy_fast as np
+import numpy as np
+from common.numpy_fast import clip
+from common.realtime import sec_since_boot
 
 from selfdrive.config import Conversions as CV
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
 from .carstate import CarState
 from .carcontroller import CarController, AH
 from selfdrive.boardd.boardd import can_capnp_to_can_list
-
 from cereal import car
 
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 
-NEW_CAN = os.getenv("OLD_CAN") is None
+
+def get_compute_gb():
+  # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
+  # where -1.0 is max brake and 1.0 is max gas
+  # see debug/dump_accel_from_fiber.py to see how those parameters were generated
+  w0 = np.array([[ 1.22056961, -0.39625418,  0.67952657],
+                 [ 1.03691769,  0.78210306, -0.41343188]])
+  b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
+  w2 = np.array([[-0.59124422,  0.42899439,  0.38660881],
+                 [ 0.79973811,  0.13178682,  0.08550351],
+                 [-0.15651935, -0.44360259,  0.76910877]])
+  b2 = np.array([ 0.15624429,  0.02294923, -0.0341086 ])
+  w4 = np.array([[-0.31521443],
+                 [-0.38626176],
+                 [ 0.52667892]])
+  b4 = np.array([-0.02922216])
+
+  def compute_output(dat, w0, b0, w2, b2, w4, b4):
+    m0 = np.dot(dat, w0) + b0
+    m0 = leakyrelu(m0, 0.1)
+    m2 = np.dot(m0, w2) + b2
+    m2 = leakyrelu(m2, 0.1)
+    m4 = np.dot(m2, w4) + b4
+    return m4
+
+  def leakyrelu(x, alpha):
+    return np.maximum(x, alpha * x)
+
+  def _compute_gb(accel, speed):
+    #linearly extrap below v1 using v1 and v2 data
+    v1 = 5.
+    v2 = 10.
+    dat = np.array([accel, speed])
+    if speed > 5.:
+      m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
+    else:
+      dat[1] = v1
+      m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
+      dat[1] = v2
+      m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
+      m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
+    return float(m4)
+
+  return _compute_gb
+
 
 # Car button codes
 class CruiseButtons:
@@ -22,6 +68,7 @@ class CruiseButtons:
   CANCEL      = 2
   MAIN        = 1
 
+
 #car chimes: enumeration from dbc file. Chimes are for alerts and warnings
 class CM:
   MUTE = 0
@@ -30,6 +77,7 @@ class CM:
   REPEATED = 1
   CONTINUOUS = 2
 
+
 #car beepss: enumeration from dbc file. Beeps are for activ and deactiv
 class BP:
   MUTE = 0
@@ -37,12 +85,17 @@ class BP:
   TRIPLE = 2
   REPEATED = 1
 
+
 class CarInterface(object):
   def __init__(self, CP, logcan, sendcan=None):
     self.logcan = logcan
     self.CP = CP
 
     self.frame = 0
+    self.last_enable_pressed = 0
+    self.last_enable_sent = 0
+    self.gas_pressed_prev = False
+    self.brake_pressed_prev = False
     self.can_invalid_count = 0
 
     # *** init the major players ***
@@ -54,7 +107,8 @@ class CarInterface(object):
       self.CC = CarController()
 
     if self.CS.accord:
-      self.accord_msg = []
+      # self.accord_msg = []
+      raise NotImplementedError
 
   @staticmethod
   def get_params(candidate, fingerprint):
@@ -68,6 +122,8 @@ class CarInterface(object):
     ret.radarName = "nidec"
     ret.carFingerprint = candidate
 
+    ret.safetyModel = car.CarParams.SafetyModels.honda
+
     ret.enableSteer = True
     ret.enableBrake = True
 
@@ -75,7 +131,7 @@ class CarInterface(object):
     ret.enableGas = 0x201 in fingerprint
     ret.enableCruise = not ret.enableGas
 
-    # FIXME: hardcoding honda civic 2016 touring wight so it can be used to 
+    # 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
@@ -86,27 +142,32 @@ class CarInterface(object):
     cR_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.steerKp, ret.steerKi = 0.8, 0.24
+      # 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']
+      ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
     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
       # Acura at comma has modified steering FW, so different tuning for the Neo in that car
-      # FIXME: using dongleId isn't great, better to identify the car than the Neo
+      is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
-      is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee'
       ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
     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.steerKp, ret.steerKi = 0.8, 0.24
     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
@@ -114,48 +175,54 @@ class CarInterface(object):
       ret.steerKp, ret.steerKi = 0.8, 0.24
     else:
       raise ValueError("unsupported car %s" % candidate)
- 
+
+    # 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. Otherwise, add 0.5 mph margin to not 
+    # 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
     # 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)
 
     # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position... all cars will have approximately similar dyn behaviors
+    # 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)
 
     # no rear steering, at least on the listed cars above
     ret.chi = 0.
 
+    # no max steer limit VS speed
+    ret.steerMaxBP = [0.]  # m/s
+    ret.steerMaxV = [1.]   # max steer allowed
+    ret.gasMaxBP = [0.]  # m/s
+    ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed
+    ret.brakeMaxBP = [5., 20.]  # m/s
+    ret.brakeMaxV = [1., 0.8]   # max brake allowed
+
     return ret
 
+  compute_gb = staticmethod(get_compute_gb())
+
   # returns a car.CarState
-  def update(self):
+  def update(self, c):
     # ******************* do can recv *******************
     can_pub_main = []
     canMonoTimes = []
 
-    if NEW_CAN:
+    self.CS.update(can_pub_main)
-      self.CS.update(can_pub_main)
-    else:
-      for a in messaging.drain_sock(self.logcan):
-        canMonoTimes.append(a.logMonoTime)
-        can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80]))
-        if self.CS.accord:
-          self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
-          self.accord_msg = self.accord_msg[-1:]
-      self.CS.update(can_pub_main)
 
     # create message
     ret = car.CarState.new_message()
 
     # speeds
     ret.vEgo = self.CS.v_ego
-    ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
+    ret.wheelSpeeds.fl = self.CS.v_wheel_fl
-    ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
+    ret.wheelSpeeds.fr = self.CS.v_wheel_fr
-    ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
+    ret.wheelSpeeds.rl = self.CS.v_wheel_rl
-    ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
+    ret.wheelSpeeds.rr = self.CS.v_wheel_rr
 
     # gas pedal
     ret.gas = self.CS.car_gas / 256.0
@@ -172,28 +239,17 @@ class CarInterface(object):
     # TODO: units
     ret.steeringAngle = self.CS.angle_steers
 
-    if self.CS.accord:
+    # gear shifter lever
-      # TODO: move this into the CAN parser
+    ret.gearShifter = self.CS.gear_shifter
-      ret.steeringTorque = 0
+
-      if len(self.accord_msg) > 0:
+    ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
-        aa = map(lambda x: ord(x)&0x7f, self.accord_msg[0][2])
+    ret.steeringPressed = self.CS.steer_override
-        if len(aa) != 5 or (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f != aa[4]:
-          print "ACCORD MSG BAD LEN OR CHECKSUM!"
-          # TODO: throw an error here?
-        else:
-          st = ((aa[0]&0xF) << 5) + (aa[1]&0x1F)
-          if st >= 256:
-            st = -(512-st)
-          ret.steeringTorque = st
-      ret.steeringPressed = abs(ret.steeringTorque) > 20
-    else:
-      ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
-      ret.steeringPressed = self.CS.steer_override
 
     # cruise state
     ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
     ret.cruiseState.available = bool(self.CS.main_on)
+    ret.cruiseState.speedOffset = self.CS.cruise_speed_offset 
 
     # TODO: button presses
     buttonEvents = []
@@ -244,38 +300,94 @@ class CarInterface(object):
       buttonEvents.append(be)
     ret.buttonEvents = buttonEvents
 
-    # errors
+    # events
     # TODO: I don't like the way capnp does enums
     # These strings aren't checked at compile time
-    errors = []
+    events = []
     if not self.CS.can_valid:
       self.can_invalid_count += 1
       if self.can_invalid_count >= 5:
-        errors.append('commIssue')
+        events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     else:
       self.can_invalid_count = 0
     if self.CS.steer_error:
-      errors.append('steerUnavailable')
+      events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     elif self.CS.steer_not_allowed:
-      errors.append('steerTempUnavailable')
+      events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
     if self.CS.brake_error:
-      errors.append('brakeUnavailable')
+      events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-    if not self.CS.gear_shifter_valid:
+    if not ret.gearShifter == 'drive':
-      errors.append('wrongGear')
+      events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.door_all_closed:
-      errors.append('doorOpen')
+      events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.seatbelt:
-      errors.append('seatbeltNotLatched')
+      events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if self.CS.esp_disabled:
-      errors.append('espDisabled')
+      events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not self.CS.main_on:
-      errors.append('wrongCarMode')
+      events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
-    if self.CS.gear_shifter == 2:
+    if ret.gearShifter == 'reverse':
-      errors.append('reverseGear')
+      events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-
+    if self.CS.brake_hold:
-    ret.errors = errors
+      events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
+    if self.CS.park_brake:
+      events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
+
+    if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
+      events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
+
+    # disable on pedals rising edge or when brake is pressed and speed isn't zero
+    if (ret.gasPressed and not self.gas_pressed_prev) or \
+       (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
+      events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+
+    #if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
+    if ret.gasPressed:
+      events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+
+    # it can happen that car cruise disables while comma system is enabled: need to
+    # keep braking if needed or if the speed is very low
+    # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
+    if self.CP.enableCruise and not ret.cruiseState.enabled and \
+       (c.actuators.brake <= 0. or ret.vEgo < 0.3):
+      events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
+
+    cur_time = sec_since_boot()
+    enable_pressed = False
+    # handle button presses
+    for b in ret.buttonEvents:
+
+      # do enable on both accel and decel buttons
+      if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+        print "enabled pressed at", cur_time
+        self.last_enable_pressed = cur_time
+        enable_pressed = True
+
+      # do disable on button down
+      if b.type == "cancel" and b.pressed:
+        events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
+
+    if self.CP.enableCruise:
+      # KEEP THIS EVENT LAST! send enable event if button is pressed and there are 
+      # NO_ENTRY events, so controlsd will display alerts. Also not send enable events
+      # too close in time, so a no_entry will not be followed by another one.
+      # TODO: button press should be the only thing that triggers enble
+      if ((cur_time - self.last_enable_pressed) < 0.2 and
+          (cur_time - self.last_enable_sent) > 0.2 and
+          ret.cruiseState.enabled) or \
+         (enable_pressed and get_events(events, [ET.NO_ENTRY])):
+        events.append(create_event('buttonEnable', [ET.ENABLE]))
+        self.last_enable_sent = cur_time
+    elif enable_pressed:
+      events.append(create_event('buttonEnable', [ET.ENABLE]))
+
+    ret.events = events
     ret.canMonoTimes = canMonoTimes
 
+    # update previous brake/gas pressed
+    self.gas_pressed_prev = ret.gasPressed
+    self.brake_pressed_prev = ret.brakePressed
+
     # cast to reader so it can't be modified
     #print ret
     return ret.as_reader()
@@ -309,10 +421,10 @@ class CarInterface(object):
       "chimeRepeated": (BP.MUTE, CM.REPEATED),
       "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
 
-    pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6)
+    pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6)
 
     self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
-      c.gas, c.brake, c.steeringTorque, \
+      c.actuators, \
       c.cruiseControl.speedOverride, \
       c.cruiseControl.override, \
       c.cruiseControl.cancel, \
@@ -324,4 +436,3 @@ class CarInterface(object):
       snd_chime = snd_chime)
 
     self.frame += 1
-    return not (c.enabled and not self.CC.controls_allowed)

selfdrive/car/honda/old_can_parser.py

@@ -36,15 +36,16 @@ class CANParser(object):
 
     for _, addr, _ in signals:
       self.vl[addr] = {}
-      self.ts[addr] = 0
+      self.ts[addr] = {}
       self.ct[addr] = sec_since_boot()
-      self.ok[addr] = False
+      self.ok[addr] = True
       self.cn[addr] = 0
       self.cn_vl[addr] = 0
       self.ck[addr] = False
 
     for name, addr, ival in signals:
       self.vl[addr][name] = ival
+      self.ts[addr][name] = 0
 
     self._msgs = [s[1] for s in signals]
     self._sgs = [s[0] for s in signals]
@@ -94,7 +95,6 @@ class CANParser(object):
           self.ok[msg] = False
 
         # update msg time stamps and counter value
-        self.ts[msg] = ts
         self.ct[msg] = self.sec_since_boot_cached
         self.cn[msg] = cn
         self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
@@ -107,12 +107,14 @@ class CANParser(object):
         for ii in idxs:
           sg = self._sgs[ii]
           self.vl[msg][sg] = out[sg]
+          self.ts[msg][sg] = ts
 
     # for each message, check if it's too long since last time we received it
     self._check_dead_msgs()
 
     # assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
     self.can_valid = True
+
     if False in self.ok.values():
       #print "CAN INVALID!"
       self.can_valid = False

selfdrive/common/cereal.mk

@@ -1,14 +1,27 @@
 UNAME_M ?= $(shell uname -m)
-ifeq ($(UNAME_M),x86_64)
+UNAME_S ?= $(shell uname -s)
 
-CEREAL_CFLAGS = -I$(ONE)/external/capnp/include
+
+ifeq ($(UNAME_S),Darwin)
+
+CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
+CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
+
+CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
+              $(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \
+              $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a
+
+else ifeq ($(UNAME_M),x86_64)
+
+CEREAL_CFLAGS = -I$(BASEDIR)/external/capnp/include
 CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
 ifeq ($(CEREAL_LIBS),)
-	CEREAL_LIBS = -L$(ONE)/external/capnp/lib \
+  CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \
 	              -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
 endif
 
 else
+
 CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
 CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
 ifeq ($(CEREAL_LIBS),)
@@ -30,4 +43,3 @@ car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \
            -c -o '$@' '$<'
-  

selfdrive/common/mat.h

@@ -65,4 +65,24 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
   return ret;
 }
 
+// scales the input and output space of a transformation matrix
+// that assumes pixel-center origin.
+static inline mat3 transform_scale_buffer(const mat3 in, float s) {
+  // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
+
+  mat3 transform_out = {{
+    1.0f/s, 0.0f, 0.5f,
+    0.0f, 1.0f/s, 0.5f,
+    0.0f, 0.0f, 1.0f,
+  }};
+
+  mat3 transform_in = {{
+    s,  0.0f, -0.5f*s,
+    0.0f, s, -0.5f*s,
+    0.0f, 0.0f, 1.0f,
+  }};
+
+  return matmul3(transform_in, matmul3(in, transform_out));
+}
+
 #endif

selfdrive/common/params.cc

@@ -11,6 +11,18 @@
 #include <stdlib.h>
 #include <stdio.h>
 
+namespace {
+
+template <typename T>
+T* null_coalesce(T* a, T* b) {
+  return a != NULL ? a : b;
+}
+
+static const char* default_params_path = null_coalesce(
+    const_cast<const char*>(getenv("PARAMS_PATH")), "/data/params");
+
+}  // namespace
+
 int write_db_value(const char* params_path, const char* key, const char* value,
                    size_t value_size) {
   int lock_fd = -1;
@@ -18,6 +30,11 @@ int write_db_value(const char* params_path, const char* key, const char* value,
   int result;
   char tmp_path[1024];
   char path[1024];
+  ssize_t bytes_written;
+
+  if (params_path == NULL) {
+    params_path = default_params_path;
+  }
 
   // Write value to temp.
   result =
@@ -27,7 +44,7 @@ int write_db_value(const char* params_path, const char* key, const char* value,
   }
 
   tmp_fd = mkstemp(tmp_path);
-  const ssize_t bytes_written = write(tmp_fd, value, value_size);
+  bytes_written = write(tmp_fd, value, value_size);
   if (bytes_written != value_size) {
     result = -20;
     goto cleanup;
@@ -79,6 +96,10 @@ int read_db_value(const char* params_path, const char* key, char** value,
   int result;
   char path[1024];
 
+  if (params_path == NULL) {
+    params_path = default_params_path;
+  }
+
   result = snprintf(path, sizeof(path), "%s/.lock", params_path);
   if (result < 0) {
     goto cleanup;
@@ -99,7 +120,7 @@ int read_db_value(const char* params_path, const char* key, char** value,
   // Read value.
   // TODO(mgraczyk): If there is a lot of contention, we can release the lock
   //                 after opening the file, before reading.
-  *value = read_file(path, value_sz);
+  *value = static_cast<char*>(read_file(path, value_sz));
   if (*value == NULL) {
     result = -22;
     goto cleanup;

selfdrive/common/params.h

@@ -7,14 +7,12 @@
 extern "C" {
 #endif
 
-#define PARAMS_PATH "/data/params"
-
 int write_db_value(const char* params_path, const char* key, const char* value,
                    size_t value_size);
 
 // Reads a value from the params database.
 // Inputs:
-//  params_path: The path of the database, eg /sdcard/params.
+//  params_path: The path of the database, or NULL to use the default.
 //  key: The key to read.
 //  value: A pointer where a newly allocated string containing the db value will
 //         be written.

selfdrive/common/swaglog.h

@@ -1,6 +1,8 @@
 #ifndef SWAGLOG_H
 #define SWAGLOG_H
 
+#include "common/timing.h"
+
 #define CLOUDLOG_DEBUG 10
 #define CLOUDLOG_INFO 20
 #define CLOUDLOG_WARNING 30
@@ -24,9 +26,43 @@ void cloudlog_bind(const char* k, const char* v);
                                            __func__, \
                                            fmt, ## __VA_ARGS__)
 
+#define cloudlog_rl(burst, millis, lvl, fmt, ...)   \
+{                                                   \
+  static uint64_t __begin = 0;                      \
+  static int __printed = 0;                         \
+  static int __missed = 0;                          \
+                                                    \
+  int __burst = (burst);                            \
+  int __millis = (millis);                          \
+  uint64_t __ts = nanos_since_boot();               \
+                                                    \
+  if (!__begin) __begin = __ts;                     \
+                                                    \
+  if (__begin + __millis*1000000ULL < __ts) {       \
+    if (__missed) {                                 \
+      cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
+    }                                               \
+    __begin = 0;                                    \
+    __printed = 0;                                  \
+    __missed = 0;                                   \
+  }                                                 \
+                                                    \
+  if (__printed < __burst) {                        \
+    cloudlog(lvl, fmt, ## __VA_ARGS__);             \
+    __printed++;                                    \
+  } else {                                          \
+    __missed++;                                     \
+  }                                                 \
+}
+
 #define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
 #define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
 #define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
 #define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
 
+#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
 #endif

selfdrive/common/timing.h

@@ -5,7 +5,7 @@
 #include <time.h>
 
 #ifdef __APPLE__
-#define CLOCK_BOOTTIME CLOCK_REALTIME
+#define CLOCK_BOOTTIME CLOCK_MONOTONIC
 #endif
 
 static inline uint64_t nanos_since_boot() {
@@ -38,4 +38,17 @@ static inline double seconds_since_epoch() {
   return (double)t.tv_sec + t.tv_nsec * 1e-9;
 }
 
+// you probably should use nanos_since_boot instead
+static inline uint64_t nanos_monotonic() {
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline uint64_t nanos_monotonic_raw() {
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC_RAW, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
 #endif

selfdrive/common/util.c

@@ -3,6 +3,10 @@
 #include <string.h>
 #include <assert.h>
 
+#ifdef __linux__
+#include <sys/prctl.h>
+#endif
+
 void* read_file(const char* path, size_t* out_len) {
   FILE* f = fopen(path, "r");
   if (!f) {
@@ -28,3 +32,10 @@ void* read_file(const char* path, size_t* out_len) {
 
   return buf;
 }
+
+void set_thread_name(const char* name) {
+#ifdef __linux__
+  // pthread_setname_np is dumb (fails instead of truncates)
+  prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
+#endif
+}

selfdrive/common/util.h

@@ -1,6 +1,9 @@
 #ifndef COMMON_UTIL_H
 #define COMMON_UTIL_H
 
+
+#ifndef __cplusplus
+
 #define min(a,b) \
  ({ __typeof__ (a) _a = (a); \
      __typeof__ (b) _b = (b); \
@@ -11,6 +14,8 @@
      __typeof__ (b) _b = (b); \
    _a > _b ? _a : _b; })
 
+#endif
+
 #define clamp(a,b,c) \
  ({ __typeof__ (a) _a = (a); \
      __typeof__ (b) _b = (b); \
@@ -19,11 +24,20 @@
 
 #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 // Reads a file into a newly allocated buffer.
 //
 // Returns NULL on failure, otherwise the NULL-terminated file contents.
 // The result must be freed by the caller.
 void* read_file(const char* path, size_t* out_len);
 
+void set_thread_name(const char* name);
+
+#ifdef __cplusplus
+}
+#endif
 
 #endif

selfdrive/common/utilpp.h

@@ -9,6 +9,10 @@
 #include <sstream>
 #include <fstream>
 
+#ifdef __x86_64
+#include <linux/limits.h>
+#endif
+
 namespace util {
 
 inline bool starts_with(std::string s, std::string prefix) {

selfdrive/common/version.h

@@ -1 +1 @@
-#define OPENPILOT_VERSION "0.3.6.1"
+#define OPENPILOT_VERSION "0.3.7"

selfdrive/common/visionipc.c

@@ -1,6 +1,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdbool.h>
+#include <string.h>
 #include <unistd.h>
 #include <assert.h>
 #include <errno.h>
@@ -126,7 +127,7 @@ int vipc_send(int fd, const VisionPacket *p2) {
   return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
 }
 
-void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
+void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
                      int num_fds, const int* fds) {
   for (int i=0; i<num_fds; i++) {
     if (bufs[i].addr) {
@@ -180,10 +181,10 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
   s->bufs_info = rp.d.stream_bufs;
 
   s->num_bufs = rp.num_fds;
-  s->bufs = calloc(s->num_bufs, sizeof(VisionBuf));
+  s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
   assert(s->bufs);
 
-  visionbufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
+  vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
 
   if (out_bufs_info) {
     *out_bufs_info = s->bufs_info;
@@ -192,7 +193,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
   return 0;
 }
 
-VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra) {
+VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
   int err;
 
   VisionPacket rp;

selfdrive/common/visionipc.h

@@ -47,9 +47,9 @@ typedef struct VisionStreamBufs {
   } buf_info;
 } VisionStreamBufs;
 
-typedef struct VisionBufExtra {
+typedef struct VIPCBufExtra {
   uint32_t frame_id; // only for yuv
-} VisionBufExtra;
+} VIPCBufExtra;
 
 typedef union VisionPacketData {
   struct {
@@ -60,7 +60,7 @@ typedef union VisionPacketData {
   struct {
     VisionStreamType type;
     int idx;
-    VisionBufExtra extra;
+    VIPCBufExtra extra;
   } stream_acq;
   struct {
     VisionStreamType type;
@@ -79,12 +79,12 @@ int vipc_connect(void);
 int vipc_recv(int fd, VisionPacket *out_p);
 int vipc_send(int fd, const VisionPacket *p);
 
-typedef struct VisionBuf {
+typedef struct VIPCBuf {
   int fd;
   size_t len;
   void* addr;
-} VisionBuf;
+} VIPCBuf;
-void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
+void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
                      int num_fds, const int* fds);
 
 
@@ -94,11 +94,11 @@ typedef struct VisionStream {
   int last_idx;
   int num_bufs;
   VisionStreamBufs bufs_info;
-  VisionBuf *bufs;
+  VIPCBuf *bufs;
 } VisionStream;
 
 int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
-VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
+VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
 void visionstream_destroy(VisionStream *s);
 
 #ifdef __cplusplus

selfdrive/controls/controlsd.py

@@ -2,24 +2,24 @@
 import os
 import json
 from copy import copy
-
 import zmq
-
 from cereal import car, log
 from common.numpy_fast import clip
 from common.fingerprints import fingerprint
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.profiler import Profiler
 from common.params import Params
-
 import selfdrive.messaging as messaging
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.services import service_list
 from selfdrive.car import get_car
 from selfdrive.controls.lib.planner import Planner
-from selfdrive.controls.lib.drive_helpers import learn_angle_offset
+from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
-from selfdrive.controls.lib.longcontrol import LongControl
+                                                 get_events, \
+                                                 create_event, \
+                                                 EventTypes as ET
+from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
 from selfdrive.controls.lib.latcontrol import LatControl
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -34,427 +34,472 @@ AWARENESS_TIME = 360.      # 6 minutes limit without user touching steering whee
 AWARENESS_PRE_TIME = 20.   # a first alert is issued 20s before start decelerating the car
 AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
 
-# class Cal
+
 class Calibration:
   UNCALIBRATED = 0
   CALIBRATED = 1
   INVALID = 2
 
-# to be used
+
-class State():
+class State:
   DISABLED = 0
   ENABLED = 1
-  SOFT_DISABLE = 2
+  PRE_ENABLED = 2
-
+  SOFT_DISABLING = 3
-class Controls(object):
-  def __init__(self, gctx, rate=100):
-    self.rate = rate
-
-    # *** log ***
-    context = zmq.Context()
-
-    # pub
-    self.live100 = messaging.pub_sock(context, service_list['live100'].port)
-    self.carstate = messaging.pub_sock(context, service_list['carState'].port)
-    self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
-    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
- 
-    # sub
-    self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
-    self.health = messaging.sub_sock(context, service_list['health'].port)
-    logcan = messaging.sub_sock(context, service_list['can'].port)
-    self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
-    
-    self.CC = car.CarControl.new_message()
-    self.CI, self.CP = get_car(logcan, sendcan)
-    self.PL = Planner(self.CP)
-    self.AM = AlertManager()
-    self.LoC = LongControl()
-    self.LaC = LatControl()
-    self.VM = VehicleModel(self.CP)
-  
-    # write CarParams
-    params = Params()
-    params.put("CarParams", self.CP.to_bytes())
-  
-    # fake plan
-    self.plan_ts = 0
-    self.plan = log.Plan.new_message()
-    self.plan.lateralValid = False
-    self.plan.longitudinalValid = False
-  
-    # controls enabled state
-    self.enabled = False
-    self.last_enable_request = 0
-  
-    # learned angle offset
-    self.angle_offset = 0
-    calibration_params = params.get("CalibrationParams")
-    if calibration_params:
-      try:
-        calibration_params = json.loads(calibration_params)
-        self.angle_offset = calibration_params["angle_offset"]
-      except (ValueError, KeyError):
-        pass
-  
-    # rear view camera state
-    self.rear_view_toggle = False
-    self.rear_view_allowed = (params.get("IsRearViewMirror") == "1")
-  
-    self.v_cruise_kph = 255
-  
-    # 0.0 - 1.0
-    self.awareness_status = 1.0
-  
-    self.soft_disable_timer = None
-  
-    self.overtemp = False
-    self.free_space = 1.0
-    self.cal_status = Calibration.UNCALIBRATED
-    self.cal_perc = 0
-  
-    self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
- 
-  def data_sample(self):
-    self.prof = Profiler()
-    self.cur_time = sec_since_boot()
-    # first read can and compute car states
-    self.CS = self.CI.update()
-
-    self.prof.checkpoint("CarInterface")
-
-    # *** thermal checking logic ***
-    # thermal data, checked every second
-    td = messaging.recv_sock(self.thermal)
-    if td is not None:
-      # Check temperature.
-      self.overtemp = any(
-          t > 950
-          for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
-                    td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
-      # under 15% of space free
-      self.free_space = td.thermal.freeSpace
-
-    # read calibration status
-    cal = messaging.recv_sock(self.cal)
-    if cal is not None:
-      self.cal_status = cal.liveCalibration.calStatus
-      self.cal_perc = cal.liveCalibration.calPerc
-    
-
-  def state_transition(self):
-    pass # for now
-
-  def state_control(self):
-    
-    # did it request to enable?
-    enable_request, enable_condition = False, False
-
-    # reset awareness status on steering
-    if self.CS.steeringPressed or not self.enabled:
-      self.awareness_status = 1.0
-    elif self.enabled:
-      # gives the user 6 minutes
-      self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME)
-      if self.awareness_status <= 0.:
-        self.AM.add("driverDistracted", self.enabled)
-      elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
-           self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
-        self.AM.add("preDriverDistracted", self.enabled)
-
-    # handle button presses
-    for b in self.CS.buttonEvents:
-      print b
-
-      # button presses for rear view
-      if b.type == "leftBlinker" or b.type == "rightBlinker":
-        if b.pressed and self.rear_view_allowed:
-          self.rear_view_toggle = True
-        else:
-          self.rear_view_toggle = False
-
-      if b.type == "altButton1" and b.pressed:
-        self.rear_view_toggle = not self.rear_view_toggle
-
-      if not self.CP.enableCruise and self.enabled and not b.pressed:
-        if b.type == "accelCruise":
-          self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
-        elif b.type == "decelCruise":
-          self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
-        self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
-
-      if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
-        enable_request = True
-
-      # do disable on button down
-      if b.type == "cancel" and b.pressed:
-        self.AM.add("disable", self.enabled)
-
-    self.prof.checkpoint("Buttons")
-    
-    # *** health checking logic ***
-    hh = messaging.recv_sock(self.health)
-    if hh is not None:
-      # if the board isn't allowing controls but somehow we are enabled!
-      # TODO: this should be in state transition with a function follower logic
-      if not hh.health.controlsAllowed and self.enabled:
-        self.AM.add("controlsMismatch", self.enabled)
-
-    # disable if the pedals are pressed while engaged, this is a user disable
-    if self.enabled:
-      if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available:
-        self.AM.add("disable", self.enabled)
-
-      # it can happen that car cruise disables while comma system is enabled: need to
-      # keep braking if needed or if the speed is very low
-      # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
-      if self.CP.enableCruise and not self.CS.cruiseState.enabled and \
-         (self.CC.brake <= 0. or self.CS.vEgo < 0.3):
-        self.AM.add("cruiseDisabled", self.enabled)
-
-    if enable_request:
-      # check for pressed pedals
-      if self.CS.gasPressed or self.CS.brakePressed:
-        self.AM.add("pedalPressed", self.enabled)
-        enable_request = False
-      else:
-        print "enabled pressed at", self.cur_time
-        self.last_enable_request = self.cur_time
 
-      # don't engage with less than 15% free
-      if self.free_space < 0.15:
-        self.AM.add("outOfSpace", self.enabled)
-        enable_request = False
 
-    if self.CP.enableCruise:
+# True when actuators are controlled
-      enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled
+def isActive(state):
-    else:
+  return state in [State.ENABLED, State.SOFT_DISABLING]
-      enable_condition = enable_request
+
+
+# True if system is engaged
+def isEnabled(state):
+  return (isActive(state) or state == State.PRE_ENABLED)
 
-    if self.CP.enableCruise and self.CS.cruiseState.enabled:
-      self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH
 
-    self.prof.checkpoint("AdaptiveCruise")
+def data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc):
 
-    # *** what's the plan ***
+  # *** read can and compute car states ***
-    plan_packet = self.PL.update(self.CS, self.LoC)
+  CS = CI.update(CC)
-    self.plan = plan_packet.plan
+  events = list(CS.events)
-    self.plan_ts = plan_packet.logMonoTime
 
-    # if user is not responsive to awareness alerts, then start a smooth deceleration
+  # *** thermal checking logic ***
-    if self.awareness_status < -0.:
+  # thermal data, checked every second
-      self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL)
+  td = messaging.recv_sock(thermal)
-      self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax)
+  if td is not None:
+    # Check temperature
+    overtemp = any(
+        t > 950
+        for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
+                  td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
+    if overtemp:
+      events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
-    if enable_request or enable_condition or self.enabled:
+    # under 15% of space free
-      # add all alerts from car
+    if td.thermal.freeSpace < 0.15:
-      for alert in self.CS.errors:
+      events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
-        self.AM.add(alert, self.enabled)
 
-      if not self.plan.longitudinalValid:
+  # *** read calibration status ***
-        self.AM.add("radarCommIssue", self.enabled)
+  cal = messaging.recv_sock(cal)
+  if cal is not None:
+    cal_status = cal.liveCalibration.calStatus
+    cal_perc = cal.liveCalibration.calPerc
 
-      if self.cal_status != Calibration.CALIBRATED:
+  if cal_status != Calibration.CALIBRATED:
-        if self.cal_status == Calibration.UNCALIBRATED:
+    if cal_status == Calibration.UNCALIBRATED:
-          self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%')
+      events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    else:
+      events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
+  # *** health checking logic ***
+  hh = messaging.recv_sock(health)
+  if hh is not None:
+    controls_allowed = hh.health.controlsAllowed
+    if not controls_allowed:
+      events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+
+  return CS, events, cal_status, cal_perc
+
+
+def calc_plan(CS, events, PL, LoC):
+   # plan runs always, independently of the state
+   plan_packet = PL.update(CS, LoC)
+   plan = plan_packet.plan
+   plan_ts = plan_packet.logMonoTime
+
+   # add events from planner
+   events += list(plan.events)
+
+   # disable if lead isn't close when system is active and brake is pressed to avoid
+   # unexpected vehicle accelerations
+   if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED:
+     events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+
+   return plan, plan_ts
+
+
+def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM):
+  # compute conditional state transitions and execute actions on state transitions
+  enabled = isEnabled(state)
+
+  # handle button presses. TODO: this should be in state_control, but a decelCruise press
+  # would have the effect of both enabling and changing speed is checked after the state transition
+  for b in CS.buttonEvents:
+    if not CP.enableCruise and enabled and not b.pressed:
+      if b.type == "accelCruise":
+        v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
+      elif b.type == "decelCruise":
+        v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
+      v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+
+  # decrease the soft disable timer at every step, as it's reset on
+  # entrance in SOFT_DISABLING state
+  soft_disable_timer = max(0, soft_disable_timer - 1)
+
+  # ***** handle state transitions *****
+
+  # DISABLED
+  if state == State.DISABLED:
+    if get_events(events, [ET.ENABLE]):
+      if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
+        for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
+          AM.add(e, enabled)
+        for e in get_events(events, [ET.NO_ENTRY]):
+          txt =  str(cal_perc) + '%' if e == 'calibrationInProgress' else ''
+          AM.add(str(e) + "NoEntry", enabled, txt)
+      else:
+        if get_events(events, [ET.PRE_ENABLE]):
+          state = State.PRE_ENABLED
         else:
-          self.AM.add("calibrationInvalid", self.enabled)
+          state = State.ENABLED
-
+        AM.add("enable", enabled)
-      if not self.plan.lateralValid:
+        # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
-        # If the model is not broadcasting, assume that it is because
+        v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
-        # the user has uploaded insufficient data for calibration.
+
-        # Other cases that would trigger this are rare and unactionable by the user.
+  # ENABLED
-        self.AM.add("dataNeeded", self.enabled)
+  elif state == State.ENABLED:
-
+    if get_events(events, [ET.USER_DISABLE]):
-      if self.overtemp:
+      state = State.DISABLED
-        self.AM.add("overheat", self.enabled)
+      AM.add("disable", enabled)
-
+
-    
+    elif get_events(events, [ET.IMMEDIATE_DISABLE]):
-    # *** angle offset learning *** 
+      state = State.DISABLED
-    if self.rk.frame % 5 == 2 and self.plan.lateralValid: 
+      for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
-      # *** run this at 20hz again *** 
+        AM.add(e, enabled)
-      self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, 
+
-                                             self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des,
+    elif get_events(events, [ET.SOFT_DISABLE]):
-                                             self.CS.steeringPressed) 
+      state = State.SOFT_DISABLING
-
+      soft_disable_timer = 300   # 3s TODO: use rate
-    # *** gas/brake PID loop *** 
+      for e in get_events(events, [ET.SOFT_DISABLE]):
-    final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, 
+        AM.add(e, enabled)
-                                        self.plan.vTarget, 
+
-                                        [self.plan.aTargetMin, self.plan.aTargetMax], 
+  # SOFT DISABLING
-                                        self.plan.jerkFactor, self.CP) 
+  elif state == State.SOFT_DISABLING:
-
+    if get_events(events, [ET.USER_DISABLE]):
-    # *** steering PID loop *** 
+      state = State.DISABLED
-    final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle, 
+      AM.add("disable", enabled)
-                                            self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM) 
+
- 
+    elif get_events(events, [ET.IMMEDIATE_DISABLE]):
-    self.prof.checkpoint("PID") 
+      state = State.DISABLED
-    
+      for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
-        # ***** handle alerts ****
+        AM.add(e, enabled)
-    # send FCW alert if triggered by planner
+
-    if self.plan.fcw:
+    elif not get_events(events, [ET.SOFT_DISABLE]):
-      self.AM.add("fcw", self.enabled)
+      # no more soft disabling condition, so go back to ENABLED
-
+      state = State.ENABLED
-    # send a "steering required alert" if saturation count has reached the limit
+
-    if sat_flag:
+    elif soft_disable_timer <= 0:
-      self.AM.add("steerSaturated", self.enabled)
+      state = State.DISABLED
-
+  
-    if self.enabled and self.AM.alertShouldDisable():
+  # TODO: PRE ENABLING
-      print "DISABLING IMMEDIATELY ON ALERT"
+  elif state == State.PRE_ENABLED:
-      self.enabled = False
+    if get_events(events, [ET.USER_DISABLE]):
-
+      state = State.DISABLED
-    if self.enabled and self.AM.alertShouldSoftDisable():
+      AM.add("disable", enabled)
-      if self.soft_disable_timer is None:
+
-        self.soft_disable_timer = 3 * self.rate
+    elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
-      elif self.soft_disable_timer == 0:
+      state = State.DISABLED
-        print "SOFT DISABLING ON ALERT"
+      for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
-        self.enabled = False
+        AM.add(e, enabled)
+
+    elif not get_events(events, [ET.PRE_ENABLE]):
+      state = State.ENABLED
+
+  return state, soft_disable_timer, v_cruise_kph
+
+
+def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status,
+                  PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
+  # Given the state, this function returns the actuators
+
+  # reset actuators to zero
+  actuators = car.CarControl.Actuators.new_message()
+
+  enabled = isEnabled(state)
+  active = isActive(state)
+
+  for b in CS.buttonEvents:
+    # any button event resets awarenesss_status
+    awareness_status = 1.
+
+    # button presses for rear view
+    if b.type == "leftBlinker" or b.type == "rightBlinker":
+      if b.pressed and rear_view_allowed:
+        rear_view_toggle = True
       else:
-        self.soft_disable_timer -= 1
+        rear_view_toggle = False
-    else:
-      self.soft_disable_timer = None
-
-    if enable_condition and not self.enabled and not self.AM.alertPresent():
-      print "*** enabling controls"
-
-      # beep for enabling
-      self.AM.add("enable", self.enabled)
-
-      # enable both lateral and longitudinal controls
-      self.enabled = True
-
-      # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
-      self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
-
-      # 6 minutes driver you're on
-      self.awareness_status = 1.0
-
-      # reset the PID loops
-      self.LaC.reset()
-      # start long control at actual speed
-      self.LoC.reset(v_pid = self.CS.vEgo)
-
-    # *** push the alerts to current ***
-    # TODO: remove output, store them inside AM class instead
-    self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time)
- 
-    # ***** control the car *****
-    self.CC.enabled = self.enabled
-
-    self.CC.gas = float(final_gas)
-    self.CC.brake = float(final_brake)
-    self.CC.steeringTorque = float(final_steer)
-
-    self.CC.cruiseControl.override = True
-    # always cancel if we have an interceptor
-    self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or 
-                                        (not self.enabled and self.CS.cruiseState.enabled))
-
-    # brake discount removes a sharp nonlinearity
-    brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0))
-    self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0)
-
-    #CC.cruiseControl.accelOverride = float(AC.a_pcm)
-    # TODO: parametrize 0.714 in interface?
-    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
-    # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
-    self.CC.cruiseControl.accelOverride = float(max(0.714, self.plan.aTargetMax/A_ACC_MAX))
-
-    self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
-    self.CC.hudControl.speedVisible = self.enabled
-    self.CC.hudControl.lanesVisible = self.enabled
-    self.CC.hudControl.leadVisible = self.plan.hasLead
-    self.CC.hudControl.visualAlert = self.visual_alert
-    self.CC.hudControl.audibleAlert = self.audible_alert
-
-    # TODO: remove it from here and put it in state_transition
-    # this alert will apply next controls cycle
-    if not self.CI.apply(self.CC):
-      self.AM.add("controlsFailed", self.enabled)
-
-  def data_send(self):
-    
-    # broadcast carControl first
-    cc_send = messaging.new_message()
-    cc_send.init('carControl')
-    cc_send.carControl = copy(self.CC)
-    self.carcontrol.send(cc_send.to_bytes())
-
-    self.prof.checkpoint("CarControl")
-
-    # broadcast carState
-    cs_send = messaging.new_message()
-    cs_send.init('carState')
-    cs_send.carState = copy(self.CS)
-    self.carstate.send(cs_send.to_bytes())
-    
-    # ***** publish state to logger *****
-
-    # publish controls state at 100Hz
-    dat = messaging.new_message()
-    dat.init('live100')
 
-    # show rear view camera on phone if in reverse gear or when button is pressed
+    if b.type == "altButton1" and b.pressed:
-    dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle
+      rear_view_toggle = not rear_view_toggle
-    dat.live100.alertText1 = self.alert_text_1
-    dat.live100.alertText2 = self.alert_text_2
-    dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0
 
-    # what packets were used to process
-    dat.live100.canMonoTimes = list(self.CS.canMonoTimes)
-    dat.live100.planMonoTime = self.plan_ts
 
-    # if controls is enabled
+  # send FCW alert if triggered by planner
-    dat.live100.enabled = self.enabled
+  if plan.fcw:
+    AM.add("fcw", enabled)
 
-    # car state
+  # ***** state specific actions *****
-    dat.live100.vEgo = self.CS.vEgo
-    dat.live100.angleSteers = self.CS.steeringAngle
-    dat.live100.steerOverride = self.CS.steeringPressed
 
-    # longitudinal control state
+  # DISABLED
-    dat.live100.vPid = float(self.LoC.v_pid)
+  if state in [State.PRE_ENABLED, State.DISABLED]:
-    dat.live100.vCruise = float(self.v_cruise_kph)
+    awareness_status = 1.
-    dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd)
+    LaC.reset()
-    dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
+    LoC.reset(v_pid=CS.vEgo)
 
-    # lateral control state
+  # ENABLED or SOFT_DISABLING
-    dat.live100.yDes = float(self.LaC.y_des)
+  elif state in [State.ENABLED, State.SOFT_DISABLING]:
-    dat.live100.angleSteersDes = float(self.LaC.angle_steers_des)
-    dat.live100.upSteer = float(self.LaC.Up_steer)
-    dat.live100.uiSteer = float(self.LaC.Ui_steer)
 
-    # processed radar state, should add a_pcm?
+    if CS.steeringPressed:
-    dat.live100.vTargetLead = float(self.plan.vTarget)
+      # reset awareness status on steering
-    dat.live100.aTargetMin = float(self.plan.aTargetMin)
+      awareness_status = 1.0
-    dat.live100.aTargetMax = float(self.plan.aTargetMax)
-    dat.live100.jerkFactor = float(self.plan.jerkFactor)
 
-    # log learned angle offset
+    # 6 minutes driver you're on
-    dat.live100.angleOffset = float(self.angle_offset)
+    awareness_status -= 0.01/(AWARENESS_TIME)
+    if awareness_status <= 0.:
+      AM.add("driverDistracted", enabled)
+    elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
+         awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
+      AM.add("preDriverDistracted", enabled)
 
-    # lag
+    # parse warnings from car specific interface
-    dat.live100.cumLagMs = -self.rk.remaining*1000.
+    for e in get_events(events, [ET.WARNING]):
+      AM.add(e, enabled)
 
-    self.live100.send(dat.to_bytes())
+  # if user is not responsive to awareness alerts, then start a smooth deceleration
+  if awareness_status < -0.:
+    plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
+    plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
 
-    self.prof.checkpoint("Live100")
+  # *** angle offset learning ***
 
-  def wait(self):
+  if rk.frame % 5 == 2 and plan.lateralValid:
-    # *** run loop at fixed rate ***
+    # *** run this at 20hz again ***
-    if self.rk.keep_time():
+    angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
-      self.prof.display()
+                                      PL.PP.c_poly, PL.PP.c_prob, LaC.y_des,
+                                      CS.steeringPressed)
+
+  # *** gas/brake PID loop ***
+  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed,
+                                              v_cruise_kph, plan.vTarget,
+                                              [plan.aTargetMin, plan.aTargetMax],
+                                              plan.jerkFactor, CP)
+
+  # *** steering PID loop ***
+  actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
+                               CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
+
+  # send a "steering required alert" if saturation count has reached the limit
+  if LaC.sat_flag:
+    AM.add("steerSaturated", enabled)
+
+  if CP.enableCruise and CS.cruiseState.enabled:
+    v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+
+  # *** process alerts ***
+
+  AM.process_alerts(sec_since_boot())
+
+  return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
+
+
+def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
+              carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
+              LaC, LoC, angle_offset):
+
+  # ***** control the car *****
+
+  CC = car.CarControl.new_message()
+
+  CC.enabled = isEnabled(state)
+
+  CC.actuators = actuators
+
+  CC.cruiseControl.override = True
+  # always cancel if we have an interceptor
+  CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
+
+  # brake discount removes a sharp nonlinearity
+  brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
+  CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
+
+  # TODO: parametrize 0.714 in interface?
+  # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
+  # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
+  CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
+
+  CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
+  CC.hudControl.speedVisible = isEnabled(state)
+  CC.hudControl.lanesVisible = isEnabled(state)
+  CC.hudControl.leadVisible = plan.hasLead
+  CC.hudControl.visualAlert = AM.visual_alert
+  CC.hudControl.audibleAlert = AM.audible_alert
+
+  # send car controls over can
+  CI.apply(CC)
+
+  # ***** publish state to logger *****
+  # publish controls state at 100Hz
+  dat = messaging.new_message()
+  dat.init('live100')
+
+  # show rear view camera on phone if in reverse gear or when button is pressed
+  dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
+  dat.live100.alertText1 = AM.alert_text_1
+  dat.live100.alertText2 = AM.alert_text_2
+  dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
+
+  # what packets were used to process
+  dat.live100.canMonoTimes = list(CS.canMonoTimes)
+  dat.live100.planMonoTime = plan_ts
+
+  # if controls is enabled
+  dat.live100.enabled = isEnabled(state)
+
+  # car state
+  dat.live100.vEgo = CS.vEgo
+  dat.live100.angleSteers = CS.steeringAngle
+  dat.live100.steerOverride = CS.steeringPressed
+
+  # longitudinal control state
+  dat.live100.vPid = float(LoC.v_pid)
+  dat.live100.vCruise = float(v_cruise_kph)
+  dat.live100.upAccelCmd = float(LoC.pid.p)
+  dat.live100.uiAccelCmd = float(LoC.pid.i)
+
+  # lateral control state
+  dat.live100.yDes = float(LaC.y_des)
+  dat.live100.angleSteersDes = float(LaC.angle_steers_des)
+  dat.live100.upSteer = float(LaC.pid.p)
+  dat.live100.uiSteer = float(LaC.pid.i)
+
+  # processed radar state, should add a_pcm?
+  dat.live100.vTargetLead = float(plan.vTarget)
+  dat.live100.aTargetMin = float(plan.aTargetMin)
+  dat.live100.aTargetMax = float(plan.aTargetMax)
+  dat.live100.jerkFactor = float(plan.jerkFactor)
+
+  # log learned angle offset
+  dat.live100.angleOffset = float(angle_offset)
+
+  # lag
+  dat.live100.cumLagMs = -rk.remaining*1000.
+
+  live100.send(dat.to_bytes())
+
+  # broadcast carState
+  cs_send = messaging.new_message()
+  cs_send.init('carState')
+  # TODO: override CS.events with all the cumulated events
+  cs_send.carState = copy(CS)
+  carstate.send(cs_send.to_bytes())
+
+  # broadcast carControl
+  cc_send = messaging.new_message()
+  cc_send.init('carControl')
+  cc_send.carControl = copy(CC)
+  carcontrol.send(cc_send.to_bytes())
+  #print [i.name for i in events]
+
+  # publish mpc state at 20Hz
+  if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
+    dat = messaging.new_message()
+    dat.init('liveMpc')
+    dat.liveMpc.x = list(LaC.mpc_solution[0].x)
+    dat.liveMpc.y = list(LaC.mpc_solution[0].y)
+    dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
+    dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
+    livempc.send(dat.to_bytes())
+
+  return CC
 
 
 def controlsd_thread(gctx, rate=100):
   # start the loop
   set_realtime_priority(2)
-  CTRLS = Controls(gctx, rate)
+
+  context = zmq.Context()
+
+  # pub
+  live100 = messaging.pub_sock(context, service_list['live100'].port)
+  carstate = messaging.pub_sock(context, service_list['carState'].port)
+  carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
+  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+  livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
+
+  # sub
+  thermal = messaging.sub_sock(context, service_list['thermal'].port)
+  health = messaging.sub_sock(context, service_list['health'].port)
+  cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
+  logcan = messaging.sub_sock(context, service_list['can'].port)
+
+  CC = car.CarControl.new_message()
+  CI, CP = get_car(logcan, sendcan)
+  PL = Planner(CP)
+  LoC = LongControl(CI.compute_gb)
+  VM = VehicleModel(CP)
+  LaC = LatControl(VM)
+  AM = AlertManager()
+
+  # write CarParams
+  params = Params()
+  params.put("CarParams", CP.to_bytes())
+
+  state = State.DISABLED
+  soft_disable_timer = 0
+  v_cruise_kph = 255
+  cal_perc = 0
+  cal_status = Calibration.UNCALIBRATED
+  rear_view_toggle = False
+  rear_view_allowed = params.get("IsRearViewMirror") == "1"
+
+  # 0.0 - 1.0
+  awareness_status = 0.
+
+  rk = Ratekeeper(rate, print_delay_threshold=2./1000)
+
+  # learned angle offset
+  angle_offset = 0.
+  calibration_params = params.get("CalibrationParams")
+  if calibration_params:
+    try:
+      calibration_params = json.loads(calibration_params)
+      angle_offset = calibration_params["angle_offset"]
+    except (ValueError, KeyError):
+      pass
+
+  prof = Profiler()
+
   while 1:
-    CTRLS.data_sample()
+
-    CTRLS.state_transition()
+    prof.reset()  # avoid memory leak
-    CTRLS.state_control()
+
-    CTRLS.data_send()
+    # sample data and compute car events
-    CTRLS.wait()
+    CS, events, cal_status, cal_perc = data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc)
+    prof.checkpoint("Sample")
+
+    # define plan
+    plan, plan_ts = calc_plan(CS, events, PL, LoC)
+    prof.checkpoint("Plan")
+
+    # update control state
+    state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM)
+    prof.checkpoint("State transition")
+
+    # compute actuators
+    actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
+                                                                            AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
+                                                                            rear_view_allowed, rear_view_toggle)
+    prof.checkpoint("State Control")
+
+    # publish data
+    CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
+                   rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
+                   rear_view_toggle, awareness_status, LaC, LoC, angle_offset)
+    prof.checkpoint("Sent")
+
+    # *** run loop at fixed rate ***
+    if rk.keep_time():
+      prof.display()
 
 
 def main(gctx=None):
@@ -462,4 +507,3 @@ def main(gctx=None):
 
 if __name__ == "__main__":
   main()
- 

selfdrive/controls/lib/adaptivecruise.py

@@ -109,7 +109,7 @@ def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
 # maximum acceleration adjustment
 _A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
 # speeds
-_A_CORR_BY_SPEED_BP = [0., 5., 20.]
+_A_CORR_BY_SPEED_BP = [0., 2., 10.]
 
 # max acceleration allowed in acc, which happens in restart
 A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
@@ -283,20 +283,12 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
 
 class AdaptiveCruise(object):
   def __init__(self):
-    self.last_cal = 0.
     self.l1, self.l2 = None, None
-    self.dead = True
+  def update(self, v_ego, angle_steers, v_pid, CP, l20):
-  def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
     if l20 is not None:
       self.l1 = l20.live20.leadOne
       self.l2 = l20.live20.leadTwo
 
-      # TODO: no longer has anything to do with calibration
-      self.last_cal = cur_time
-      self.dead = False
-    elif cur_time - self.last_cal > 0.5:
-      self.dead = True
-
     self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
       compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
     self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE

selfdrive/controls/lib/alertmanager.py

@@ -2,22 +2,31 @@ from cereal import car
 from selfdrive.swaglog import cloudlog
 import copy
 
-class ET:
+
-  ENABLE = 0
+# Priority
-  NO_ENTRY = 1
+class PT:
-  WARNING = 2
+  HIGH = 3
-  USER_DISABLE = 3
+  MID = 2
-  SOFT_DISABLE = 4
+  LOW = 1
-  IMMEDIATE_DISABLE = 5
+
-
+
-class alert(object):
+class Alert(object):
-  def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text):
+  def __init__(self, 
+               alert_text_1,
+               alert_text_2,
+               alert_priority,
+               visual_alert,
+               audible_alert, 
+               duration_sound,
+               duration_hud_alert,
+               duration_text):
+
     self.alert_text_1 = alert_text_1
     self.alert_text_2 = alert_text_2
-    self.alert_type = alert_type
+    self.alert_priority = alert_priority
     self.visual_alert = visual_alert if visual_alert is not None else "none"
     self.audible_alert = audible_alert if audible_alert is not None else "none"
-
+ 
     self.duration_sound = duration_sound
     self.duration_hud_alert = duration_hud_alert
     self.duration_text = duration_text
@@ -28,45 +37,297 @@ class alert(object):
     tst.hudControl.audibleAlert = self.audible_alert
 
   def __str__(self):
-    return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_type) + "  " + str(self.visual_alert) + " " + str(self.audible_alert)
+    return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + "  " + str(
+      self.visual_alert) + " " + str(self.audible_alert)
 
   def __gt__(self, alert2):
-    return self.alert_type > alert2.alert_type
+    return self.alert_priority > alert2.alert_priority
+
 
 class AlertManager(object):
   alerts = {
-    "enable":                alert("", "",                                                    ET.ENABLE,       None,               "beepSingle", .2, 0., 0.),
+
-    "disable":               alert("", "",                                                    ET.USER_DISABLE, None,               "beepSingle", .2, 0., 0.),
+    # Miscellaneous alerts
-    "pedalPressed":          alert("Comma Unavailable",        "Pedal Pressed",               ET.NO_ENTRY,     "brakePressed",     "chimeDouble", .4, 2., 3.),
+    "enable": Alert(
-    "preDriverDistracted":   alert("Take Control ",            "User Distracted",             ET.WARNING,      "steerRequired",    "chimeDouble", .4, 2., 3.),
+        "",
-    "driverDistracted":      alert("Take Control to Regain Speed", "User Distracted",         ET.WARNING,      "steerRequired", "chimeRepeated", .5, .5, .5),
+        "",
-    "steerSaturated":        alert("Take Control",             "Turn Exceeds Limit",          ET.WARNING,      "steerRequired", "chimeSingle", 1., 2., 3.),
+        PT.MID, None, "beepSingle", .2, 0., 0.),
-    "overheat":              alert("Take Control Immediately", "System Overheated",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
-    "controlsMismatch":      alert("Take Control Immediately", "Controls Mismatch",           ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "disable": Alert(
-    "radarCommIssue":        alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "",
-    "calibrationInvalid":    alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "",
-    "calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ",     ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        PT.MID, None, "beepSingle", .2, 0., 0.),
-    "modelCommIssue":        alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
-    "controlsFailed":        alert("Take Control Immediately", "Controls Failed",             ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "fcw": Alert(
-    "fcw":                   alert("", "",                                                    ET.WARNING,           None,            None,            .1, .1, .1),
+        "", 
-    # car errors
+        "", 
-    "commIssue":             alert("Take Control Immediately","CAN Error: Restart the Car",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        PT.LOW, None, None, .1, .1, .1),
-    "steerUnavailable":      alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
-    "steerTempUnavailable":  alert("Take Control", "Steer Temporarily Unavailable",           ET.WARNING,           "steerRequired", "chimeDouble",   .4, 2., 3.),
+    "steerSaturated": Alert(
-    "brakeUnavailable":      alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Take Control", 
-    "gasUnavailable":        alert("Take Control Immediately","Gas Error: Restart the Car",   ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Turn Exceeds Limit", 
-    "wrongGear":             alert("Take Control Immediately","Gear not D",                   ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
-    "reverseGear":           alert("Take Control Immediately","Car in Reverse",               ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
-    "doorOpen":              alert("Take Control Immediately","Door Open",                    ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+    "steerTempUnavailable": Alert(
-    "seatbeltNotLatched":    alert("Take Control Immediately","Seatbelt Unlatched",           ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Take Control", 
-    "espDisabled":           alert("Take Control Immediately","ESP Off",                      ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "Steer Temporarily Unavailable", 
-    "cruiseDisabled":        alert("Take Control Immediately","Cruise Is Off",                ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
-    "wrongCarMode":          alert("Comma Unavailable","Main Switch Off",                     ET.NO_ENTRY,     None, "chimeDouble", .4, 0., 3.),
+
-    "outOfSpace":            alert("Comma Unavailable","Out of Space",                        ET.NO_ENTRY,     None, "chimeDouble", .4, 0., 3.),
+    "preDriverDistracted": Alert(
-    "dataNeeded":            alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
+        "Take Control ", 
-    "ethicalDilemma":        alert("Take Control Immediately","Ethical Dilemma Detected",     ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+        "User Distracted", 
-    "startup":               alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.),
+        PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
+
+    "driverDistracted": Alert(
+        "Take Control to Regain Speed", 
+        "User Distracted", 
+        PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5),
+
+    "startup": Alert(
+        "Always Keep Hands on Wheel", 
+        "Be Ready to Take Over Any Time", 
+        PT.LOW, None, None, 0., 0., 15.),
+
+    "ethicalDilemma": Alert(
+        "Take Control Immediately", 
+        "Ethical Dilemma Detected", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "steerTempUnavailableNoEntry": Alert(
+        "Comma Unavailable", 
+        "Steer Temporary Unavailable", 
+        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+
+    # Non-entry only alerts
+    "wrongCarModeNoEntry": Alert(
+        "Comma Unavailable", 
+        "Main Switch Off", 
+        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+
+    "dataNeededNoEntry": Alert(
+        "Comma Unavailable", 
+        "Data needed for calibration. Upload drive, try again", 
+        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+
+    "outOfSpaceNoEntry": Alert(
+        "Comma Unavailable", 
+        "Out of Space", 
+        PT.LOW, None, "chimeDouble", .4, 0., 3.),
+
+    "pedalPressedNoEntry": Alert(
+        "Comma Unavailable", 
+        "Pedal Pressed", 
+        PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
+
+    "speedTooLowNoEntry": Alert(
+        "Comma Unavailable", 
+        "Speed Too Low", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "brakeHoldNoEntry": Alert(
+        "Comma Unavailable", 
+        "Brake Hold Active", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "parkBrakeNoEntry": Alert(
+        "Comma Unavailable", 
+        "Park Brake Engaged", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    # Cancellation alerts causing disabling
+    "overheat": Alert(
+        "Take Control Immediately", 
+        "System Overheated", 
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "wrongGear": Alert(
+        "Take Control Immediately", 
+        "Gear not D", 
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "calibrationInvalid": Alert(
+        "Take Control Immediately", 
+        "Calibration Invalid: Reposition Neo and Recalibrate", 
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "calibrationInProgress": Alert(
+        "Take Control Immediately", 
+        "Calibration in Progress",
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "doorOpen": Alert(
+        "Take Control Immediately", 
+        "Door Open", 
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "seatbeltNotLatched": Alert(
+        "Take Control Immediately", 
+        "Seatbelt Unlatched", 
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "espDisabled": Alert(
+        "Take Control Immediately", 
+        "ESP Off", 
+        PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "radarCommIssue": Alert(
+        "Take Control Immediately", 
+        "Radar Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "radarFault": Alert(
+        "Take Control Immediately", 
+        "Radar Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "modelCommIssue": Alert(
+        "Take Control Immediately", 
+        "Model Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "controlsFailed": Alert(
+        "Take Control Immediately", 
+        "Controls Failed", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "controlsMismatch": Alert(
+        "Take Control Immediately", 
+        "Controls Mismatch", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "commIssue": Alert(
+        "Take Control Immediately", 
+        "CAN Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "steerUnavailable": Alert(
+        "Take Control Immediately", 
+        "Steer Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "brakeUnavailable": Alert(
+        "Take Control Immediately", 
+        "Brake Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "gasUnavailable": Alert(
+        "Take Control Immediately", 
+        "Gas Error: Restart the Car", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "reverseGear": Alert(
+        "Take Control Immediately", 
+        "Reverse Gear", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    "cruiseDisabled": Alert(
+        "Take Control Immediately", 
+        "Cruise Is Off", 
+        PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
+
+    # not loud cancellations (user is in control)
+    "noTarget": Alert(
+        "Comma Canceled",
+        "No Close Lead", 
+        PT.HIGH, None, "chimeDouble", .4, 2., 3.),
+
+    # Cancellation alerts causing non-entry
+    "overheatNoEntry": Alert(
+        "Comma Unavailable", 
+        "System Overheated", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "wrongGearNoEntry": Alert(
+        "Comma Unavailable", 
+        "Gear not D", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "calibrationInvalidNoEntry": Alert(
+        "Comma Unavailable", 
+        "Calibration Invalid: Reposition Neo and Recalibrate", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "calibrationInProgressNoEntry": Alert(
+        "Comma Unavailable", 
+        "Calibration in Progress: ", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "doorOpenNoEntry": Alert(
+        "Comma Unavailable", 
+        "Door Open", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "seatbeltNotLatchedNoEntry": Alert(
+        "Comma Unavailable", 
+        "Seatbelt Unlatched", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+ 
+    "espDisabledNoEntry": Alert(
+        "Comma Unavailable", 
+        "ESP Off", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "radarCommIssueNoEntry": Alert(
+        "Comma Unavailable", 
+        "Radar Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "radarFaultNoEntry": Alert(
+        "Comma Unavailable", 
+        "Radar Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "modelCommIssueNoEntry": Alert(
+        "Comma Unavailable", 
+        "Model Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "controlsFailedNoEntry": Alert(
+        "Comma Unavailable", 
+        "Controls Failed", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "controlsMismatchNoEntry": Alert(
+        "Comma Unavailable", 
+        "Controls Mismatch", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "commIssueNoEntry": Alert(
+        "Comma Unavailable", 
+        "CAN Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "steerUnavailableNoEntry": Alert(
+        "Comma Unavailable", 
+        "Steer Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "brakeUnavailableNoEntry": Alert(
+        "Comma Unavailable", 
+        "Brake Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "gasUnavailableNoEntry": Alert(
+        "Comma Unavailable", 
+        "Gas Error: Restart the Car", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "reverseGearNoEntry": Alert(
+        "Comma Unavailable", 
+        "Reverse Gear", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "cruiseDisabledNoEntry": Alert(
+        "Comma Unavailable", 
+        "Cruise is Off", 
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
+    "noTargetNoEntry": Alert(
+        "Comma Unavailable", 
+        "No Close Lead",
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
   }
+
   def __init__(self):
     self.activealerts = []
     self.current_alert = None
@@ -75,31 +336,16 @@ class AlertManager(object):
   def alertPresent(self):
     return len(self.activealerts) > 0
 
-  def alertShouldSoftDisable(self):
-    return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts)
-
-  def alertShouldDisable(self):
-    return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts)
-
   def add(self, alert_type, enabled=True, extra_text=''):
     alert_type = str(alert_type)
     this_alert = copy.copy(self.alerts[alert_type])
     this_alert.alert_text_2 += extra_text
-    # downgrade the alert if we aren't enabled, except if it's FCW, which remains the same 
-    # TODO: remove this 'if' by adding more alerts
-    if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \
-      and this_alert != self.alerts['fcw']:
-      this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.)
-
-    # ignore no entries if we are enabled 
-    if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]:
-      return
 
     # if new alert is higher priority, log it
     if self.current_alert is None or this_alert > self.current_alert:
       cloudlog.event('alert_add',
-        alert_type=alert_type,
+                     alert_type=alert_type,
-        enabled=enabled)
+                     enabled=enabled)
 
     self.activealerts.append(this_alert)
     self.activealerts.sort()
@@ -109,29 +355,29 @@ class AlertManager(object):
       self.alert_start_time = cur_time
       self.current_alert = self.activealerts[0]
       print self.current_alert
-    alert_text_1 = ""
+
-    alert_text_2 = ""
+    # start with assuming no alerts
-    visual_alert = "none"
+    self.alert_text_1 = ""
-    audible_alert = "none"
+    self.alert_text_2 = ""
+    self.visual_alert = "none"
+    self.audible_alert = "none"
 
     if self.current_alert is not None:
       # ewwwww
       if self.alert_start_time + self.current_alert.duration_sound > cur_time:
-        audible_alert = self.current_alert.audible_alert
+        self.audible_alert = self.current_alert.audible_alert
 
       if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
-        visual_alert = self.current_alert.visual_alert
+        self.visual_alert = self.current_alert.visual_alert
 
       if self.alert_start_time + self.current_alert.duration_text > cur_time:
-        alert_text_1 = self.current_alert.alert_text_1
+        self.alert_text_1 = self.current_alert.alert_text_1
-        alert_text_2 = self.current_alert.alert_text_2
+        self.alert_text_2 = self.current_alert.alert_text_2
 
       # disable current alert
-      if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, self.current_alert.duration_text) < cur_time:
+      if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert,
+                                     self.current_alert.duration_text) < cur_time:
         self.current_alert = None
 
     # reset
     self.activealerts = []
-
-    return alert_text_1, alert_text_2, visual_alert, audible_alert
-

selfdrive/controls/lib/drive_helpers.py

@@ -1,8 +1,38 @@
 from common.numpy_fast import clip
+from cereal import car
+
+
+class EventTypes:
+  ENABLE = 'enable'
+  PRE_ENABLE = 'preEnable'
+  NO_ENTRY = 'noEntry'
+  WARNING = 'warning'
+  USER_DISABLE = 'userDisable'
+  SOFT_DISABLE = 'softDisable'
+  IMMEDIATE_DISABLE = 'immediateDisable'
+
+
+def create_event(name, types):
+  event = car.CarEvent.new_message()
+  event.name = name
+  for t in types:
+    setattr(event, t, True)
+  return event
+
+
+def get_events(events, types):
+  out = []
+  for e in events:
+    for t in types:
+      if getattr(e, t):
+        out.append(e.name)
+  return out
+
 
 def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
+
 def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
   # simple integral controller that learns how much steering offset to put to have the car going straight
   # while being in the middle of the lane

selfdrive/controls/lib/latcontrol.py

@@ -1,133 +1,80 @@
 import math
-import numpy as np
+from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from common.numpy_fast import clip, interp
-from selfdrive.config import Conversions as CV
+
-
+
-_K_CURV_V = [1., 0.6]
+# 100ms is a rule of thumb estimation of lag from image processing to actuator command
-_K_CURV_BP = [0., 0.002]
+ACTUATORS_DELAY = 0.1
-
+
-def calc_d_lookahead(v_ego, d_poly):
+
-  #*** this function computes how far too look for lateral control
+def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
-  # howfar we look ahead is function of speed and how much curvy is the path
+  states[0].x = v_ego * ACTUATORS_DELAY
-  offset_lookahead = 1.
+  states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * ACTUATORS_DELAY
-  k_lookahead = 7.
+  return states
-  # integrate abs value of second derivative of poly to get a measure of path curvature
+
-  pts_len = 50.  # m
+
-  if len(d_poly)>0:
+def get_steer_max(CP, v_ego):
-    pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len))
+  return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
-  else:
+
-    pts = 0.
-  curv = np.sum(np.abs(pts))/pts_len
-
-  k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
-
-  # sqrt on speed is needed to keep, for a given curvature, the y_des
-  # proportional to speed. Indeed, y_des is prop to d_lookahead^2
-  # 36m at 25m/s
-  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
-  return d_lookahead
-
-def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
-  #*** this function returns the lateral offset given the steering angle, speed and the lookahead distance
-  sa = (angle_steers - angle_offset) * CV.DEG_TO_RAD
-  curvature = VM.calc_curvature(sa, v_ego)
-  # clip is to avoid arcsin NaNs due to too sharp turns
-  y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
-  return y_actual, curvature
-
-def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
-  # inverse of the above function
-  curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
-  steer_des = VM.get_steer_from_curvature(curvature, v_ego) * CV.RAD_TO_DEG + angle_offset
-  return steer_des, curvature
-
-def pid_lateral_control(v_ego, sa_actual, sa_des, Ui_steer, steer_max,
-                        steer_override, sat_count, enabled, Kp, Ki, rate):
-
-  sat_count_rate = 1./rate
-  sat_count_limit = 0.8      # after 0.8s of continuous saturation, an alert will be sent
-
-  error_steer = sa_des - sa_actual
-  Ui_unwind_speed = 0.3/rate   #.3 per second
-
-  Up_steer = error_steer*Kp
-  Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
-  output_steer_new = Ui_steer_new + Up_steer
-
-  # Anti-wind up for integrator: do not integrate if we are against the steer limits
-  if (
-    (error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
-    (error_steer <= 0. and
-     (output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
-    #update integrator
-    Ui_steer = Ui_steer_new
-  # unwind integrator if driver is maneuvering the steering wheel
-  elif steer_override:
-    Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
-
-  # still, intergral term should not be bigger then limits
-  Ui_steer = clip(Ui_steer, -steer_max, steer_max)
-
-  output_steer = Up_steer + Ui_steer
-
-  # don't run steer control if at very low speed
-  if v_ego < 0.3 or not enabled:
-    output_steer = 0.
-    Ui_steer = 0.
-
-  # useful to know if control is against the limit
-  lateral_control_sat = False
-  if abs(output_steer) > steer_max:
-    lateral_control_sat = True
-
-  output_steer = clip(output_steer, -steer_max, steer_max)
-
-  # if lateral control is saturated for a certain period of time, send an alert for taking control of the car
-  # wind
-  if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
-    sat_count += sat_count_rate
-  # unwind
-  else:
-    sat_count -= sat_count_rate
-
-  sat_flag = False
-  if sat_count >= sat_count_limit:
-    sat_flag = True
-
-  sat_count = clip(sat_count, 0, 1)
-
-  return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
 
 class LatControl(object):
-  def __init__(self):
+  def __init__(self, VM):
-    self.Up_steer = 0.
+    self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0)
-    self.sat_count = 0
+    self.setup_mpc()
-    self.y_des = 0.0
+
-    self.lateral_control_sat = False
+    self.y_des = -1  # Legacy
-    self.Ui_steer = 0.
+
-    self.reset()
+  def setup_mpc(self):
+    self.libmpc = libmpc_py.libmpc
+    self.libmpc.init()
+
+    self.mpc_solution = libmpc_py.ffi.new("log_t *")
+    self.cur_state = libmpc_py.ffi.new("state_t *")
+    self.mpc_updated = False
+    self.cur_state[0].x = 0.0
+    self.cur_state[0].y = 0.0
+    self.cur_state[0].psi = 0.0
+    self.cur_state[0].delta = 0.0
+
+    self.last_mpc_ts = 0.0
+    self.angle_steers_des = 0
 
   def reset(self):
-    self.Ui_steer = 0.
+    self.pid.reset()
+
+  def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
+    self.mpc_updated = False
+    if self.last_mpc_ts + 0.001 < PL.last_md_ts:
+      self.last_mpc_ts = PL.last_md_ts
+
+      curvature_factor = VM.curvature_factor(v_ego)
 
-  def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM):
+      l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
-    rate = 100
+      r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
+      p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
 
-    steer_max = 1.0
+      # account for actuation delay
+      self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
 
-    # how far we look ahead is function of speed and desired path
+      self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-    d_lookahead = calc_d_lookahead(v_ego, d_poly)
+                          l_poly, r_poly, p_poly,
+                          PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width)
 
-    # desired lookahead offset
+      delta_desired = self.mpc_solution[0].delta[1]
-    self.y_des = np.polyval(d_poly, d_lookahead)
+      self.cur_state[0].delta = delta_desired
 
-    # calculate actual offset at the lookahead point
+      self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset
-    self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des,
+      self.mpc_updated = True
-                                                d_lookahead, VM, angle_offset)
 
-    output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
+    if v_ego < 0.3 or not active:
-      v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max,
+      output_steer = 0.0
-      steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate)
+      self.pid.reset()
+    else:
+      steer_max = get_steer_max(VM.CP, v_ego)
+      self.pid.pos_limit = steer_max
+      self.pid.neg_limit = -steer_max
+      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override)
 
-    final_steer = clip(output_steer, -steer_max, steer_max)
+    self.sat_flag = self.pid.saturated
-    return final_steer, sat_flag
+    return output_steer

selfdrive/controls/lib/latcontrol_helpers.py

@@ -0,0 +1,89 @@
+import numpy as np
+import math
+from common.numpy_fast import interp
+
+_K_CURV_V = [1., 0.6]
+_K_CURV_BP = [0., 0.002]
+
+# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
+_LANE_WIDTH_V = [3., 3.8]
+
+# break points of speed
+_LANE_WIDTH_BP = [0., 31.]
+
+
+def calc_d_lookahead(v_ego, d_poly):
+  # this function computes how far too look for lateral control
+  # howfar we look ahead is function of speed and how much curvy is the path
+  offset_lookahead = 1.
+  k_lookahead = 7.
+  # integrate abs value of second derivative of poly to get a measure of path curvature
+  pts_len = 50.  # m
+  if len(d_poly) > 0:
+    pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
+  else:
+    pts = 0.
+  curv = np.sum(np.abs(pts)) / pts_len
+
+  k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
+
+  # sqrt on speed is needed to keep, for a given curvature, the y_des
+  # proportional to speed. Indeed, y_des is prop to d_lookahead^2
+  # 36m at 25m/s
+  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
+  return d_lookahead
+
+
+def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
+  # this function returns the lateral offset given the steering angle, speed and the lookahead distance
+  sa = math.radians(angle_steers - angle_offset)
+  curvature = VM.calc_curvature(sa, v_ego)
+  # clip is to avoid arcsin NaNs due to too sharp turns
+  y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
+  return y_actual, curvature
+
+
+def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
+  # inverse of the above function
+  curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
+  steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
+  return steer_des, curvature
+
+
+def compute_path_pinv():
+  deg = 3
+  x = np.arange(50.0)
+  X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
+  pinv = np.linalg.pinv(X)
+  return pinv
+
+
+def model_polyfit(points, path_pinv):
+  return np.dot(path_pinv, map(float, points))
+
+
+def calc_desired_path(l_poly,
+                      r_poly,
+                      p_poly,
+                      l_prob,
+                      r_prob,
+                      p_prob,
+                      speed,
+                      lane_width=None):
+  # this function computes the poly for the center of the lane, averaging left and right polys
+  if lane_width is None:
+    lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
+
+  # lanes in US are ~3.6m wide
+  half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
+  if l_prob + r_prob > 0.01:
+    c_poly = ((l_poly - half_lane_poly) * l_prob +
+              (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
+    c_prob = l_prob + r_prob - l_prob * r_prob
+  else:
+    c_poly = np.zeros(4)
+    c_prob = 0.
+
+  p_weight = 1.  # predicted path weight relatively to the center of the lane
+  d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight))
+  return d_poly, c_poly, c_prob

selfdrive/controls/lib/lateral_mpc/Makefile

@@ -0,0 +1,75 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../../../phonelibs
+
+UNAME_M := $(shell uname -m)
+
+CFLAGS = -O3 -fPIC -I.
+CXXFLAGS = -O3 -fPIC -I.
+
+QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC 
+
+ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
+
+ifeq ($(UNAME_M),aarch64)
+ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
+else
+ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
+endif
+
+OBJS = \
+  $(PHONELIBS)/qpoases/SRC/Bounds.o \
+  $(PHONELIBS)/qpoases/SRC/Constraints.o \
+  $(PHONELIBS)/qpoases/SRC/CyclingManager.o \
+  $(PHONELIBS)/qpoases/SRC/Indexlist.o \
+  $(PHONELIBS)/qpoases/SRC/MessageHandling.o \
+  $(PHONELIBS)/qpoases/SRC/QProblem.o \
+  $(PHONELIBS)/qpoases/SRC/QProblemB.o \
+  $(PHONELIBS)/qpoases/SRC/SubjectTo.o \
+  $(PHONELIBS)/qpoases/SRC/Utils.o \
+  $(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \
+  mpc_export/acado_qpoases_interface.o \
+  mpc_export/acado_integrator.o \
+  mpc_export/acado_solver.o \
+  mpc_export/acado_auxiliary_functions.o \
+  mpc.o
+
+DEPS := $(OBJS:.o=.d)
+
+.PHONY: all
+all: libcommampc.so
+
+libcommampc.so: $(OBJS)
+	$(CXX) -shared -o '$@' $^ -lm
+
+%.o: %.cpp
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+generator: generator.cpp
+	$(CXX) -Wall -std=c++11 \
+         generator.cpp \
+         -o generator \
+         $(ACADO_FLAGS) \
+         $(ACADO_LIBS)
+
+.PHONY: generate
+generate: generator
+	./generator
+
+.PHONY: clean
+clean:
+	rm -f libcommampc.so generator $(OBJS) $(DEPS)
+
+-include $(DEPS)

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -0,0 +1,137 @@
+#include <acado_code_generation.hpp>
+
+#define PI 3.1415926536
+#define deg2rad(d) (d/180.0*PI)
+
+const int controlHorizon = 50;
+const double samplingTime = 0.05; // 20 Hz
+
+using namespace std;
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState xx; // x position
+  DifferentialState yy; // y position
+  DifferentialState psi; // vehicle heading
+  DifferentialState delta;
+
+  OnlineData curvature_factor;
+  OnlineData v_ref; // m/s
+  OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
+  OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
+  OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
+  OnlineData l_prob, r_prob, p_prob;
+  OnlineData lane_width;
+
+  Control t;
+
+  // Equations of motion
+  f << dot(xx) == v_ref * cos(psi);
+  f << dot(yy) == v_ref * sin(psi);
+  f << dot(psi) == v_ref * delta * curvature_factor;
+  f << dot(delta) == t;
+
+  auto lr_prob = l_prob + r_prob - l_prob * r_prob;
+
+  auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
+  auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
+  auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
+
+  auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
+  auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
+  auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
+
+  auto c_left_lane = exp(-(poly_l - yy));
+  auto c_right_lane = exp(poly_r - yy);
+
+  auto r_phantom = poly_l - lane_width/2.0;
+  auto l_phantom = poly_r + lane_width/2.0;
+
+  auto path = lr_prob       * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+              + (1-lr_prob) * poly_p;
+
+  auto angle = lr_prob      * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+               + (1-lr_prob) * angle_p;
+
+  // Running cost
+  Function h;
+
+  // Distance errors
+  h << path - yy;
+  h << l_prob * c_left_lane;
+  h << r_prob * c_right_lane;
+
+  // Heading error
+  h << (v_ref + 1.0 ) * (angle - psi);
+
+  // Angular rate error
+  h << (v_ref + 1.0 ) * t;
+
+  DMatrix Q(5,5);
+  Q(0,0) = 1.0;
+  Q(1,1) = 1.0;
+  Q(2,2) = 1.0;
+
+  Q(3,3) = 1.0;
+
+  Q(4,4) = 1.0;
+
+  // Terminal cost
+  Function hN;
+
+  // Distance errors
+  hN << path - yy;
+  hN << l_prob * c_left_lane;
+  hN << r_prob * c_right_lane;
+
+  // Heading errors
+  hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
+
+  DMatrix QN(4,4);
+  QN(0,0) = 1.0;
+  QN(1,1) = 1.0;
+  QN(2,2) = 1.0;
+
+  QN(3,3) = 1.0;
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = samplingTime * controlHorizon;
+
+  OCP ocp( tStart, tEnd, controlHorizon );
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
+  ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
+  ocp.subjectTo( -0.1 <= t <= 0.1);
+  ocp.setNOD(18);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, 250 );
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -0,0 +1,30 @@
+import os
+import subprocess
+
+from cffi import FFI
+
+mpc_dir = os.path.dirname(os.path.abspath(__file__))
+
+libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
+subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
+
+ffi = FFI()
+ffi.cdef("""
+typedef struct {
+    double x, y, psi, delta, t;
+} state_t;
+
+typedef struct {
+    double x[50];
+    double y[50];
+    double psi[50];
+    double delta[50];
+} log_t;
+
+void init();
+void run_mpc(state_t * x0, log_t * solution,
+             double l_poly[4], double r_poly[4], double p_poly[4],
+             double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
+""")
+
+libmpc = ffi.dlopen(libmpc_fn)

selfdrive/controls/lib/lateral_mpc/mpc.c

@@ -0,0 +1,100 @@
+#include "acado_common.h"
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+#define NX          ACADO_NX  /* Number of differential state variables.  */
+#define NXA         ACADO_NXA /* Number of algebraic variables. */
+#define NU          ACADO_NU  /* Number of control inputs. */
+#define NOD         ACADO_NOD  /* Number of online data values. */
+
+#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
+#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
+
+#define N           ACADO_N   /* Number of intervals in the horizon. */
+
+ACADOvariables acadoVariables;
+ACADOworkspace acadoWorkspace;
+
+typedef struct {
+  double x, y, psi, delta, t;
+} state_t;
+
+
+typedef struct {
+  double x[N];
+  double y[N];
+  double psi[N];
+	double delta[N];
+} log_t;
+
+void init(){
+  acado_initializeSolver();
+  int    i;
+
+  /* Initialize the states and controls. */
+  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.1;
+
+  /* Initialize the measurements/reference. */
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+
+  /* MPC: initialize the current state feedback. */
+  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+}
+
+void run_mpc(state_t * x0, log_t * solution,
+             double l_poly[4], double r_poly[4], double p_poly[4],
+             double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
+
+  int    i;
+
+  for (i = 0; i <= NOD * N; i+= NOD){
+    acadoVariables.od[i] = curvature_factor;
+    acadoVariables.od[i+1] = v_ref;
+
+    acadoVariables.od[i+2] = l_poly[0];
+    acadoVariables.od[i+3] = l_poly[1];
+    acadoVariables.od[i+4] = l_poly[2];
+    acadoVariables.od[i+5] = l_poly[3];
+
+    acadoVariables.od[i+6] = r_poly[0];
+    acadoVariables.od[i+7] = r_poly[1];
+    acadoVariables.od[i+8] = r_poly[2];
+    acadoVariables.od[i+9] = r_poly[3];
+
+    acadoVariables.od[i+10] = p_poly[0];
+    acadoVariables.od[i+11] = p_poly[1];
+    acadoVariables.od[i+12] = p_poly[2];
+    acadoVariables.od[i+13] = p_poly[3];
+
+
+    acadoVariables.od[i+14] = l_prob;
+    acadoVariables.od[i+15] = r_prob;
+    acadoVariables.od[i+16] = p_prob;
+    acadoVariables.od[i+17] = lane_width;
+
+  }
+
+  acadoVariables.x0[0] = x0->x;
+  acadoVariables.x0[1] = x0->y;
+  acadoVariables.x0[2] = x0->psi;
+  acadoVariables.x0[3] = x0->delta;
+
+
+  acado_preparationStep();
+  acado_feedbackStep( );
+
+  acado_shiftStates(2, 0, 0);
+  acado_shiftControls( 0 );
+
+	for (i = 0; i <= N; i++){
+		solution->x[i] = acadoVariables.x[i*NX];
+		solution->y[i] = acadoVariables.x[i*NX+1];
+		solution->psi[i] = acadoVariables.x[i*NX+2];
+		solution->delta[i] = acadoVariables.x[i*NX+3];
+	}
+
+  return;
+}

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

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357
+size 4906

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270
+size 3428

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c78a95b2550fdfdc0182add44508c59ec7af9d1c58d776dcd32ee85ea52a771e
+size 8680

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

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:963720039f3655eb3926e5bb3bbde21d92c824245055588ebaee223223accc02
+size 18490

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:10ff52f5d8dbe27def800fe490cdee82a7c054183d2fb1888752609ea00bbea1
+size 1949

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:55ef4b230fd392bb43a49b2bd4fbf6e478b6f7b6144f65df97fe3a952d6b0b49
+size 1822

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

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20
+size 194688