openpilot v0.3.6 release

old-commit-hash: 99cb610b12

RELEASES.md

@@ -1,3 +1,11 @@
+Version 0.3.6 (2017-08-08)
+==========================
+ * Fix alpha CR-V support
+ * Improved GPS
+ * Fix display of target speed not always matching HUD
+ * Increased acceleration after stop
+ * Mitigated some vehicles driving too close to the right line
+
 Version 0.3.5 (2017-07-30)
 ==========================
  * Fix bug where new devices would not begin calibration

cereal/build_from_src.mk

@@ -3,7 +3,7 @@ SRCS := log.capnp car.capnp
 GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
 
 
-UNAME_M := $(shell uname -m)
+UNAME_M ?= $(shell uname -m)
 
 # only generate C++ for docker tests
 ifneq ($(OPTEST),1)

cereal/car.capnp

@@ -180,23 +180,29 @@ struct CarControl {
 struct CarParams {
   carName @0: Text;
   radarName @1: Text;
-  carFingerprint @11: Text;
+  carFingerprint @2: Text;
 
-  enableSteer @2: Bool;
+  enableSteer @3: Bool;
-  enableGas @3: Bool;
+  enableGas @4: Bool;
-  enableBrake @4: Bool;
+  enableBrake @5: Bool;
-  enableCruise @5: Bool;
+  enableCruise @6: Bool;
 
   # things about the car in the manual
-  wheelBase @6: Float32;     # in meters
+  m @7: Float32;     # [kg] running weight
-  steerRatio @7: Float32;
+  l @8: Float32;     # [m] wheelbase
+  sR @9: Float32;    # [] steering ratio
+  aF @10: Float32;   # [m] GC distance to front axle
+  aR @11: Float32;   # [m] GC distance to rear axle
+  chi @12: Float32;  # [] rear steering ratio wrt front steering (usually 0)
 
   # things we can derive
-  slipFactor @8: Float32;
+  j @13: Float32;    # [kg*m2] body rot inertia
+  cF @14: Float32;   # [N/rad] front tire coeff of stiff
+  cR @15: Float32;   # [N/rad] rear tire coeff of stiff
 
   # Kp and Ki for the lateral control
-  steerKp @9: Float32;
+  steerKp @16: Float32;
-  steerKi @10: Float32;
+  steerKi @17: Float32;
 
   # TODO: Kp and Ki for long control, perhaps not needed?
 }

cereal/log.capnp

@@ -26,6 +26,8 @@ struct InitData {
 
   deviceType @3 :DeviceType;
   version @4 :Text;
+  gitCommit @10 :Text;
+  gitBranch @11 :Text;
 
   androidBuildInfo @5 :AndroidBuildInfo;
   androidSensors @6 :List(AndroidSensor);
@@ -326,14 +328,15 @@ struct Live100Data {
   vTargetLead @3 :Float32;
   upAccelCmd @4 :Float32;
   uiAccelCmd @5 :Float32;
-  yActual @6 :Float32;
+  yActualDEPRECATED @6 :Float32;
   yDes @7 :Float32;
   upSteer @8 :Float32;
   uiSteer @9 :Float32;
   aTargetMin @10 :Float32;
   aTargetMax @11 :Float32;
   jerkFactor @12 :Float32;
-  angleSteers @13 :Float32;  # Steering angle in degrees.
+  angleSteers @13 :Float32;     # Steering angle in degrees.
+  angleSteersDes @29 :Float32;
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
 
@@ -701,10 +704,66 @@ struct QcomGnss {
   union {
     measurementReport @1 :MeasurementReport;
     clockReport @2 :ClockReport;
+    drMeasurementReport @3 :DrMeasurementReport;
+  }
+
+  enum MeasurementSource @0xd71a12b6faada7ee {
+    gps @0;
+    glonass @1;
+    beidou @2;
+  }
+
+  enum SVObservationState @0xe81e829a0d6c83e9 {
+    idle @0;
+    search @1;
+    searchVerify @2;
+    bitEdge @3;
+    trackVerify @4;
+    track @5;
+    restart @6;
+    dpo @7;
+    glo10msBe @8;
+    glo10msAt @9;
+  }
+
+  struct MeasurementStatus @0xe501010e1bcae83b {
+    subMillisecondIsValid @0 :Bool;
+    subBitTimeIsKnown @1 :Bool;
+    satelliteTimeIsKnown @2 :Bool;
+    bitEdgeConfirmedFromSignal @3 :Bool;
+    measuredVelocity @4 :Bool;
+    fineOrCoarseVelocity @5 :Bool;
+    lockPointValid @6 :Bool;
+    lockPointPositive @7 :Bool;
+    lastUpdateFromDifference @8 :Bool;
+    lastUpdateFromVelocityDifference @9 :Bool;
+    strongIndicationOfCrossCorelation @10 :Bool;
+    tentativeMeasurement @11 :Bool;
+    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;
+    
+    imdJammingIndicator @24 :Bool;
+    lteB13TxJammingIndicator @25 :Bool;
+    freshMeasurementIndicator @26 :Bool;
+
+    multipathEstimateIsValid @27 :Bool;
+    directionIsValid @28 :Bool;
   }
 
   struct MeasurementReport {
-    source @0 :Source;
+    source @0 :MeasurementSource;
 
     fCount @1 :UInt32;
 
@@ -720,11 +779,6 @@ struct QcomGnss {
 
     sv @10 :List(SV);
 
-    enum Source {
-      gps @0;
-      glonass @1;
-    }
-
     struct SV {
       svId @0 :UInt8;
       observationState @2 :SVObservationState;
@@ -736,7 +790,7 @@ struct QcomGnss {
       filterStages @7 :UInt8;
       carrierNoise @8 :UInt16;
       latency @9 :Int16;
-      predetectIntegration @10 :UInt8;
+      predetectInterval @10 :UInt8;
       postdetections @11 :UInt16;
 
       unfilteredMeasurementIntegral @12 :UInt32;
@@ -753,55 +807,6 @@ struct QcomGnss {
       fineSpeed @23 :Float32;
       fineSpeedUncertainty @24 :Float32;
       cycleSlipCount @25 :UInt8;
-
-      struct MeasurementStatus {
-        subMillisecondIsValid @0 :Bool;
-        subBitTimeIsKnown @1 :Bool;
-        satelliteTimeIsKnown @2 :Bool;
-        bitEdgeConfirmedFromSignal @3 :Bool;
-        measuredVelocity @4 :Bool;
-        fineOrCoarseVelocity @5 :Bool;
-        lockPointValid @6 :Bool;
-        lockPointPositive @7 :Bool;
-        lastUpdateFromDifference @8 :Bool;
-        lastUpdateFromVelocityDifference @9 :Bool;
-        strongIndicationOfCrossCorelation @10 :Bool;
-        tentativeMeasurement @11 :Bool;
-        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;
-        
-        imdJammingIndicator @24 :Bool;
-        lteB13TxJammingIndicator @25 :Bool;
-        freshMeasurementIndicator @26 :Bool;
-
-        multipathEstimateIsValid @27 :Bool;
-        directionIsValid @28 :Bool;
-      }
-
-      enum SVObservationState {
-        idle @0;
-        search @1;
-        searchVerify @2;
-        bitEdge @3;
-        trackVerify @4;
-        track @5;
-        restart @6;
-        dpo @7;
-        glo10msBe @8;
-        glo10msAt @9;
-      }
     }
 
   }
@@ -810,8 +815,8 @@ struct QcomGnss {
     hasFCount @0 :Bool;
     fCount @1 :UInt32;
 
-    hasGpsWeekNumber @2 :Bool;
+    hasGpsWeek @2 :Bool;
-    gpsWeekNumber @3 :UInt16;
+    gpsWeek @3 :UInt16;
     hasGpsMilliseconds @4 :Bool;
     gpsMilliseconds @5 :UInt32;
     gpsTimeBias @6 :Float32;
@@ -866,6 +871,88 @@ struct QcomGnss {
     lpmRtcCount @49 :UInt32;
     clockResets @50 :UInt32;
   }
+
+  struct DrMeasurementReport {
+
+    reason @0 :UInt8;
+    seqNum @1 :UInt8;
+    seqMax @2 :UInt8;
+    rfLoss @3 :UInt16;
+
+    systemRtcValid @4 :Bool;
+    fCount @5 :UInt32;
+    clockResets @6 :UInt32;
+    systemRtcTime @7 :UInt64;
+
+    gpsLeapSeconds @8 :UInt8;
+    gpsLeapSecondsUncertainty @9 :UInt8;
+    gpsToGlonassTimeBiasMilliseconds @10 :Float32;
+    gpsToGlonassTimeBiasMillisecondsUncertainty @11 :Float32;
+
+    gpsWeek @12 :UInt16;
+    gpsMilliseconds @13 :UInt32;
+    gpsTimeBiasMs @14 :UInt32;
+    gpsClockTimeUncertaintyMs @15 :UInt32;
+    gpsClockSource @16 :UInt8;
+
+    glonassClockSource @17 :UInt8;
+    glonassYear @18 :UInt8;
+    glonassDay @19 :UInt16;
+    glonassMilliseconds @20 :UInt32;
+    glonassTimeBias @21 :Float32;
+    glonassClockTimeUncertainty @22 :Float32;
+
+    clockFrequencyBias @23 :Float32;
+    clockFrequencyUncertainty @24 :Float32;
+    frequencySource @25 :UInt8;
+
+    source @26 :MeasurementSource;
+
+    sv @27 :List(SV);
+
+    struct SV {
+      svId @0 :UInt8;
+      glonassFrequencyIndex @1 :Int8;
+      observationState @2 :SVObservationState;
+      observations @3 :UInt8;
+      goodObservations @4 :UInt8;
+      filterStages @5 :UInt8;
+      predetectInterval @6 :UInt8;
+      cycleSlipCount @7 :UInt8;
+      postdetections @8 :UInt16;
+
+      measurementStatus @9 :MeasurementStatus;
+
+      carrierNoise @10 :UInt16;
+      rfLoss @11 :UInt16;
+      latency @12 :Int16;
+
+      filteredMeasurementFraction @13 :Float32;
+      filteredMeasurementIntegral @14 :UInt32;
+      filteredTimeUncertainty @15 :Float32;
+      filteredSpeed @16 :Float32;
+      filteredSpeedUncertainty @17 :Float32;
+
+      unfilteredMeasurementFraction @18 :Float32;
+      unfilteredMeasurementIntegral @19 :UInt32;
+      unfilteredTimeUncertainty @20 :Float32;
+      unfilteredSpeed @21 :Float32;
+      unfilteredSpeedUncertainty @22 :Float32;
+
+      multipathEstimate @23 :UInt32;
+      azimuth @24 :Float32;
+      elevation @25 :Float32;
+      dopplerAcceleration @26 :Float32;
+      fineSpeed @27 :Float32;
+      fineSpeedUncertainty @28 :Float32;
+
+      carrierPhase @29 :Float64;
+      fCount @30 :UInt32;
+
+      parityErrorCount @31 :UInt16;
+      goodParity @32 :Bool;
+    }
+  }
 }
 
 struct LidarPts {

common/params.py

@@ -57,7 +57,7 @@ keys = {
   "IsMetric": TxType.PERSISTANT,
   "IsRearViewMirror": TxType.PERSISTANT,
 # written: visiond
-# read:    visiond
+# read:    visiond, controlsd
   "CalibrationParams": TxType.PERSISTANT,
 # written: visiond
 # read:    visiond, ui

opendbc

@@ -1 +1 @@
-Subproject commit b77861eb00d45e25af501f78bbed155d2df06159
+Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba

panda

@@ -1 +1 @@
-Subproject commit 5409c51041cfe8f139650a4e0decf4f6d863eb07
+Subproject commit ff8a4bd4e844c2b0a0dd7efb321b31781d8034c8

selfdrive/car/honda/carstate.py

@@ -21,6 +21,7 @@ def get_can_parser(CP):
       ("WHEEL_SPEED_FL", 0x1d0, 0),
       ("WHEEL_SPEED_FR", 0x1d0, 0),
       ("WHEEL_SPEED_RL", 0x1d0, 0),
+      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x14a, 0),
       ("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x191, 0),
@@ -33,13 +34,13 @@ def get_can_parser(CP):
       ("SEATBELT_DRIVER_LAMP", 0x305, 1),
       ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
       ("BRAKE_PRESSED", 0x17c, 0),
+      ("BRAKE_SWITCH", 0x17c, 0),
       ("CAR_GAS", 0x130, 0),
       ("CRUISE_BUTTONS", 0x296, 0),
       ("ESP_DISABLED", 0x1a4, 1),
       ("HUD_LEAD", 0x30c, 0),
       ("USER_BRAKE", 0x1a4, 0),
       ("STEER_STATUS", 0x18f, 5),
-      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("BRAKE_ERROR_1", 0x1b0, 1),
       ("BRAKE_ERROR_2", 0x1b0, 1),
       ("GEAR_SHIFTER", 0x191, 0),
@@ -74,6 +75,7 @@ def get_can_parser(CP):
       ("WHEEL_SPEED_FL", 0x1d0, 0),
       ("WHEEL_SPEED_FR", 0x1d0, 0),
       ("WHEEL_SPEED_RL", 0x1d0, 0),
+      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x156, 0),
       ("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x1a3, 0),
@@ -86,13 +88,13 @@ def get_can_parser(CP):
       ("SEATBELT_DRIVER_LAMP", 0x305, 1),
       ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
       ("BRAKE_PRESSED", 0x17c, 0),
+      ("BRAKE_SWITCH", 0x17c, 0),
       ("CAR_GAS", 0x130, 0),
       ("CRUISE_BUTTONS", 0x1a6, 0),
       ("ESP_DISABLED", 0x1a4, 1),
       ("HUD_LEAD", 0x30c, 0),
       ("USER_BRAKE", 0x1a4, 0),
       ("STEER_STATUS", 0x18f, 5),
-      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("BRAKE_ERROR_1", 0x1b0, 1),
       ("BRAKE_ERROR_2", 0x1b0, 1),
       ("GEAR_SHIFTER", 0x1a3, 0),
@@ -125,6 +127,7 @@ def get_can_parser(CP):
       ("WHEEL_SPEED_FL", 0x1d0, 0),
       ("WHEEL_SPEED_FR", 0x1d0, 0),
       ("WHEEL_SPEED_RL", 0x1d0, 0),
+      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x156, 0),
       #("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x191, 0),
@@ -137,6 +140,7 @@ def get_can_parser(CP):
       ("SEATBELT_DRIVER_LAMP", 0x305, 1),
       ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
       ("BRAKE_PRESSED", 0x17c, 0),
+      ("BRAKE_SWITCH", 0x17c, 0),
       #("CAR_GAS", 0x130, 0),
       ("PEDAL_GAS", 0x17C, 0),
       ("CRUISE_BUTTONS", 0x1a6, 0),
@@ -144,7 +148,6 @@ def get_can_parser(CP):
       ("HUD_LEAD", 0x30c, 0),
       ("USER_BRAKE", 0x1a4, 0),
       #("STEER_STATUS", 0x18f, 5),
-      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("BRAKE_ERROR_1", 0x1b0, 1),
       ("BRAKE_ERROR_2", 0x1b0, 1),
       ("GEAR_SHIFTER", 0x191, 0),
@@ -161,7 +164,7 @@ def get_can_parser(CP):
       (0x156, 100),
       (0x158, 100),
       (0x17c, 100),
-      #(0x1a3, 50),
+      (0x191, 100),
       (0x1a4, 50),
       (0x1a6, 50),
       (0x1b0, 50),
@@ -177,6 +180,7 @@ def get_can_parser(CP):
       ("WHEEL_SPEED_FL", 0x1d0, 0),
       ("WHEEL_SPEED_FR", 0x1d0, 0),
       ("WHEEL_SPEED_RL", 0x1d0, 0),
+      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x156, 0),
       ("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x191, 0),
@@ -189,13 +193,13 @@ def get_can_parser(CP):
       ("SEATBELT_DRIVER_LAMP", 0x305, 1),
       ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
       ("BRAKE_PRESSED", 0x17c, 0),
+      ("BRAKE_SWITCH", 0x17c, 0),
       #("CAR_GAS", 0x130, 0),
       ("CRUISE_BUTTONS", 0x1a6, 0),
       ("ESP_DISABLED", 0x1a4, 1),
       ("HUD_LEAD", 0x30c, 0),
       ("USER_BRAKE", 0x1a4, 0),
       ("STEER_STATUS", 0x18f, 5),
-      ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("BRAKE_ERROR_1", 0x1b0, 1),
       ("BRAKE_ERROR_2", 0x1b0, 1),
       ("GEAR_SHIFTER", 0x191, 0),
@@ -213,7 +217,6 @@ def get_can_parser(CP):
       (0x158, 100),
       (0x17c, 100),
       (0x191, 100),
-      (0x1a3, 50),
       (0x1a4, 50),
       (0x1a6, 50),
       (0x1b0, 50),
@@ -379,7 +382,7 @@ class CarState(object):
     else:
       self.car_gas = cp.vl[0x130]['CAR_GAS']
       self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
-    self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
+    self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or cp.vl[0x17C]['BRAKE_SWITCH']
     self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
     self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
     self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']

selfdrive/car/honda/interface.py

@@ -59,8 +59,8 @@ class CarInterface(object):
   @staticmethod
   def get_params(candidate, fingerprint):
 
-    # pedal
+    # kg of standard extra cargo to count for drive, gas, etc...
-    brake_only = 0x201 not in fingerprint
+    std_cargo = 136
 
     ret = car.CarParams.new_message()
 
@@ -70,32 +70,63 @@ class CarInterface(object):
 
     ret.enableSteer = True
     ret.enableBrake = True
-    ret.enableGas = not brake_only
-    ret.enableCruise = brake_only
-    #ret.enableCruise = False
 
-    # TODO: those parameters should be platform dependent
+    # pedal
-    ret.wheelBase = 2.67
+    ret.enableGas = 0x201 in fingerprint
-    ret.slipFactor = 0.0014
+    ret.enableCruise = not ret.enableGas
+
+    # FIXME: hardcoding honda civic 2016 touring wight so it can be used to 
+    # scale unknown params for other cars
+    m_civic = 2923./2.205 + std_cargo
+    l_civic = 2.70
+    aF_civic = l_civic * 0.4
+    aR_civic = l_civic - aF_civic
+    j_civic = 2500
+    cF_civic = 85400
+    cR_civic = 90000
 
     if candidate == "HONDA CIVIC 2016 TOURING":
-      ret.steerRatio = 13.0
+      ret.m = m_civic
-      ret.steerKp, ret.steerKi = 6.0, 1.4
+      ret.l = l_civic
+      ret.aF = aF_civic
+      ret.sR = 13.0
+      ret.steerKp, ret.steerKi = 1.0, 0.24
     elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
-      ret.steerRatio = 15.3
+      ret.m = 3095./2.205 + std_cargo
-      if not brake_only:
+      ret.l = 2.67
-        # assuming if we have an interceptor we also have a torque mod
+      ret.aF = ret.l * 0.37
-        ret.steerKp, ret.steerKi = 3.0, 0.7
+      ret.sR = 15.3
-      else:
+      # Acura at comma has modified steering FW, so different tuning for the Neo in that car
-        ret.steerKp, ret.steerKi = 6.0, 1.4
+      # FIXME: using dongleId isn't great, better to identify the car than the Neo
+      is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee'
+      ret.steerKp, ret.steerKi = [0.5, 0.12] if is_fw_modified else [1.0, 0.24]
     elif candidate == "HONDA ACCORD 2016 TOURING":
-      ret.steerRatio = 15.3
+      ret.m = 3580./2.205 + std_cargo
-      ret.steerKp, ret.steerKi = 6.0, 1.4
+      ret.l = 2.74
+      ret.aF = ret.l * 0.38
+      ret.sR = 15.3
+      ret.steerKp, ret.steerKi = 1.0, 0.24
     elif candidate == "HONDA CR-V 2016 TOURING":
-      ret.steerRatio = 15.3
+      ret.m = 3572./2.205 + std_cargo
-      ret.steerKp, ret.steerKi = 3.0, 0.7
+      ret.l = 2.62
+      ret.aF = ret.l * 0.41
+      ret.sR = 15.3
+      ret.steerKp, ret.steerKi = 0.5, 0.12
     else:
       raise ValueError("unsupported car %s" % candidate)
+ 
+    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
+    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.
 
     return ret
 
@@ -278,7 +309,7 @@ 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/1.4,0,1)*0xc6)
+    pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6)
 
     self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
       c.gas, c.brake, c.steeringTorque, \

selfdrive/common/cereal.mk

@@ -1,3 +1,14 @@
+UNAME_M ?= $(shell uname -m)
+ifeq ($(UNAME_M),x86_64)
+
+CEREAL_CFLAGS = -I$(ONE)/external/capnp/include
+CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
+ifeq ($(CEREAL_LIBS),)
+	CEREAL_LIBS = -L$(ONE)/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),)
@@ -5,6 +16,9 @@ ifeq ($(CEREAL_LIBS),)
                 -L$(PHONELIBS)/capnp-c/aarch64/lib/ \
                 -l:libcapn.a -l:libcapnp.a -l:libkj.a
 endif
+
+endif
+
 CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o
 
 log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++

selfdrive/common/params.c

@@ -2,7 +2,10 @@
 
 #include "selfdrive/common/util.h"
 
+#ifndef _GNU_SOURCE
 #define _GNU_SOURCE
+#endif  // _GNU_SOURCE
+
 #include <sys/file.h>
 #include <unistd.h>
 #include <stdlib.h>

selfdrive/common/params.h

@@ -7,6 +7,8 @@
 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);
 

selfdrive/common/version.h

@@ -1 +1 @@
-#define OPENPILOT_VERSION "0.3.5"
+#define OPENPILOT_VERSION "0.3.6"

selfdrive/config.py

@@ -1,14 +1,16 @@
 import numpy as np
 
 class Conversions:
-  MPH_TO_MS = 1.609/3.6
+  MPH_TO_KPH = 1.609344
-  MS_TO_MPH = 3.6/1.609
+  KPH_TO_MPH = 1. / MPH_TO_KPH
-  KPH_TO_MS = 1./3.6
   MS_TO_KPH = 3.6
-  MPH_TO_KPH = 1.609
+  KPH_TO_MS = 1. / MS_TO_KPH
-  KPH_TO_MPH = 1./1.609
+  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
-  KNOTS_TO_MS = 1/1.9438
+  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
   MS_TO_KNOTS = 1.9438
+  KNOTS_TO_MS = 1. / MS_TO_KNOTS
+  DEG_TO_RAD = np.pi/180.
+  RAD_TO_DEG = 1. / DEG_TO_RAD
 
   # Car decode decimal minutes into decimal degrees, can work with numpy arrays as input
   @staticmethod

selfdrive/controls/controlsd.py

@@ -1,29 +1,29 @@
 #!/usr/bin/env python
 import os
-import zmq
+import json
-import selfdrive.messaging as messaging
 from copy import copy
 
-from cereal import car, log
+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.longcontrol import LongControl
 from selfdrive.controls.lib.latcontrol import LatControl
-
 from selfdrive.controls.lib.alertmanager import AlertManager
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.adaptivecruise import A_ACC_MAX
 
 V_CRUISE_MAX = 144
 V_CRUISE_MIN = 8
@@ -71,6 +71,7 @@ class Controls(object):
     self.AM = AlertManager()
     self.LoC = LongControl()
     self.LaC = LatControl()
+    self.VM = VehicleModel(self.CP)
   
     # write CarParams
     params = Params()
@@ -88,6 +89,13 @@ class Controls(object):
   
     # 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
@@ -276,7 +284,7 @@ class Controls(object):
 
     # *** steering PID loop *** 
     final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle, 
-                                            self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.CP) 
+                                            self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM) 
  
     self.prof.checkpoint("PID") 
     
@@ -345,8 +353,10 @@ class Controls(object):
     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: fix this
+    # TODO: parametrize 0.714 in interface?
-    self.CC.cruiseControl.accelOverride = float(1.0)
+    # 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
@@ -407,8 +417,8 @@ class Controls(object):
     dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
 
     # lateral control state
-    dat.live100.yActual = float(self.LaC.y_actual)
     dat.live100.yDes = float(self.LaC.y_des)
+    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)
 

selfdrive/controls/lib/adaptivecruise.py

@@ -3,6 +3,8 @@ import numpy as np
 from common.numpy_fast import clip, interp
 import selfdrive.messaging as messaging
 
+# TODO: we compute a_pcm but we don't use it, as accelOverride is hardcoded in controlsd
+
 # lookup tables VS speed to determine min and max accels in cruise
 _A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
 _A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.]
@@ -25,7 +27,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP):
   deg_to_rad = np.pi / 180.  # from can reading to rad
 
   a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
-  a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.steerRatio * CP.wheelBase)
+  a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
   a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
 
   a_target[1] = min(a_target[1], a_x_allowed)
@@ -109,12 +111,15 @@ _A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
 # speeds
 _A_CORR_BY_SPEED_BP = [0., 5., 20.]
 
+# max acceleration allowed in acc, which happens in restart
+A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
+
 def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
   a_coast_min = -1.0   # never coast faster then -1m/s^2
   # coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
   # regardless v_target
   if v_ref > min(v_coast, v_target):
-    # for smooth coast we can be agrressive and target a point where car would actually crash
+    # for smooth coast we can be aggressive and target a point where car would actually crash
     v_offset_coast = 0.
     d_offset_coast = d_des/2. - 4.
 
@@ -127,9 +132,8 @@ def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_c
     else:
       a_max = a_coast_min
   else:
-    # same as cruise accel, but add a small correction based on lead acceleration at low speeds
+    # same as cruise accel, plus add a small correction based on relative lead speed
-    # when lead car accelerates faster, we can do the same, and vice versa
+    # if the lead car is faster, we can accelerate more, if the car is slower, then we can reduce acceleration
-
     a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
                   * clip(-v_rel / 4., -.5, 1)
   return a_max
@@ -154,7 +158,7 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
                           v_target, v_coast, a_target, a_pcm):
   #*** compute max accel ***
   # v_rel is now your velocity in lead car frame
-  v_rel = -v_rel  # this simplifiess things when thinking in d_rel-v_rel diagram
+  v_rel *= -1  # this simplifies things when thinking in d_rel-v_rel diagram
 
   v_rel_pid = v_pid - v_lead
 
@@ -227,8 +231,9 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
 
   #*** set accel limits as cruise accel/decel limits ***
   a_target = calc_cruise_accel_limits(v_ego)
-  # Always 1 for now.
+
-  a_pcm = 1
+  # start with 1
+  a_pcm = 1.
 
   #*** limit max accel in sharp turns
   a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP)

selfdrive/controls/lib/drive_helpers.py

@@ -5,8 +5,8 @@ def rate_limit(new_value, last_value, dw_step, up_step):
 
 def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
   # simple integral controller that learns how much steering offset to put to have the car going straight
-  min_offset = -1.  # deg
+  min_offset = -5.  # deg
-  max_offset =  1.  # deg
+  max_offset =  5.  # deg
   alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
   min_learn_speed = 1.
 

selfdrive/controls/lib/latcontrol.py

@@ -1,12 +1,7 @@
 import math
 import numpy as np
 from common.numpy_fast import clip, interp
-
+from selfdrive.config import Conversions as CV
-def calc_curvature(v_ego, angle_steers, CP, angle_offset=0):
-  deg_to_rad = np.pi/180.
-  angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad
-  curvature = angle_steers_rad/(CP.steerRatio * CP.wheelBase * (1. + CP.slipFactor * v_ego**2))
-  return curvature
 
 _K_CURV_V = [1., 0.6]
 _K_CURV_BP = [0., 0.002]
@@ -26,27 +21,33 @@ def calc_d_lookahead(v_ego, d_poly):
 
   k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
 
-  # sqrt on speed is needed to keep, for a given curvature, the y_offset
+  # sqrt on speed is needed to keep, for a given curvature, the y_des
-  # proportional to speed. Indeed, y_offset is prop to d_lookahead^2
+  # 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, CP, angle_offset):
+def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
-  #*** this function return teh lateral offset given the steering angle, speed and the lookahead distance
+  #*** this function returns the lateral offset given the steering angle, speed and the lookahead distance
-  curvature = calc_curvature(v_ego, angle_steers, CP, angle_offset)
+  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 pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
+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 = y_des - y_actual
+  error_steer = sa_des - sa_actual
   Ui_unwind_speed = 0.3/rate   #.3 per second
 
   Up_steer = error_steer*Kp
@@ -109,7 +110,7 @@ class LatControl(object):
   def reset(self):
     self.Ui_steer = 0.
 
-  def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP):
+  def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM):
     rate = 100
 
     steer_max = 1.0
@@ -117,16 +118,16 @@ class LatControl(object):
     # how far we look ahead is function of speed and desired path
     d_lookahead = calc_d_lookahead(v_ego, d_poly)
 
-    # calculate actual offset at the lookahead point
-    self.y_actual, _ = calc_lookahead_offset(v_ego, angle_steers,
-                                             d_lookahead, CP, angle_offset)
-
     # desired lookahead offset
     self.y_des = np.polyval(d_poly, d_lookahead)
 
+    # calculate actual offset at the lookahead point
+    self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des,
+                                                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(
-      v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
+      v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max,
-      steer_override, self.sat_count, enabled, CP.steerKp, CP.steerKi, rate)
+      steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate)
 
     final_steer = clip(output_steer, -steer_max, steer_max)
     return final_steer, sat_flag

selfdrive/controls/lib/vehicle_model.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+import numpy as np
+from numpy.linalg import inv
+from selfdrive.car.honda.interface import CarInterface
+
+## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
+
+# Xdot = A*X + B*U
+# where X = [v, r], with v and r lateral speed and rotational speed, respectively
+# and U is the steering angle (controller input)
+# 
+# A depends on longitudinal speed, u, and vehicle parameters CP
+
+def create_dyn_state_matrices(u, CP):
+  A = np.zeros((2,2))
+  B = np.zeros((2,1))
+  A[0,0] = - (CP.cF + CP.cR)/(CP.m*u)
+  A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u
+  A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u)
+  A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u)
+  B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR
+  B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR
+  return A, B
+
+
+def kin_ss_sol(sa, u, CP):
+  # kinematic solution, useful when speed ~ 0
+  K = np.zeros((2,1))
+  K[0,0] = CP.aR / CP.sR / CP.l * u
+  K[1,0] = 1. / CP.sR / CP.l * u
+  return K * sa
+
+
+def dyn_ss_sol(sa, u, CP):
+  # Dynamic solution, useful when speed > 0
+  A, B = create_dyn_state_matrices(u, CP)
+  return - np.matmul(inv(A), B) * sa
+
+def calc_slip_factor(CP):
+  # the slip factor is a measure of how the curvature changes with speed
+  # it's positive for Oversteering vehicle, negative (usual case) otherwise
+  return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR)
+
+class VehicleModel(object):
+  def __init__(self, CP, init_state=np.asarray([[0.],[0.]])):
+    self.dt = 0.1
+    lookahead = 2.    # s
+    self.steps = int(lookahead / self.dt)
+    self.update_state(init_state)
+    self.state_pred = np.zeros((self.steps, self.state.shape[0]))
+    self.CP = CP
+
+  def update_state(self, state):
+    self.state = state
+
+  def steady_state_sol(self, sa, u):
+    # if the speed is too small we can't use the dynamic model 
+    # (tire slip is undefined), we then use the kinematic model
+    if u > 0.1:
+      return dyn_ss_sol(sa, u, self.CP)
+    else:
+      return kin_ss_sol(sa, u, self.CP)
+
+  def calc_curvature(self, sa, u):
+    # this formula can be derived from state equations in steady state conditions
+    sf = calc_slip_factor(self.CP)
+    return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l
+
+  def get_steer_from_curvature(self, curv, u):
+    # this function is the exact inverse of calc_curvature, returning steer angle given curvature
+    sf = calc_slip_factor(self.CP)
+    return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv
+
+  def state_prediction(self, sa, u):
+    # U is the matrix of the controls
+    # u is the long speed
+    A, B = create_dyn_state_matrices(u, self.CP)
+    return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
+
+
+if __name__ == '__main__':
+  # load car params
+  CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
+  print CP
+  VM = VehicleModel(CP)
+  print VM.steady_state_sol(.1, 0.15)
+  

selfdrive/controls/radard.py

@@ -4,18 +4,15 @@ import zmq
 import numpy as np
 import numpy.matlib
 from collections import defaultdict
-
 from fastcluster import linkage_vector
-
 import selfdrive.messaging as messaging
 from selfdrive.services import service_list
 from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
 from selfdrive.controls.lib.pathplanner import PathPlanner
 from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
+from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.swaglog import cloudlog
-
 from cereal import car
-
 from common.params import Params
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.kalman.ekf import EKF, SimpleSensor
@@ -51,6 +48,7 @@ def radard_thread(gctx=None):
   # wait for stats about the car to come in from controls
   cloudlog.info("radard is waiting for CarParams")
   CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+  VM = VehicleModel(CP)
   cloudlog.info("radard got CarParams")
 
   # import the radar from the fingerprint
@@ -142,7 +140,7 @@ def radard_thread(gctx=None):
     if enabled:    # use path from model path_poly
       path_y = np.polyval(PP.d_poly, path_x)
     else:          # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset
-      path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, CP, angle_offset=0)[0]
+      path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
 
     # *** remove missing points from meta data ***
     for ids in tracks.keys():

selfdrive/debug/getframes/Makefile

@@ -0,0 +1,26 @@
+CC = clang
+CXX = clang++
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+
+.PHONY: all
+all: libvisionipc.so
+
+visionipc.o: ../../common/visionipc.c ../../common/visionipc.h 
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -MMD \
+          -I../.. -I../../.. \
+          -c -o '$@' ../../common/visionipc.c
+
+libvisionipc.so: visionipc.o
+	$(CC) -shared -fPIC -o '$@' visionipc.o
+
+.PHONY: clean
+clean:
+	rm visionipc.o libvisionipc.so

selfdrive/debug/getframes/getframes.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+import os
+import time
+import subprocess
+from cffi import FFI
+import ctypes
+
+import numpy as np
+
+gf_dir = os.path.dirname(os.path.abspath(__file__))
+
+subprocess.check_call(["make"], cwd=gf_dir)
+
+
+ffi = FFI()
+ffi.cdef("""
+
+typedef enum VisionStreamType {
+  VISION_STREAM_UI_BACK,
+  VISION_STREAM_UI_FRONT,
+  VISION_STREAM_YUV,
+  VISION_STREAM_MAX,
+} VisionStreamType;
+
+typedef struct VisionUIInfo {
+  int big_box_x, big_box_y;
+  int big_box_width, big_box_height;
+  int transformed_width, transformed_height;
+
+  int front_box_x, front_box_y;
+  int front_box_width, front_box_height;
+} VisionUIInfo;
+
+typedef struct VisionStreamBufs {
+  VisionStreamType type;
+
+  int width, height, stride;
+  size_t buf_len;
+
+  union {
+    VisionUIInfo ui_info;
+  } buf_info;
+} VisionStreamBufs;
+
+typedef struct VisionBuf {
+  int fd;
+  size_t len;
+  void* addr;
+} VisionBuf;
+
+typedef struct VisionBufExtra {
+  uint32_t frame_id; // only for yuv
+} VisionBufExtra;
+
+typedef struct VisionStream {
+  int ipc_fd;
+  int last_idx;
+  int num_bufs;
+  VisionStreamBufs bufs_info;
+  VisionBuf *bufs;
+} VisionStream;
+
+int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
+VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
+void visionstream_destroy(VisionStream *s);
+
+"""
+)
+
+clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so"))
+
+
+def getframes():
+  s = ffi.new("VisionStream*")
+  buf_info = ffi.new("VisionStreamBufs*")
+  err = clib.visionstream_init(s, clib.VISION_STREAM_UI_BACK, True, buf_info)
+  assert err == 0
+
+  w = buf_info.width
+  h = buf_info.height
+  assert buf_info.stride == w*3
+  assert buf_info.buf_len == w*h*3
+
+  while True:
+    buf = clib.visionstream_get(s, ffi.NULL)
+
+    pbuf = ffi.buffer(buf.addr, buf.len)
+    yield np.frombuffer(pbuf, dtype=np.uint8).reshape((h, w, 3))
+
+
+if __name__ == "__main__":
+  for buf in getframes():
+    print buf.shape, buf[101, 101]

selfdrive/loggerd/loggerd

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:71026624dc5524417a9f569d35655deb790289631c641453c835046a50ba2a2a
+oid sha256:51d01404e86f6e1f1d2ecea67bfea72c8e568f03b4f839b4480b40c27993e685
-size 1365152
+size 1367088

selfdrive/messaging.py

@@ -13,8 +13,10 @@ def pub_sock(context, port, addr="*"):
   sock.bind("tcp://%s:%d" % (addr, port))
   return sock
 
-def sub_sock(context, port, poller=None, addr="127.0.0.1"):
+def sub_sock(context, port, poller=None, addr="127.0.0.1", conflate=False):
   sock = context.socket(zmq.SUB)
+  if conflate:
+    sock.setsockopt(zmq.CONFLATE, 1)
   sock.connect("tcp://%s:%d" % (addr, port))
   sock.setsockopt(zmq.SUBSCRIBE, "")
   if poller is not None:
@@ -50,3 +52,12 @@ def recv_sock(sock, wait=False):
   if dat is not None:
     dat = log.Event.from_bytes(dat)
   return dat
+
+def recv_one(sock):
+  return log.Event.from_bytes(sock.recv())
+
+def recv_one_or_none(sock):
+  try:
+    return log.Event.from_bytes(sock.recv(zmq.NOBLOCK))
+  except zmq.error.Again:
+    return None

selfdrive/sensord/sensord

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:58e476760c0f87f13b68b75d64860fb5c57b5509b4174d31f466074e0423e7eb
+oid sha256:fc4b862553d2ec9d63740b554997e5069808c531d9d722b4ce2cf83a2f4429ed
-size 918928
+size 935360

selfdrive/test/plant/plant.py

@@ -17,31 +17,15 @@ from selfdrive.car.honda.carstate import get_can_parser
 from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
 
 from selfdrive.car.honda.can_parser import CANParser
+from selfdrive.car.honda.interface import CarInterface
 
 from cereal import car
 from common.dbc import dbc
 acura = dbc(os.path.join(DBC_PATH, "acura_ilx_2016_can.dbc"))
 
-def fake_car_params():
+# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
-  ret = car.CarParams.new_message()
+CP = CarInterface.get_params("ACURA ILX 2016 ACURAWATCH PLUS", {0x201})
 
-  # largely copied from honda
-  ret.carName = "honda"
-  ret.radarName = "nidec"
-  ret.carFingerprint = "ACURA ILX 2016 ACURAWATCH PLUS"
-
-  ret.enableSteer = True
-  ret.enableBrake = True
-  ret.enableGas = True
-  ret.enableCruise = False
-
-  ret.wheelBase = 2.67
-  ret.steerRatio = 15.3
-  ret.slipFactor = 0.0014
-
-  ret.steerKp, ret.steerKi = 12.0, 1.0
-
-  return ret
 
 def car_plant(pos, speed, grade, gas, brake):
   # vehicle parameters
@@ -111,6 +95,7 @@ class Plant(object):
       Plant.logcan = messaging.pub_sock(context, service_list['can'].port)
       Plant.sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
       Plant.model = messaging.pub_sock(context, service_list['model'].port)
+      Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port)
       Plant.live100 = messaging.sub_sock(context, service_list['live100'].port)
       Plant.messaging_initialized = True
 
@@ -158,7 +143,7 @@ class Plant(object):
 
   def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True):
     # dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
-    cp2 = get_can_parser(fake_car_params())
+    cp2 = get_can_parser(CP)
     sgs = cp2._sgs
     msgs = cp2._msgs
     cks_msgs = cp2.msgs_ck
@@ -263,17 +248,22 @@ class Plant(object):
     can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
     Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
 
-    # ******** publish a fake model going straight ********
+    # ******** publish a fake model going straight and fake calibration ********
     if publish_model:
       md = messaging.new_message()
+      cal = messaging.new_message()
       md.init('model')
+      cal.init('liveCalibration')
       md.model.frameId = 0
       for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
         x.points = [0.0]*50
         x.prob = 1.0
         x.std = 1.0
+      cal.liveCalibration.calStatus = 1
+      cal.liveCalibration.calPerc = 100
       # fake values?
       Plant.model.send(md.to_bytes())
+      Plant.cal.send(cal.to_bytes())
 
     # ******** update prevs ********
     self.speed = speed

selfdrive/ui/ui.c

@@ -753,9 +753,11 @@ static void ui_draw_vision(UIState *s) {
         snprintf(speed_str, sizeof(speed_str), "%3d KPH",
                  (int)(scene->v_cruise + 0.5));
       } else {
-        // Convert KPH to MPH.
+        /* Convert KPH to MPH. Using an approximated mph to kph 
+        conversion factor of 1.609 because this is what the Honda
+        hud seems to be using */
         snprintf(speed_str, sizeof(speed_str), "%3d MPH",
-                 (int)(scene->v_cruise * 0.621371 + 0.5));
+                 (int)(scene->v_cruise * 0.621504 + 0.5));
       }
       nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE);
       nvgText(s->vg, 480, 95, speed_str, NULL);

selfdrive/visiond/visiond

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2441337ea200d9750160c1e83a57da0431af2972801e4f2acb26dc110f46be77
+oid sha256:e2dc36a454c0447387826dd0de64ee47e170012df0ec041189ae77d64aa6de3f
-size 16399784
+size 16407976