openpilot v0.6.3 release

old-commit-hash: d5f9caa82d

selfdrive/controls/controlsd.py

@@ -12,7 +12,7 @@ from selfdrive.config import Conversions as CV
 from selfdrive.services import service_list
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car.car_helpers import get_car, get_startup_alert
-from selfdrive.controls.lib.model_parser import CAMERA_OFFSET
+from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import get_events, \
                                                  create_event, \
                                                  EventTypes as ET, \
@@ -21,6 +21,7 @@ from selfdrive.controls.lib.drive_helpers import get_events, \
 from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
@@ -90,11 +91,16 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
     cal_status = sm['liveCalibration'].calStatus
     cal_perc = sm['liveCalibration'].calPerc
 
+  cal_rpy = [0,0,0]
   if cal_status != Calibration.CALIBRATED:
     if cal_status == Calibration.UNCALIBRATED:
       events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
     else:
       events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+  else:
+    rpy = sm['liveCalibration'].rpyCalib
+    if len(rpy) == 3:
+      cal_rpy = rpy
 
   # When the panda and controlsd do not agree on controls_allowed
   # we want to disengage openpilot. However the status from the panda goes through
@@ -112,7 +118,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
 
   # Driver monitoring
   if sm.updated['driverMonitoring']:
-    driver_status.get_pose(sm['driverMonitoring'], params)
+    driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)
 
   if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
     events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
@@ -255,8 +261,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
   # Steering PID loop and lateral MPC
-  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
-                                                              CS.steeringPressed, CP, VM, path_plan)
+  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan)
 
   # Send a "steering required alert" if saturation count has reached the limit
   if LaC.sat_flag and CP.steerLimitAlert:
@@ -310,12 +315,11 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
   ldw_allowed = CS.vEgo > 12.5 and not blinker
 
   if len(list(sm['pathPlan'].rPoly)) == 4:
-    CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
+    CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET) and right_lane_visible)
   if len(list(sm['pathPlan'].lPoly)) == 4:
-    CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
+    CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET) and left_lane_visible)
 
   CC.hudControl.visualAlert = AM.visual_alert
-  CC.hudControl.audibleAlert = AM.audible_alert
 
   if not read_only:
     # send car controls over can
@@ -335,7 +339,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
     "alertStatus": AM.alert_status,
     "alertBlinkingRate": AM.alert_rate,
     "alertType": AM.alert_type,
-    "alertSound": "",  # no EON sounds yet
+    "alertSound": AM.audible_alert,
     "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
     "driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
     "canMonoTimes": list(CS.canMonoTimes),
@@ -372,7 +376,9 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
 
   if CP.lateralTuning.which() == 'pid':
     dat.controlsState.lateralControlState.pidState = lac_log
-  else:
+  elif CP.lateralTuning.which() == 'lqr':
+    dat.controlsState.lateralControlState.lqrState = lac_log
+  elif CP.lateralTuning.which() == 'indi':
     dat.controlsState.lateralControlState.indiState = lac_log
   controlsstate.send(dat.to_bytes())
 
@@ -466,8 +472,10 @@ def controlsd_thread(gctx=None):
 
   if CP.lateralTuning.which() == 'pid':
     LaC = LatControlPID(CP)
-  else:
+  elif CP.lateralTuning.which() == 'indi':
     LaC = LatControlINDI(CP)
+  elif CP.lateralTuning.which() == 'lqr':
+    LaC = LatControlLQR(CP)
 
   driver_status = DriverStatus()
 
@@ -486,6 +494,9 @@ def controlsd_thread(gctx=None):
   sm['pathPlan'].sensorValid = True
   sm['pathPlan'].posenetValid = True
 
+  # detect sound card presence
+  sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
+
   # controlsd is driven by can recv, expected at 100Hz
   rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -518,6 +529,8 @@ def controlsd_thread(gctx=None):
       events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if not CS.canValid:
       events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if not sounds_available:
+      events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
 
     # Only allow engagement with brake pressed when stopped behind another stopped car
     if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:

selfdrive/controls/lib/alertmanager.py

@@ -1,4 +1,4 @@
-from cereal import log
+from cereal import car, log
 from common.realtime import DT_CTRL
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.alerts import ALERTS
@@ -7,7 +7,8 @@ import copy
 
 AlertSize = log.ControlsState.AlertSize
 AlertStatus = log.ControlsState.AlertStatus
-
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 
 class AlertManager(object):
 
@@ -49,8 +50,8 @@ class AlertManager(object):
     self.alert_text_2 = ""
     self.alert_status = AlertStatus.normal
     self.alert_size = AlertSize.none
-    self.visual_alert = "none"
-    self.audible_alert = "none"
+    self.visual_alert = VisualAlert.none
+    self.audible_alert = AudibleAlert.none
     self.alert_rate = 0.
 
     if current_alert:

selfdrive/controls/lib/alerts.py

@@ -298,6 +298,13 @@ ALERTS = [
       AlertStatus.normal, AlertSize.mid,
       Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
 
+  Alert(
+      "soundsUnavailableNoEntry",
+      "openpilot Unavailable",
+      "Speaker not found",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
   Alert(
       "tooDistractedNoEntry",
       "openpilot Unavailable",
@@ -311,84 +318,84 @@ ALERTS = [
       "TAKE CONTROL IMMEDIATELY",
       "System Overheated",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "wrongGear",
       "TAKE CONTROL IMMEDIATELY",
       "Gear not D",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "calibrationInvalid",
       "TAKE CONTROL IMMEDIATELY",
       "Calibration Invalid: Reposition EON and Recalibrate",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "calibrationIncomplete",
       "TAKE CONTROL IMMEDIATELY",
       "Calibration in Progress",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "doorOpen",
       "TAKE CONTROL IMMEDIATELY",
       "Door Open",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "seatbeltNotLatched",
       "TAKE CONTROL IMMEDIATELY",
       "Seatbelt Unlatched",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "espDisabled",
       "TAKE CONTROL IMMEDIATELY",
       "ESP Off",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "lowBattery",
       "TAKE CONTROL IMMEDIATELY",
       "Low Battery",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "commIssue",
       "TAKE CONTROL IMMEDIATELY",
       "Communication Issue between Processes",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "radarCanError",
       "TAKE CONTROL IMMEDIATELY",
       "Radar Error: Restart the Car",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "radarFault",
       "TAKE CONTROL IMMEDIATELY",
       "Radar Error: Restart the Car",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   Alert(
       "posenetInvalid",
       "TAKE CONTROL IMMEDIATELY",
       "Vision Failure: Check Camera View",
       AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
 
   # Cancellation alerts causing immediate disabling
   Alert(
@@ -674,6 +681,13 @@ ALERTS = [
       AlertStatus.normal, AlertSize.mid,
       Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
 
+  Alert(
+      "soundsUnavailablePermanent",
+      "Speaker not found",
+      "Reboot your EON",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
   Alert(
       "vehicleModelInvalid",
       "Vehicle Parameter Identification Failed",

selfdrive/controls/lib/driver_monitor.py

@@ -3,31 +3,40 @@ from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from common.filter_simple import FirstOrderFilter
 
-_AWARENESS_TIME = 180        # 3 minutes limit without user touching steering wheels make the car enter a terminal status
-_AWARENESS_PRE_TIME = 20.    # a first alert is issued 20s before expiration
-_AWARENESS_PROMPT_TIME = 5.  # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 7.
-_DISTRACTED_PRE_TIME = 4.
-_DISTRACTED_PROMPT_TIME = 2.
-# model output refers to center of cropped image, so need to apply the x displacement offset
+_AWARENESS_TIME = 90.        # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_PRE_TIME_TILL_TERMINAL = 20.    # a first alert is issued 20s before expiration
+_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5.  # a second alert is issued 5s before start decelerating the car
+_DISTRACTED_TIME = 10.
+_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
+_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
+
 _FACE_THRESHOLD = 0.4
-_PITCH_WEIGHT = 1.5  # pitch matters a lot more
+_EYE_THRESHOLD = 0.4
+_BLINK_THRESHOLD = 0.2 # 0.225
+_PITCH_WEIGHT = 1.35 # 1.5  # pitch matters a lot more
 _METRIC_THRESHOLD = 0.4
-_PITCH_POS_ALLOWANCE = 0.08   # rad, to not be too sensitive on positive pitch
-_PITCH_NATURAL_OFFSET = 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
-_YAW_NATURAL_OFFSET = 0.08    # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
-_STD_THRESHOLD = 0.1          # above this standard deviation consider the measurement invalid
+_PITCH_POS_ALLOWANCE = 0.04 # 0.08  # rad, to not be too sensitive on positive pitch
+_PITCH_NATURAL_OFFSET = 0.12  # 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
+_YAW_NATURAL_OFFSET = 0.08  # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
 _DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 _VARIANCE_FILTER_TS = 20.     # 0.008Hz
 
 MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
+
+# model output refers to center of cropped image, so need to apply the x displacement offset
 RESIZED_FOCAL = 320.0
 H, W, FULL_W = 320, 160, 426
 
-def head_orientation_from_descriptor(angles_desc, pos_desc):
+class DistractedType(object):
+  NOT_DISTRACTED = 0
+  BAD_POSE = 1
+  BAD_BLINK = 2
+
+def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
   # the output of these angles are in device frame
   # so from driver's perspective, pitch is up and yaw is right
-  # TODO this should be calibrated
+
+  # TODO: calibrate based on position
   pitch_prnet = angles_desc[0]
   yaw_prnet = angles_desc[1]
   roll_prnet = angles_desc[2]
@@ -39,6 +48,11 @@ def head_orientation_from_descriptor(angles_desc, pos_desc):
   roll = roll_prnet
   pitch = pitch_prnet + pitch_focal_angle
   yaw = -yaw_prnet + yaw_focal_angle
+
+  # no calib for roll
+  pitch -= rpy_calib[1]
+  yaw -= rpy_calib[2]
+
   return np.array([roll, pitch, yaw])
 
 
@@ -50,15 +64,21 @@ class _DriverPose():
     self.yaw_offset = 0.
     self.pitch_offset = 0.
 
+class _DriverBlink():
+  def __init__(self):
+    self.left_blink = 0.
+    self.right_blink = 0.
+
+
 
 def _monitor_hysteresis(variance_level, monitor_valid_prev):
   var_thr = 0.63 if monitor_valid_prev else 0.37
   return variance_level < var_thr
 
-
 class DriverStatus():
   def __init__(self, monitor_on=False):
     self.pose = _DriverPose()
+    self.blink = _DriverBlink()
     self.monitor_on = monitor_on
     self.monitor_param_on = monitor_on
     self.monitor_valid = True   # variance needs to be low
@@ -70,52 +90,59 @@ class DriverStatus():
     self.ts_last_check = 0.
     self.face_detected = False
     self.terminal_alert_cnt = 0
-    self._set_timers()
+    self.step_change = 0.
+    self._set_timers(self.monitor_on)
 
   def _reset_filters(self):
     self.driver_distraction_filter.x = 0.
     self.variance_filter.x = 0.
     self.monitor_valid = True
 
-  def _set_timers(self):
-    if self.monitor_on:
-      self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME
-      self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME
+  def _set_timers(self, active_monitoring):
+    if active_monitoring:
+      # when falling back from passive mode to active mode, reset awareness to avoid false alert
+      if self.step_change == DT_CTRL / _AWARENESS_TIME:
+        self.awareness = 1.
+      self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
+      self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
       self.step_change = DT_CTRL / _DISTRACTED_TIME
     else:
-      self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME
-      self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME
+      self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
+      self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
       self.step_change = DT_CTRL / _AWARENESS_TIME
 
-  def _is_driver_distracted(self, pose):
-    # to be tuned and to learn the driver's normal pose
+  def _is_driver_distracted(self, pose, blink):
+    # TODO: natural pose calib of each driver
     pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
     yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
     # add positive pitch allowance
     if pitch_error > 0.:
       pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
     pitch_error *= _PITCH_WEIGHT
-    metric = np.sqrt(yaw_error**2 + pitch_error**2)
-    # TODO: do something with the eye states and combine them with head pose
-    return 1 if metric > _METRIC_THRESHOLD else 0
+    pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
+
+    if pose_metric > _METRIC_THRESHOLD:
+      return DistractedType.BAD_POSE 
+    elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
+      return DistractedType.BAD_BLINK
+    else:
+      return DistractedType.NOT_DISTRACTED
 
 
-  def get_pose(self, driver_monitoring, params):
+  def get_pose(self, driver_monitoring, params, cal_rpy):
     if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
       return
 
-    self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition)
+    self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
+    self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
+    self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
     self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
 
-    self.driver_distracted = self._is_driver_distracted(self.pose)
+    self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
     # first order filters
     self.driver_distraction_filter.update(self.driver_distracted)
-    self.variance_high = False #driver_monitoring.std > _STD_THRESHOLD
-
-    self.variance_filter.update(self.variance_high)
 
     monitor_param_on_prev = self.monitor_param_on
-    monitor_valid_prev = self.monitor_valid
 
     # don't check for param too often as it's a kernel call
     ts = sec_since_boot()
@@ -123,24 +150,26 @@ class DriverStatus():
       self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
       self.ts_last_check = ts
 
-    self.monitor_valid = _monitor_hysteresis(self.variance_filter.x, monitor_valid_prev)
     self.monitor_on = self.monitor_valid and self.monitor_param_on
     if monitor_param_on_prev != self.monitor_param_on:
       self._reset_filters()
-    self._set_timers()
+    self._set_timers(self.monitor_on and self.face_detected)
 
 
   def update(self, events, driver_engaged, ctrl_active, standstill):
+    if driver_engaged:
+      self.awareness = 1.
+      return events
 
     driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
     awareness_prev = self.awareness
 
-    if (driver_engaged and self.awareness > 0.) or not ctrl_active:
+    if (driver_engaged and self.awareness > 0) or not ctrl_active:
       # always reset if driver is in control (unless we are in red alert state) or op isn't active
-      self.awareness = 1.
+      self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)
 
-    # only update if face is detected, driver is distracted and distraction filter is high
-    if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
+    # should always be counting if distracted unless at standstill and reaching orange
+    if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
        not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
       self.awareness = max(self.awareness - self.step_change, -0.1)
 

selfdrive/controls/lib/lane_planner.py

@@ -0,0 +1,74 @@
+from common.numpy_fast import interp
+import numpy as np
+from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv
+
+CAMERA_OFFSET = 0.06  # m from center car to camera
+
+
+def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
+  # This will improve behaviour when lanes suddenly widen
+  lane_width = min(4.0, lane_width)
+  l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
+  r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
+
+  path_from_left_lane = l_poly.copy()
+  path_from_left_lane[3] -= lane_width / 2.0
+  path_from_right_lane = r_poly.copy()
+  path_from_right_lane[3] += lane_width / 2.0
+
+  lr_prob = l_prob + r_prob - l_prob * r_prob
+
+  d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
+  return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
+
+
+class LanePlanner(object):
+  def __init__(self):
+    self.l_poly = [0., 0., 0., 0.]
+    self.r_poly = [0., 0., 0., 0.]
+    self.p_poly = [0., 0., 0., 0.]
+    self.d_poly = [0., 0., 0., 0.]
+
+    self.lane_width_estimate = 3.7
+    self.lane_width_certainty = 1.0
+    self.lane_width = 3.7
+
+    self.l_prob = 0.
+    self.r_prob = 0.
+    self.lr_prob = 0.
+
+    self._path_pinv = compute_path_pinv()
+    self.x_points = np.arange(50)
+
+  def parse_model(self, md):
+    if len(md.leftLane.poly):
+      self.l_poly = np.array(md.leftLane.poly)
+      self.r_poly = np.array(md.rightLane.poly)
+      self.p_poly = np.array(md.path.poly)
+    else:
+      self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
+      self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
+      self.p_poly = model_polyfit(md.path.points, self._path_pinv)  # predicted path
+    self.l_prob = md.leftLane.prob  # left line prob
+    self.r_prob = md.rightLane.prob  # right line prob
+
+  def update_lane(self, v_ego):
+    # only offset left and right lane lines; offsetting p_poly does not make sense
+    self.l_poly[3] += CAMERA_OFFSET
+    self.r_poly[3] += CAMERA_OFFSET
+
+    self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob
+
+    # Find current lanewidth
+    self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
+    current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
+    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
+    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
+    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
+                      (1 - self.lane_width_certainty) * speed_lane_width
+
+    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
+
+  def update(self, v_ego, md):
+    self.parse_model(md)
+    self.update_lane(v_ego)

selfdrive/controls/lib/latcontrol_helpers.py

@@ -60,30 +60,3 @@ def compute_path_pinv(l=50):
 
 def model_polyfit(points, path_pinv):
   return np.dot(path_pinv, [float(x) for x in 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/latcontrol_indi.py

@@ -47,7 +47,7 @@ class LatControlINDI(object):
     self.output_steer = 0.
     self.counter = 0
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
     # Update Kalman filter
     y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
     self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)

selfdrive/controls/lib/latcontrol_lqr.py

@@ -0,0 +1,72 @@
+import numpy as np
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from common.numpy_fast import clip
+from cereal import log
+
+
+class LatControlLQR(object):
+  def __init__(self, CP, rate=100):
+    self.sat_flag = False
+    self.scale = CP.lateralTuning.lqr.scale
+    self.ki = CP.lateralTuning.lqr.ki
+
+
+    self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2))
+    self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1))
+    self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2))
+    self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2))
+    self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1))
+    self.dc_gain = CP.lateralTuning.lqr.dcGain
+
+    self.x_hat = np.array([[0], [0]])
+    self.i_unwind_rate = 0.3 / rate
+    self.i_rate = 1.0 / rate
+
+    self.reset()
+
+  def reset(self):
+    self.i_lqr = 0.0
+    self.output_steer = 0.0
+
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
+    lqr_log = log.ControlsState.LateralLQRState.new_message()
+
+    torque_scale = (0.45 + v_ego / 60.0)**2  # Scale actuator model with speed
+
+    # Subtract offset. Zero angle should correspond to zero torque
+    self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
+    angle_steers -= path_plan.angleOffset
+
+    # Update Kalman filter
+    angle_steers_k = float(self.C.dot(self.x_hat))
+    e = angle_steers - angle_steers_k
+    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
+
+    if v_ego < 0.3 or not active:
+      lqr_log.active = False
+      self.reset()
+    else:
+      lqr_log.active = True
+
+      # LQR
+      u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+
+      # Integrator
+      if steer_override:
+        self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
+      else:
+        self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k)
+
+      lqr_output = torque_scale * u_lqr / self.scale
+      self.i_lqr = clip(self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
+
+      self.output_steer = lqr_output + self.i_lqr
+
+      # Clip output
+      steers_max = get_steer_max(CP, v_ego)
+      self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+
+    lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
+    lqr_log.i = self.i_lqr
+    lqr_log.output = self.output_steer
+    return self.output_steer, float(self.angle_steers_des), lqr_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -14,7 +14,7 @@ class LatControlPID(object):
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steerAngle = float(angle_steers)
     pid_log.steerRate = float(angle_steers_rate)

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -23,8 +23,8 @@ int main( )
   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 d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3;
+  OnlineData l_prob, r_prob;
   OnlineData lane_width;
 
   Control t;
@@ -39,26 +39,13 @@ int main( )
 
   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 poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_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);
-
-  // given the lane width estimate, this is where we estimate the path given lane lines
-  auto path_from_left_lane = poly_l - lane_width/2.0;
-  auto path_from_right_lane = poly_r + lane_width/2.0;
-
-  // if the lanes are visible, drive in the center, otherwise follow the path
-  auto path = lr_prob * (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (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;
+  auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2);
 
   // When the lane is not visible, use an estimate of its position
-  auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (path + lane_width/2.0);
-  auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (path - lane_width/2.0);
+  auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0);
+  auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0);
 
   auto c_left_lane = exp(-(weighted_left_lane - yy));
   auto c_right_lane = exp(weighted_right_lane - yy);
@@ -67,12 +54,12 @@ int main( )
   Function h;
 
   // Distance errors
-  h << path - yy;
+  h << poly_d - yy;
   h << lr_prob * c_left_lane;
   h << lr_prob * c_right_lane;
 
   // Heading error
-  h << (v_ref + 1.0 ) * (angle - psi);
+  h << (v_ref + 1.0 ) * (angle_d - psi);
 
   // Angular rate error
   h << (v_ref + 1.0 ) * t;
@@ -88,12 +75,12 @@ int main( )
   Function hN;
 
   // Distance errors
-  hN << path - yy;
+  hN << poly_d - yy;
   hN << l_prob * c_left_lane;
   hN << r_prob * c_right_lane;
 
   // Heading errors
-  hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
+  hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi);
 
   BMatrix QN(4,4); QN.setAll(true);
   // QN(0,0) = 1.0;
@@ -125,7 +112,7 @@ int main( )
   ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
   // more than absolute max steer angle
   ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
-  ocp.setNOD(18);
+  ocp.setNOD(17);
 
   OCPexport mpc(ocp);
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );

selfdrive/controls/lib/lateral_mpc/lateral_mpc.c

@@ -65,8 +65,8 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
 }
 
 int 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){
+             double l_poly[4], double r_poly[4], double d_poly[4],
+             double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){
 
   int    i;
 
@@ -84,16 +84,15 @@ int run_mpc(state_t * x0, log_t * solution,
     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+10] = d_poly[0];
+    acadoVariables.od[i+11] = d_poly[1];
+    acadoVariables.od[i+12] = d_poly[2];
+    acadoVariables.od[i+13] = d_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.od[i+16] = lane_width;
 
   }
 

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

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f1d93e7b412f1573e2b6b22b11587bbed076d59a1c1c6d8f6d69eddc3998518c
+oid sha256:b175a66de26ad7bd788086a2d6a7ef6243eb2a0aac1ddcff39b00554a8960d97
 size 8823

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

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:5cf12c96cffb69f1e659a20760c9ad3ab74ecce3d88aba1e50af359ab14c88da
-size 18662
+oid sha256:5848ec6e7975d6fee93187e0f41d6cba57cc0ebee6edf63ebddf3c7ad6f8f52c
+size 18622

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

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:269cf8ba0c80202e59352e7474d5aa768fa1ffc8268e051496d28629fa8cb144
-size 400285
+oid sha256:a2c030dd09379475b0247609d8a02f161f3e468e85480740d4abcf9c80868de0
+size 390405

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -24,8 +24,8 @@ typedef struct {
 
 void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
 int 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);
+             double l_poly[4], double r_poly[4], double d_poly[4],
+             double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width);
 """)
 
 libmpc = ffi.dlopen(libmpc_fn)

selfdrive/controls/lib/model_parser.py

@@ -1,66 +0,0 @@
-from common.numpy_fast import interp
-import numpy as np
-from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
-
-CAMERA_OFFSET = 0.06  # m from center car to camera
-
-
-class ModelParser(object):
-  def __init__(self):
-    self.d_poly = [0., 0., 0., 0.]
-    self.c_poly = [0., 0., 0., 0.]
-    self.c_prob = 0.
-    self.last_model = 0.
-    self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
-    self._path_pinv = compute_path_pinv()
-
-    self.lane_width_estimate = 3.7
-    self.lane_width_certainty = 1.0
-    self.lane_width = 3.7
-    self.l_prob = 0.
-    self.r_prob = 0.
-    self.x_points = np.arange(50)
-
-  def update(self, v_ego, md):
-    if len(md.leftLane.poly):
-      l_poly = np.array(md.leftLane.poly)
-      r_poly = np.array(md.rightLane.poly)
-      p_poly = np.array(md.path.poly)
-    else:
-      l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
-      r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
-      p_poly = model_polyfit(md.path.points, self._path_pinv)  # predicted path
-
-    # only offset left and right lane lines; offsetting p_poly does not make sense
-    l_poly[3] += CAMERA_OFFSET
-    r_poly[3] += CAMERA_OFFSET
-
-    p_prob = 1.  # model does not tell this probability yet, so set to 1 for now
-    l_prob = md.leftLane.prob  # left line prob
-    r_prob = md.rightLane.prob  # right line prob
-
-    # Find current lanewidth
-    lr_prob = l_prob * r_prob
-    self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty)
-    current_lane_width = abs(l_poly[3] - r_poly[3])
-    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
-    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
-    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
-                      (1 - self.lane_width_certainty) * speed_lane_width
-
-    self.lead_dist = md.lead.dist
-    self.lead_prob = md.lead.prob
-    self.lead_var = md.lead.std**2
-
-    # compute target path
-    self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
-      l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
-
-    self.r_poly = r_poly
-    self.r_prob = r_prob
-
-    self.l_poly = l_poly
-    self.l_prob = l_prob
-
-    self.p_poly = p_poly
-    self.p_prob = p_prob

selfdrive/controls/lib/pathplanner.py

@@ -2,12 +2,13 @@ import os
 import math
 import numpy as np
 
+# from common.numpy_fast import clip
 from common.realtime import sec_since_boot
 from selfdrive.services import service_list
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
-from selfdrive.controls.lib.model_parser import ModelParser
+from selfdrive.controls.lib.lane_planner import LanePlanner
 import selfdrive.messaging as messaging
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
@@ -21,10 +22,7 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
 
 class PathPlanner(object):
   def __init__(self, CP):
-    self.MP = ModelParser()
-
-    self.l_poly = [0., 0., 0., 0.]
-    self.r_poly = [0., 0., 0., 0.]
+    self.LP = LanePlanner()
 
     self.last_cloudlog_t = 0
 
@@ -33,6 +31,7 @@ class PathPlanner(object):
 
     self.setup_mpc(CP.steerRateCost)
     self.solution_invalid_cnt = 0
+    self.path_offset_i = 0.0
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -50,10 +49,6 @@ class PathPlanner(object):
     self.angle_steers_des_prev = 0.0
     self.angle_steers_des_time = 0.0
 
-    self.l_poly = libmpc_py.ffi.new("double[4]")
-    self.r_poly = libmpc_py.ffi.new("double[4]")
-    self.p_poly = libmpc_py.ffi.new("double[4]")
-
   def update(self, sm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
@@ -62,23 +57,28 @@ class PathPlanner(object):
     angle_offset_average = sm['liveParameters'].angleOffsetAverage
     angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
 
-    self.MP.update(v_ego, sm['model'])
+    self.LP.update(v_ego, sm['model'])
 
     # Run MPC
     self.angle_steers_des_prev = self.angle_steers_des_mpc
     VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
     curvature_factor = VM.curvature_factor(v_ego)
-    self.l_poly = list(self.MP.l_poly)
-    self.r_poly = list(self.MP.r_poly)
-    self.p_poly = list(self.MP.p_poly)
+
+    # TODO: Check for active, override, and saturation
+    # if active:
+    #   self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
+    #   self.path_offset_i = clip(self.path_offset_i, -0.5,  0.5)
+    #   self.LP.d_poly[3] += self.path_offset_i
+    # else:
+    #   self.path_offset_i = 0.0
 
     # account for actuation delay
     self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        self.l_poly, self.r_poly, self.p_poly,
-                        self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width)
+                        list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
+                        self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)
 
     # reset to current steer angle if not active or overriding
     if active:
@@ -112,17 +112,16 @@ class PathPlanner(object):
     plan_send = messaging.new_message()
     plan_send.init('pathPlan')
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
-    plan_send.pathPlan.laneWidth = float(self.MP.lane_width)
-    plan_send.pathPlan.dPoly = [float(x) for x in self.MP.d_poly]
-    plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly]
-    plan_send.pathPlan.cProb = float(self.MP.c_prob)
-    plan_send.pathPlan.lPoly = [float(x) for x in self.l_poly]
-    plan_send.pathPlan.lProb = float(self.MP.l_prob)
-    plan_send.pathPlan.rPoly = [float(x) for x in self.r_poly]
-    plan_send.pathPlan.rProb = float(self.MP.r_prob)
+    plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
+    plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
+    plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
+    plan_send.pathPlan.lProb = float(self.LP.l_prob)
+    plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
+    plan_send.pathPlan.rProb = float(self.LP.r_prob)
+
     plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
     plan_send.pathPlan.rateSteers = float(rate_desired)
-    plan_send.pathPlan.angleOffset = float(angle_offset_average)
+    plan_send.pathPlan.angleOffset = float(self.path_offset_i)
     plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
     plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
     plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)

selfdrive/controls/lib/planner.py

@@ -3,7 +3,6 @@ import math
 import numpy as np
 from common.params import Params
 from common.numpy_fast import interp
-from common.kalman.simple_kalman import KF1D
 
 import selfdrive.messaging as messaging
 from cereal import car
@@ -95,9 +94,7 @@ class Planner(object):
     self.longitudinalPlanSource = 'cruise'
     self.fcw_checker = FCWChecker()
     self.fcw_enabled = fcw_enabled
-
-    self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
-    self.model_v_kf_ready = False
+    self.path_x = np.arange(192)
 
     self.params = Params()
 
@@ -112,7 +109,6 @@ class Planner(object):
       slowest = min(solutions, key=solutions.get)
 
       self.longitudinalPlanSource = slowest
-
       # Choose lowest of MPC and cruise
       if slowest == 'mpc1':
         self.v_acc = self.mpc1.v_mpc
@@ -145,15 +141,21 @@ class Planner(object):
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 
-    if not self.model_v_kf_ready:
-      self.model_v_kf.x = [[v_ego],[0.0]]
-      self.model_v_kf_ready = True
+    if len(sm['model'].path.poly):
+      path = list(sm['model'].path.poly)
 
-    if len(sm['model'].speed):
-      self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
+      # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
+      # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
+      # k = y'' / (1 + y'^2)^1.5
+      y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
+      y_pp = 6 * path[0] * self.path_x + 2 * path[1]
+      curv = y_pp / (1. + y_p**2)**1.5
 
-    if self.params.get("LimitSetSpeedNeural") == "1":
-      model_speed = self.model_v_kf.x[0][0]
+      a_y_max = 2.975 - v_ego * 0.0375  # ~1.85 @ 75mph, ~2.6 @ 25mph
+      v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
+      model_speed = np.min(v_curvature)
+      # print(model_speed * CV.MS_TO_MPH, model_speed)
+      model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
     else:
       model_speed = MAX_SPEED
 
@@ -174,11 +176,9 @@ class Planner(object):
                                                     jerk_limits[1], jerk_limits[0],
                                                     LON_MPC_STEP)
 
-      # accel and jerk up limits are higher here to make model not limiting accel
-      # mainly done to prevent flickering of slowdown icon
       self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
                                                     model_speed,
-                                                    2*accel_limits[1], 3*accel_limits[0],
+                                                    2*accel_limits[1], accel_limits[0],
                                                     2*jerk_limits[1], jerk_limits[0],
                                                     LON_MPC_STEP)
 
@@ -234,13 +234,13 @@ class Planner(object):
     plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
 
     # longitudal plan
-    plan_send.plan.vCruise = self.v_cruise
-    plan_send.plan.aCruise = self.a_cruise
-    plan_send.plan.vStart = self.v_acc_start
-    plan_send.plan.aStart = self.a_acc_start
-    plan_send.plan.vTarget = self.v_acc
-    plan_send.plan.aTarget = self.a_acc
-    plan_send.plan.vTargetFuture = self.v_acc_future
+    plan_send.plan.vCruise = float(self.v_cruise)
+    plan_send.plan.aCruise = float(self.a_cruise)
+    plan_send.plan.vStart = float(self.v_acc_start)
+    plan_send.plan.aStart = float(self.a_acc_start)
+    plan_send.plan.vTarget = float(self.v_acc)
+    plan_send.plan.aTarget = float(self.a_acc)
+    plan_send.plan.vTargetFuture = float(self.v_acc_future)
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 

selfdrive/controls/lib/radar_helpers.py

@@ -25,14 +25,9 @@ _VLEAD_K = [[0.1988689], [0.28555364]]
 class Track(object):
   def __init__(self):
     self.ekf = None
-    self.initted = False
+    self.cnt = 0
 
   def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
-    if self.initted:
-      # pylint: disable=access-member-before-definition
-      self.vLeadPrev = self.vLead
-      self.vRelPrev = self.vRel
-
     # relative values, copy
     self.dRel = d_rel   # LONG_DIST
     self.yRel = y_rel   # -LAT_DIST
@@ -42,17 +37,12 @@ class Track(object):
     # computed velocity and accelerations
     self.vLead = self.vRel + v_ego_t_aligned
 
-    if not self.initted:
-      self.initted = True
-      self.aLeadTau = _LEAD_ACCEL_TAU
-      self.cnt = 1
-      self.vision_cnt = 0
-      self.vision = False
+    if self.cnt == 0:
       self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
     else:
       self.kf.update(self.vLead)
 
-      self.cnt += 1
+    self.cnt += 1
 
     self.vLeadK = float(self.kf.x[SPEED][0])
     self.aLeadK = float(self.kf.x[ACCEL][0])
@@ -67,6 +57,10 @@ class Track(object):
     # Weigh y higher since radar is inaccurate in this dimension
     return [self.dRel, self.yRel*2, self.vRel]
 
+  def reset_a_lead(self, aLeadK, aLeadTau):
+    self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
+    self.aLeadK = aLeadK
+    self.aLeadTau = aLeadTau
 
 def mean(l):
   return sum(l) / len(l)
@@ -115,11 +109,17 @@ class Cluster(object):
 
   @property
   def aLeadK(self):
-    return mean([t.aLeadK for t in self.tracks])
+    if all(t.cnt <= 1 for t in self.tracks):
+      return 0.
+    else:
+      return mean([t.aLeadK for t in self.tracks if t.cnt > 1])
 
   @property
   def aLeadTau(self):
-    return mean([t.aLeadTau for t in self.tracks])
+    if all(t.cnt <= 1 for t in self.tracks):
+      return _LEAD_ACCEL_TAU
+    else:
+      return mean([t.aLeadTau for t in self.tracks if t.cnt > 1])
 
   @property
   def measured(self):

selfdrive/controls/radard.py

@@ -143,15 +143,24 @@ class RadarD(object):
           clusters[cluster_i] = Cluster()
         clusters[cluster_i].add(self.tracks[idens[idx]])
     elif len(track_pts) == 1:
+      # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1
+      cluster_idxs = [0]
       clusters = [Cluster()]
       clusters[0].add(self.tracks[idens[0]])
     else:
       clusters = []
 
+    # if a new point, reset accel to the rest of the cluster
+    for idx in xrange(len(track_pts)):
+      if self.tracks[idens[idx]].cnt <= 1:
+        aLeadK = clusters[cluster_idxs[idx]].aLeadK
+        aLeadTau = clusters[cluster_idxs[idx]].aLeadTau
+        self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau)
+
     # *** publish radarState ***
     dat = messaging.new_message()
     dat.init('radarState')
-    dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
+    dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model'])
     dat.radarState.mdMonoTime = self.last_md_ts
     dat.radarState.canMonoTimes = list(rr.canMonoTimes)
     dat.radarState.radarErrors = list(rr.errors)

selfdrive/controls/tests/test_lateral_mpc.py

@@ -1,9 +1,9 @@
 import unittest
-import copy
 import numpy as np
 from selfdrive.car.honda.interface import CarInterface
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.lane_planner import calc_d_poly
 
 
 def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
@@ -16,13 +16,17 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
   mpc_solution = libmpc_py.ffi.new("log_t *")
 
-  p_l = copy.copy(poly_l)
+  p_l = poly_l.copy()
   p_l[3] += poly_shift
-  p_r = copy.copy(poly_r)
+
+  p_r = poly_r.copy()
   p_r[3] += poly_shift
-  p_p = copy.copy(poly_p)
+
+  p_p = poly_p.copy()
   p_p[3] += poly_shift
 
+  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width)
+
   CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
   VM = VehicleModel(CP)
 
@@ -31,7 +35,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
   l_poly = libmpc_py.ffi.new("double[4]", map(float, p_l))
   r_poly = libmpc_py.ffi.new("double[4]", map(float, p_r))
-  p_poly = libmpc_py.ffi.new("double[4]", map(float, p_p))
+  d_poly = libmpc_py.ffi.new("double[4]", map(float, d_poly))
 
   cur_state = libmpc_py.ffi.new("state_t *")
   cur_state[0].x = x_init
@@ -41,7 +45,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
   # converge in no more than 20 iterations
   for _ in range(20):
-    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, p_prob,
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob,
                    curvature_factor, v_ref, lane_width)
 
   return mpc_solution
@@ -119,3 +123,7 @@ class TestLateralMpc(unittest.TestCase):
     sol = run_mpc(y_init=y_init)
     for y in list(sol[0].y):
       self.assertGreaterEqual(y_init, abs(y))
+
+
+if __name__ == "__main__":
+  unittest.main()