selfdrive/controls

selfdrive/controls/.gitignore

@@ -0,0 +1,2 @@
+calibration_param
+traces

selfdrive/controls/controlsd.py

@@ -0,0 +1,626 @@
+#!/usr/bin/env python3
+import os
+import gc
+import capnp
+from cereal import car, log
+from common.numpy_fast import clip
+from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
+from common.profiler import Profiler
+from common.params import Params, put_nonblocking
+import cereal.messaging as messaging
+from selfdrive.config import Conversions as CV
+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.lane_planner import CAMERA_OFFSET
+from selfdrive.controls.lib.drive_helpers import get_events, \
+                                                 create_event, \
+                                                 EventTypes as ET, \
+                                                 update_v_cruise, \
+                                                 initialize_v_cruise
+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, MAX_TERMINAL_DURATION
+from selfdrive.controls.lib.planner import LON_MPC_STEP
+from selfdrive.controls.lib.gps_helpers import is_rhd_region
+from selfdrive.locationd.calibration_helpers import Calibration, Filter
+
+LANE_DEPARTURE_THRESHOLD = 0.1
+
+ThermalStatus = log.ThermalData.ThermalStatus
+State = log.ControlsState.OpenpilotState
+HwType = log.HealthData.HwType
+
+LaneChangeState = log.PathPlan.LaneChangeState
+LaneChangeDirection = log.PathPlan.LaneChangeDirection
+
+
+def add_lane_change_event(events, path_plan):
+  if path_plan.laneChangeState == LaneChangeState.preLaneChange:
+    if path_plan.laneChangeDirection == LaneChangeDirection.left:
+      events.append(create_event('preLaneChangeLeft', [ET.WARNING]))
+    else:
+      events.append(create_event('preLaneChangeRight', [ET.WARNING]))
+  elif path_plan.laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]:
+      events.append(create_event('laneChange', [ET.WARNING]))
+
+
+def isActive(state):
+  """Check if the actuators are enabled"""
+  return state in [State.enabled, State.softDisabling]
+
+
+def isEnabled(state):
+  """Check if openpilot is engaged"""
+  return (isActive(state) or state == State.preEnabled)
+
+def events_to_bytes(events):
+  # optimization when comparing capnp structs: str() or tree traverse are much slower
+  ret = []
+  for e in events:
+    if isinstance(e, capnp.lib.capnp._DynamicStructReader):
+      e = e.as_builder()
+    ret.append(e.to_bytes())
+  return ret
+
+
+def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params):
+  """Receive data from sockets and create events for battery, temperature and disk space"""
+
+  # Update carstate from CAN and create events
+  can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+  CS = CI.update(CC, can_strs)
+
+  sm.update(0)
+
+  events = list(CS.events)
+  add_lane_change_event(events, sm['pathPlan'])
+  enabled = isEnabled(state)
+
+  # Check for CAN timeout
+  if not can_strs:
+    can_error_counter += 1
+    events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+
+  overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
+  free_space = sm['thermal'].freeSpace < 0.07  # under 7% of space free no enable allowed
+  low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError  # at zero percent battery, while discharging, OP should not allowed
+  mem_low = sm['thermal'].memUsedPercent > 90
+
+  # Create events for battery, temperature and disk space
+  if low_battery:
+    events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+  if overtemp:
+    events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+  if free_space:
+    events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
+  if mem_low:
+    events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
+
+  if CS.stockAeb:
+    events.append(create_event('stockAeb', []))
+
+  # GPS coords RHD parsing, once every restart
+  if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
+    is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
+    driver_status.is_rhd_region = is_rhd
+    driver_status.is_rhd_region_checked = True
+    put_nonblocking("IsRHD", "1" if is_rhd else "0")
+
+  # Handle calibration
+  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
+  # another socket other than the CAN messages and one can arrive earlier than the other.
+  # Therefore we allow a mismatch for two samples, then we trigger the disengagement.
+  if not enabled:
+    mismatch_counter = 0
+
+  controls_allowed = sm['health'].controlsAllowed
+  if not controls_allowed and enabled:
+    mismatch_counter += 1
+  if mismatch_counter >= 200:
+    events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
+
+  # Driver monitoring
+  if sm.updated['model']:
+    driver_status.set_policy(sm['model'])
+
+  if sm.updated['driverMonitoring']:
+    driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled)
+
+  if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
+    events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
+
+  return CS, events, cal_perc, mismatch_counter, can_error_counter
+
+
+def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
+  """Compute conditional state transitions and execute actions on state transitions"""
+  enabled = isEnabled(state)
+
+  v_cruise_kph_last = v_cruise_kph
+
+  # if stock cruise is completely disabled, then we can use our own set speed logic
+  if not CP.enableCruise:
+    v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
+  elif CP.enableCruise and CS.cruiseState.enabled:
+    v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+
+  # decrease the soft disable timer at every step, as it's reset on
+  # entrance in SOFT_DISABLING state
+  soft_disable_timer = max(0, soft_disable_timer - 1)
+
+  # DISABLED
+  if state == State.disabled:
+    if get_events(events, [ET.ENABLE]):
+      if get_events(events, [ET.NO_ENTRY]):
+        for e in get_events(events, [ET.NO_ENTRY]):
+          AM.add(frame, str(e) + "NoEntry", enabled)
+
+      else:
+        if get_events(events, [ET.PRE_ENABLE]):
+          state = State.preEnabled
+        else:
+          state = State.enabled
+        AM.add(frame, "enable", enabled)
+        v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last)
+
+  # ENABLED
+  elif state == State.enabled:
+    if get_events(events, [ET.USER_DISABLE]):
+      state = State.disabled
+      AM.add(frame, "disable", enabled)
+
+    elif get_events(events, [ET.IMMEDIATE_DISABLE]):
+      state = State.disabled
+      for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
+        AM.add(frame, e, enabled)
+
+    elif get_events(events, [ET.SOFT_DISABLE]):
+      state = State.softDisabling
+      soft_disable_timer = 300   # 3s
+      for e in get_events(events, [ET.SOFT_DISABLE]):
+        AM.add(frame, e, enabled)
+
+  # SOFT DISABLING
+  elif state == State.softDisabling:
+    if get_events(events, [ET.USER_DISABLE]):
+      state = State.disabled
+      AM.add(frame, "disable", enabled)
+
+    elif get_events(events, [ET.IMMEDIATE_DISABLE]):
+      state = State.disabled
+      for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
+        AM.add(frame, e, enabled)
+
+    elif not get_events(events, [ET.SOFT_DISABLE]):
+      # no more soft disabling condition, so go back to ENABLED
+      state = State.enabled
+
+    elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
+      for e in get_events(events, [ET.SOFT_DISABLE]):
+        AM.add(frame, e, enabled)
+
+    elif soft_disable_timer <= 0:
+      state = State.disabled
+
+  # PRE ENABLING
+  elif state == State.preEnabled:
+    if get_events(events, [ET.USER_DISABLE]):
+      state = State.disabled
+      AM.add(frame, "disable", enabled)
+
+    elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
+      state = State.disabled
+      for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
+        AM.add(frame, e, enabled)
+
+    elif not get_events(events, [ET.PRE_ENABLE]):
+      state = State.enabled
+
+  return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
+
+
+def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
+                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame):
+  """Given the state, this function returns an actuators packet"""
+
+  actuators = car.CarControl.Actuators.new_message()
+
+  enabled = isEnabled(state)
+  active = isActive(state)
+
+  # check if user has interacted with the car
+  driver_engaged = len(CS.buttonEvents) > 0 or \
+                   v_cruise_kph != v_cruise_kph_last or \
+                   CS.steeringPressed
+
+  if CS.leftBlinker or CS.rightBlinker:
+    last_blinker_frame = frame
+
+  # add eventual driver distracted events
+  events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill)
+
+  if plan.fcw:
+    # send FCW alert if triggered by planner
+    AM.add(frame, "fcw", enabled)
+
+  elif CS.stockFcw:
+    # send a silent alert when stock fcw triggers, since the car is already beeping
+    AM.add(frame, "fcwStock", enabled)
+
+  # State specific actions
+
+  if state in [State.preEnabled, State.disabled]:
+    LaC.reset()
+    LoC.reset(v_pid=CS.vEgo)
+
+  elif state in [State.enabled, State.softDisabling]:
+    # parse warnings from car specific interface
+    for e in get_events(events, [ET.WARNING]):
+      extra_text = ""
+      if e == "belowSteerSpeed":
+        if is_metric:
+          extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
+        else:
+          extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
+      AM.add(frame, e, enabled, extra_text_2=extra_text)
+
+  plan_age = DT_CTRL * (frame - rcv_frame['plan'])
+  dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL  # no greater than dt mpc + dt, to prevent too high extraps
+
+  a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
+  v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
+
+  # Gas/Brake PID loop
+  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.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan)
+
+  # Send a "steering required alert" if saturation count has reached the limit
+  if lac_log.saturated and not CS.steeringPressed:
+    # Check if we deviated from the path
+    left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
+    right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
+
+    if left_deviation or right_deviation:
+      AM.add(frame, "steerSaturated", enabled)
+
+  # Parse permanent warnings to display constantly
+  for e in get_events(events, [ET.PERMANENT]):
+    extra_text_1, extra_text_2 = "", ""
+    if e == "calibrationIncomplete":
+      extra_text_1 = str(cal_perc) + "%"
+      if is_metric:
+        extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
+      else:
+        extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
+    AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
+
+  return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame
+
+
+def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
+              driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
+              last_blinker_frame, is_ldw_enabled, can_error_counter):
+  """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
+
+  CC = car.CarControl.new_message()
+  CC.enabled = isEnabled(state)
+  CC.actuators = actuators
+
+  CC.cruiseControl.override = True
+  CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
+
+  # Some override values for Honda
+  brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))  # brake discount removes a sharp nonlinearity
+  CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
+  CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget)
+
+  CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
+  CC.hudControl.speedVisible = isEnabled(state)
+  CC.hudControl.lanesVisible = isEnabled(state)
+  CC.hudControl.leadVisible = sm['plan'].hasLead
+
+  right_lane_visible = sm['pathPlan'].rProb > 0.5
+  left_lane_visible = sm['pathPlan'].lProb > 0.5
+  CC.hudControl.rightLaneVisible = bool(right_lane_visible)
+  CC.hudControl.leftLaneVisible = bool(left_lane_visible)
+
+  recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
+  ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state)
+
+  md = sm['model']
+  if len(md.meta.desirePrediction):
+    l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
+    r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
+
+    l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
+    r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
+
+    if ldw_allowed:
+      CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
+      CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
+
+  if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
+    AM.add(sm.frame, 'ldwPermanent', False)
+    events.append(create_event('ldw', [ET.PERMANENT]))
+
+  AM.process_alerts(sm.frame)
+  CC.hudControl.visualAlert = AM.visual_alert
+
+  if not read_only:
+    # send car controls over can
+    can_sends = CI.apply(CC)
+    pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+
+  force_decel = driver_status.awareness < 0.
+
+  # controlsState
+  dat = messaging.new_message()
+  dat.init('controlsState')
+  dat.valid = CS.canValid
+  dat.controlsState = {
+    "alertText1": AM.alert_text_1,
+    "alertText2": AM.alert_text_2,
+    "alertSize": AM.alert_size,
+    "alertStatus": AM.alert_status,
+    "alertBlinkingRate": AM.alert_rate,
+    "alertType": AM.alert_type,
+    "alertSound": AM.audible_alert,
+    "awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0,
+    "driverMonitoringOn": bool(driver_status.face_detected),
+    "canMonoTimes": list(CS.canMonoTimes),
+    "planMonoTime": sm.logMonoTime['plan'],
+    "pathPlanMonoTime": sm.logMonoTime['pathPlan'],
+    "enabled": isEnabled(state),
+    "active": isActive(state),
+    "vEgo": CS.vEgo,
+    "vEgoRaw": CS.vEgoRaw,
+    "angleSteers": CS.steeringAngle,
+    "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
+    "steerOverride": CS.steeringPressed,
+    "state": state,
+    "engageable": not bool(get_events(events, [ET.NO_ENTRY])),
+    "longControlState": LoC.long_control_state,
+    "vPid": float(LoC.v_pid),
+    "vCruise": float(v_cruise_kph),
+    "upAccelCmd": float(LoC.pid.p),
+    "uiAccelCmd": float(LoC.pid.i),
+    "ufAccelCmd": float(LoC.pid.f),
+    "angleSteersDes": float(LaC.angle_steers_des),
+    "vTargetLead": float(v_acc),
+    "aTarget": float(a_acc),
+    "jerkFactor": float(sm['plan'].jerkFactor),
+    "gpsPlannerActive": sm['plan'].gpsPlannerActive,
+    "vCurvature": sm['plan'].vCurvature,
+    "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
+    "cumLagMs": -rk.remaining * 1000.,
+    "startMonoTime": int(start_time * 1e9),
+    "mapValid": sm['plan'].mapValid,
+    "forceDecel": bool(force_decel),
+    "canErrorCounter": can_error_counter,
+  }
+
+  if CP.lateralTuning.which() == 'pid':
+    dat.controlsState.lateralControlState.pidState = lac_log
+  elif CP.lateralTuning.which() == 'lqr':
+    dat.controlsState.lateralControlState.lqrState = lac_log
+  elif CP.lateralTuning.which() == 'indi':
+    dat.controlsState.lateralControlState.indiState = lac_log
+  pm.send('controlsState', dat)
+
+  # carState
+  cs_send = messaging.new_message()
+  cs_send.init('carState')
+  cs_send.valid = CS.canValid
+  cs_send.carState = CS
+  cs_send.carState.events = events
+  pm.send('carState', cs_send)
+
+  # carEvents - logged every second or on change
+  events_bytes = events_to_bytes(events)
+  if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev):
+    ce_send = messaging.new_message()
+    ce_send.init('carEvents', len(events))
+    ce_send.carEvents = events
+    pm.send('carEvents', ce_send)
+
+  # carParams - logged every 50 seconds (> 1 per segment)
+  if (sm.frame % int(50. / DT_CTRL) == 0):
+    cp_send = messaging.new_message()
+    cp_send.init('carParams')
+    cp_send.carParams = CP
+    pm.send('carParams', cp_send)
+
+  # carControl
+  cc_send = messaging.new_message()
+  cc_send.init('carControl')
+  cc_send.valid = CS.canValid
+  cc_send.carControl = CC
+  pm.send('carControl', cc_send)
+
+  return CC, events_bytes
+
+
+def controlsd_thread(sm=None, pm=None, can_sock=None):
+  gc.disable()
+
+  # start the loop
+  set_realtime_priority(3)
+
+  params = Params()
+
+  is_metric = params.get("IsMetric", encoding='utf8') == "1"
+  is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
+  passive = params.get("Passive", encoding='utf8') == "1"
+  openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
+  community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
+
+  passive = passive or not openpilot_enabled_toggle
+
+  # Pub/Sub Sockets
+  if pm is None:
+    pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
+
+  if sm is None:
+    sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
+                              'model', 'gpsLocation'], ignore_alive=['gpsLocation'])
+
+
+  if can_sock is None:
+    can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
+    can_sock = messaging.sub_sock('can', timeout=can_timeout)
+
+  # wait for health and CAN packets
+  hw_type = messaging.recv_one(sm.sock['health']).health.hwType
+  has_relay = hw_type in [HwType.blackPanda, HwType.uno]
+  print("Waiting for CAN messages...")
+  messaging.get_one_can(can_sock)
+
+  CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
+
+  car_recognized = CP.carName != 'mock'
+  # If stock camera is disconnected, we loaded car controls and it's not chffrplus
+  controller_available = CP.enableCamera and CI.CC is not None and not passive
+  community_feature_disallowed = CP.communityFeature and not community_feature_toggle
+  read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
+  if read_only:
+    CP.safetyModel = car.CarParams.SafetyModel.noOutput
+
+  # Write CarParams for radard and boardd safety mode
+  params.put("CarParams", CP.to_bytes())
+  params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
+
+  CC = car.CarControl.new_message()
+  AM = AlertManager()
+
+  startup_alert = get_startup_alert(car_recognized, controller_available)
+  AM.add(sm.frame, startup_alert, False)
+
+  LoC = LongControl(CP, CI.compute_gb)
+  VM = VehicleModel(CP)
+
+  if CP.lateralTuning.which() == 'pid':
+    LaC = LatControlPID(CP)
+  elif CP.lateralTuning.which() == 'indi':
+    LaC = LatControlINDI(CP)
+  elif CP.lateralTuning.which() == 'lqr':
+    LaC = LatControlLQR(CP)
+
+  driver_status = DriverStatus()
+  is_rhd = params.get("IsRHD")
+  if is_rhd is not None:
+    driver_status.is_rhd = bool(int(is_rhd))
+
+  state = State.disabled
+  soft_disable_timer = 0
+  v_cruise_kph = 255
+  v_cruise_kph_last = 0
+  mismatch_counter = 0
+  can_error_counter = 0
+  last_blinker_frame = 0
+  events_prev = []
+
+  sm['liveCalibration'].calStatus = Calibration.INVALID
+  sm['pathPlan'].sensorValid = True
+  sm['pathPlan'].posenetValid = True
+  sm['thermal'].freeSpace = 1.
+
+  # 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)
+
+  internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
+
+  prof = Profiler(False)  # off by default
+
+  while True:
+    start_time = sec_since_boot()
+    prof.checkpoint("Ratekeeper", ignore=True)
+
+    # Sample data and compute car events
+    CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params)
+    prof.checkpoint("Sample")
+
+    # Create alerts
+    if not sm.alive['plan'] and sm.alive['pathPlan']:  # only plan not being received: radar not communicating
+      events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    elif not sm.all_alive_and_valid():
+      events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if not sm['pathPlan'].mpcSolutionValid:
+      events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if not sm['pathPlan'].sensorValid:
+      events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
+    if not sm['pathPlan'].paramsValid:
+      events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
+    if not sm['pathPlan'].posenetValid:
+      events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
+    if not sm['plan'].radarValid:
+      events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if sm['plan'].radarCanError:
+      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]))
+    if internet_needed:
+      events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
+    if community_feature_disallowed:
+      events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
+    if read_only and not passive:
+      events.append(create_event('carUnrecognized', [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:
+      events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+
+    if not read_only:
+      # update control state
+      state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
+        state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
+      prof.checkpoint("State transition")
+
+    # Compute actuators (runs PID loops and lateral MPC)
+    actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \
+      state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
+                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame)
+
+    prof.checkpoint("State Control")
+
+    # Publish data
+    CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
+                                LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame,
+                                is_ldw_enabled, can_error_counter)
+    prof.checkpoint("Sent")
+
+    rk.monitor_time()
+    prof.display()
+
+
+def main(sm=None, pm=None, logcan=None):
+  controlsd_thread(sm, pm, logcan)
+
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/gps_plannerd.py

@@ -0,0 +1,381 @@
+#!/usr/bin/env python
+import os
+import zmq
+import json
+import time
+import numpy as np
+from numpy import linalg as LA
+from threading import Thread
+from scipy.spatial import cKDTree
+
+from selfdrive.swaglog import cloudlog
+from cereal.services import service_list
+from common.realtime import Ratekeeper
+from common.kalman.ned import geodetic2ecef, NED
+import cereal.messaging as messaging
+from cereal import log
+import warnings
+from selfdrive.config import Conversions as CV
+
+
+if os.getenv('EON_LIVE') == '1':
+  _REMOTE_ADDR = "192.168.5.11"
+else:
+  _REMOTE_ADDR = "127.0.0.1"
+
+LOOP = 'small_loop'
+
+TRACK_SNAP_DIST = 17.  # snap to a track below this distance
+TRACK_LOST_DIST = 30.  # lose a track above this distance
+INSTRUCTION_APPROACHING_DIST = 200.
+INSTRUCTION_ACTIVE_DIST = 20.
+
+ROT_CENTER_TO_LOC = 1.2
+
+class INSTRUCTION_STATE:
+  NONE = log.UiNavigationEvent.Status.none
+  PASSIVE = log.UiNavigationEvent.Status.passive
+  APPROACHING = log.UiNavigationEvent.Status.approaching
+  ACTIVE = log.UiNavigationEvent.Status.active
+
+
+def convert_ecef_to_capnp(points):
+  points_capnp = []
+  for p in points:
+    point = log.ECEFPoint.new_message()
+    point.x, point.y, point.z = map(float, p[0:3])
+    points_capnp.append(point)
+  return points_capnp
+
+
+def get_spaced_points(track, start_index, cur_ecef, v_ego):
+  active_points = []
+  look_ahead = 5.0 + 1.5 * v_ego  # 5m + 1.5s
+
+  # forward and backward passes for better poly fit
+  for idx_sign in [1, -1]:
+    for i in range(0, 1000):
+      index = start_index + i * idx_sign
+      # loop around 
+      p = track[index % len(track)]
+
+      distance = LA.norm(cur_ecef - p[0:3])
+      if i > 5 and distance > look_ahead:
+        break
+
+      active_points.append([p, index])
+
+  # sort points by index
+  active_points = sorted(active_points, key=lambda pt: pt[1])
+  active_points = [p[0] for p in active_points]
+
+  return active_points
+
+
+def fit_poly(points, cur_ecef, cur_heading, ned_converter):
+  relative_points = []
+  for point in points.points:
+    p = np.array([point.x, point.y, point.z])
+    relative_points.append(ned_converter.ecef_to_ned_matrix.dot(p - cur_ecef))
+
+  relative_points = np.matrix(np.vstack(relative_points))
+
+  # Calculate relative postions and rotate wrt to heading of car
+  c, s = np.cos(-cur_heading), np.sin(-cur_heading)
+  R = np.array([[c, -s], [s, c]])
+
+  n, e = relative_points[:, 0], relative_points[:, 1]
+  relative_points = np.hstack([e, n])
+  rotated_points = relative_points.dot(R)
+
+  rotated_points = np.array(rotated_points)
+  x, y = rotated_points[:, 1], -rotated_points[:, 0]
+
+  warnings.filterwarnings('error')
+
+  # delete points that go backward
+  max_x = x[0]
+  x_new = []
+  y_new = []
+
+  for xi, yi in zip(x, y):
+    if xi > max_x:
+      max_x = xi
+      x_new.append(xi)
+      y_new.append(yi)
+
+  x = np.array(x_new)
+  y = np.array(y_new)
+
+  if len(x) > 10:
+    poly = map(float, np.polyfit(x + ROT_CENTER_TO_LOC, y, 3))  # 1.2m in front
+  else:
+    poly = [0.0, 0.0, 0.0, 0.0]
+  return poly, float(max_x + ROT_CENTER_TO_LOC)
+
+
+def get_closest_track(tracks, track_trees, cur_ecef):
+
+  track_list = [(name, track_trees[name].query(cur_ecef, 1)) for name in track_trees]
+  closest_name, [closest_distance, closest_idx] = min(track_list, key=lambda x: x[1][0])
+
+  return {'name': closest_name,
+          'distance': closest_distance,
+          'idx': closest_idx,
+          'speed': tracks[closest_name][closest_idx][3],
+          'accel': tracks[closest_name][closest_idx][4]}
+
+
+def get_track_from_name(tracks, track_trees, track_name, cur_ecef):
+  if track_name is None:
+    return None
+  else:
+    track_distance, track_idx = track_trees[track_name].query(cur_ecef, 1)
+  return {'name': track_name,
+          'distance': track_distance,
+          'idx': track_idx,
+          'speed': tracks[track_name][track_idx][3],
+          'accel': tracks[track_name][track_idx][4]}
+
+
+def get_tracks_from_instruction(tracks,instruction, track_trees, cur_ecef):
+  if instruction is None:
+    return None, None
+  else:
+    source_track = get_track_from_name(tracks, track_trees, instruction['source'], cur_ecef)
+    target_track = get_track_from_name(tracks, track_trees, instruction['target'], cur_ecef)
+    return source_track, target_track
+
+
+def get_next_instruction_distance(track, instruction, cur_ecef):
+  if instruction is None:
+    return None
+  else:
+    return np.linalg.norm(cur_ecef - track[instruction['start_idx']][0:3])
+
+
+def update_current_track(tracks, cur_track, cur_ecef, track_trees):
+
+  closest_track = get_closest_track(tracks, track_trees, cur_ecef)
+
+  # have we lost current track?
+  if cur_track is not None:
+    cur_track = get_track_from_name(tracks, track_trees, cur_track['name'], cur_ecef)
+    if cur_track['distance'] > TRACK_LOST_DIST:
+      cur_track = None
+
+  # did we snap to a new track?
+  if cur_track is None and closest_track['distance'] < TRACK_SNAP_DIST:
+    cur_track = closest_track
+
+  return cur_track, closest_track
+
+
+def update_instruction(instruction, instructions, cur_track, source_track, state, cur_ecef, tracks):
+
+  if state == INSTRUCTION_STATE.ACTIVE:  # instruction frozen, just update distance
+    instruction['distance'] = get_next_instruction_distance(tracks[source_track['name']], instruction, cur_ecef)
+    return instruction
+
+  elif cur_track is None:
+    return None
+
+  else:
+    instruction_list = [i for i in instructions[cur_track['name']] if i['start_idx'] > cur_track['idx']]
+    if len(instruction_list) > 0:
+      next_instruction = min(instruction_list, key=lambda x: x['start_idx'])
+      next_instruction['distance'] = get_next_instruction_distance(tracks[cur_track['name']], next_instruction, cur_ecef)
+      return next_instruction
+    else:
+      return None
+
+
+def calc_instruction_state(state, cur_track, closest_track, source_track, target_track, instruction):
+
+  lost_track_or_instruction = cur_track is None or instruction is None
+
+  if state == INSTRUCTION_STATE.NONE:
+    if lost_track_or_instruction:
+      pass
+    else:
+      state = INSTRUCTION_STATE.PASSIVE
+
+  elif state == INSTRUCTION_STATE.PASSIVE:
+    if lost_track_or_instruction:
+      state = INSTRUCTION_STATE.NONE
+    elif instruction['distance'] < INSTRUCTION_APPROACHING_DIST:
+      state = INSTRUCTION_STATE.APPROACHING
+
+  elif state == INSTRUCTION_STATE.APPROACHING:
+    if lost_track_or_instruction:
+      state = INSTRUCTION_STATE.NONE
+    elif instruction['distance'] < INSTRUCTION_ACTIVE_DIST:
+      state = INSTRUCTION_STATE.ACTIVE
+
+  elif state == INSTRUCTION_STATE.ACTIVE:
+    if lost_track_or_instruction:
+      state = INSTRUCTION_STATE.NONE
+    elif target_track['distance'] < TRACK_SNAP_DIST and \
+         source_track['idx'] > instruction['start_idx'] and \
+         instruction['distance'] > 10.:
+      state = INSTRUCTION_STATE.NONE
+      cur_track = target_track
+
+  return state, cur_track
+
+
+def gps_planner_point_selection():
+
+  DECIMATION = 1
+
+  cloudlog.info("Starting gps_plannerd point selection")
+
+  rk = Ratekeeper(10.0, print_delay_threshold=np.inf)
+
+  context = zmq.Context()
+  live_location = messaging.sub_sock(context, 'liveLocation', conflate=True, addr=_REMOTE_ADDR)
+  car_state = messaging.sub_sock(context, 'carState', conflate=True)
+  gps_planner_points = messaging.pub_sock(context, 'gpsPlannerPoints')
+  ui_navigation_event = messaging.pub_sock(context, 'uiNavigationEvent')
+
+  # Load tracks and instructions from disk
+  basedir = os.environ['BASEDIR']
+  tracks = np.load(os.path.join(basedir, 'selfdrive/controls/tracks/%s.npy' % LOOP)).item()
+  instructions = json.loads(open(os.path.join(basedir, 'selfdrive/controls/tracks/instructions_%s.json' % LOOP)).read())
+
+  # Put tracks into KD-trees
+  track_trees = {}
+  for name in tracks:
+    tracks[name] = tracks[name][::DECIMATION]
+    track_trees[name] = cKDTree(tracks[name][:,0:3]) # xyz
+  cur_track = None
+  source_track = None
+  target_track = None
+  instruction = None
+  v_ego = 0.
+  state = INSTRUCTION_STATE.NONE
+
+  counter = 0
+
+  while True:
+    counter += 1
+    ll = messaging.recv_one(live_location)
+    ll = ll.liveLocation
+    cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
+    cs = messaging.recv_one_or_none(car_state)
+    if cs is not None:
+      v_ego = cs.carState.vEgo
+
+    cur_track, closest_track = update_current_track(tracks, cur_track, cur_ecef, track_trees)
+    #print cur_track
+
+    instruction = update_instruction(instruction, instructions, cur_track, source_track, state, cur_ecef, tracks)
+
+    source_track, target_track = get_tracks_from_instruction(tracks, instruction, track_trees, cur_ecef)
+
+    state, cur_track = calc_instruction_state(state, cur_track, closest_track, source_track, target_track, instruction)
+
+    active_points = []
+
+    # Make list of points used by gpsPlannerPlan
+    if cur_track is not None:
+      active_points = get_spaced_points(tracks[cur_track['name']], cur_track['idx'], cur_ecef, v_ego)
+
+    cur_pos = log.ECEFPoint.new_message()
+    cur_pos.x, cur_pos.y, cur_pos.z = map(float, cur_ecef)
+    m = messaging.new_message()
+    m.init('gpsPlannerPoints')
+    m.gpsPlannerPoints.curPos = cur_pos
+    m.gpsPlannerPoints.points = convert_ecef_to_capnp(active_points)
+    m.gpsPlannerPoints.valid = len(active_points) > 10
+    m.gpsPlannerPoints.trackName = "none" if cur_track is None else cur_track['name']
+    m.gpsPlannerPoints.speedLimit = 100. if cur_track is None else float(cur_track['speed'])
+    m.gpsPlannerPoints.accelTarget = 0. if cur_track is None else float(cur_track['accel'])
+    gps_planner_points.send(m.to_bytes())
+
+    m = messaging.new_message()
+    m.init('uiNavigationEvent')
+    m.uiNavigationEvent.status = state
+    m.uiNavigationEvent.type = "none" if instruction is None else instruction['type']
+    m.uiNavigationEvent.distanceTo = 0. if instruction is None else float(instruction['distance'])
+    endRoadPoint = log.ECEFPoint.new_message()
+    m.uiNavigationEvent.endRoadPoint = endRoadPoint
+    ui_navigation_event.send(m.to_bytes())
+
+    rk.keep_time()
+
+
+def gps_planner_plan():
+
+  context = zmq.Context()
+
+  live_location = messaging.sub_sock(context, 'liveLocation', conflate=True, addr=_REMOTE_ADDR)
+  gps_planner_points = messaging.sub_sock(context, 'gpsPlannerPoints', conflate=True)
+  gps_planner_plan = messaging.pub_sock(context, 'gpsPlannerPlan')
+
+  points = messaging.recv_one(gps_planner_points).gpsPlannerPoints
+
+  target_speed = 100. * CV.MPH_TO_MS
+  target_accel = 0.
+
+  last_ecef = np.array([0., 0., 0.])
+
+  while True:
+    ll = messaging.recv_one(live_location)
+    ll = ll.liveLocation
+    p = messaging.recv_one_or_none(gps_planner_points)
+    if p is not None:
+      points = p.gpsPlannerPoints
+      target_speed = p.gpsPlannerPoints.speedLimit
+      target_accel = p.gpsPlannerPoints.accelTarget
+
+    cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
+
+    # TODO: make NED initialization much faster so we can run this every time step
+    if np.linalg.norm(last_ecef - cur_ecef) > 200.:
+      ned_converter = NED(ll.lat, ll.lon, ll.alt)
+      last_ecef = cur_ecef
+
+    cur_heading = np.radians(ll.heading)
+
+    if points.valid:
+      poly, x_lookahead = fit_poly(points, cur_ecef, cur_heading, ned_converter)
+    else:
+      poly, x_lookahead = [0.0, 0.0, 0.0, 0.0], 0.
+
+    valid = points.valid
+
+    m = messaging.new_message()
+    m.init('gpsPlannerPlan')
+    m.gpsPlannerPlan.valid = valid
+    m.gpsPlannerPlan.poly = poly
+    m.gpsPlannerPlan.trackName = points.trackName
+    r = []
+    for p in points.points:
+      point = log.ECEFPoint.new_message()
+      point.x, point.y, point.z = p.x, p.y, p.z
+      r.append(point)
+    m.gpsPlannerPlan.points = r
+    m.gpsPlannerPlan.speed = target_speed
+    m.gpsPlannerPlan.acceleration = target_accel
+    m.gpsPlannerPlan.xLookahead = x_lookahead
+    gps_planner_plan.send(m.to_bytes())
+
+
+def main(gctx=None):
+  cloudlog.info("Starting gps_plannerd main thread")
+
+  point_thread = Thread(target=gps_planner_point_selection)
+  point_thread.daemon = True
+  control_thread = Thread(target=gps_planner_plan)
+  control_thread.daemon = True
+
+  point_thread.start()
+  control_thread.start()
+
+  while True:
+    time.sleep(1)
+
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/lib/alertmanager.py

@@ -0,0 +1,71 @@
+from cereal import car, log
+from common.realtime import DT_CTRL
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.alerts import ALERTS
+import copy
+
+
+AlertSize = log.ControlsState.AlertSize
+AlertStatus = log.ControlsState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+
+class AlertManager():
+
+  def __init__(self):
+    self.activealerts = []
+    self.alerts = {alert.alert_type: alert for alert in ALERTS}
+
+  def alertPresent(self):
+    return len(self.activealerts) > 0
+
+  def add(self, frame, alert_type, enabled=True, extra_text_1='', extra_text_2=''):
+    alert_type = str(alert_type)
+    added_alert = copy.copy(self.alerts[alert_type])
+    added_alert.alert_text_1 += extra_text_1
+    added_alert.alert_text_2 += extra_text_2
+    added_alert.start_time = frame * DT_CTRL
+
+    # if new alert is higher priority, log it
+    if not self.alertPresent() or added_alert.alert_priority > self.activealerts[0].alert_priority:
+          cloudlog.event('alert_add', alert_type=alert_type, enabled=enabled)
+
+    self.activealerts.append(added_alert)
+
+    # sort by priority first and then by start_time
+    self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True)
+
+  def process_alerts(self, frame):
+    cur_time = frame * DT_CTRL
+
+    # first get rid of all the expired alerts
+    self.activealerts = [a for a in self.activealerts if a.start_time +
+                         max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time]
+
+    current_alert = self.activealerts[0] if self.alertPresent() else None
+
+    # start with assuming no alerts
+    self.alert_type = ""
+    self.alert_text_1 = ""
+    self.alert_text_2 = ""
+    self.alert_status = AlertStatus.normal
+    self.alert_size = AlertSize.none
+    self.visual_alert = VisualAlert.none
+    self.audible_alert = AudibleAlert.none
+    self.alert_rate = 0.
+
+    if current_alert:
+      self.alert_type = current_alert.alert_type
+
+      if current_alert.start_time + current_alert.duration_sound > cur_time:
+        self.audible_alert = current_alert.audible_alert
+
+      if current_alert.start_time + current_alert.duration_hud_alert > cur_time:
+        self.visual_alert = current_alert.visual_alert
+
+      if current_alert.start_time + current_alert.duration_text > cur_time:
+        self.alert_text_1 = current_alert.alert_text_1
+        self.alert_text_2 = current_alert.alert_text_2
+        self.alert_status = current_alert.alert_status
+        self.alert_size = current_alert.alert_size
+        self.alert_rate = current_alert.alert_rate

selfdrive/controls/lib/alerts.py

@@ -0,0 +1,775 @@
+from cereal import car, log
+
+# Priority
+class Priority:
+  LOWEST = 0
+  LOW_LOWEST = 1
+  LOW = 2
+  MID = 3
+  HIGH = 4
+  HIGHEST = 5
+
+AlertSize = log.ControlsState.AlertSize
+AlertStatus = log.ControlsState.AlertStatus
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+
+class Alert():
+  def __init__(self,
+               alert_type,
+               alert_text_1,
+               alert_text_2,
+               alert_status,
+               alert_size,
+               alert_priority,
+               visual_alert,
+               audible_alert,
+               duration_sound,
+               duration_hud_alert,
+               duration_text,
+               alert_rate=0.):
+
+    self.alert_type = alert_type
+    self.alert_text_1 = alert_text_1
+    self.alert_text_2 = alert_text_2
+    self.alert_status = alert_status
+    self.alert_size = alert_size
+    self.alert_priority = alert_priority
+    self.visual_alert = visual_alert
+    self.audible_alert = audible_alert
+
+    self.duration_sound = duration_sound
+    self.duration_hud_alert = duration_hud_alert
+    self.duration_text = duration_text
+
+    self.start_time = 0.
+    self.alert_rate = alert_rate
+
+    # typecheck that enums are valid on startup
+    tst = car.CarControl.new_message()
+    tst.hudControl.visualAlert = self.visual_alert
+
+  def __str__(self):
+    return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + "  " + str(
+      self.visual_alert) + " " + str(self.audible_alert)
+
+  def __gt__(self, alert2):
+    return self.alert_priority > alert2.alert_priority
+
+
+ALERTS = [
+  # Miscellaneous alerts
+  Alert(
+      "enable",
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.MID, VisualAlert.none, AudibleAlert.chimeEngage, .2, 0., 0.),
+
+  Alert(
+      "disable",
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.MID, VisualAlert.none, AudibleAlert.chimeDisengage, .2, 0., 0.),
+
+  Alert(
+      "fcw",
+      "BRAKE!",
+      "Risk of Collision",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeat, 1., 2., 2.),
+
+  Alert(
+      "fcwStock",
+      "BRAKE!",
+      "Risk of Collision",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.),  # no EON chime for stock FCW
+
+  Alert(
+      "steerSaturated",
+      "TAKE CONTROL",
+      "Turn Exceeds Steering Limit",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.),
+
+  Alert(
+      "steerTempUnavailable",
+      "TAKE CONTROL",
+      "Steering Temporarily Unavailable",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
+
+  Alert(
+      "steerTempUnavailableMute",
+      "TAKE CONTROL",
+      "Steering Temporarily Unavailable",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, .2, .2),
+
+  Alert(
+      "preDriverDistracted",
+      "KEEP EYES ON ROAD: User Appears Distracted",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
+
+  Alert(
+      "promptDriverDistracted",
+      "KEEP EYES ON ROAD",
+      "User Appears Distracted",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
+
+  Alert(
+      "driverDistracted",
+      "DISENGAGE IMMEDIATELY",
+      "User Was Distracted",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
+
+  Alert(
+      "preDriverUnresponsive",
+      "TOUCH STEERING WHEEL: No Face Detected",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
+
+  Alert(
+      "promptDriverUnresponsive",
+      "TOUCH STEERING WHEEL",
+      "User Is Unresponsive",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
+
+  Alert(
+      "driverUnresponsive",
+      "DISENGAGE IMMEDIATELY",
+      "User Was Unresponsive",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
+
+  Alert(
+      "driverMonitorLowAcc",
+      "CHECK DRIVER FACE VISIBILITY",
+      "Driver Monitor Model Output Uncertain",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 0., 4.),
+
+  Alert(
+      "geofence",
+      "DISENGAGEMENT REQUIRED",
+      "Not in Geofenced Area",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
+
+  Alert(
+      "startup",
+      "Be ready to take over at any time",
+      "Always keep hands on wheel and eyes on road",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
+
+  Alert(
+      "startupNoControl",
+      "Dashcam mode",
+      "Always keep hands on wheel and eyes on road",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
+
+  Alert(
+      "startupNoCar",
+      "Dashcam mode with unsupported car",
+      "Always keep hands on wheel and eyes on road",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
+
+  Alert(
+      "ethicalDilemma",
+      "TAKE CONTROL IMMEDIATELY",
+      "Ethical Dilemma Detected",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 3.),
+
+  Alert(
+      "steerTempUnavailableNoEntry",
+      "openpilot Unavailable",
+      "Steering Temporarily Unavailable",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
+
+  Alert(
+      "manualRestart",
+      "TAKE CONTROL",
+      "Resume Driving Manually",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "resumeRequired",
+      "STOPPED",
+      "Press Resume to Move",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "belowSteerSpeed",
+      "TAKE CONTROL",
+      "Steer Unavailable Below ",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3),
+
+  Alert(
+      "debugAlert",
+      "DEBUG ALERT",
+      "",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1),
+  Alert(
+      "preLaneChangeLeft",
+      "Steer Left to Start Lane Change",
+      "Monitor Other Vehicles",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
+
+  Alert(
+      "preLaneChangeRight",
+      "Steer Right to Start Lane Change",
+      "Monitor Other Vehicles",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
+
+  Alert(
+      "laneChange",
+      "Changing Lane",
+      "Monitor Other Vehicles",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1),
+
+  Alert(
+      "posenetInvalid",
+      "TAKE CONTROL",
+      "Vision Model Output Uncertain",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
+
+  # Non-entry only alerts
+  Alert(
+      "wrongCarModeNoEntry",
+      "openpilot Unavailable",
+      "Main Switch Off",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
+
+  Alert(
+      "dataNeededNoEntry",
+      "openpilot Unavailable",
+      "Data Needed for Calibration. Upload Drive, Try Again",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
+
+  Alert(
+      "outOfSpaceNoEntry",
+      "openpilot Unavailable",
+      "Out of Storage Space",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.),
+
+  Alert(
+      "pedalPressedNoEntry",
+      "openpilot Unavailable",
+      "Pedal Pressed During Attempt",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, "brakePressed", AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "speedTooLowNoEntry",
+      "openpilot Unavailable",
+      "Speed Too Low",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "brakeHoldNoEntry",
+      "openpilot Unavailable",
+      "Brake Hold Active",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "parkBrakeNoEntry",
+      "openpilot Unavailable",
+      "Park Brake Engaged",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "lowSpeedLockoutNoEntry",
+      "openpilot Unavailable",
+      "Cruise Fault: Restart the Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "lowBatteryNoEntry",
+      "openpilot Unavailable",
+      "Low Battery",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "sensorDataInvalidNoEntry",
+      "openpilot Unavailable",
+      "No Data from EON Sensors",
+      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",
+      "Distraction Level Too High",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  # Cancellation alerts causing soft disabling
+  Alert(
+      "overheat",
+      "TAKE CONTROL IMMEDIATELY",
+      "System Overheated",
+      AlertStatus.critical, AlertSize.full,
+      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.chimeWarningRepeat, .1, 2., 2.),
+
+  Alert(
+      "calibrationInvalid",
+      "TAKE CONTROL IMMEDIATELY",
+      "Calibration Invalid: Reposition EON and Recalibrate",
+      AlertStatus.critical, AlertSize.full,
+      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.chimeWarningRepeat, .1, 2., 2.),
+
+  Alert(
+      "doorOpen",
+      "TAKE CONTROL IMMEDIATELY",
+      "Door Open",
+      AlertStatus.critical, AlertSize.full,
+      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.chimeWarningRepeat, .1, 2., 2.),
+
+  Alert(
+      "espDisabled",
+      "TAKE CONTROL IMMEDIATELY",
+      "ESP Off",
+      AlertStatus.critical, AlertSize.full,
+      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.chimeWarningRepeat, .1, 2., 2.),
+
+  Alert(
+      "commIssue",
+      "TAKE CONTROL IMMEDIATELY",
+      "Communication Issue between Processes",
+      AlertStatus.critical, AlertSize.full,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
+
+  Alert(
+      "radarCommIssue",
+      "TAKE CONTROL IMMEDIATELY",
+      "Radar Communication Issue",
+      AlertStatus.critical, AlertSize.full,
+      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.chimeWarningRepeat, .1, 2., 2.),
+
+  Alert(
+      "radarFault",
+      "TAKE CONTROL IMMEDIATELY",
+      "Radar Error: Restart the Car",
+      AlertStatus.critical, AlertSize.full,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
+
+
+  Alert(
+      "lowMemory",
+      "TAKE CONTROL IMMEDIATELY",
+      "Low Memory: Reboot Your EON",
+      AlertStatus.critical, AlertSize.full,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
+
+  # Cancellation alerts causing immediate disabling
+  Alert(
+      "controlsFailed",
+      "TAKE CONTROL IMMEDIATELY",
+      "Controls Failed",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "controlsMismatch",
+      "TAKE CONTROL IMMEDIATELY",
+      "Controls Mismatch",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "canError",
+      "TAKE CONTROL IMMEDIATELY",
+      "CAN Error: Check Connections",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "steerUnavailable",
+      "TAKE CONTROL IMMEDIATELY",
+      "LKAS Fault: Restart the Car",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "brakeUnavailable",
+      "TAKE CONTROL IMMEDIATELY",
+      "Cruise Fault: Restart the Car",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "gasUnavailable",
+      "TAKE CONTROL IMMEDIATELY",
+      "Gas Fault: Restart the Car",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "reverseGear",
+      "TAKE CONTROL IMMEDIATELY",
+      "Reverse Gear",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "cruiseDisabled",
+      "TAKE CONTROL IMMEDIATELY",
+      "Cruise Is Off",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  Alert(
+      "plannerError",
+      "TAKE CONTROL IMMEDIATELY",
+      "Planner Solution Error",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
+
+  # not loud cancellations (user is in control)
+  Alert(
+      "noTarget",
+      "openpilot Canceled",
+      "No close lead car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
+
+  Alert(
+      "speedTooLow",
+      "openpilot Canceled",
+      "Speed too low",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
+
+  # Cancellation alerts causing non-entry
+  Alert(
+      "overheatNoEntry",
+      "openpilot Unavailable",
+      "System overheated",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "wrongGearNoEntry",
+      "openpilot Unavailable",
+      "Gear not D",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "calibrationInvalidNoEntry",
+      "openpilot Unavailable",
+      "Calibration Invalid: Reposition EON and Recalibrate",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "calibrationIncompleteNoEntry",
+      "openpilot Unavailable",
+      "Calibration in Progress",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "doorOpenNoEntry",
+      "openpilot Unavailable",
+      "Door open",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "seatbeltNotLatchedNoEntry",
+      "openpilot Unavailable",
+      "Seatbelt unlatched",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "espDisabledNoEntry",
+      "openpilot Unavailable",
+      "ESP Off",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "geofenceNoEntry",
+      "openpilot Unavailable",
+      "Not in Geofenced Area",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.MID, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "radarCanErrorNoEntry",
+      "openpilot Unavailable",
+      "Radar Error: Restart the Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "radarFaultNoEntry",
+      "openpilot Unavailable",
+      "Radar Error: Restart the Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "posenetInvalidNoEntry",
+      "openpilot Unavailable",
+      "Vision Model Output Uncertain",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "controlsFailedNoEntry",
+      "openpilot Unavailable",
+      "Controls Failed",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "canErrorNoEntry",
+      "openpilot Unavailable",
+      "CAN Error: Check Connections",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "steerUnavailableNoEntry",
+      "openpilot Unavailable",
+      "LKAS Fault: Restart the Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "brakeUnavailableNoEntry",
+      "openpilot Unavailable",
+      "Cruise Fault: Restart the Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "gasUnavailableNoEntry",
+      "openpilot Unavailable",
+      "Gas Error: Restart the Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "reverseGearNoEntry",
+      "openpilot Unavailable",
+      "Reverse Gear",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "cruiseDisabledNoEntry",
+      "openpilot Unavailable",
+      "Cruise is Off",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "noTargetNoEntry",
+      "openpilot Unavailable",
+      "No Close Lead Car",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "plannerErrorNoEntry",
+      "openpilot Unavailable",
+      "Planner Solution Error",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+  Alert(
+      "commIssueNoEntry",
+      "openpilot Unavailable",
+      "Communication Issue between Processes",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
+
+  Alert(
+      "radarCommIssueNoEntry",
+      "openpilot Unavailable",
+      "Radar Communication Issue",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
+
+  Alert(
+      "internetConnectivityNeededNoEntry",
+      "openpilot Unavailable",
+      "Please Connect to Internet",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
+
+  Alert(
+      "lowMemoryNoEntry",
+      "openpilot Unavailable",
+      "Low Memory: Reboot Your EON",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
+
+  # permanent alerts
+  Alert(
+      "steerUnavailablePermanent",
+      "LKAS Fault: Restart the car to engage",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "brakeUnavailablePermanent",
+      "Cruise Fault: Restart the car to engage",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "lowSpeedLockoutPermanent",
+      "Cruise Fault: Restart the car to engage",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "calibrationIncompletePermanent",
+      "Calibration in Progress: ",
+      "Drive Above ",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "invalidGiraffeToyotaPermanent",
+      "Unsupported Giraffe Configuration",
+      "Visit comma.ai/tg",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "internetConnectivityNeededPermanent",
+      "Please connect to Internet",
+      "An Update Check Is Required to Engage",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "communityFeatureDisallowedPermanent",
+      "Community Feature Detected",
+      "Enable Community Features in Developer Settings",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),  # LOW priority to overcome Cruise Error
+
+  Alert(
+      "sensorDataInvalidPermanent",
+      "No Data from EON Sensors",
+      "Reboot your EON",
+      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(
+      "lowMemoryPermanent",
+      "RAM Critically Low",
+      "Reboot your EON",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "carUnrecognizedPermanent",
+      "Dashcam Mode",
+      "Car Unrecognized",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+  Alert(
+      "vehicleModelInvalid",
+      "Vehicle Parameter Identification Failed",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1),
+
+  # offroad alerts
+  Alert(
+      "ldwPermanent",
+      "TAKE CONTROL",
+      "Lane Departure Detected",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.),
+]

selfdrive/controls/lib/alerts_offroad.json

@@ -0,0 +1,31 @@
+{
+  "Offroad_ChargeDisabled": {
+    "text": "EON charging disabled after car being off for more than 30 hours. Turn ignition on to start charging again.",
+    "severity": 0
+  },
+  "Offroad_TemperatureTooHigh": {
+    "text": "EON temperature too high. System won't start.",
+    "severity": 1
+  },
+  "Offroad_ConnectivityNeededPrompt": {
+    "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in ",
+    "severity": 0,
+    "_comment": "Append the number of days at the end of the text"
+  },
+  "Offroad_ConnectivityNeeded": {
+    "text": "Connect to internet to check for updates. openpilot won't engage until it connects to internet to check for updates.",
+    "severity": 1
+  },
+  "Offroad_PandaFirmwareMismatch": {
+    "text": "Unexpected panda firmware version. System won't start. Reboot your EON to reflash panda.",
+    "severity": 1
+  },
+  "Offroad_InvalidTime": {
+    "text": "Invalid date and time settings, system won't start. Connect to internet to set time.",
+    "severity": 1
+  },
+  "Offroad_IsTakingSnapshot": {
+    "text": "Taking camera snapshots. System won't start until finished.",
+    "severity": 0
+  }
+}

selfdrive/controls/lib/cluster/.gitignore

@@ -0,0 +1 @@
+test

selfdrive/controls/lib/cluster/LICENSE

@@ -0,0 +1,13 @@
+Copyright:
+  * fastcluster_dm.cpp & fastcluster_R_dm.cpp:
+     © 2011 Daniel Müllner <http://danifold.net>
+  * fastcluster.(h|cpp) & demo.cpp & plotresult.r:
+     © 2018 Christoph Dalitz <http://www.hsnr.de/ipattern/>
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

selfdrive/controls/lib/cluster/README

@@ -0,0 +1,79 @@
+C++ interface to fast hierarchical clustering algorithms
+========================================================
+
+This is a simplified C++ interface to fast implementations of hierarchical
+clustering by Daniel Müllner. The original library with interfaces to R
+and Python is described in:
+
+Daniel Müllner: "fastcluster: Fast Hierarchical, Agglomerative Clustering
+Routines for R and Python." Journal of Statistical Software 53 (2013),
+no. 9, pp. 1–18, http://www.jstatsoft.org/v53/i09/
+
+
+Usage of the library
+--------------------
+
+For using the library, the following source files are needed:
+
+fastcluster_dm.cpp, fastcluster_R_dm.cpp
+   original code by Daniel Müllner
+   these are included by fastcluster.cpp via #include, and therefore
+   need not be compiled to object code
+
+fastcluster.[h|cpp]
+   simplified C++ interface
+   fastcluster.cpp is the only file that must be compiled
+
+The library provides the clustering function *hclust_fast* for
+creating the dendrogram information in an encoding as used by the
+R function *hclust*. For a description of the parameters, see fastcluster.h.
+Its parameter *method* can be one of
+
+HCLUST_METHOD_SINGLE
+  single link with the minimum spanning tree algorithm (Rohlf, 1973)
+
+HHCLUST_METHOD_COMPLETE
+  complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984)
+
+HCLUST_METHOD_AVERAGE
+  complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984)
+
+HCLUST_METHOD_MEDIAN
+  median link with the generic algorithm (Müllner, 2011)
+
+For splitting the dendrogram into clusters, the two functions *cutree_k*
+and *cutree_cdist* are provided.
+
+Note that output parameters must be allocated beforehand, e.g.
+  int* merge = new int[2*(npoints-1)];
+For a complete usage example, see lines 135-142 of demo.cpp.
+
+
+Demonstration program
+---------------------
+
+A simple demo is implemented in demo.cpp, which can be compiled and run with
+
+   make
+   ./hclust-demo -m complete lines.csv
+
+It creates two clusters of line segments such that the segment angle between
+line segments of different clusters have a maximum (cosine) dissimilarity.
+For visualizing the result, plotresult.r can be used as follows
+(requires R <https://r-project.org> to be installed):
+
+  ./hclust-demo -m complete lines.csv | Rscript plotresult.r
+
+
+Authors & Copyright
+-------------------
+
+Daniel Müllner, 2011, <http://danifold.net>
+Christoph Dalitz, 2018, <http://www.hsnr.de/ipattern/>
+
+
+License
+-------
+
+This code is provided under a BSD-style license.
+See the file LICENSE for details.

selfdrive/controls/lib/cluster/SConscript

@@ -0,0 +1,8 @@
+Import('env')
+
+fc = env.SharedLibrary("fastcluster", "fastcluster.cpp")
+
+# TODO: how do I gate on test
+#env.Program("test", ["test.cpp"], LIBS=[fc])
+#valgrind --leak-check=full ./test
+

selfdrive/controls/lib/cluster/fastcluster.cpp

@@ -0,0 +1,218 @@
+//
+// C++ standalone verion of fastcluster by Daniel Müllner
+//
+// Copyright: Christoph Dalitz, 2018
+//            Daniel Müllner, 2011
+// License:   BSD style license
+//            (see the file LICENSE for details)
+//
+
+
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+
+extern "C" {
+#include "fastcluster.h"
+}
+
+// Code by Daniel Müllner
+// workaround to make it usable as a standalone version (without R)
+bool fc_isnan(double x) { return false; }
+#include "fastcluster_dm.cpp"
+#include "fastcluster_R_dm.cpp"
+
+extern "C" {
+//
+// Assigns cluster labels (0, ..., nclust-1) to the n points such
+// that the cluster result is split into nclust clusters.
+//
+// Input arguments:
+//   n      = number of observables
+//   merge  = clustering result in R format
+//   nclust = number of clusters
+// Output arguments:
+//   labels = allocated integer array of size n for result
+//
+  void cutree_k(int n, const int* merge, int nclust, int* labels) {
+
+    int k,m1,m2,j,l;
+
+    if (nclust > n || nclust < 2) {
+      for (j=0; j<n; j++) labels[j] = 0;
+      return;
+    }
+
+    // assign to each observable the number of its last merge step
+    // beware: indices of observables in merge start at 1 (R convention)
+    std::vector<int> last_merge(n, 0);
+    for (k=1; k<=(n-nclust); k++) {
+      // (m1,m2) = merge[k,]
+      m1 = merge[k-1];
+      m2 = merge[n-1+k-1];
+      if (m1 < 0 && m2 < 0) { // both single observables
+        last_merge[-m1-1] = last_merge[-m2-1] = k;
+      }
+      else if (m1 < 0 || m2 < 0) { // one is a cluster
+        if(m1 < 0) { j = -m1; m1 = m2; } else j = -m2;
+        // merging single observable and cluster
+        for(l = 0; l < n; l++)
+          if (last_merge[l] == m1)
+            last_merge[l] = k;
+        last_merge[j-1] = k;
+      }
+      else { // both cluster
+        for(l=0; l < n; l++) {
+          if( last_merge[l] == m1 || last_merge[l] == m2 )
+            last_merge[l] = k;
+        }
+      }
+    }
+
+    // assign cluster labels
+    int label = 0;
+    std::vector<int> z(n,-1);
+    for (j=0; j<n; j++) {
+      if (last_merge[j] == 0) { // still singleton
+        labels[j] = label++;
+      } else {
+        if (z[last_merge[j]] < 0) {
+          z[last_merge[j]] = label++;
+        }
+        labels[j] = z[last_merge[j]];
+      }
+    }
+  }
+
+  //
+  // Assigns cluster labels (0, ..., nclust-1) to the n points such
+  // that the hierarchical clustering is stopped when cluster distance >= cdist
+  //
+  // Input arguments:
+  //   n      = number of observables
+  //   merge  = clustering result in R format
+  //   height = cluster distance at each merge step
+  //   cdist  = cutoff cluster distance
+  // Output arguments:
+  //   labels = allocated integer array of size n for result
+  //
+  void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels) {
+
+    int k;
+
+    for (k=0; k<(n-1); k++) {
+      if (height[k] >= cdist) {
+        break;
+      }
+    }
+    cutree_k(n, merge, n-k, labels);
+  }
+
+
+  //
+  // Hierarchical clustering with one of Daniel Muellner's fast algorithms
+  //
+  // Input arguments:
+  //   n       = number of observables
+  //   distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
+  //             the upper triangle (without diagonal elements) of the distance
+  //             matrix, e.g. for n=4:
+  //               d00 d01 d02 d03
+  //               d10 d11 d12 d13   ->  d01 d02 d03 d12 d13 d23
+  //               d20 d21 d22 d23
+  //               d30 d31 d32 d33
+  //   method  = cluster metric (see enum method_code)
+  // Output arguments:
+  //   merge   = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
+  //             Result follows R hclust convention:
+  //              - observabe indices start with one
+  //              - merge[i][] contains the merged nodes in step i
+  //              - merge[i][j] is negative when the node is an atom
+  //   height  = allocated (n-1) array with distances at each merge step
+  // Return code:
+  //   0 = ok
+  //   1 = invalid method
+  //
+  int hclust_fast(int n, double* distmat, int method, int* merge, double* height) {
+
+    // call appropriate culstering function
+    cluster_result Z2(n-1);
+    if (method == HCLUST_METHOD_SINGLE) {
+      // single link
+      MST_linkage_core(n, distmat, Z2);
+    }
+    else if (method == HCLUST_METHOD_COMPLETE) {
+      // complete link
+      NN_chain_core<METHOD_METR_COMPLETE, t_float>(n, distmat, NULL, Z2);
+    }
+    else if (method == HCLUST_METHOD_AVERAGE) {
+      // best average distance
+      double* members = new double[n];
+      for (int i=0; i<n; i++) members[i] = 1;
+      NN_chain_core<METHOD_METR_AVERAGE, t_float>(n, distmat, members, Z2);
+      delete[] members;
+    }
+    else if (method == HCLUST_METHOD_MEDIAN) {
+      // best median distance (beware: O(n^3))
+      generic_linkage<METHOD_METR_MEDIAN, t_float>(n, distmat, NULL, Z2);
+    }
+    else if (method == HCLUST_METHOD_CENTROID) {
+      // best centroid distance (beware: O(n^3))
+      double* members = new double[n];
+      for (int i=0; i<n; i++) members[i] = 1;
+      generic_linkage<METHOD_METR_CENTROID, t_float>(n, distmat, members, Z2);
+      delete[] members;
+    }
+    else {
+      return 1;
+    }
+
+    int* order = new int[n];
+    if (method == HCLUST_METHOD_MEDIAN || method == HCLUST_METHOD_CENTROID) {
+      generate_R_dendrogram<true>(merge, height, order, Z2, n);
+    } else {
+      generate_R_dendrogram<false>(merge, height, order, Z2, n);
+    }
+    delete[] order; // only needed for visualization
+
+    return 0;
+  }
+
+
+  // Build condensed distance matrix
+  // Input arguments:
+  //   n  = number of observables
+  //   m  = dimension of observable
+  // Output arguments:
+  //   out = allocated integer array of size n * (n - 1) / 2 for result
+  void hclust_pdist(int n, int m, double* pts, double* out) {
+    int ii = 0;
+    for (int i = 0; i < n; i++){
+      for (int j = i + 1; j < n; j++){
+        // Compute euclidian distance
+        double d = 0;
+        for (int k = 0; k < m; k ++){
+          double error = pts[i * m + k] - pts[j * m + k];
+          d += (error * error);
+        }
+        out[ii] = d;//sqrt(d);
+        ii++;
+      }
+    }
+  }
+
+  void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx) {
+    double* pdist = new double[n * (n - 1) / 2];
+    int* merge = new int[2 * (n - 1)];
+    double* height = new double[n - 1];
+
+    hclust_pdist(n, m, pts, pdist);
+    hclust_fast(n, pdist, HCLUST_METHOD_CENTROID, merge, height);
+    cutree_cdist(n, merge, height, dist, idx);
+
+    delete[] pdist;
+    delete[] merge;
+    delete[] height;
+  }
+}

selfdrive/controls/lib/cluster/fastcluster.h

@@ -0,0 +1,77 @@
+//
+// C++ standalone verion of fastcluster by Daniel Muellner
+//
+// Copyright: Daniel Muellner, 2011
+//            Christoph Dalitz, 2018
+// License:   BSD style license
+//            (see the file LICENSE for details)
+//
+
+#ifndef fastclustercpp_H
+#define fastclustercpp_H
+
+//
+// Assigns cluster labels (0, ..., nclust-1) to the n points such
+// that the cluster result is split into nclust clusters.
+//
+// Input arguments:
+//   n      = number of observables
+//   merge  = clustering result in R format
+//   nclust = number of clusters
+// Output arguments:
+//   labels = allocated integer array of size n for result
+//
+void cutree_k(int n, const int* merge, int nclust, int* labels);
+
+//
+// Assigns cluster labels (0, ..., nclust-1) to the n points such
+// that the hierarchical clsutering is stopped at cluster distance cdist
+//
+// Input arguments:
+//   n      = number of observables
+//   merge  = clustering result in R format
+//   height = cluster distance at each merge step
+//   cdist  = cutoff cluster distance
+// Output arguments:
+//   labels = allocated integer array of size n for result
+//
+void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
+
+//
+// Hierarchical clustering with one of Daniel Muellner's fast algorithms
+//
+// Input arguments:
+//   n       = number of observables
+//   distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
+//             the upper triangle (without diagonal elements) of the distance
+//             matrix, e.g. for n=4:
+//               d00 d01 d02 d03
+//               d10 d11 d12 d13   ->  d01 d02 d03 d12 d13 d23
+//               d20 d21 d22 d23
+//               d30 d31 d32 d33
+//   method  = cluster metric (see enum method_code)
+// Output arguments:
+//   merge   = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
+//             Result follows R hclust convention:
+//              - observabe indices start with one
+//              - merge[i][] contains the merged nodes in step i
+//              - merge[i][j] is negative when the node is an atom
+//   height  = allocated (n-1) array with distances at each merge step
+// Return code:
+//   0 = ok
+//   1 = invalid method
+//
+int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
+enum hclust_fast_methods {
+  HCLUST_METHOD_SINGLE = 0,
+  HCLUST_METHOD_COMPLETE = 1,
+  HCLUST_METHOD_AVERAGE = 2,
+  HCLUST_METHOD_MEDIAN = 3,
+  HCLUST_METHOD_CENTROID = 5,
+};
+
+void hclust_pdist(int n, int m, double* pts, double* out);
+void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
+
+
+#endif

selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp

@@ -0,0 +1,115 @@
+//
+// Excerpt from fastcluster_R.cpp
+//
+// Copyright: Daniel Müllner, 2011 <http://danifold.net>
+//
+
+struct pos_node {
+  t_index pos;
+  int node;
+};
+
+void order_nodes(const int N, const int * const merge, const t_index * const node_size, int * const order) {
+  /* Parameters:
+     N         : number of data points
+     merge     : (N-1)×2 array which specifies the node indices which are
+                 merged in each step of the clustering procedure.
+                 Negative entries -1...-N point to singleton nodes, while
+                 positive entries 1...(N-1) point to nodes which are themselves
+                 parents of other nodes.
+     node_size : array of node sizes - makes it easier
+     order     : output array of size N
+
+     Runtime: Θ(N)
+  */
+  auto_array_ptr<pos_node> queue(N/2);
+
+  int parent;
+  int child;
+  t_index pos = 0;
+
+  queue[0].pos = 0;
+  queue[0].node = N-2;
+  t_index idx = 1;
+
+  do {
+    --idx;
+    pos = queue[idx].pos;
+    parent = queue[idx].node;
+
+    // First child
+    child = merge[parent];
+    if (child<0) { // singleton node, write this into the 'order' array.
+      order[pos] = -child;
+      ++pos;
+    }
+    else { /* compound node: put it on top of the queue and decompose it
+              in a later iteration. */
+      queue[idx].pos = pos;
+      queue[idx].node = child-1; // convert index-1 based to index-0 based
+      ++idx;
+      pos += node_size[child-1];
+    }
+    // Second child
+    child = merge[parent+N-1];
+    if (child<0) {
+      order[pos] = -child;
+    }
+    else {
+      queue[idx].pos = pos;
+      queue[idx].node = child-1;
+      ++idx;
+    }
+  } while (idx>0);
+}
+
+#define size_(r_) ( ((r_<N) ? 1 : node_size[r_-N]) )
+
+template <const bool sorted>
+void generate_R_dendrogram(int * const merge, double * const height, int * const order, cluster_result & Z2, const int N) {
+  // The array "nodes" is a union-find data structure for the cluster
+  // identites (only needed for unsorted cluster_result input).
+  union_find nodes(sorted ? 0 : N);
+  if (!sorted) {
+    std::stable_sort(Z2[0], Z2[N-1]);
+  }
+
+  t_index node1, node2;
+  auto_array_ptr<t_index> node_size(N-1);
+
+  for (t_index i=0; i<N-1; ++i) {
+    // Get two data points whose clusters are merged in step i.
+    // Find the cluster identifiers for these points.
+    if (sorted) {
+      node1 = Z2[i]->node1;
+      node2 = Z2[i]->node2;
+    }
+    else {
+      node1 = nodes.Find(Z2[i]->node1);
+      node2 = nodes.Find(Z2[i]->node2);
+      // Merge the nodes in the union-find data structure by making them
+      // children of a new node.
+      nodes.Union(node1, node2);
+    }
+    // Sort the nodes in the output array.
+    if (node1>node2) {
+      t_index tmp = node1;
+      node1 = node2;
+      node2 = tmp;
+    }
+    /* Conversion between labeling conventions.
+       Input:  singleton nodes 0,...,N-1
+               compound nodes  N,...,2N-2
+       Output: singleton nodes -1,...,-N
+               compound nodes  1,...,N
+    */
+    merge[i]     = (node1<N) ? -static_cast<int>(node1)-1
+                              : static_cast<int>(node1)-N+1;
+    merge[i+N-1] = (node2<N) ? -static_cast<int>(node2)-1
+                              : static_cast<int>(node2)-N+1;
+    height[i] = Z2[i]->dist;
+    node_size[i] = size_(node1) + size_(node2);
+  }
+
+  order_nodes(N, merge, node_size, order);
+}

selfdrive/controls/lib/cluster/fastcluster_py.py

@@ -0,0 +1,27 @@
+import os
+import numpy as np
+
+from cffi import FFI
+
+cluster_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
+cluster_fn = os.path.join(cluster_dir, "libfastcluster.so")
+
+ffi = FFI()
+ffi.cdef("""
+int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
+void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
+void hclust_pdist(int n, int m, double* pts, double* out);
+void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
+""")
+
+hclust = ffi.dlopen(cluster_fn)
+
+
+def cluster_points_centroid(pts, dist):
+  pts = np.ascontiguousarray(pts, dtype=np.float64)
+  pts_ptr = ffi.cast("double *", pts.ctypes.data)
+  n, m = pts.shape
+
+  labels_ptr = ffi.new("int[]", n)
+  hclust.cluster_points_centroid(n, m, pts_ptr, dist**2, labels_ptr)
+  return list(labels_ptr)

selfdrive/controls/lib/cluster/test.cpp

@@ -0,0 +1,35 @@
+#include <cassert>
+
+extern "C" {
+#include "fastcluster.h"
+}
+
+
+int main(int argc, const char* argv[]){
+  const int n = 11;
+  const int m = 3;
+  double* pts = new double[n*m]{59.26000137, -9.35999966, -5.42500019,
+                                91.61999817, -0.31999999, -2.75,
+                                31.38000031, 0.40000001, -0.2,
+                                89.57999725, -8.07999992, -18.04999924,
+                                53.42000122, 0.63999999, -0.175,
+                                31.38000031, 0.47999999, -0.2,
+                                36.33999939, 0.16, -0.2,
+                                53.33999939, 0.95999998, -0.175,
+                                59.26000137, -9.76000023, -5.44999981,
+                                33.93999977, 0.40000001, -0.22499999,
+                                106.74000092, -5.76000023, -18.04999924};
+
+  int * idx = new int[n];
+  int * correct_idx = new int[n]{0, 1, 2, 3, 4, 2, 5, 4, 0, 5, 6};
+
+  cluster_points_centroid(n, m, pts, 2.5 * 2.5, idx);
+
+  for (int i = 0; i < n; i++){
+    assert(idx[i] == correct_idx[i]);
+  }
+
+  delete[] idx;
+  delete[] correct_idx;
+  delete[] pts;
+}

selfdrive/controls/lib/drive_helpers.py

@@ -0,0 +1,82 @@
+from cereal import car
+from common.numpy_fast import clip, interp
+from selfdrive.config import Conversions as CV
+
+# kph
+V_CRUISE_MAX = 144
+V_CRUISE_MIN = 8
+V_CRUISE_DELTA = 8
+V_CRUISE_ENABLE_MIN = 40
+
+
+class MPC_COST_LAT:
+  PATH = 1.0
+  LANE = 3.0
+  HEADING = 1.0
+  STEER_RATE = 1.0
+
+
+class MPC_COST_LONG:
+  TTC = 5.0
+  DISTANCE = 0.1
+  ACCELERATION = 10.0
+  JERK = 20.0
+
+
+class EventTypes:
+  ENABLE = 'enable'
+  PRE_ENABLE = 'preEnable'
+  NO_ENTRY = 'noEntry'
+  WARNING = 'warning'
+  USER_DISABLE = 'userDisable'
+  SOFT_DISABLE = 'softDisable'
+  IMMEDIATE_DISABLE = 'immediateDisable'
+  PERMANENT = 'permanent'
+
+
+def create_event(name, types):
+  event = car.CarEvent.new_message()
+  event.name = name
+  for t in types:
+    setattr(event, t, True)
+  return event
+
+
+def get_events(events, types):
+  out = []
+  for e in events:
+    for t in types:
+      if getattr(e, t):
+        out.append(e.name)
+  return out
+
+
+def rate_limit(new_value, last_value, dw_step, up_step):
+  return clip(new_value, last_value + dw_step, last_value + up_step)
+
+
+def get_steer_max(CP, v_ego):
+  return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
+
+
+def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
+  # handle button presses. TODO: this should be in state_control, but a decelCruise press
+  # would have the effect of both enabling and changing speed is checked after the state transition
+  for b in buttonEvents:
+    if enabled and not b.pressed:
+      if b.type == "accelCruise":
+        v_cruise_kph += V_CRUISE_DELTA - (v_cruise_kph % V_CRUISE_DELTA)
+      elif b.type == "decelCruise":
+        v_cruise_kph -= V_CRUISE_DELTA - ((V_CRUISE_DELTA - v_cruise_kph) % V_CRUISE_DELTA)
+      v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+
+  return v_cruise_kph
+
+
+def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last):
+  for b in buttonEvents:
+    # 250kph or above probably means we never had a set speed
+    if b.type == "accelCruise" and v_cruise_last < 250:
+      return v_cruise_last
+
+  return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX)))

selfdrive/controls/lib/driver_monitor.py

@@ -0,0 +1,252 @@
+import numpy as np
+from common.realtime import DT_CTRL, DT_DMON
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
+from common.filter_simple import FirstOrderFilter
+from common.stat_live import RunningStatFilter
+
+_AWARENESS_TIME = 100.  # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_PRE_TIME_TILL_TERMINAL = 25.  # a first alert is issued 25s before expiration
+_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15.  # a second alert is issued 15s before start decelerating the car
+_DISTRACTED_TIME = 11.
+_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
+_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
+
+_FACE_THRESHOLD = 0.4
+_EYE_THRESHOLD = 0.6
+_BLINK_THRESHOLD = 0.5 # 0.225
+_BLINK_THRESHOLD_SLACK = 0.65
+_BLINK_THRESHOLD_STRICT = 0.5
+_PITCH_WEIGHT = 1.35 # 1.5  # pitch matters a lot more
+_POSESTD_THRESHOLD = 0.14
+_METRIC_THRESHOLD = 0.4
+_METRIC_THRESHOLD_SLACK = 0.55
+_METRIC_THRESHOLD_STRICT = 0.4
+_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
+_PITCH_NATURAL_OFFSET = 0.02 # 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)
+
+_HI_STD_TIMEOUT = 2
+_DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
+
+_POSE_CALIB_MIN_SPEED = 13 # 30 mph
+_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
+_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
+
+_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
+_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
+
+MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
+MAX_TERMINAL_DURATION = 3000 # 30s
+
+# 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
+
+class DistractedType():
+  NOT_DISTRACTED = 0
+  BAD_POSE = 1
+  BAD_BLINK = 2
+
+def face_orientation_from_net(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
+
+  pitch_net = angles_desc[0]
+  yaw_net = angles_desc[1]
+  roll_net = angles_desc[2]
+
+  face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
+  yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL)
+  pitch_focal_angle = np.arctan2(face_pixel_position[1] - H//2, RESIZED_FOCAL)
+
+  roll = roll_net
+  pitch = pitch_net + pitch_focal_angle
+  yaw = -yaw_net + yaw_focal_angle
+
+  # no calib for roll
+  pitch -= rpy_calib[1]
+  yaw -= rpy_calib[2]
+  return np.array([roll, pitch, yaw])
+
+class DriverPose():
+  def __init__(self):
+    self.yaw = 0.
+    self.pitch = 0.
+    self.roll = 0.
+    self.yaw_std = 0.
+    self.pitch_std = 0.
+    self.roll_std = 0.
+    self.pitch_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
+    self.yaw_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
+    self.low_std = True
+    self.cfactor = 1.
+
+class DriverBlink():
+  def __init__(self):
+    self.left_blink = 0.
+    self.right_blink = 0.
+    self.cfactor = 1.
+
+class DriverStatus():
+  def __init__(self):
+    self.pose = DriverPose()
+    self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
+                            self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
+    self.blink = DriverBlink()
+    self.awareness = 1.
+    self.awareness_active = 1.
+    self.awareness_passive = 1.
+    self.driver_distracted = False
+    self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON)
+    self.face_detected = False
+    self.terminal_alert_cnt = 0
+    self.terminal_time = 0
+    self.step_change = 0.
+    self.active_monitoring_mode = True
+    self.hi_stds = 0
+    self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
+
+    self.is_rhd_region = False
+    self.is_rhd_region_checked = False
+
+    self._set_timers(active_monitoring=True)
+
+  def _set_timers(self, active_monitoring):
+    if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
+      if active_monitoring:
+        self.step_change = DT_CTRL / _DISTRACTED_TIME
+      else:
+        self.step_change = 0.
+      return # no exploit after orange alert
+    elif self.awareness <= 0.:
+      return
+
+    if active_monitoring:
+      # when falling back from passive mode to active mode, reset awareness to avoid false alert
+      if not self.active_monitoring_mode:
+        self.awareness_passive = self.awareness
+        self.awareness = self.awareness_active
+
+      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
+      self.active_monitoring_mode = True
+    else:
+      if self.active_monitoring_mode:
+        self.awareness_active = self.awareness
+        self.awareness = self.awareness_passive
+
+      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
+      self.active_monitoring_mode = False
+
+  def _is_driver_distracted(self, pose, blink):
+    if not self.pose_calibrated:
+      pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
+      yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
+    else:
+      pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
+      yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
+
+    # positive pitch allowance
+    if pitch_error > 0.:
+      pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
+    pitch_error *= _PITCH_WEIGHT
+    pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
+
+    if pose_metric > _METRIC_THRESHOLD*pose.cfactor:
+      return DistractedType.BAD_POSE
+    elif (blink.left_blink + blink.right_blink)*0.5 > _BLINK_THRESHOLD*blink.cfactor:
+      return DistractedType.BAD_BLINK
+    else:
+      return DistractedType.NOT_DISTRACTED
+
+  def set_policy(self, model_data):
+    ep = min(model_data.meta.engagedProb, 0.8) / 0.8
+    self.pose.cfactor = np.interp(ep, [0, 0.5, 1], [_METRIC_THRESHOLD_STRICT, _METRIC_THRESHOLD, _METRIC_THRESHOLD_SLACK])/_METRIC_THRESHOLD
+    self.blink.cfactor = np.interp(ep, [0, 0.5, 1], [_BLINK_THRESHOLD_STRICT, _BLINK_THRESHOLD, _BLINK_THRESHOLD_SLACK])/_BLINK_THRESHOLD
+
+  def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged):
+    # 10 Hz
+    if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0 or len(driver_monitoring.faceOrientationStd) == 0 or len(driver_monitoring.facePositionStd) == 0:
+      return
+
+    self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
+    self.pose.pitch_std = driver_monitoring.faceOrientationStd[0]
+    self.pose.yaw_std = driver_monitoring.faceOrientationStd[1]
+    # self.pose.roll_std = driver_monitoring.faceOrientationStd[2]
+    max_std = np.max([self.pose.pitch_std, self.pose.yaw_std])
+    self.pose.low_std = max_std < _POSESTD_THRESHOLD
+    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 and \
+                          abs(driver_monitoring.facePosition[0]) <= 0.4 and abs(driver_monitoring.facePosition[1]) <= 0.45 and \
+                          not self.is_rhd_region
+
+    self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0
+    # first order filters
+    self.driver_distraction_filter.update(self.driver_distracted)
+
+    # update offseter
+    # only update when driver is actively driving the car above a certain speed
+    if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
+      self.pose.pitch_offseter.push_and_update(self.pose.pitch)
+      self.pose.yaw_offseter.push_and_update(self.pose.yaw)
+
+    self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
+                            self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
+
+    self._set_timers(self.face_detected)
+    if self.face_detected and not self.pose.low_std:
+      self.step_change *= max(0, (max_std-0.5)*(max_std-2))
+      self.hi_stds += 1
+    elif self.face_detected and self.pose.low_std:
+      self.hi_stds = 0
+
+  def update(self, events, driver_engaged, ctrl_active, standstill):
+    if (driver_engaged and self.awareness > 0) or not ctrl_active:
+      # reset only when on disengagement if red reached
+      self.awareness = 1.
+      self.awareness_active = 1.
+      self.awareness_passive = 1.
+      return events
+
+    driver_attentive = self.driver_distraction_filter.x < 0.37
+    awareness_prev = self.awareness
+
+    if self.face_detected and self.hi_stds * DT_DMON > _HI_STD_TIMEOUT:
+      events.append(create_event('driverMonitorLowAcc', [ET.WARNING]))
+
+    if (driver_attentive and self.face_detected and self.awareness > 0):
+      # only restore awareness when paying attention and alert is not red
+      self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
+      if self.awareness == 1.:
+        self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
+      # don't display alert banner when awareness is recovering and has cleared orange
+      if self.awareness > self.threshold_prompt:
+        return events
+
+    # should always be counting if distracted unless at standstill and reaching orange
+    if (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)
+
+    alert = None
+    if self.awareness <= 0.:
+      # terminal red alert: disengagement required
+      alert = 'driverDistracted' if self.active_monitoring_mode else 'driverUnresponsive'
+      self.terminal_time += 1
+      if awareness_prev > 0.:
+        self.terminal_alert_cnt += 1
+    elif self.awareness <= self.threshold_prompt:
+      # prompt orange alert
+      alert = 'promptDriverDistracted' if self.active_monitoring_mode else 'promptDriverUnresponsive'
+    elif self.awareness <= self.threshold_pre:
+      # pre green alert
+      alert = 'preDriverDistracted' if self.active_monitoring_mode else 'preDriverUnresponsive'
+
+    if alert is not None:
+      events.append(create_event(alert, [ET.WARNING]))
+
+    return events

selfdrive/controls/lib/fcw.py

@@ -0,0 +1,77 @@
+import numpy as np
+from collections import defaultdict
+
+from common.numpy_fast import interp
+
+_FCW_A_ACT_V = [-3., -2.]
+_FCW_A_ACT_BP = [0., 30.]
+
+
+class FCWChecker():
+  def __init__(self):
+    self.reset_lead(0.0)
+    self.common_counters = defaultdict(lambda: 0)
+
+  def reset_lead(self, cur_time):
+    self.last_fcw_a = 0.0
+    self.v_lead_max = 0.0
+    self.lead_seen_t = cur_time
+    self.last_fcw_time = 0.0
+    self.last_min_a = 0.0
+
+    self.counters = defaultdict(lambda: 0)
+
+  @staticmethod
+  def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
+    max_ttc = 5.0
+
+    v_rel = v_ego - v_lead
+    a_rel = a_ego - a_lead
+
+    # assuming that closing gap ARel comes from lead vehicle decel,
+    # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
+    # This helps underweighting ARel when v_lead is close to zero.
+    t_decel = 2.
+    a_rel = np.minimum(a_rel, v_lead / t_decel)
+
+    # delta of the quadratic equation to solve for ttc
+    delta = v_rel**2 + 2 * x_lead * a_rel
+
+    # assign an arbitrary high ttc value if there is no solution to ttc
+    if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
+      ttc = max_ttc
+    else:
+      ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
+    return ttc
+
+  def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+    mpc_solution_a = list(mpc_solution[0].a_ego)
+
+    self.last_min_a = min(mpc_solution_a)
+    self.v_lead_max = max(self.v_lead_max, v_lead)
+
+    self.common_counters['blinkers'] = self.common_counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
+    self.common_counters['v_ego'] = self.common_counters['v_ego'] + 1 if v_ego > 5.0 else 0
+
+    if (fcw_lead > 0.99):
+      ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
+      self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
+      self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
+      self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
+      self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
+      self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
+      self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
+
+      a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
+      a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
+
+      future_fcw_allowed = all(c >= 10 for c in self.counters.values())
+      future_fcw_allowed = future_fcw_allowed and all(c >= 10 for c in self.common_counters.values())
+      future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
+
+      if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
+        self.last_fcw_time = cur_time
+        self.last_fcw_a = self.last_min_a
+        return True
+
+    return False

selfdrive/controls/lib/geofence.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+import os
+import json
+import numpy as np
+from common.basedir import BASEDIR
+from common.realtime import sec_since_boot
+from shapely.geometry import Point, Polygon
+
+LATITUDE_DEG_TO_M = 111133 # ~111km per deg is the mean between equator and poles
+GEOFENCE_THRESHOLD = 8.
+LOC_FILTER_F = 0.5   # 0.5Hz
+DT = 0.1             # external gps packets are at 10Hz
+LOC_FILTER_K = 2 * np.pi * LOC_FILTER_F * DT / (1 + 2 * np.pi * LOC_FILTER_F * DT)
+
+class Geofence():
+  def __init__(self, active):
+    self.lat_filt = None
+    self.lon_filt = None
+    self.ts_last_check = 0.
+    self.active = active
+    # hack: does not work at north/south poles, and when longitude is ~180
+    self.in_geofence = not active  # initialize false if geofence is active
+    # get full geofenced polygon in lat and lon coordinates
+    geofence_polygon = np.load(os.path.join(BASEDIR, 'selfdrive/controls/geofence_routes/press_demo.npy'))
+    # for small latitude variations, we can assume constant conversion between longitude and meters (use the first point)
+    self.longitude_deg_to_m = LATITUDE_DEG_TO_M * np.cos(np.radians(geofence_polygon[0,0]))
+    # convert to m
+    geofence_polygon_m = geofence_polygon * LATITUDE_DEG_TO_M
+    geofence_polygon_m[:, 1] = geofence_polygon[:,1] * self.longitude_deg_to_m
+    self.geofence_polygon = Polygon(geofence_polygon_m)
+
+
+  def update_geofence_status(self, gps_loc, params):
+
+    if self.lat_filt is None:
+      # first time we get a location packet
+      self.latitude = gps_loc.latitude
+      self.longitude = gps_loc.longitude
+    else:
+      # apply a filter
+      self.latitude = LOC_FILTER_K * gps_loc.latitude + (1. - LOC_FILTER_K) * self.latitude
+      self.longitude = LOC_FILTER_K * gps_loc.longitude + (1. - LOC_FILTER_K) * self.longitude
+
+    ts = sec_since_boot()
+
+    if ts - self.ts_last_check > 1.:  # tun check at 1Hz, since is computationally intense
+      self.active = params.get("IsGeofenceEnabled") == "1"
+      self.ts_last_check = ts
+
+      p = Point([self.latitude * LATITUDE_DEG_TO_M, self.longitude * self.longitude_deg_to_m])
+
+      # histeresys
+      geofence_distance = self.geofence_polygon.distance(p)
+      if self.in_geofence and geofence_distance > GEOFENCE_THRESHOLD and self.active:
+        self.in_geofence = False
+      elif (not self.in_geofence and geofence_distance < 1.) or not self.active:
+        self.in_geofence = True
+
+
+if __name__ == "__main__":
+  from common.params import Params
+  # for tests
+  params = Params()
+  gf = Geofence(True)
+  class GpsPos():
+    def __init__(self, lat, lon):
+      self.latitude = lat
+      self.longitude = lon
+
+  #pos = GpsPos(37.687347, -122.471117)  # True
+  pos = GpsPos(37.687347, -122.571117)  # False
+  gf.update_geofence_status(pos, params)
+  print(gf.in_geofence)

selfdrive/controls/lib/gps_helpers.py

@@ -0,0 +1,18 @@
+_RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \
+                    ['IN1', 6.75, 28.10, 68.17, 97.4], \
+                    ['IN2', 28.09, 35.99, 72.18, 80.87], \
+                    ['IRL', 51.42, 55.38, -10.58, -5.99], \
+                    ['JP1', 32.66, 45.52, 137.27, 146.02], \
+                    ['JP2', 32.79, 37.60, 131.41, 137.28], \
+                    ['JP3', 24.04, 34.78, 122.93, 131.42], \
+                    ['MY', 0.86, 7.36, 99.64, 119.27], \
+                    ['NZ', -52.61, -29.24, 166, 178.84], \
+                    ['SF', -35.14, -22.13, 16.07, 33.21], \
+                    ['UK', 49.9, 60.84, -8.62, 1.77] ]
+
+def is_rhd_region(latitude, longitude):
+  for region in _RHD_REGION_MAP:
+    if region[1] <= latitude <= region[2] and \
+      region[3] <= longitude <= region[4]:
+      return True
+  return False

selfdrive/controls/lib/lane_planner.py

@@ -0,0 +1,89 @@
+from common.numpy_fast import interp
+import numpy as np
+from cereal import log
+
+CAMERA_OFFSET = 0.06  # m from center car to camera
+
+def compute_path_pinv(l=50):
+  deg = 3
+  x = np.arange(l*1.0)
+  X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
+  pinv = np.linalg.pinv(X)
+  return pinv
+
+
+def model_polyfit(points, path_pinv):
+  return np.dot(path_pinv, [float(x) for x in points])
+
+
+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():
+  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.l_lane_change_prob = 0.
+    self.r_lane_change_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
+
+    if len(md.meta.desirePrediction):
+      self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
+      self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
+
+  def update_d_poly(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
+
+    # 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_d_poly(v_ego)

selfdrive/controls/lib/latcontrol_indi.py

@@ -0,0 +1,122 @@
+import math
+import numpy as np
+
+from cereal import log
+from common.realtime import DT_CTRL
+from common.numpy_fast import clip
+from selfdrive.car.toyota.values import SteerLimitParams
+from selfdrive.car import apply_toyota_steer_torque_limits
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+
+
+class LatControlINDI():
+  def __init__(self, CP):
+    self.angle_steers_des = 0.
+
+    A = np.matrix([[1.0, DT_CTRL, 0.0],
+                   [0.0, 1.0, DT_CTRL],
+                   [0.0, 0.0, 1.0]])
+    C = np.matrix([[1.0, 0.0, 0.0],
+                   [0.0, 1.0, 0.0]])
+
+    # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
+    # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
+
+    # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
+    # K = np.transpose(K)
+    K = np.matrix([[7.30262179e-01, 2.07003658e-04],
+                   [7.29394177e+00, 1.39159419e-02],
+                   [1.71022442e+01, 3.38495381e-02]])
+
+    self.K = K
+    self.A_K = A - np.dot(K, C)
+    self.x = np.matrix([[0.], [0.], [0.]])
+
+    self.enforce_rate_limit = CP.carName == "toyota"
+
+    self.RC = CP.lateralTuning.indi.timeConstant
+    self.G = CP.lateralTuning.indi.actuatorEffectiveness
+    self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
+    self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
+    self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
+
+    self.sat_count_rate = 1.0 * DT_CTRL
+    self.sat_limit = CP.steerLimitTimer
+
+    self.reset()
+
+  def reset(self):
+    self.delayed_output = 0.
+    self.output_steer = 0.
+    self.sat_count = 0.0
+
+  def _check_saturation(self, control, check_saturation, limit):
+    saturated = abs(control) == limit
+
+    if saturated and check_saturation:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+
+    self.sat_count = clip(self.sat_count, 0.0, 1.0)
+
+    return self.sat_count > self.sat_limit
+
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, 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)
+
+    indi_log = log.ControlsState.LateralINDIState.new_message()
+    indi_log.steerAngle = math.degrees(self.x[0])
+    indi_log.steerRate = math.degrees(self.x[1])
+    indi_log.steerAccel = math.degrees(self.x[2])
+
+    if v_ego < 0.3 or not active:
+      indi_log.active = False
+      self.output_steer = 0.0
+      self.delayed_output = 0.0
+    else:
+      self.angle_steers_des = path_plan.angleSteers
+      self.rate_steers_des = path_plan.rateSteers
+
+      steers_des = math.radians(self.angle_steers_des)
+      rate_des = math.radians(self.rate_steers_des)
+
+      # Expected actuator value
+      self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha)
+
+      # Compute acceleration error
+      rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
+      accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
+      accel_error = accel_sp - self.x[2]
+
+      # Compute change in actuator
+      g_inv = 1. / self.G
+      delta_u = g_inv * accel_error
+
+      # Enforce rate limit
+      if self.enforce_rate_limit:
+        steer_max = float(SteerLimitParams.STEER_MAX)
+        new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
+        prev_output_steer_cmd = steer_max * self.output_steer
+        new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams)
+        self.output_steer = new_output_steer_cmd / steer_max
+      else:
+        self.output_steer = self.delayed_output + delta_u
+
+      steers_max = get_steer_max(CP, v_ego)
+      self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+
+      indi_log.active = True
+      indi_log.rateSetPoint = float(rate_sp)
+      indi_log.accelSetPoint = float(accel_sp)
+      indi_log.accelError = float(accel_error)
+      indi_log.delayedOutput = float(self.delayed_output)
+      indi_log.delta = float(delta_u)
+      indi_log.output = float(self.output_steer)
+
+      check_saturation = (v_ego > 10.) and not rate_limited and not steer_override
+      indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
+
+    return float(self.output_steer), float(self.angle_steers_des), indi_log

selfdrive/controls/lib/latcontrol_lqr.py

@@ -0,0 +1,95 @@
+import numpy as np
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from common.numpy_fast import clip
+from common.realtime import DT_CTRL
+from cereal import log
+
+
+class LatControlLQR():
+  def __init__(self, CP):
+    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 * DT_CTRL
+    self.i_rate = 1.0 * DT_CTRL
+
+    self.sat_count_rate = 1.0 * DT_CTRL
+    self.sat_limit = CP.steerLimitTimer
+
+    self.reset()
+
+  def reset(self):
+    self.i_lqr = 0.0
+    self.output_steer = 0.0
+    self.sat_count = 0.0
+
+  def _check_saturation(self, control, check_saturation, limit):
+    saturated = abs(control) == limit
+
+    if saturated and check_saturation:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+
+    self.sat_count = clip(self.sat_count, 0.0, 1.0)
+
+    return self.sat_count > self.sat_limit
+
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+    lqr_log = log.ControlsState.LateralLQRState.new_message()
+
+    steers_max = get_steer_max(CP, v_ego)
+    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
+      lqr_output = 0.
+      self.reset()
+    else:
+      lqr_log.active = True
+
+      # LQR
+      u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+      lqr_output = torque_scale * u_lqr / self.scale
+
+      # Integrator
+      if steer_override:
+        self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
+      else:
+        error = self.angle_steers_des - angle_steers_k
+        i = self.i_lqr + self.ki * self.i_rate * error
+        control = lqr_output + i
+
+        if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
+            (error <= 0 and (control >= -steers_max or i > 0.0))):
+          self.i_lqr = i
+
+      self.output_steer = lqr_output + self.i_lqr
+      self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+
+    check_saturation = (v_ego > 10) and not rate_limited and not steer_override
+    saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
+
+    lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
+    lqr_log.i = self.i_lqr
+    lqr_log.output = self.output_steer
+    lqr_log.lqrOutput = lqr_output
+    lqr_log.saturated = saturated
+    return self.output_steer, float(self.angle_steers_des), lqr_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -0,0 +1,49 @@
+from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from cereal import car
+from cereal import log
+
+
+class LatControlPID():
+  def __init__(self, CP):
+    self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+                            (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                            k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer)
+    self.angle_steers_des = 0.
+
+  def reset(self):
+    self.pid.reset()
+
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+    pid_log = log.ControlsState.LateralPIDState.new_message()
+    pid_log.steerAngle = float(angle_steers)
+    pid_log.steerRate = float(angle_steers_rate)
+
+    if v_ego < 0.3 or not active:
+      output_steer = 0.0
+      pid_log.active = False
+      self.pid.reset()
+    else:
+      self.angle_steers_des = path_plan.angleSteers  # get from MPC/PathPlanner
+
+      steers_max = get_steer_max(CP, v_ego)
+      self.pid.pos_limit = steers_max
+      self.pid.neg_limit = -steers_max
+      steer_feedforward = self.angle_steers_des   # feedforward desired angle
+      if CP.steerControlType == car.CarParams.SteerControlType.torque:
+        # TODO: feedforward something based on path_plan.rateSteers
+        steer_feedforward -= path_plan.angleOffset   # subtract the offset, since it does not contribute to resistive torque
+        steer_feedforward *= v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
+      deadzone = 0.0
+
+      check_saturation = (v_ego > 10) and not rate_limited and not steer_override
+      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override,
+                                     feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
+      pid_log.active = True
+      pid_log.p = self.pid.p
+      pid_log.i = self.pid.i
+      pid_log.f = self.pid.f
+      pid_log.output = output_steer
+      pid_log.saturated = bool(self.pid.saturated)
+
+    return output_steer, float(self.angle_steers_des), pid_log

selfdrive/controls/lib/lateral_mpc/.gitignore

@@ -0,0 +1,2 @@
+generator
+lib_qp/

selfdrive/controls/lib/lateral_mpc/SConscript

@@ -0,0 +1,29 @@
+Import('env', 'arch')
+
+cpp_path = [
+    "#phonelibs/acado/include",
+    "#phonelibs/acado/include/acado",
+    "#phonelibs/qpoases/INCLUDE",
+    "#phonelibs/qpoases/INCLUDE/EXTRAS",
+    "#phonelibs/qpoases/SRC/",
+    "#phonelibs/qpoases",
+    "lib_mpc_export"
+
+]
+
+mpc_files = [
+    "lateral_mpc.c",
+    Glob("lib_mpc_export/*.c"),
+    Glob("lib_mpc_export/*.cpp"),
+]
+
+interface_dir = Dir('lib_mpc_export')
+
+SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+
+env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+# if arch != "aarch64":
+#     acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_casadi.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_csparse.a")]
+#     env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path)

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -0,0 +1,139 @@
+#include <acado_code_generation.hpp>
+
+#define PI 3.1415926536
+#define deg2rad(d) (d/180.0*PI)
+
+const int controlHorizon = 50;
+
+using namespace std;
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState xx; // x position
+  DifferentialState yy; // y position
+  DifferentialState psi; // vehicle heading
+  DifferentialState delta;
+
+  OnlineData curvature_factor;
+  OnlineData v_ref; // m/s
+  OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
+  OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
+  OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3;
+  OnlineData l_prob, r_prob;
+  OnlineData lane_width;
+
+  Control t;
+
+  // Equations of motion
+  f << dot(xx) == v_ref * cos(psi);
+  f << dot(yy) == v_ref * sin(psi);
+  f << dot(psi) == v_ref * delta * curvature_factor;
+  f << dot(delta) == t;
+
+  auto lr_prob = l_prob + r_prob - l_prob * r_prob;
+
+  auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
+  auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
+  auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3;
+
+  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) * (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);
+
+  // Running cost
+  Function h;
+
+  // Distance errors
+  h << poly_d - yy;
+  h << lr_prob * c_left_lane;
+  h << lr_prob * c_right_lane;
+
+  // Heading error
+  h << (v_ref + 1.0 ) * (angle_d - psi);
+
+  // Angular rate error
+  h << (v_ref + 1.0 ) * t;
+
+  BMatrix Q(5,5); Q.setAll(true);
+  // Q(0,0) = 1.0;
+  // Q(1,1) = 1.0;
+  // Q(2,2) = 1.0;
+  // Q(3,3) = 1.0;
+  // Q(4,4) = 2.0;
+
+  // Terminal cost
+  Function hN;
+
+  // Distance errors
+  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_d - psi);
+
+  BMatrix QN(4,4); QN.setAll(true);
+  // QN(0,0) = 1.0;
+  // QN(1,1) = 1.0;
+  // QN(2,2) = 1.0;
+  // QN(3,3) = 1.0;
+
+  // Non uniform time grid
+  // First 5 timesteps are 0.05, after that it's 0.15
+  DMatrix numSteps(20, 1);
+  for (int i = 0; i < 5; i++){
+    numSteps(i) = 1;
+  }
+  for (int i = 5; i < 20; i++){
+    numSteps(i) = 3;
+  }
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = 2.5;
+
+  OCP ocp( tStart, tEnd, numSteps);
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  // car can't go backward to avoid "circles"
+  ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
+  // more than absolute max steer angle
+  ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
+  ocp.setNOD(17);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}

selfdrive/controls/lib/lateral_mpc/lateral_mpc.c

@@ -0,0 +1,134 @@
+#include "acado_common.h"
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+#define NX          ACADO_NX  /* Number of differential state variables.  */
+#define NXA         ACADO_NXA /* Number of algebraic variables. */
+#define NU          ACADO_NU  /* Number of control inputs. */
+#define NOD         ACADO_NOD  /* Number of online data values. */
+
+#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
+#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
+
+#define N           ACADO_N   /* Number of intervals in the horizon. */
+
+ACADOvariables acadoVariables;
+ACADOworkspace acadoWorkspace;
+
+typedef struct {
+  double x, y, psi, delta, t;
+} state_t;
+
+
+typedef struct {
+  double x[N+1];
+  double y[N+1];
+  double psi[N+1];
+  double delta[N+1];
+  double rate[N];
+  double cost;
+} log_t;
+
+void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost){
+  int    i;
+  const int STEP_MULTIPLIER = 3;
+
+  for (i = 0; i < N; i++) {
+    int f = 1;
+    if (i > 4){
+      f = STEP_MULTIPLIER;
+    }
+    // Setup diagonal entries
+    acadoVariables.W[NY*NY*i + (NY+1)*0] = pathCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*1] = laneCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*2] = laneCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*3] = headingCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*4] = steerRateCost * f;
+  }
+  acadoVariables.WN[(NYN+1)*0] = pathCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*1] = laneCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*2] = laneCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*3] = headingCost * STEP_MULTIPLIER;
+}
+
+void init(double pathCost, double laneCost, double headingCost, double steerRateCost){
+  acado_initializeSolver();
+  int    i;
+
+  /* Initialize the states and controls. */
+  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.1;
+
+  /* Initialize the measurements/reference. */
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+
+  /* MPC: initialize the current state feedback. */
+  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+
+  init_weights(pathCost, laneCost, headingCost, steerRateCost);
+}
+
+int run_mpc(state_t * x0, log_t * solution,
+             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;
+
+  for (i = 0; i <= NOD * N; i+= NOD){
+    acadoVariables.od[i] = curvature_factor;
+    acadoVariables.od[i+1] = v_ref;
+
+    acadoVariables.od[i+2] = l_poly[0];
+    acadoVariables.od[i+3] = l_poly[1];
+    acadoVariables.od[i+4] = l_poly[2];
+    acadoVariables.od[i+5] = l_poly[3];
+
+    acadoVariables.od[i+6] = r_poly[0];
+    acadoVariables.od[i+7] = r_poly[1];
+    acadoVariables.od[i+8] = r_poly[2];
+    acadoVariables.od[i+9] = r_poly[3];
+
+    acadoVariables.od[i+10] = 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] = lane_width;
+
+  }
+
+  acadoVariables.x0[0] = x0->x;
+  acadoVariables.x0[1] = x0->y;
+  acadoVariables.x0[2] = x0->psi;
+  acadoVariables.x0[3] = x0->delta;
+
+
+  acado_preparationStep();
+  acado_feedbackStep();
+
+  /* printf("lat its: %d\n", acado_getNWSR());  // n iterations
+  printf("Objective: %.6f\n", acado_getObjective());  // solution cost */
+
+  for (i = 0; i <= N; i++){
+    solution->x[i] = acadoVariables.x[i*NX];
+    solution->y[i] = acadoVariables.x[i*NX+1];
+    solution->psi[i] = acadoVariables.x[i*NX+2];
+    solution->delta[i] = acadoVariables.x[i*NX+3];
+    if (i < N){
+      solution->rate[i] = acadoVariables.u[i];
+    }
+  }
+  solution->cost = acado_getObjective();
+
+  // Dont shift states here. Current solution is closer to next timestep than if
+  // we use the old solution as a starting point
+  //acado_shiftStates(2, 0, 0);
+  //acado_shiftControls( 0 );
+
+  return acado_getNWSR();
+}

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

@@ -0,0 +1,212 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+real_t* acado_getVariablesX( )
+{
+	return acadoVariables.x;
+}
+
+real_t* acado_getVariablesU( )
+{
+	return acadoVariables.u;
+}
+
+#if ACADO_NY > 0
+real_t* acado_getVariablesY( )
+{
+	return acadoVariables.y;
+}
+#endif
+
+#if ACADO_NYN > 0
+real_t* acado_getVariablesYN( )
+{
+	return acadoVariables.yN;
+}
+#endif
+
+real_t* acado_getVariablesX0( )
+{
+#if ACADO_INITIAL_VALUE_FIXED
+	return acadoVariables.x0;
+#else
+	return 0;
+#endif
+}
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( )
+{
+	int i, j;
+	printf("\nDifferential variables:\n[\n");
+	for (i = 0; i < ACADO_N + 1; ++i)
+	{
+		for (j = 0; j < ACADO_NX; ++j)
+			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print control variables. */
+void acado_printControlVariables( )
+{
+	int i, j;
+	printf("\nControl variables:\n[\n");
+	for (i = 0; i < ACADO_N; ++i)
+	{
+		for (j = 0; j < ACADO_NU; ++j)
+			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( )
+{
+	printf(
+		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
+		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
+		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
+	);
+	
+	printf(
+		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
+		"supervision of Moritz Diehl. All rights reserved.\n\n"
+		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
+		"General Public License 3 in the hope that it will be useful,\n"
+		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
+		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
+		"GNU Lesser General Public License for more details.\n\n"
+	);
+}
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+void acado_tic( acado_timer* t )
+{
+	QueryPerformanceFrequency(&t->freq);
+	QueryPerformanceCounter(&t->tic);
+}
+
+real_t acado_toc( acado_timer* t )
+{
+	QueryPerformanceCounter(&t->toc);
+	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
+}
+
+
+#elif (defined __APPLE__)
+
+void acado_tic( acado_timer* t )
+{
+    /* read current clock cycles */
+    t->tic = mach_absolute_time();
+}
+
+real_t acado_toc( acado_timer* t )
+{
+
+    uint64_t duration; /* elapsed time in clock cycles*/
+    
+    t->toc = mach_absolute_time();
+    duration = t->toc - t->tic;
+    
+    /*conversion from clock cycles to nanoseconds*/
+    mach_timebase_info(&(t->tinfo));
+    duration *= t->tinfo.numer;
+    duration /= t->tinfo.denom;
+
+    return (real_t)duration / 1e9;
+}
+
+#else
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	gettimeofday(&t->tic, 0);
+}
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timeval temp;
+	
+	gettimeofday(&t->toc, 0);
+    
+	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
+}
+
+#else
+/* ANSI */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	clock_gettime(CLOCK_MONOTONIC, &t->tic);
+}
+
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timespec temp;
+    
+	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
+    
+	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
+}
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || _WIN64) */
+
+#endif

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

@@ -0,0 +1,138 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_AUXILIARY_FUNCTIONS_H
+#define ACADO_AUXILIARY_FUNCTIONS_H
+
+#include "acado_common.h"
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** Get pointer to the matrix with differential variables. */
+real_t* acado_getVariablesX( );
+
+/** Get pointer to the matrix with control variables. */
+real_t* acado_getVariablesU( );
+
+#if ACADO_NY > 0
+/** Get pointer to the matrix with references/measurements. */
+real_t* acado_getVariablesY( );
+#endif
+
+#if ACADO_NYN > 0
+/** Get pointer to the vector with references/measurement on the last node. */
+real_t* acado_getVariablesYN( );
+#endif
+
+/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
+real_t* acado_getVariablesX0( );
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( );
+
+/** Print control variables. */
+void acado_printControlVariables( );
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( );
+
+/*
+ * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
+ * providing us with the following timing routines.
+ */
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+/* Use Windows QueryPerformanceCounter for timing. */
+#include <Windows.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	LARGE_INTEGER tic;
+	LARGE_INTEGER toc;
+	LARGE_INTEGER freq;
+} acado_timer;
+
+
+#elif (defined __APPLE__)
+
+#include "unistd.h"
+#include <mach/mach_time.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	uint64_t tic;
+	uint64_t toc;
+	mach_timebase_info_data_t tinfo;
+} acado_timer;
+
+#else
+
+/* Use POSIX clock_gettime() for timing on non-Windows machines. */
+#include <time.h>
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode of operation. */
+
+#include <sys/stat.h>
+#include <sys/time.h>
+
+typedef struct acado_timer_
+{
+	struct timeval tic;
+	struct timeval toc;
+} acado_timer;
+
+#else
+/* ANSI C */
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	struct timespec tic;
+	struct timespec toc;
+} acado_timer;
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || defined _WIN64) */
+
+/** A function for measurement of the current time. */
+void acado_tic( acado_timer* t );
+
+/** A function which returns the elapsed time. */
+real_t acado_toc( acado_timer* t );
+
+#endif
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_AUXILIARY_FUNCTIONS_H */

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

@@ -0,0 +1,357 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_COMMON_H
+#define ACADO_COMMON_H
+
+#include <math.h>
+#include <string.h>
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** \defgroup ACADO ACADO CGT generated module. */
+/** @{ */
+
+/** qpOASES QP solver indicator. */
+#define ACADO_QPOASES  0
+#define ACADO_QPOASES3 1
+/** FORCES QP solver indicator.*/
+#define ACADO_FORCES   2
+/** qpDUNES QP solver indicator.*/
+#define ACADO_QPDUNES  3
+/** HPMPC QP solver indicator. */
+#define ACADO_HPMPC    4
+#define ACADO_GENERIC    5
+
+/** Indicator for determining the QP solver used by the ACADO solver code. */
+#define ACADO_QP_SOLVER ACADO_QPOASES
+
+#include "acado_qpoases_interface.hpp"
+
+
+/*
+ * Common definitions
+ */
+/** User defined block based condensing. */
+#define ACADO_BLOCK_CONDENSING 0
+/** Compute covariance matrix of the last state estimate. */
+#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
+/** Flag indicating whether constraint values are hard-coded or not. */
+#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
+/** Indicator for fixed initial state. */
+#define ACADO_INITIAL_STATE_FIXED 1
+/** Number of control/estimation intervals. */
+#define ACADO_N 20
+/** Number of online data values. */
+#define ACADO_NOD 17
+/** Number of path constraints. */
+#define ACADO_NPAC 0
+/** Number of control variables. */
+#define ACADO_NU 1
+/** Number of differential variables. */
+#define ACADO_NX 4
+/** Number of algebraic variables. */
+#define ACADO_NXA 0
+/** Number of differential derivative variables. */
+#define ACADO_NXD 0
+/** Number of references/measurements per node on the first N nodes. */
+#define ACADO_NY 5
+/** Number of references/measurements on the last (N + 1)st node. */
+#define ACADO_NYN 4
+/** Total number of QP optimization variables. */
+#define ACADO_QP_NV 24
+/** Number of Runge-Kutta stages per integration step. */
+#define ACADO_RK_NSTAGES 4
+/** Providing interface for arrival cost. */
+#define ACADO_USE_ARRIVAL_COST 0
+/** Indicator for usage of non-hard-coded linear terms in the objective. */
+#define ACADO_USE_LINEAR_TERMS 0
+/** Indicator for type of fixed weighting matrices. */
+#define ACADO_WEIGHTING_MATRICES_TYPE 2
+
+
+/*
+ * Globally used structure definitions
+ */
+
+/** The structure containing the user data.
+ * 
+ *  Via this structure the user "communicates" with the solver code.
+ */
+typedef struct ACADOvariables_
+{
+int dummy;
+/** Matrix of size: 21 x 4 (row major format)
+ * 
+ *  Matrix containing 21 differential variable vectors.
+ */
+real_t x[ 84 ];
+
+/** Column vector of size: 20
+ * 
+ *  Matrix containing 20 control variable vectors.
+ */
+real_t u[ 20 ];
+
+/** Matrix of size: 21 x 17 (row major format)
+ * 
+ *  Matrix containing 21 online data vectors.
+ */
+real_t od[ 357 ];
+
+/** Column vector of size: 100
+ * 
+ *  Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes.
+ */
+real_t y[ 100 ];
+
+/** Column vector of size: 4
+ * 
+ *  Reference/measurement vector for the 21. node.
+ */
+real_t yN[ 4 ];
+
+/** Matrix of size: 100 x 5 (row major format) */
+real_t W[ 500 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t WN[ 16 ];
+
+/** Column vector of size: 4
+ * 
+ *  Current state feedback vector.
+ */
+real_t x0[ 4 ];
+
+
+} ACADOvariables;
+
+/** Private workspace used by the auto-generated code.
+ * 
+ *  Data members of this structure are private to the solver.
+ *  In other words, the user code should not modify values of this 
+ *  structure. 
+ */
+typedef struct ACADOworkspace_
+{
+/** Column vector of size: 14 */
+real_t rhs_aux[ 14 ];
+
+real_t rk_ttt;
+
+/** Row vector of size: 42 */
+real_t rk_xxx[ 42 ];
+
+/** Matrix of size: 4 x 24 (row major format) */
+real_t rk_kkk[ 96 ];
+
+/** Row vector of size: 42 */
+real_t state[ 42 ];
+
+/** Column vector of size: 80 */
+real_t d[ 80 ];
+
+/** Column vector of size: 100 */
+real_t Dy[ 100 ];
+
+/** Column vector of size: 4 */
+real_t DyN[ 4 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t evGx[ 320 ];
+
+/** Column vector of size: 80 */
+real_t evGu[ 80 ];
+
+/** Column vector of size: 11 */
+real_t objAuxVar[ 11 ];
+
+/** Row vector of size: 22 */
+real_t objValueIn[ 22 ];
+
+/** Row vector of size: 30 */
+real_t objValueOut[ 30 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t Q1[ 320 ];
+
+/** Matrix of size: 80 x 5 (row major format) */
+real_t Q2[ 400 ];
+
+/** Column vector of size: 20 */
+real_t R1[ 20 ];
+
+/** Matrix of size: 20 x 5 (row major format) */
+real_t R2[ 100 ];
+
+/** Column vector of size: 80 */
+real_t S1[ 80 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t QN1[ 16 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t QN2[ 16 ];
+
+/** Column vector of size: 4 */
+real_t Dx0[ 4 ];
+
+/** Matrix of size: 4 x 4 (row major format) */
+real_t T[ 16 ];
+
+/** Column vector of size: 840 */
+real_t E[ 840 ];
+
+/** Column vector of size: 840 */
+real_t QE[ 840 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t QGx[ 320 ];
+
+/** Column vector of size: 80 */
+real_t Qd[ 80 ];
+
+/** Column vector of size: 84 */
+real_t QDy[ 84 ];
+
+/** Matrix of size: 20 x 4 (row major format) */
+real_t H10[ 80 ];
+
+/** Matrix of size: 24 x 24 (row major format) */
+real_t H[ 576 ];
+
+/** Matrix of size: 40 x 24 (row major format) */
+real_t A[ 960 ];
+
+/** Column vector of size: 24 */
+real_t g[ 24 ];
+
+/** Column vector of size: 24 */
+real_t lb[ 24 ];
+
+/** Column vector of size: 24 */
+real_t ub[ 24 ];
+
+/** Column vector of size: 40 */
+real_t lbA[ 40 ];
+
+/** Column vector of size: 40 */
+real_t ubA[ 40 ];
+
+/** Column vector of size: 24 */
+real_t x[ 24 ];
+
+/** Column vector of size: 64 */
+real_t y[ 64 ];
+
+
+} ACADOworkspace;
+
+/* 
+ * Forward function declarations. 
+ */
+
+
+/** Performs the integration and sensitivity propagation for one shooting interval.
+ *
+ *  \param rk_eta Working array to pass the input values and return the results.
+ *  \param resetIntegrator The internal memory of the integrator can be reset.
+ *  \param rk_index Number of the shooting interval.
+ *
+ *  \return Status code of the integrator.
+ */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
+
+/** Export of an ACADO symbolic function.
+ *
+ *  \param in Input to the exported function.
+ *  \param out Output of the exported function.
+ */
+void acado_rhs_forw(const real_t* in, real_t* out);
+
+/** Preparation step of the RTI scheme.
+ *
+ *  \return Status of the integration module. =0: OK, otherwise the error code.
+ */
+int acado_preparationStep(  );
+
+/** Feedback/estimation step of the RTI scheme.
+ *
+ *  \return Status code of the qpOASES QP solver.
+ */
+int acado_feedbackStep(  );
+
+/** Solver initialization. Must be called once before any other function call.
+ *
+ *  \return =0: OK, otherwise an error code of a QP solver.
+ */
+int acado_initializeSolver(  );
+
+/** Initialize shooting nodes by a forward simulation starting from the first node.
+ */
+void acado_initializeNodesByForwardSimulation(  );
+
+/** Shift differential variables vector by one interval.
+ *
+ *  \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation.
+ *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
+
+/** Shift controls vector by one interval.
+ *
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftControls( real_t* const uEnd );
+
+/** Get the KKT tolerance of the current iterate.
+ *
+ *  \return The KKT tolerance value.
+ */
+real_t acado_getKKT(  );
+
+/** Calculate the objective value.
+ *
+ *  \return Value of the objective function.
+ */
+real_t acado_getObjective(  );
+
+
+/* 
+ * Extern declarations. 
+ */
+
+extern ACADOworkspace acadoWorkspace;
+extern ACADOvariables acadoVariables;
+
+/** @} */
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_COMMON_H */

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

@@ -0,0 +1,255 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_common.h"
+
+
+void acado_rhs_forw(const real_t* in, real_t* out)
+{
+const real_t* xd = in;
+const real_t* u = in + 24;
+const real_t* od = in + 25;
+/* Vector of auxiliary variables; number of elements: 14. */
+real_t* a = acadoWorkspace.rhs_aux;
+
+/* Compute intermediate quantities: */
+a[0] = (cos(xd[2]));
+a[1] = (sin(xd[2]));
+a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2])));
+a[3] = (xd[12]*a[2]);
+a[4] = (xd[13]*a[2]);
+a[5] = (xd[14]*a[2]);
+a[6] = (xd[15]*a[2]);
+a[7] = (cos(xd[2]));
+a[8] = (xd[12]*a[7]);
+a[9] = (xd[13]*a[7]);
+a[10] = (xd[14]*a[7]);
+a[11] = (xd[15]*a[7]);
+a[12] = (xd[22]*a[2]);
+a[13] = (xd[22]*a[7]);
+
+/* Compute outputs: */
+out[0] = (od[1]*a[0]);
+out[1] = (od[1]*a[1]);
+out[2] = ((od[1]*xd[3])*od[0]);
+out[3] = u[0];
+out[4] = (od[1]*a[3]);
+out[5] = (od[1]*a[4]);
+out[6] = (od[1]*a[5]);
+out[7] = (od[1]*a[6]);
+out[8] = (od[1]*a[8]);
+out[9] = (od[1]*a[9]);
+out[10] = (od[1]*a[10]);
+out[11] = (od[1]*a[11]);
+out[12] = ((od[1]*xd[16])*od[0]);
+out[13] = ((od[1]*xd[17])*od[0]);
+out[14] = ((od[1]*xd[18])*od[0]);
+out[15] = ((od[1]*xd[19])*od[0]);
+out[16] = (real_t)(0.0000000000000000e+00);
+out[17] = (real_t)(0.0000000000000000e+00);
+out[18] = (real_t)(0.0000000000000000e+00);
+out[19] = (real_t)(0.0000000000000000e+00);
+out[20] = (od[1]*a[12]);
+out[21] = (od[1]*a[13]);
+out[22] = ((od[1]*xd[23])*od[0]);
+out[23] = (real_t)(1.0000000000000000e+00);
+}
+
+/* Fixed step size:0.05 */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
+{
+int error;
+
+int run1;
+int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
+int numInts = numSteps[rk_index];
+acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
+rk_eta[4] = 1.0000000000000000e+00;
+rk_eta[5] = 0.0000000000000000e+00;
+rk_eta[6] = 0.0000000000000000e+00;
+rk_eta[7] = 0.0000000000000000e+00;
+rk_eta[8] = 0.0000000000000000e+00;
+rk_eta[9] = 1.0000000000000000e+00;
+rk_eta[10] = 0.0000000000000000e+00;
+rk_eta[11] = 0.0000000000000000e+00;
+rk_eta[12] = 0.0000000000000000e+00;
+rk_eta[13] = 0.0000000000000000e+00;
+rk_eta[14] = 1.0000000000000000e+00;
+rk_eta[15] = 0.0000000000000000e+00;
+rk_eta[16] = 0.0000000000000000e+00;
+rk_eta[17] = 0.0000000000000000e+00;
+rk_eta[18] = 0.0000000000000000e+00;
+rk_eta[19] = 1.0000000000000000e+00;
+rk_eta[20] = 0.0000000000000000e+00;
+rk_eta[21] = 0.0000000000000000e+00;
+rk_eta[22] = 0.0000000000000000e+00;
+rk_eta[23] = 0.0000000000000000e+00;
+acadoWorkspace.rk_xxx[24] = rk_eta[24];
+acadoWorkspace.rk_xxx[25] = rk_eta[25];
+acadoWorkspace.rk_xxx[26] = rk_eta[26];
+acadoWorkspace.rk_xxx[27] = rk_eta[27];
+acadoWorkspace.rk_xxx[28] = rk_eta[28];
+acadoWorkspace.rk_xxx[29] = rk_eta[29];
+acadoWorkspace.rk_xxx[30] = rk_eta[30];
+acadoWorkspace.rk_xxx[31] = rk_eta[31];
+acadoWorkspace.rk_xxx[32] = rk_eta[32];
+acadoWorkspace.rk_xxx[33] = rk_eta[33];
+acadoWorkspace.rk_xxx[34] = rk_eta[34];
+acadoWorkspace.rk_xxx[35] = rk_eta[35];
+acadoWorkspace.rk_xxx[36] = rk_eta[36];
+acadoWorkspace.rk_xxx[37] = rk_eta[37];
+acadoWorkspace.rk_xxx[38] = rk_eta[38];
+acadoWorkspace.rk_xxx[39] = rk_eta[39];
+acadoWorkspace.rk_xxx[40] = rk_eta[40];
+acadoWorkspace.rk_xxx[41] = rk_eta[41];
+
+for (run1 = 0; run1 < 1; ++run1)
+{
+for(run1 = 0; run1 < numInts; run1++ ) {
+acadoWorkspace.rk_xxx[0] = + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
+acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
+acadoWorkspace.rk_xxx[23] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
+rk_eta[0] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[72];
+rk_eta[1] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[73];
+rk_eta[2] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[74];
+rk_eta[3] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[75];
+rk_eta[4] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[76];
+rk_eta[5] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[77];
+rk_eta[6] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[78];
+rk_eta[7] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[79];
+rk_eta[8] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[80];
+rk_eta[9] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[81];
+rk_eta[10] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[82];
+rk_eta[11] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[83];
+rk_eta[12] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[84];
+rk_eta[13] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[85];
+rk_eta[14] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[86];
+rk_eta[15] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[87];
+rk_eta[16] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[88];
+rk_eta[17] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[89];
+rk_eta[18] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[90];
+rk_eta[19] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[91];
+rk_eta[20] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[92];
+rk_eta[21] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[93];
+rk_eta[22] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[94];
+rk_eta[23] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[95];
+acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
+}
+}
+error = 0;
+return error;
+}
+

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

@@ -0,0 +1,70 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+extern "C"
+{
+#include "acado_common.h"
+}
+
+#include "INCLUDE/QProblem.hpp"
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+static int acado_nWSR;
+
+
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+static SolutionAnalysis acado_sa;
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+int acado_solve( void )
+{
+	acado_nWSR = QPOASES_NWSRMAX;
+
+	QProblem qp(24, 40);
+	
+	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
+
+    qp.getPrimalSolution( acadoWorkspace.x );
+    qp.getDualSolution( acadoWorkspace.y );
+	
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+
+	if (retVal != SUCCESSFUL_RETURN)
+		return (int)retVal;
+		
+	retVal = acado_sa.getHessianInverse( &qp,var );
+
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+	return (int)retVal;
+}
+
+int acado_getNWSR( void )
+{
+	return acado_nWSR;
+}
+
+const char* acado_getErrorString( int error )
+{
+	return MessageHandling::getErrorString( error );
+}

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

@@ -0,0 +1,65 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef QPOASES_HEADER
+#define QPOASES_HEADER
+
+#ifdef PC_DEBUG
+#include <stdio.h>
+#endif /* PC_DEBUG */
+
+#include <math.h>
+
+#ifdef __cplusplus
+#define EXTERNC extern "C"
+#else
+#define EXTERNC
+#endif
+
+/*
+ * A set of options for qpOASES
+ */
+
+/** Maximum number of optimization variables. */
+#define QPOASES_NVMAX      24
+/** Maximum number of constraints. */
+#define QPOASES_NCMAX      40
+/** Maximum number of working set recalculations. */
+#define QPOASES_NWSRMAX    500
+/** Print level for qpOASES. */
+#define QPOASES_PRINTLEVEL PL_NONE
+/** The value of EPS */
+#define QPOASES_EPS        2.221e-16
+/** Internally used floating point type */
+typedef double real_t;
+
+/*
+ * Forward function declarations
+ */
+
+/** A function that calls the QP solver */
+EXTERNC int acado_solve( void );
+
+/** Get the number of active set changes */
+EXTERNC int acado_getNWSR( void );
+
+/** Get the error string. */
+const char* acado_getErrorString( int error );
+
+#endif /* QPOASES_HEADER */

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -0,0 +1,30 @@
+import os
+
+from cffi import FFI
+
+mpc_dir = os.path.dirname(os.path.abspath(__file__))
+libmpc_fn = os.path.join(mpc_dir, "libmpc.so")
+
+ffi = FFI()
+ffi.cdef("""
+typedef struct {
+    double x, y, psi, delta, t;
+} state_t;
+
+typedef struct {
+    double x[21];
+    double y[21];
+    double psi[21];
+    double delta[21];
+    double rate[20];
+    double cost;
+} log_t;
+
+void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
+void init_weights(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 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/long_mpc.py

@@ -0,0 +1,123 @@
+import os
+import math
+
+import cereal.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from common.realtime import sec_since_boot
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
+
+LOG_MPC = os.environ.get('LOG_MPC', False)
+
+
+class LongitudinalMpc():
+  def __init__(self, mpc_id):
+    self.mpc_id = mpc_id
+
+    self.setup_mpc()
+    self.v_mpc = 0.0
+    self.v_mpc_future = 0.0
+    self.a_mpc = 0.0
+    self.v_cruise = 0.0
+    self.prev_lead_status = False
+    self.prev_lead_x = 0.0
+    self.new_lead = False
+
+    self.last_cloudlog_t = 0.0
+
+  def send_mpc_solution(self, pm, qp_iterations, calculation_time):
+    qp_iterations = max(0, qp_iterations)
+    dat = messaging.new_message()
+    dat.init('liveLongitudinalMpc')
+    dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
+    dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
+    dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
+    dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
+    dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
+    dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
+    dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
+    dat.liveLongitudinalMpc.qpIterations = qp_iterations
+    dat.liveLongitudinalMpc.mpcId = self.mpc_id
+    dat.liveLongitudinalMpc.calculationTime = calculation_time
+    pm.send('liveLongitudinalMpc', dat)
+
+  def setup_mpc(self):
+    ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
+    self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+                     MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+
+    self.mpc_solution = ffi.new("log_t *")
+    self.cur_state = ffi.new("state_t *")
+    self.cur_state[0].v_ego = 0
+    self.cur_state[0].a_ego = 0
+    self.a_lead_tau = _LEAD_ACCEL_TAU
+
+  def set_cur_state(self, v, a):
+    self.cur_state[0].v_ego = v
+    self.cur_state[0].a_ego = a
+
+  def update(self, pm, CS, lead, v_cruise_setpoint):
+    v_ego = CS.vEgo
+
+    # Setup current mpc state
+    self.cur_state[0].x_ego = 0.0
+
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = max(0.0, lead.vLead)
+      a_lead = lead.aLeadK
+
+      if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
+        v_lead = 0.0
+        a_lead = 0.0
+
+      self.a_lead_tau = lead.aLeadTau
+      self.new_lead = False
+      if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
+        self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
+        self.new_lead = True
+
+      self.prev_lead_status = True
+      self.prev_lead_x = x_lead
+      self.cur_state[0].x_l = x_lead
+      self.cur_state[0].v_l = v_lead
+    else:
+      self.prev_lead_status = False
+      # Fake a fast lead car, so mpc keeps running
+      self.cur_state[0].x_l = 50.0
+      self.cur_state[0].v_l = v_ego + 10.0
+      a_lead = 0.0
+      self.a_lead_tau = _LEAD_ACCEL_TAU
+
+    # Calculate mpc
+    t = sec_since_boot()
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
+    duration = int((sec_since_boot() - t) * 1e9)
+
+    if LOG_MPC:
+      self.send_mpc_solution(pm, n_its, duration)
+
+    # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
+    self.v_mpc = self.mpc_solution[0].v_ego[1]
+    self.a_mpc = self.mpc_solution[0].a_ego[1]
+    self.v_mpc_future = self.mpc_solution[0].v_ego[10]
+
+    # Reset if NaN or goes through lead car
+    crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
+    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
+    backwards = min(self.mpc_solution[0].v_ego) < -0.01
+
+    if ((backwards or crashing) and self.prev_lead_status) or nans:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
+                          self.mpc_id, backwards, crashing, nans))
+
+      self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+                       MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+      self.cur_state[0].v_ego = v_ego
+      self.cur_state[0].a_ego = 0.0
+      self.v_mpc = v_ego
+      self.a_mpc = CS.aEgo
+      self.prev_lead_status = False

selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,130 @@
+from cereal import log
+from common.numpy_fast import clip, interp
+from selfdrive.controls.lib.pid import PIController
+
+LongCtrlState = log.ControlsState.LongControlState
+
+STOPPING_EGO_SPEED = 0.5
+MIN_CAN_SPEED = 0.3  # TODO: parametrize this in car interface
+STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
+STARTING_TARGET_SPEED = 0.5
+BRAKE_THRESHOLD_TO_PID = 0.2
+
+STOPPING_BRAKE_RATE = 0.2  # brake_travel/s while trying to stop
+STARTING_BRAKE_RATE = 0.8  # brake_travel/s while releasing on restart
+BRAKE_STOPPING_TARGET = 0.5  # apply at least this amount of brake to maintain the vehicle stationary
+
+_MAX_SPEED_ERROR_BP = [0., 30.]  # speed breakpoints
+_MAX_SPEED_ERROR_V = [1.5, .8]  # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
+
+RATE = 100.0
+
+
+def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
+                             output_gb, brake_pressed, cruise_standstill):
+  """Update longitudinal control state machine"""
+  stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
+                       (v_ego < STOPPING_EGO_SPEED and \
+                        ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or
+                        brake_pressed))
+
+  starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill
+
+  if not active:
+    long_control_state = LongCtrlState.off
+
+  else:
+    if long_control_state == LongCtrlState.off:
+      if active:
+        long_control_state = LongCtrlState.pid
+
+    elif long_control_state == LongCtrlState.pid:
+      if stopping_condition:
+        long_control_state = LongCtrlState.stopping
+
+    elif long_control_state == LongCtrlState.stopping:
+      if starting_condition:
+        long_control_state = LongCtrlState.starting
+
+    elif long_control_state == LongCtrlState.starting:
+      if stopping_condition:
+        long_control_state = LongCtrlState.stopping
+      elif output_gb >= -BRAKE_THRESHOLD_TO_PID:
+        long_control_state = LongCtrlState.pid
+
+  return long_control_state
+
+
+class LongControl():
+  def __init__(self, CP, compute_gb):
+    self.long_control_state = LongCtrlState.off  # initialized to off
+    self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                            (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                            rate=RATE,
+                            sat_limit=0.8,
+                            convert=compute_gb)
+    self.v_pid = 0.0
+    self.last_output_gb = 0.0
+
+  def reset(self, v_pid):
+    """Reset PID controller and change setpoint"""
+    self.pid.reset()
+    self.v_pid = v_pid
+
+  def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP):
+    """Update longitudinal control. This updates the state machine and runs a PID loop"""
+    # Actuation limits
+    gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
+    brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
+
+    # Update state machine
+    output_gb = self.last_output_gb
+    self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,
+                                                       v_target_future, self.v_pid, output_gb,
+                                                       brake_pressed, cruise_standstill)
+
+    v_ego_pid = max(v_ego, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+
+    if self.long_control_state == LongCtrlState.off:
+      self.v_pid = v_ego_pid
+      self.pid.reset()
+      output_gb = 0.
+
+    # tracking objects and driving
+    elif self.long_control_state == LongCtrlState.pid:
+      self.v_pid = v_target
+      self.pid.pos_limit = gas_max
+      self.pid.neg_limit = - brake_max
+
+      # Toyota starts braking more when it thinks you want to stop
+      # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
+      prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
+      deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
+
+      output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
+
+      if prevent_overshoot:
+        output_gb = min(output_gb, 0.0)
+
+    # Intention is to stop, switch to a different brake control until we stop
+    elif self.long_control_state == LongCtrlState.stopping:
+      # Keep applying brakes until the car is stopped
+      if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
+        output_gb -= STOPPING_BRAKE_RATE / RATE
+      output_gb = clip(output_gb, -brake_max, gas_max)
+
+      self.v_pid = v_ego
+      self.pid.reset()
+
+    # Intention is to move again, release brake fast before handing control to PID
+    elif self.long_control_state == LongCtrlState.starting:
+      if output_gb < -0.2:
+        output_gb += STARTING_BRAKE_RATE / RATE
+      self.v_pid = v_ego
+      self.pid.reset()
+
+    self.last_output_gb = output_gb
+    final_gas = clip(output_gb, 0., gas_max)
+    final_brake = -clip(output_gb, -brake_max, 0.)
+
+    return final_gas, final_brake

selfdrive/controls/lib/longitudinal_mpc/.gitignore

@@ -0,0 +1,2 @@
+generator
+lib_qp/

selfdrive/controls/lib/longitudinal_mpc/SConscript

@@ -0,0 +1,32 @@
+Import('env', 'arch')
+
+
+cpp_path = [
+    "#phonelibs/acado/include",
+    "#phonelibs/acado/include/acado",
+    "#phonelibs/qpoases/INCLUDE",
+    "#phonelibs/qpoases/INCLUDE/EXTRAS",
+    "#phonelibs/qpoases/SRC/",
+    "#phonelibs/qpoases",
+    "lib_mpc_export"
+
+]
+
+mpc_files = [
+    "longitudinal_mpc.c",
+    Glob("lib_mpc_export/*.c"),
+    Glob("lib_mpc_export/*.cpp"),
+]
+
+interface_dir = Dir('lib_mpc_export')
+
+SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+
+env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+env.SharedLibrary('mpc2', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+
+# if arch != "aarch64":
+#     acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_casadi.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_csparse.a")]
+#     env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path)

selfdrive/controls/lib/longitudinal_mpc/generator.cpp

@@ -0,0 +1,97 @@
+#include <acado_code_generation.hpp>
+
+const int controlHorizon = 50;
+
+using namespace std;
+
+#define G 9.81
+#define TR 1.8
+
+#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
+#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1))
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState x_ego, v_ego, a_ego;
+  OnlineData x_l, v_l;
+
+  Control j_ego;
+
+  auto desired = 4.0 + RW(v_ego, v_l);
+  auto d_l = x_l - x_ego;
+
+  // Equations of motion
+  f << dot(x_ego) == v_ego;
+  f << dot(v_ego) == a_ego;
+  f << dot(a_ego) == j_ego;
+
+  // Running cost
+  Function h;
+  h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
+  h << (d_l - desired) / (0.05 * v_ego + 0.5);
+  h << a_ego * (0.1 * v_ego + 1.0);
+  h << j_ego * (0.1 * v_ego + 1.0);
+
+  // Weights are defined in mpc.
+  BMatrix Q(4,4); Q.setAll(true);
+
+  // Terminal cost
+  Function hN;
+  hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
+  hN << (d_l - desired) / (0.05 * v_ego + 0.5);
+  hN << a_ego * (0.1 * v_ego + 1.0);
+
+  // Weights are defined in mpc.
+  BMatrix QN(3,3); QN.setAll(true);
+
+  // Non uniform time grid
+  // First 5 timesteps are 0.2, after that it's 0.6
+  DMatrix numSteps(20, 1);
+  for (int i = 0; i < 5; i++){
+    numSteps(i) = 1;
+  }
+  for (int i = 5; i < 20; i++){
+    numSteps(i) = 3;
+  }
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = 10.0;
+
+  OCP ocp( tStart, tEnd, numSteps);
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  ocp.subjectTo( 0.0 <= v_ego);
+  ocp.setNOD(2);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.c

@@ -0,0 +1,212 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+
+real_t* acado_getVariablesX( )
+{
+	return acadoVariables.x;
+}
+
+real_t* acado_getVariablesU( )
+{
+	return acadoVariables.u;
+}
+
+#if ACADO_NY > 0
+real_t* acado_getVariablesY( )
+{
+	return acadoVariables.y;
+}
+#endif
+
+#if ACADO_NYN > 0
+real_t* acado_getVariablesYN( )
+{
+	return acadoVariables.yN;
+}
+#endif
+
+real_t* acado_getVariablesX0( )
+{
+#if ACADO_INITIAL_VALUE_FIXED
+	return acadoVariables.x0;
+#else
+	return 0;
+#endif
+}
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( )
+{
+	int i, j;
+	printf("\nDifferential variables:\n[\n");
+	for (i = 0; i < ACADO_N + 1; ++i)
+	{
+		for (j = 0; j < ACADO_NX; ++j)
+			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print control variables. */
+void acado_printControlVariables( )
+{
+	int i, j;
+	printf("\nControl variables:\n[\n");
+	for (i = 0; i < ACADO_N; ++i)
+	{
+		for (j = 0; j < ACADO_NU; ++j)
+			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
+		printf("\n");
+	}
+	printf("]\n\n");
+}
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( )
+{
+	printf(
+		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
+		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
+		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
+	);
+	
+	printf(
+		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
+		"supervision of Moritz Diehl. All rights reserved.\n\n"
+		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
+		"General Public License 3 in the hope that it will be useful,\n"
+		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
+		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
+		"GNU Lesser General Public License for more details.\n\n"
+	);
+}
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+void acado_tic( acado_timer* t )
+{
+	QueryPerformanceFrequency(&t->freq);
+	QueryPerformanceCounter(&t->tic);
+}
+
+real_t acado_toc( acado_timer* t )
+{
+	QueryPerformanceCounter(&t->toc);
+	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
+}
+
+
+#elif (defined __APPLE__)
+
+void acado_tic( acado_timer* t )
+{
+    /* read current clock cycles */
+    t->tic = mach_absolute_time();
+}
+
+real_t acado_toc( acado_timer* t )
+{
+
+    uint64_t duration; /* elapsed time in clock cycles*/
+    
+    t->toc = mach_absolute_time();
+    duration = t->toc - t->tic;
+    
+    /*conversion from clock cycles to nanoseconds*/
+    mach_timebase_info(&(t->tinfo));
+    duration *= t->tinfo.numer;
+    duration /= t->tinfo.denom;
+
+    return (real_t)duration / 1e9;
+}
+
+#else
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	gettimeofday(&t->tic, 0);
+}
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timeval temp;
+	
+	gettimeofday(&t->toc, 0);
+    
+	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
+}
+
+#else
+/* ANSI */
+
+/* read current time */
+void acado_tic( acado_timer* t )
+{
+	clock_gettime(CLOCK_MONOTONIC, &t->tic);
+}
+
+
+/* return time passed since last call to tic on this timer */
+real_t acado_toc( acado_timer* t )
+{
+	struct timespec temp;
+    
+	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
+    
+	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	else
+	{
+		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
+	}
+	
+	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
+}
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || _WIN64) */
+
+#endif

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.h

@@ -0,0 +1,138 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_AUXILIARY_FUNCTIONS_H
+#define ACADO_AUXILIARY_FUNCTIONS_H
+
+#include "acado_common.h"
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** Get pointer to the matrix with differential variables. */
+real_t* acado_getVariablesX( );
+
+/** Get pointer to the matrix with control variables. */
+real_t* acado_getVariablesU( );
+
+#if ACADO_NY > 0
+/** Get pointer to the matrix with references/measurements. */
+real_t* acado_getVariablesY( );
+#endif
+
+#if ACADO_NYN > 0
+/** Get pointer to the vector with references/measurement on the last node. */
+real_t* acado_getVariablesYN( );
+#endif
+
+/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
+real_t* acado_getVariablesX0( );
+
+/** Print differential variables. */
+void acado_printDifferentialVariables( );
+
+/** Print control variables. */
+void acado_printControlVariables( );
+
+/** Print ACADO code generation notice. */
+void acado_printHeader( );
+
+/*
+ * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
+ * providing us with the following timing routines.
+ */
+
+#if !(defined _DSPACE)
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+/* Use Windows QueryPerformanceCounter for timing. */
+#include <Windows.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	LARGE_INTEGER tic;
+	LARGE_INTEGER toc;
+	LARGE_INTEGER freq;
+} acado_timer;
+
+
+#elif (defined __APPLE__)
+
+#include "unistd.h"
+#include <mach/mach_time.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	uint64_t tic;
+	uint64_t toc;
+	mach_timebase_info_data_t tinfo;
+} acado_timer;
+
+#else
+
+/* Use POSIX clock_gettime() for timing on non-Windows machines. */
+#include <time.h>
+
+#if __STDC_VERSION__ >= 199901L
+/* C99 mode of operation. */
+
+#include <sys/stat.h>
+#include <sys/time.h>
+
+typedef struct acado_timer_
+{
+	struct timeval tic;
+	struct timeval toc;
+} acado_timer;
+
+#else
+/* ANSI C */
+
+/** A structure for keeping internal timer data. */
+typedef struct acado_timer_
+{
+	struct timespec tic;
+	struct timespec toc;
+} acado_timer;
+
+#endif /* __STDC_VERSION__ >= 199901L */
+
+#endif /* (defined _WIN32 || defined _WIN64) */
+
+/** A function for measurement of the current time. */
+void acado_tic( acado_timer* t );
+
+/** A function which returns the elapsed time. */
+real_t acado_toc( acado_timer* t );
+
+#endif
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_AUXILIARY_FUNCTIONS_H */

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

@@ -0,0 +1,354 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef ACADO_COMMON_H
+#define ACADO_COMMON_H
+
+#include <math.h>
+#include <string.h>
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+/** \defgroup ACADO ACADO CGT generated module. */
+/** @{ */
+
+/** qpOASES QP solver indicator. */
+#define ACADO_QPOASES  0
+#define ACADO_QPOASES3 1
+/** FORCES QP solver indicator.*/
+#define ACADO_FORCES   2
+/** qpDUNES QP solver indicator.*/
+#define ACADO_QPDUNES  3
+/** HPMPC QP solver indicator. */
+#define ACADO_HPMPC    4
+#define ACADO_GENERIC    5
+
+/** Indicator for determining the QP solver used by the ACADO solver code. */
+#define ACADO_QP_SOLVER ACADO_QPOASES
+
+#include "acado_qpoases_interface.hpp"
+
+
+/*
+ * Common definitions
+ */
+/** User defined block based condensing. */
+#define ACADO_BLOCK_CONDENSING 0
+/** Compute covariance matrix of the last state estimate. */
+#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
+/** Flag indicating whether constraint values are hard-coded or not. */
+#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
+/** Indicator for fixed initial state. */
+#define ACADO_INITIAL_STATE_FIXED 1
+/** Number of control/estimation intervals. */
+#define ACADO_N 20
+/** Number of online data values. */
+#define ACADO_NOD 2
+/** Number of path constraints. */
+#define ACADO_NPAC 0
+/** Number of control variables. */
+#define ACADO_NU 1
+/** Number of differential variables. */
+#define ACADO_NX 3
+/** Number of algebraic variables. */
+#define ACADO_NXA 0
+/** Number of differential derivative variables. */
+#define ACADO_NXD 0
+/** Number of references/measurements per node on the first N nodes. */
+#define ACADO_NY 4
+/** Number of references/measurements on the last (N + 1)st node. */
+#define ACADO_NYN 3
+/** Total number of QP optimization variables. */
+#define ACADO_QP_NV 23
+/** Number of Runge-Kutta stages per integration step. */
+#define ACADO_RK_NSTAGES 4
+/** Providing interface for arrival cost. */
+#define ACADO_USE_ARRIVAL_COST 0
+/** Indicator for usage of non-hard-coded linear terms in the objective. */
+#define ACADO_USE_LINEAR_TERMS 0
+/** Indicator for type of fixed weighting matrices. */
+#define ACADO_WEIGHTING_MATRICES_TYPE 2
+
+
+/*
+ * Globally used structure definitions
+ */
+
+/** The structure containing the user data.
+ * 
+ *  Via this structure the user "communicates" with the solver code.
+ */
+typedef struct ACADOvariables_
+{
+int dummy;
+/** Matrix of size: 21 x 3 (row major format)
+ * 
+ *  Matrix containing 21 differential variable vectors.
+ */
+real_t x[ 63 ];
+
+/** Column vector of size: 20
+ * 
+ *  Matrix containing 20 control variable vectors.
+ */
+real_t u[ 20 ];
+
+/** Matrix of size: 21 x 2 (row major format)
+ * 
+ *  Matrix containing 21 online data vectors.
+ */
+real_t od[ 42 ];
+
+/** Column vector of size: 80
+ * 
+ *  Matrix containing 20 reference/measurement vectors of size 4 for first 20 nodes.
+ */
+real_t y[ 80 ];
+
+/** Column vector of size: 3
+ * 
+ *  Reference/measurement vector for the 21. node.
+ */
+real_t yN[ 3 ];
+
+/** Matrix of size: 80 x 4 (row major format) */
+real_t W[ 320 ];
+
+/** Matrix of size: 3 x 3 (row major format) */
+real_t WN[ 9 ];
+
+/** Column vector of size: 3
+ * 
+ *  Current state feedback vector.
+ */
+real_t x0[ 3 ];
+
+
+} ACADOvariables;
+
+/** Private workspace used by the auto-generated code.
+ * 
+ *  Data members of this structure are private to the solver.
+ *  In other words, the user code should not modify values of this 
+ *  structure. 
+ */
+typedef struct ACADOworkspace_
+{
+real_t rk_ttt;
+
+/** Row vector of size: 18 */
+real_t rk_xxx[ 18 ];
+
+/** Matrix of size: 4 x 15 (row major format) */
+real_t rk_kkk[ 60 ];
+
+/** Row vector of size: 18 */
+real_t state[ 18 ];
+
+/** Column vector of size: 60 */
+real_t d[ 60 ];
+
+/** Column vector of size: 80 */
+real_t Dy[ 80 ];
+
+/** Column vector of size: 3 */
+real_t DyN[ 3 ];
+
+/** Matrix of size: 60 x 3 (row major format) */
+real_t evGx[ 180 ];
+
+/** Column vector of size: 60 */
+real_t evGu[ 60 ];
+
+/** Column vector of size: 13 */
+real_t objAuxVar[ 13 ];
+
+/** Row vector of size: 6 */
+real_t objValueIn[ 6 ];
+
+/** Row vector of size: 20 */
+real_t objValueOut[ 20 ];
+
+/** Matrix of size: 60 x 3 (row major format) */
+real_t Q1[ 180 ];
+
+/** Matrix of size: 60 x 4 (row major format) */
+real_t Q2[ 240 ];
+
+/** Column vector of size: 20 */
+real_t R1[ 20 ];
+
+/** Matrix of size: 20 x 4 (row major format) */
+real_t R2[ 80 ];
+
+/** Column vector of size: 60 */
+real_t S1[ 60 ];
+
+/** Matrix of size: 3 x 3 (row major format) */
+real_t QN1[ 9 ];
+
+/** Matrix of size: 3 x 3 (row major format) */
+real_t QN2[ 9 ];
+
+/** Column vector of size: 3 */
+real_t Dx0[ 3 ];
+
+/** Matrix of size: 3 x 3 (row major format) */
+real_t T[ 9 ];
+
+/** Column vector of size: 630 */
+real_t E[ 630 ];
+
+/** Column vector of size: 630 */
+real_t QE[ 630 ];
+
+/** Matrix of size: 60 x 3 (row major format) */
+real_t QGx[ 180 ];
+
+/** Column vector of size: 60 */
+real_t Qd[ 60 ];
+
+/** Column vector of size: 63 */
+real_t QDy[ 63 ];
+
+/** Matrix of size: 20 x 3 (row major format) */
+real_t H10[ 60 ];
+
+/** Matrix of size: 23 x 23 (row major format) */
+real_t H[ 529 ];
+
+/** Matrix of size: 20 x 23 (row major format) */
+real_t A[ 460 ];
+
+/** Column vector of size: 23 */
+real_t g[ 23 ];
+
+/** Column vector of size: 23 */
+real_t lb[ 23 ];
+
+/** Column vector of size: 23 */
+real_t ub[ 23 ];
+
+/** Column vector of size: 20 */
+real_t lbA[ 20 ];
+
+/** Column vector of size: 20 */
+real_t ubA[ 20 ];
+
+/** Column vector of size: 23 */
+real_t x[ 23 ];
+
+/** Column vector of size: 43 */
+real_t y[ 43 ];
+
+
+} ACADOworkspace;
+
+/* 
+ * Forward function declarations. 
+ */
+
+
+/** Performs the integration and sensitivity propagation for one shooting interval.
+ *
+ *  \param rk_eta Working array to pass the input values and return the results.
+ *  \param resetIntegrator The internal memory of the integrator can be reset.
+ *  \param rk_index Number of the shooting interval.
+ *
+ *  \return Status code of the integrator.
+ */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
+
+/** Export of an ACADO symbolic function.
+ *
+ *  \param in Input to the exported function.
+ *  \param out Output of the exported function.
+ */
+void acado_rhs_forw(const real_t* in, real_t* out);
+
+/** Preparation step of the RTI scheme.
+ *
+ *  \return Status of the integration module. =0: OK, otherwise the error code.
+ */
+int acado_preparationStep(  );
+
+/** Feedback/estimation step of the RTI scheme.
+ *
+ *  \return Status code of the qpOASES QP solver.
+ */
+int acado_feedbackStep(  );
+
+/** Solver initialization. Must be called once before any other function call.
+ *
+ *  \return =0: OK, otherwise an error code of a QP solver.
+ */
+int acado_initializeSolver(  );
+
+/** Initialize shooting nodes by a forward simulation starting from the first node.
+ */
+void acado_initializeNodesByForwardSimulation(  );
+
+/** Shift differential variables vector by one interval.
+ *
+ *  \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation.
+ *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
+
+/** Shift controls vector by one interval.
+ *
+ *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
+ */
+void acado_shiftControls( real_t* const uEnd );
+
+/** Get the KKT tolerance of the current iterate.
+ *
+ *  \return The KKT tolerance value.
+ */
+real_t acado_getKKT(  );
+
+/** Calculate the objective value.
+ *
+ *  \return Value of the objective function.
+ */
+real_t acado_getObjective(  );
+
+
+/* 
+ * Extern declarations. 
+ */
+
+extern ACADOworkspace acadoWorkspace;
+extern ACADOvariables acadoVariables;
+
+/** @} */
+
+#ifndef __MATLAB__
+#ifdef __cplusplus
+} /* extern "C" */
+#endif /* __cplusplus */
+#endif /* __MATLAB__ */
+
+#endif /* ACADO_COMMON_H */

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

@@ -0,0 +1,159 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#include "acado_common.h"
+
+
+void acado_rhs_forw(const real_t* in, real_t* out)
+{
+const real_t* xd = in;
+const real_t* u = in + 15;
+
+/* Compute outputs: */
+out[0] = xd[1];
+out[1] = xd[2];
+out[2] = u[0];
+out[3] = xd[6];
+out[4] = xd[7];
+out[5] = xd[8];
+out[6] = xd[9];
+out[7] = xd[10];
+out[8] = xd[11];
+out[9] = (real_t)(0.0000000000000000e+00);
+out[10] = (real_t)(0.0000000000000000e+00);
+out[11] = (real_t)(0.0000000000000000e+00);
+out[12] = xd[13];
+out[13] = xd[14];
+out[14] = (real_t)(1.0000000000000000e+00);
+}
+
+/* Fixed step size:0.2 */
+int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
+{
+int error;
+
+int run1;
+int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
+int numInts = numSteps[rk_index];
+acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
+rk_eta[3] = 1.0000000000000000e+00;
+rk_eta[4] = 0.0000000000000000e+00;
+rk_eta[5] = 0.0000000000000000e+00;
+rk_eta[6] = 0.0000000000000000e+00;
+rk_eta[7] = 1.0000000000000000e+00;
+rk_eta[8] = 0.0000000000000000e+00;
+rk_eta[9] = 0.0000000000000000e+00;
+rk_eta[10] = 0.0000000000000000e+00;
+rk_eta[11] = 1.0000000000000000e+00;
+rk_eta[12] = 0.0000000000000000e+00;
+rk_eta[13] = 0.0000000000000000e+00;
+rk_eta[14] = 0.0000000000000000e+00;
+acadoWorkspace.rk_xxx[15] = rk_eta[15];
+acadoWorkspace.rk_xxx[16] = rk_eta[16];
+acadoWorkspace.rk_xxx[17] = rk_eta[17];
+
+for (run1 = 0; run1 < 1; ++run1)
+{
+for(run1 = 0; run1 < numInts; run1++ ) {
+acadoWorkspace.rk_xxx[0] = + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + rk_eta[14];
+acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
+acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 15 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[14];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 30 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[30] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[31] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[32] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[33] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[34] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[35] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[36] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[37] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[38] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[39] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[40] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[41] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[42] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[43] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[44] + rk_eta[14];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 45 ]) );
+rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45];
+rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46];
+rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47];
+rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[48];
+rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[49];
+rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[50];
+rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[51];
+rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[52];
+rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[53];
+rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[54];
+rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[55];
+rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[56];
+rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[57];
+rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[58];
+rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[59];
+acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
+}
+}
+error = 0;
+return error;
+}
+

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp

@@ -0,0 +1,70 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+extern "C"
+{
+#include "acado_common.h"
+}
+
+#include "INCLUDE/QProblem.hpp"
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+static int acado_nWSR;
+
+
+
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+static SolutionAnalysis acado_sa;
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+int acado_solve( void )
+{
+	acado_nWSR = QPOASES_NWSRMAX;
+
+	QProblem qp(23, 20);
+	
+	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
+
+    qp.getPrimalSolution( acadoWorkspace.x );
+    qp.getDualSolution( acadoWorkspace.y );
+	
+#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
+
+	if (retVal != SUCCESSFUL_RETURN)
+		return (int)retVal;
+		
+	retVal = acado_sa.getHessianInverse( &qp,var );
+
+#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
+
+	return (int)retVal;
+}
+
+int acado_getNWSR( void )
+{
+	return acado_nWSR;
+}
+
+const char* acado_getErrorString( int error )
+{
+	return MessageHandling::getErrorString( error );
+}

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp

@@ -0,0 +1,65 @@
+/*
+ *    This file was auto-generated using the ACADO Toolkit.
+ *    
+ *    While ACADO Toolkit is free software released under the terms of
+ *    the GNU Lesser General Public License (LGPL), the generated code
+ *    as such remains the property of the user who used ACADO Toolkit
+ *    to generate this code. In particular, user dependent data of the code
+ *    do not inherit the GNU LGPL license. On the other hand, parts of the
+ *    generated code that are a direct copy of source code from the
+ *    ACADO Toolkit or the software tools it is based on, remain, as derived
+ *    work, automatically covered by the LGPL license.
+ *    
+ *    ACADO Toolkit is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *    
+ */
+
+
+#ifndef QPOASES_HEADER
+#define QPOASES_HEADER
+
+#ifdef PC_DEBUG
+#include <stdio.h>
+#endif /* PC_DEBUG */
+
+#include <math.h>
+
+#ifdef __cplusplus
+#define EXTERNC extern "C"
+#else
+#define EXTERNC
+#endif
+
+/*
+ * A set of options for qpOASES
+ */
+
+/** Maximum number of optimization variables. */
+#define QPOASES_NVMAX      23
+/** Maximum number of constraints. */
+#define QPOASES_NCMAX      20
+/** Maximum number of working set recalculations. */
+#define QPOASES_NWSRMAX    500
+/** Print level for qpOASES. */
+#define QPOASES_PRINTLEVEL PL_NONE
+/** The value of EPS */
+#define QPOASES_EPS        2.221e-16
+/** Internally used floating point type */
+typedef double real_t;
+
+/*
+ * Forward function declarations
+ */
+
+/** A function that calls the QP solver */
+EXTERNC int acado_solve( void );
+
+/** Get the number of active set changes */
+EXTERNC int acado_getNWSR( void );
+
+/** Get the error string. */
+const char* acado_getErrorString( int error );
+
+#endif /* QPOASES_HEADER */

selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py

@@ -0,0 +1,40 @@
+import os
+
+from cffi import FFI
+
+mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
+
+def _get_libmpc(mpc_id):
+    libmpc_fn = os.path.join(mpc_dir, "libmpc%d.so" % mpc_id)
+
+    ffi = FFI()
+    ffi.cdef("""
+    typedef struct {
+    double x_ego, v_ego, a_ego, x_l, v_l, a_l;
+    } state_t;
+
+
+    typedef struct {
+    double x_ego[21];
+    double v_ego[21];
+    double a_ego[21];
+    double j_ego[20];
+    double x_l[21];
+    double v_l[21];
+    double a_l[21];
+    double t[21];
+    double cost;
+    } log_t;
+
+    void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
+    void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
+    int run_mpc(state_t * x0, log_t * solution,
+                double l, double a_l_0);
+    """)
+
+    return (ffi, ffi.dlopen(libmpc_fn))
+
+mpcs = [_get_libmpc(1), _get_libmpc(2)]
+
+def get_libmpc(mpc_id):
+    return mpcs[mpc_id - 1]

selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c

@@ -0,0 +1,173 @@
+#include "acado_common.h"
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+#include <math.h>
+
+#define NX          ACADO_NX  /* Number of differential state variables.  */
+#define NXA         ACADO_NXA /* Number of algebraic variables. */
+#define NU          ACADO_NU  /* Number of control inputs. */
+#define NOD         ACADO_NOD  /* Number of online data values. */
+
+#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
+#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
+
+#define N           ACADO_N   /* Number of intervals in the horizon. */
+
+ACADOvariables acadoVariables;
+ACADOworkspace acadoWorkspace;
+
+typedef struct {
+  double x_ego, v_ego, a_ego, x_l, v_l, a_l;
+} state_t;
+
+
+typedef struct {
+  double x_ego[N+1];
+  double v_ego[N+1];
+  double a_ego[N+1];
+  double j_ego[N];
+  double x_l[N+1];
+  double v_l[N+1];
+  double a_l[N+1];
+  double t[N+1];
+  double cost;
+} log_t;
+
+void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
+  acado_initializeSolver();
+  int    i;
+  const int STEP_MULTIPLIER = 3;
+
+  /* Initialize the states and controls. */
+  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+
+  /* Initialize the measurements/reference. */
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+
+  /* MPC: initialize the current state feedback. */
+  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+  // Set weights
+
+  for (i = 0; i < N; i++) {
+    int f = 1;
+    if (i > 4){
+      f = STEP_MULTIPLIER;
+    }
+    // Setup diagonal entries
+    acadoVariables.W[NY*NY*i + (NY+1)*0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
+    acadoVariables.W[NY*NY*i + (NY+1)*1] = distanceCost * f; // desired distance
+    acadoVariables.W[NY*NY*i + (NY+1)*2] = accelerationCost * f; // acceleration
+    acadoVariables.W[NY*NY*i + (NY+1)*3] = jerkCost * f; // jerk
+  }
+  acadoVariables.WN[(NYN+1)*0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone
+  acadoVariables.WN[(NYN+1)*1] = distanceCost * STEP_MULTIPLIER; // desired distance
+  acadoVariables.WN[(NYN+1)*2] = accelerationCost * STEP_MULTIPLIER; // acceleration
+
+}
+
+void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){
+  int i;
+
+  double x_l = x_l_0;
+  double v_l = v_l_0;
+  double a_l = a_l_0;
+
+  double x_ego = 0.0;
+  double a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
+
+  if (a_ego > 0){
+    a_ego = 0.0;
+  }
+
+
+  double dt = 0.2;
+  double t = 0.;
+
+  for (i = 0; i < N + 1; ++i){
+    if (i > 4){
+      dt = 0.6;
+    }
+
+    /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_ego, v_ego, a_l); */
+    acadoVariables.x[i*NX] = x_ego;
+    acadoVariables.x[i*NX+1] = v_ego;
+    acadoVariables.x[i*NX+2] = a_ego;
+
+    v_ego += a_ego * dt;
+
+    if (v_ego <= 0.0) {
+      v_ego = 0.0;
+      a_ego = 0.0;
+    }
+
+    x_ego += v_ego * dt;
+    t += dt;
+  }
+
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+}
+
+int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
+  // Calculate lead vehicle predictions
+  int i;
+  double t = 0.;
+  double dt = 0.2;
+  double x_l = x0->x_l;
+  double v_l = x0->v_l;
+  double a_l = a_l_0;
+
+  /* printf("t\tx_l\t_v_l\t_al\n"); */
+  for (i = 0; i < N + 1; ++i){
+    if (i > 4){
+      dt = 0.6;
+    }
+
+    /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_l, v_l, a_l); */
+
+    acadoVariables.od[i*NOD] = x_l;
+    acadoVariables.od[i*NOD+1] = v_l;
+
+    solution->x_l[i] = x_l;
+    solution->v_l[i] = v_l;
+    solution->a_l[i] = a_l;
+    solution->t[i] = t;
+
+    a_l = a_l_0 * exp(-l * t * t / 2);
+    x_l += v_l * dt;
+    v_l += a_l * dt;
+    if (v_l < 0.0){
+      a_l = 0.0;
+      v_l = 0.0;
+    }
+
+    t += dt;
+  }
+
+  acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
+  acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
+  acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
+
+  acado_preparationStep();
+  acado_feedbackStep();
+
+  for (i = 0; i <= N; i++){
+    solution->x_ego[i] = acadoVariables.x[i*NX];
+    solution->v_ego[i] = acadoVariables.x[i*NX+1];
+    solution->a_ego[i] = acadoVariables.x[i*NX+2];
+
+    if (i < N){
+      solution->j_ego[i] = acadoVariables.u[i];
+    }
+  }
+  solution->cost = acado_getObjective();
+
+  // Dont shift states here. Current solution is closer to next timestep than if
+  // we shift by 0.2 seconds.
+
+  return acado_getNWSR();
+}

selfdrive/controls/lib/pathplanner.py

@@ -0,0 +1,230 @@
+import os
+import math
+from common.realtime import sec_since_boot, DT_MDL
+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.lane_planner import LanePlanner
+from selfdrive.config import Conversions as CV
+import cereal.messaging as messaging
+from cereal import log
+
+LaneChangeState = log.PathPlan.LaneChangeState
+LaneChangeDirection = log.PathPlan.LaneChangeDirection
+
+LOG_MPC = os.environ.get('LOG_MPC', False)
+
+LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
+LANE_CHANGE_TIME_MAX = 10.
+
+DESIRES = {
+  LaneChangeDirection.none: {
+    LaneChangeState.off: log.PathPlan.Desire.none,
+    LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
+    LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none,
+    LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none,
+  },
+  LaneChangeDirection.left: {
+    LaneChangeState.off: log.PathPlan.Desire.none,
+    LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
+    LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft,
+    LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft,
+  },
+  LaneChangeDirection.right: {
+    LaneChangeState.off: log.PathPlan.Desire.none,
+    LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
+    LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight,
+    LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight,
+  },
+}
+
+def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
+  states[0].x = v_ego * delay
+  states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
+  return states
+
+
+class PathPlanner():
+  def __init__(self, CP):
+    self.LP = LanePlanner()
+
+    self.last_cloudlog_t = 0
+    self.steer_rate_cost = CP.steerRateCost
+
+    self.setup_mpc()
+    self.solution_invalid_cnt = 0
+    self.path_offset_i = 0.0
+    self.lane_change_state = LaneChangeState.off
+    self.lane_change_timer = 0.0
+    self.prev_one_blinker = False
+
+  def setup_mpc(self):
+    self.libmpc = libmpc_py.libmpc
+    self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
+
+    self.mpc_solution = libmpc_py.ffi.new("log_t *")
+    self.cur_state = libmpc_py.ffi.new("state_t *")
+    self.cur_state[0].x = 0.0
+    self.cur_state[0].y = 0.0
+    self.cur_state[0].psi = 0.0
+    self.cur_state[0].delta = 0.0
+
+    self.angle_steers_des = 0.0
+    self.angle_steers_des_mpc = 0.0
+    self.angle_steers_des_prev = 0.0
+    self.angle_steers_des_time = 0.0
+
+  def update(self, sm, pm, CP, VM):
+    v_ego = sm['carState'].vEgo
+    angle_steers = sm['carState'].steeringAngle
+    active = sm['controlsState'].active
+
+    angle_offset = sm['liveParameters'].angleOffset
+
+    # 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.LP.parse_model(sm['model'])
+
+    # Lane change logic
+    lane_change_direction = LaneChangeDirection.none
+    one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
+    below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
+
+    if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
+      self.lane_change_state = LaneChangeState.off
+    else:
+      if sm['carState'].leftBlinker:
+        lane_change_direction = LaneChangeDirection.left
+      elif sm['carState'].rightBlinker:
+        lane_change_direction = LaneChangeDirection.right
+
+      torque_applied = sm['carState'].steeringPressed and \
+                       ((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \
+                        (sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right))
+
+      lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
+
+      # State transitions
+      # off
+      if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
+        self.lane_change_state = LaneChangeState.preLaneChange
+
+      # pre
+      elif self.lane_change_state == LaneChangeState.preLaneChange:
+        if not one_blinker or below_lane_change_speed:
+          self.lane_change_state = LaneChangeState.off
+        elif torque_applied:
+          self.lane_change_state = LaneChangeState.laneChangeStarting
+
+      # starting
+      elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
+        self.lane_change_state = LaneChangeState.laneChangeFinishing
+
+      # finishing
+      elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
+        if one_blinker:
+          self.lane_change_state = LaneChangeState.preLaneChange
+        else:
+          self.lane_change_state = LaneChangeState.off
+
+    if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
+      self.lane_change_timer = 0.0
+    else:
+      self.lane_change_timer += DT_MDL
+
+    self.prev_one_blinker = one_blinker
+
+    desire = DESIRES[lane_change_direction][self.lane_change_state]
+
+    # Turn off lanes during lane change
+    if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
+      self.LP.l_prob = 0.
+      self.LP.r_prob = 0.
+      self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
+    else:
+      self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
+
+    self.LP.update_d_poly(v_ego)
+
+
+    # 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, 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,
+                        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:
+      delta_desired = self.mpc_solution[0].delta[1]
+      rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
+    else:
+      delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
+      rate_desired = 0.0
+
+    self.cur_state[0].delta = delta_desired
+
+    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
+
+    #  Check for infeasable MPC solution
+    mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
+    t = sec_since_boot()
+    if mpc_nans:
+      self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
+      self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
+
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Lateral mpc - nan: True")
+
+    if self.mpc_solution[0].cost > 20000. or mpc_nans:   # TODO: find a better way to detect when MPC did not converge
+      self.solution_invalid_cnt += 1
+    else:
+      self.solution_invalid_cnt = 0
+    plan_solution_valid = self.solution_invalid_cnt < 2
+
+    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.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(sm['liveParameters'].angleOffsetAverage)
+    plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
+    plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
+    plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
+    plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
+
+    plan_send.pathPlan.desire = desire
+    plan_send.pathPlan.laneChangeState = self.lane_change_state
+    plan_send.pathPlan.laneChangeDirection = lane_change_direction
+
+    pm.send('pathPlan', plan_send)
+
+    if LOG_MPC:
+      dat = messaging.new_message()
+      dat.init('liveMpc')
+      dat.liveMpc.x = list(self.mpc_solution[0].x)
+      dat.liveMpc.y = list(self.mpc_solution[0].y)
+      dat.liveMpc.psi = list(self.mpc_solution[0].psi)
+      dat.liveMpc.delta = list(self.mpc_solution[0].delta)
+      dat.liveMpc.cost = self.mpc_solution[0].cost
+      pm.send('liveMpc', dat)

selfdrive/controls/lib/pid.py

@@ -0,0 +1,88 @@
+import numpy as np
+from common.numpy_fast import clip, interp
+
+def apply_deadzone(error, deadzone):
+  if error > deadzone:
+    error -= deadzone
+  elif error < - deadzone:
+    error += deadzone
+  else:
+    error = 0.
+  return error
+
+class PIController():
+  def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+    self._k_p = k_p # proportional gain
+    self._k_i = k_i # integral gain
+    self.k_f = k_f  # feedforward gain
+
+    self.pos_limit = pos_limit
+    self.neg_limit = neg_limit
+
+    self.sat_count_rate = 1.0 / rate
+    self.i_unwind_rate = 0.3 / rate
+    self.i_rate = 1.0 / rate
+    self.sat_limit = sat_limit
+    self.convert = convert
+
+    self.reset()
+
+  @property
+  def k_p(self):
+    return interp(self.speed, self._k_p[0], self._k_p[1])
+
+  @property
+  def k_i(self):
+    return interp(self.speed, self._k_i[0], self._k_i[1])
+
+  def _check_saturation(self, control, check_saturation, error):
+    saturated = (control < self.neg_limit) or (control > self.pos_limit)
+
+    if saturated and check_saturation and abs(error) > 0.1:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+
+    self.sat_count = clip(self.sat_count, 0.0, 1.0)
+
+    return self.sat_count > self.sat_limit
+
+  def reset(self):
+    self.p = 0.0
+    self.i = 0.0
+    self.f = 0.0
+    self.sat_count = 0.0
+    self.saturated = False
+    self.control = 0
+
+  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
+    self.speed = speed
+
+    error = float(apply_deadzone(setpoint - measurement, deadzone))
+    self.p = error * self.k_p
+    self.f = feedforward * self.k_f
+
+    if override:
+      self.i -= self.i_unwind_rate * float(np.sign(self.i))
+    else:
+      i = self.i + error * self.k_i * self.i_rate
+      control = self.p + self.f + i
+
+      if self.convert is not None:
+        control = self.convert(control, speed=self.speed)
+
+      # Update when changing i will move the control away from the limits
+      # or when i will move towards the sign of the error
+      if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
+          (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
+         not freeze_integrator:
+        self.i = i
+
+    control = self.p + self.f + self.i
+    if self.convert is not None:
+      control = self.convert(control, speed=self.speed)
+
+    self.saturated = self._check_saturation(control, check_saturation, error)
+
+    self.control = clip(control, self.neg_limit, self.pos_limit)
+    return self.control

selfdrive/controls/lib/planner.py

@@ -0,0 +1,253 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+from common.params import Params
+from common.numpy_fast import interp
+
+import cereal.messaging as messaging
+from cereal import car
+from common.realtime import sec_since_boot
+from selfdrive.swaglog import cloudlog
+from selfdrive.config import Conversions as CV
+from selfdrive.controls.lib.speed_smoother import speed_smoother
+from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
+from selfdrive.controls.lib.fcw import FCWChecker
+from selfdrive.controls.lib.long_mpc import LongitudinalMpc
+
+MAX_SPEED = 255.0
+
+LON_MPC_STEP = 0.2  # first step is 0.2s
+MAX_SPEED_ERROR = 2.0
+AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
+
+# lookup tables VS speed to determine min and max accels in cruise
+# make sure these accelerations are smaller than mpc limits
+_A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
+_A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.]
+
+# need fast accel at very low speed for stop and go
+# make sure these accelerations are smaller than mpc limits
+_A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4]
+_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4]
+_A_CRUISE_MAX_BP = [0.,  6.4, 22.5, 40.]
+
+# Lookup table for turns
+_A_TOTAL_MAX_V = [1.7, 3.2]
+_A_TOTAL_MAX_BP = [20., 40.]
+
+# 75th percentile
+SPEED_PERCENTILE_IDX = 7
+
+
+def calc_cruise_accel_limits(v_ego, following):
+  a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+
+  if following:
+    a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING)
+  else:
+    a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
+  return np.vstack([a_cruise_min, a_cruise_max])
+
+
+def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
+  """
+  This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
+  this should avoid accelerating when losing the target in turns
+  """
+
+  a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
+  a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
+
+  return [a_target[0], min(a_target[1], a_x_allowed)]
+
+
+class Planner():
+  def __init__(self, CP):
+    self.CP = CP
+
+    self.mpc1 = LongitudinalMpc(1)
+    self.mpc2 = LongitudinalMpc(2)
+
+    self.v_acc_start = 0.0
+    self.a_acc_start = 0.0
+
+    self.v_acc = 0.0
+    self.v_acc_future = 0.0
+    self.a_acc = 0.0
+    self.v_cruise = 0.0
+    self.a_cruise = 0.0
+    self.v_model = 0.0
+    self.a_model = 0.0
+
+    self.longitudinalPlanSource = 'cruise'
+    self.fcw_checker = FCWChecker()
+    self.path_x = np.arange(192)
+
+    self.params = Params()
+    self.first_loop = True
+
+  def choose_solution(self, v_cruise_setpoint, enabled):
+    if enabled:
+      solutions = {'model': self.v_model, 'cruise': self.v_cruise}
+      if self.mpc1.prev_lead_status:
+        solutions['mpc1'] = self.mpc1.v_mpc
+      if self.mpc2.prev_lead_status:
+        solutions['mpc2'] = self.mpc2.v_mpc
+
+      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
+        self.a_acc = self.mpc1.a_mpc
+      elif slowest == 'mpc2':
+        self.v_acc = self.mpc2.v_mpc
+        self.a_acc = self.mpc2.a_mpc
+      elif slowest == 'cruise':
+        self.v_acc = self.v_cruise
+        self.a_acc = self.a_cruise
+      elif slowest == 'model':
+        self.v_acc = self.v_model
+        self.a_acc = self.a_model
+
+    self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
+
+  def update(self, sm, pm, CP, VM, PP):
+    """Gets called when new radarState is available"""
+    cur_time = sec_since_boot()
+    v_ego = sm['carState'].vEgo
+
+    long_control_state = sm['controlsState'].longControlState
+    v_cruise_kph = sm['controlsState'].vCruise
+    force_slow_decel = sm['controlsState'].forceDecel
+    v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
+
+    lead_1 = sm['radarState'].leadOne
+    lead_2 = sm['radarState'].leadTwo
+
+    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 len(sm['model'].path.poly):
+      path = list(sm['model'].path.poly)
+
+      # 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
+      # TODO: compute max speed without using a list of points and without numpy
+      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
+
+      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)
+      model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
+    else:
+      model_speed = MAX_SPEED
+
+    # Calculate speed for normal cruise control
+    if enabled and not self.first_loop:
+      accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
+      jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]  # TODO: make a separate lookup for jerk tuning
+      accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
+
+      if force_slow_decel:
+        # if required so, force a smooth deceleration
+        accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
+        accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
+
+      self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
+                                                    v_cruise_setpoint,
+                                                    accel_limits_turns[1], accel_limits_turns[0],
+                                                    jerk_limits[1], jerk_limits[0],
+                                                    LON_MPC_STEP)
+
+      self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
+                                                    model_speed,
+                                                    2*accel_limits[1], accel_limits[0],
+                                                    2*jerk_limits[1], jerk_limits[0],
+                                                    LON_MPC_STEP)
+
+      # cruise speed can't be negative even is user is distracted
+      self.v_cruise = max(self.v_cruise, 0.)
+    else:
+      starting = long_control_state == LongCtrlState.starting
+      a_ego = min(sm['carState'].aEgo, 0.0)
+      reset_speed = MIN_CAN_SPEED if starting else v_ego
+      reset_accel = self.CP.startAccel if starting else a_ego
+      self.v_acc = reset_speed
+      self.a_acc = reset_accel
+      self.v_acc_start = reset_speed
+      self.a_acc_start = reset_accel
+      self.v_cruise = reset_speed
+      self.a_cruise = reset_accel
+
+    self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
+    self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
+
+    self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
+    self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
+
+    self.choose_solution(v_cruise_setpoint, enabled)
+
+    # determine fcw
+    if self.mpc1.new_lead:
+      self.fcw_checker.reset_lead(cur_time)
+
+    blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
+    fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
+                                  sm['controlsState'].active,
+                                  v_ego, sm['carState'].aEgo,
+                                  lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
+                                  lead_1.yRel, lead_1.vLat,
+                                  lead_1.fcw, blinkers) and not sm['carState'].brakePressed
+    if fcw:
+      cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
+
+    radar_dead = not sm.alive['radarState']
+
+    radar_errors = list(sm['radarState'].radarErrors)
+    radar_fault = car.RadarData.Error.fault in radar_errors
+    radar_can_error = car.RadarData.Error.canError in radar_errors
+
+    # **** send the plan ****
+    plan_send = messaging.new_message()
+    plan_send.init('plan')
+
+    plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
+
+    plan_send.plan.mdMonoTime = sm.logMonoTime['model']
+    plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
+
+    # longitudal plan
+    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
+
+    radar_valid = not (radar_dead or radar_fault)
+    plan_send.plan.radarValid = bool(radar_valid)
+    plan_send.plan.radarCanError = bool(radar_can_error)
+
+    plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
+
+    # Send out fcw
+    plan_send.plan.fcw = fcw
+
+    pm.send('plan', plan_send)
+
+    # Interpolate 0.05 seconds and save as starting point for next iteration
+    a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
+    v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
+    self.v_acc_start = v_acc_sol
+    self.a_acc_start = a_acc_sol
+
+    self.first_loop = False

selfdrive/controls/lib/radar_helpers.py

@@ -0,0 +1,159 @@
+from common.kalman.simple_kalman import KF1D
+from selfdrive.config import RADAR_TO_CAMERA
+
+
+# the longer lead decels, the more likely it will keep decelerating
+# TODO is this a good default?
+_LEAD_ACCEL_TAU = 1.5
+
+# radar tracks
+SPEED, ACCEL = 0, 1   # Kalman filter states enum
+
+# stationary qualification parameters
+v_ego_stationary = 4.   # no stationary object flag below this speed
+
+
+class Track():
+  def __init__(self, v_lead, kalman_params):
+    self.cnt = 0
+    self.aLeadTau = _LEAD_ACCEL_TAU
+    self.K_A = kalman_params.A
+    self.K_C = kalman_params.C
+    self.K_K = kalman_params.K
+    self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
+
+  def update(self, d_rel, y_rel, v_rel, v_lead, measured):
+    # relative values, copy
+    self.dRel = d_rel   # LONG_DIST
+    self.yRel = y_rel   # -LAT_DIST
+    self.vRel = v_rel   # REL_SPEED
+    self.vLead = v_lead
+    self.measured = measured   # measured or estimate
+
+    # computed velocity and accelerations
+    if self.cnt > 0:
+      self.kf.update(self.vLead)
+
+    self.vLeadK = float(self.kf.x[SPEED][0])
+    self.aLeadK = float(self.kf.x[ACCEL][0])
+
+    # Learn if constant acceleration
+    if abs(self.aLeadK) < 0.5:
+      self.aLeadTau = _LEAD_ACCEL_TAU
+    else:
+      self.aLeadTau *= 0.9
+
+    self.cnt += 1
+
+  def get_key_for_cluster(self):
+    # 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]], self.K_A, self.K_C, self.K_K)
+    self.aLeadK = aLeadK
+    self.aLeadTau = aLeadTau
+
+def mean(l):
+  return sum(l) / len(l)
+
+
+class Cluster():
+  def __init__(self):
+    self.tracks = set()
+
+  def add(self, t):
+    # add the first track
+    self.tracks.add(t)
+
+  # TODO: make generic
+  @property
+  def dRel(self):
+    return mean([t.dRel for t in self.tracks])
+
+  @property
+  def yRel(self):
+    return mean([t.yRel for t in self.tracks])
+
+  @property
+  def vRel(self):
+    return mean([t.vRel for t in self.tracks])
+
+  @property
+  def aRel(self):
+    return mean([t.aRel for t in self.tracks])
+
+  @property
+  def vLead(self):
+    return mean([t.vLead for t in self.tracks])
+
+  @property
+  def dPath(self):
+    return mean([t.dPath for t in self.tracks])
+
+  @property
+  def vLat(self):
+    return mean([t.vLat for t in self.tracks])
+
+  @property
+  def vLeadK(self):
+    return mean([t.vLeadK for t in self.tracks])
+
+  @property
+  def aLeadK(self):
+    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):
+    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):
+    return any(t.measured for t in self.tracks)
+
+  def get_RadarState(self, model_prob=0.0):
+    return {
+      "dRel": float(self.dRel),
+      "yRel": float(self.yRel),
+      "vRel": float(self.vRel),
+      "vLead": float(self.vLead),
+      "vLeadK": float(self.vLeadK),
+      "aLeadK": float(self.aLeadK),
+      "status": True,
+      "fcw": self.is_potential_fcw(model_prob),
+      "modelProb": model_prob,
+      "radar": True,
+      "aLeadTau": float(self.aLeadTau)
+    }
+
+  def get_RadarState_from_vision(self, lead_msg, v_ego):
+    return {
+      "dRel": float(lead_msg.dist - RADAR_TO_CAMERA),
+      "yRel": float(lead_msg.relY),
+      "vRel": float(lead_msg.relVel),
+      "vLead": float(v_ego + lead_msg.relVel),
+      "vLeadK": float(v_ego + lead_msg.relVel),
+      "aLeadK": float(0),
+      "aLeadTau": _LEAD_ACCEL_TAU,
+      "fcw": False,
+      "modelProb": float(lead_msg.prob),
+      "radar": False,
+      "status": True
+    }
+
+  def __str__(self):
+    ret = "x: %4.1f  y: %4.1f  v: %4.1f  a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
+    return ret
+
+  def potential_low_speed_lead(self, v_ego):
+    # stop for stuff in front of you and low speed, even without model confirmation
+    return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25
+
+  def is_potential_fcw(self, model_prob):
+    return model_prob > .9

selfdrive/controls/lib/speed_smoother.py

@@ -0,0 +1,99 @@
+import numpy as np
+
+
+def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin):
+
+  tDelta = 0.
+  if aEgo > aMax:
+    tDelta = (aMax - aEgo) / jMin
+  elif aEgo < aMin:
+    tDelta = (aMin - aEgo) / jMax
+
+  return tDelta
+
+
+def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
+
+  dV = vT - vEgo
+
+  # recover quickly if dV is positive and aEgo is negative or viceversa
+  if dV > 0. and aEgo < 0.:
+    jMax *= 3.
+  elif dV < 0. and aEgo > 0.:
+    jMin *= 3.
+
+  tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
+
+  if (ts <= tDelta):
+    if (aEgo < aMin):
+      vEgo += ts * aEgo + 0.5 * ts**2 * jMax
+      aEgo += ts * jMax
+      return vEgo, aEgo
+    elif (aEgo > aMax):
+      vEgo += ts * aEgo + 0.5 * ts**2 * jMin
+      aEgo += ts * jMin
+      return vEgo, aEgo
+
+  if aEgo > aMax:
+    dV -= 0.5 * (aMax**2 - aEgo**2) / jMin
+    vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin
+    aEgo += tDelta * jMin
+  elif aEgo < aMin:
+    dV -= 0.5 * (aMin**2 - aEgo**2) / jMax
+    vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax
+    aEgo += tDelta * jMax
+
+  ts -= tDelta
+
+  jLim = jMin if aEgo >= 0 else jMax
+  # if we reduce the accel to zero immediately, how much delta speed we generate?
+  dv_min_shift = - 0.5 * aEgo**2 / jLim
+
+  # flip signs so we can consider only one case
+  flipped = False
+  if dV < dv_min_shift:
+    flipped = True
+    dV *= -1
+    vEgo *= -1
+    aEgo *= -1
+    aMax = -aMin
+    jMaxcopy = -jMin
+    jMin = -jMax
+    jMax = jMaxcopy
+
+  # small addition needed to avoid numerical issues with sqrt of ~zero
+  aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin))
+
+  if aPeak > aMax:
+    aPeak = aMax
+    t1 = (aPeak - aEgo) / jMax
+    if aPeak <= 0: # there is no solution, so stop after t1
+      t2 = t1 + ts + 1e-9
+      t3 = t2
+    else:
+      vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin
+      if vChange < aPeak * ts:
+        t2 = t1 + vChange / aPeak
+      else:
+        t2 = t1 + ts
+      t3 = t2 - aPeak / jMin
+  else:
+    t1 = (aPeak - aEgo) / jMax
+    t2 = t1
+    t3 = t2 - aPeak / jMin
+
+  dt1 = min(ts, t1)
+  dt2 = max(min(ts, t2) - t1, 0.)
+  dt3 = max(min(ts, t3) - t2, 0.)
+
+  if ts > t3:
+    vEgo += dV
+    aEgo = 0.
+  else:
+    vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin
+    aEgo += jMax * dt1 + dt3 * jMin
+
+  vEgo *= -1 if flipped else 1
+  aEgo *= -1 if flipped else 1
+
+  return float(vEgo), float(aEgo)

selfdrive/controls/lib/vehicle_model.py

@@ -0,0 +1,206 @@
+#!/usr/bin/env python3
+import numpy as np
+from numpy.linalg import solve
+
+"""
+Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"
+
+The state is x = [v, r]^T
+with v lateral speed [m/s], and r rotational speed [rad/s]
+
+The input u is the steering angle [rad]
+
+The system is defined by
+x_dot = A*x + B*u
+
+A depends on longitudinal speed, u [m/s], and vehicle parameters CP
+"""
+
+
+def create_dyn_state_matrices(u, VM):
+  """Returns the A and B matrix for the dynamics system
+
+  Args:
+    u: Vehicle speed [m/s]
+    VM: Vehicle model
+
+  Returns:
+    A tuple with the 2x2 A matrix, and 2x1 B matrix
+
+  Parameters in the vehicle model:
+    cF: Tire stiffnes Front [N/rad]
+    cR: Tire stiffnes Front [N/rad]
+    aF: Distance from CG to front wheels [m]
+    aR: Distance from CG to rear wheels [m]
+    m: Mass [kg]
+    j: Rotational inertia [kg m^2]
+    sR: Steering ratio [-]
+    chi: Steer ratio rear [-]
+  """
+  A = np.zeros((2, 2))
+  B = np.zeros((2, 1))
+  A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
+  A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
+  A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
+  A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
+  B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
+  B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
+  return A, B
+
+
+def kin_ss_sol(sa, u, VM):
+  """Calculate the steady state solution at low speeds
+  At low speeds the tire slip is undefined, so a kinematic
+  model is used.
+
+  Args:
+    sa: Steering angle [rad]
+    u: Speed [m/s]
+    VM: Vehicle model
+
+  Returns:
+    2x1 matrix with steady state solution
+  """
+  K = np.zeros((2, 1))
+  K[0, 0] = VM.aR / VM.sR / VM.l * u
+  K[1, 0] = 1. / VM.sR / VM.l * u
+  return K * sa
+
+
+def dyn_ss_sol(sa, u, VM):
+  """Calculate the steady state solution when x_dot = 0,
+  Ax + Bu = 0 => x = A^{-1} B u
+
+  Args:
+    sa: Steering angle [rad]
+    u: Speed [m/s]
+    VM: Vehicle model
+
+  Returns:
+    2x1 matrix with steady state solution
+  """
+  A, B = create_dyn_state_matrices(u, VM)
+  return -solve(A, B) * sa
+
+
+def calc_slip_factor(VM):
+  """The slip factor is a measure of how the curvature changes with speed
+  it's positive for Oversteering vehicle, negative (usual case) otherwise.
+  """
+  return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)
+
+
+class VehicleModel():
+  def __init__(self, CP):
+    """
+    Args:
+      CP: Car Parameters
+    """
+    # for math readability, convert long names car params into short names
+    self.m = CP.mass
+    self.j = CP.rotationalInertia
+    self.l = CP.wheelbase
+    self.aF = CP.centerToFront
+    self.aR = CP.wheelbase - CP.centerToFront
+    self.chi = CP.steerRatioRear
+
+    self.cF_orig = CP.tireStiffnessFront
+    self.cR_orig = CP.tireStiffnessRear
+    self.update_params(1.0, CP.steerRatio)
+
+  def update_params(self, stiffness_factor, steer_ratio):
+    """Update the vehicle model with a new stiffness factor and steer ratio"""
+    self.cF = stiffness_factor * self.cF_orig
+    self.cR = stiffness_factor * self.cR_orig
+    self.sR = steer_ratio
+
+  def steady_state_sol(self, sa, u):
+    """Returns the steady state solution.
+
+    If the speed is too small we can't use the dynamic model (tire slip is undefined),
+    we then have to use the kinematic model
+
+    Args:
+      sa: Steering wheel angle [rad]
+      u: Speed [m/s]
+
+    Returns:
+      2x1 matrix with steady state solution (lateral speed, rotational speed)
+    """
+    if u > 0.1:
+      return dyn_ss_sol(sa, u, self)
+    else:
+      return kin_ss_sol(sa, u, self)
+
+  def calc_curvature(self, sa, u):
+    """Returns the curvature. Multiplied by the speed this will give the yaw rate.
+
+    Args:
+      sa: Steering wheel angle [rad]
+      u: Speed [m/s]
+
+    Returns:
+      Curvature factor [1/m]
+    """
+    return self.curvature_factor(u) * sa / self.sR
+
+  def curvature_factor(self, u):
+    """Returns the curvature factor.
+    Multiplied by wheel angle (not steering wheel angle) this will give the curvature.
+
+    Args:
+      u: Speed [m/s]
+
+    Returns:
+      Curvature factor [1/m]
+    """
+    sf = calc_slip_factor(self)
+    return (1. - self.chi) / (1. - sf * u**2) / self.l
+
+  def get_steer_from_curvature(self, curv, u):
+    """Calculates the required steering wheel angle for a given curvature
+
+    Args:
+      curv: Desired curvature [1/m]
+      u: Speed [m/s]
+
+    Returns:
+      Steering wheel angle [rad]
+    """
+
+    return curv * self.sR * 1.0 / self.curvature_factor(u)
+
+  def get_steer_from_yaw_rate(self, yaw_rate, u):
+    """Calculates the required steering wheel angle for a given yaw_rate
+
+    Args:
+      yaw_rate: Desired yaw rate [rad/s]
+      u: Speed [m/s]
+
+    Returns:
+      Steering wheel angle [rad]
+    """
+    curv = yaw_rate / u
+    return self.get_steer_from_curvature(curv, u)
+
+  def yaw_rate(self, sa, u):
+    """Calculate yaw rate
+
+    Args:
+      sa: Steering wheel angle [rad]
+      u: Speed [m/s]
+
+    Returns:
+      Yaw rate [rad/s]
+    """
+    return self.calc_curvature(sa, u) * u
+
+
+if __name__ == '__main__':
+  import math
+  from selfdrive.car.honda.interface import CarInterface
+  from selfdrive.car.honda.values import CAR
+
+  CP = CarInterface.get_params(CAR.CIVIC)
+  VM = VehicleModel(CP)
+  print(VM.yaw_rate(math.radians(20), 10.))

selfdrive/controls/plannerd.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+import gc
+
+from cereal import car
+from common.params import Params
+from common.realtime import set_realtime_priority
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.planner import Planner
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.pathplanner import PathPlanner
+import cereal.messaging as messaging
+
+
+def plannerd_thread(sm=None, pm=None):
+  gc.disable()
+
+  # start the loop
+  set_realtime_priority(2)
+
+  cloudlog.info("plannerd is waiting for CarParams")
+  CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+  cloudlog.info("plannerd got CarParams: %s", CP.carName)
+
+  PL = Planner(CP)
+  PP = PathPlanner(CP)
+
+  VM = VehicleModel(CP)
+
+  if sm is None:
+    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
+
+  if pm is None:
+    pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
+
+  sm['liveParameters'].valid = True
+  sm['liveParameters'].sensorValid = True
+  sm['liveParameters'].steerRatio = CP.steerRatio
+  sm['liveParameters'].stiffnessFactor = 1.0
+
+  while True:
+    sm.update()
+
+    if sm.updated['model']:
+      PP.update(sm, pm, CP, VM)
+    if sm.updated['radarState']:
+      PL.update(sm, pm, CP, VM, PP)
+
+
+def main(sm=None, pm=None):
+  plannerd_thread(sm, pm)
+
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/radard.py

@@ -0,0 +1,246 @@
+#!/usr/bin/env python3
+import importlib
+import math
+from collections import defaultdict, deque
+
+import cereal.messaging as messaging
+from cereal import car
+from common.numpy_fast import interp
+from common.params import Params
+from common.realtime import Ratekeeper, set_realtime_priority
+from selfdrive.config import RADAR_TO_CAMERA
+from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
+from selfdrive.controls.lib.radar_helpers import Cluster, Track
+from selfdrive.swaglog import cloudlog
+
+
+class KalmanParams():
+  def __init__(self, dt):
+    # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
+    # hardcoding a lookup table to compute K for values of radar_ts between 0.1s and 1.0s
+    assert dt > .01 and dt < .1, "Radar time step must be between .01s and 0.1s"
+    self.A = [[1.0, dt], [0.0, 1.0]]
+    self.C = [1.0, 0.0]
+    #Q = np.matrix([[10., 0.0], [0.0, 100.]])
+    #R = 1e3
+    #K = np.matrix([[ 0.05705578], [ 0.03073241]])
+    dts = [dt * 0.01 for dt in range(1, 11)]
+    K0 = [0.12288, 0.14557, 0.16523, 0.18282, 0.19887, 0.21372, 0.22761, 0.24069, 0.2531, 0.26491]
+    K1 = [0.29666, 0.29331, 0.29043, 0.28787, 0.28555, 0.28342, 0.28144, 0.27958, 0.27783, 0.27617]
+    self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
+
+
+def laplacian_cdf(x, mu, b):
+  b = max(b, 1e-4)
+  return math.exp(-abs(x-mu)/b)
+
+
+def match_vision_to_cluster(v_ego, lead, clusters):
+  # match vision point to best statistical cluster match
+  offset_vision_dist = lead.dist - RADAR_TO_CAMERA
+
+  def prob(c):
+    prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
+    prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
+    prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
+
+    # This is isn't exactly right, but good heuristic
+    return prob_d * prob_y * prob_v
+
+  cluster = max(clusters, key=prob)
+
+  # if no 'sane' match is found return -1
+  # stationary radar points can be false positives
+  dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+  vel_sane = (abs(cluster.vRel - lead.relVel) < 10) or (v_ego + cluster.vRel > 2)
+  if dist_sane and vel_sane:
+    return cluster
+  else:
+    return None
+
+
+def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
+  # Determine leads, this is where the essential logic happens
+  if len(clusters) > 0 and ready and lead_msg.prob > .5:
+    cluster = match_vision_to_cluster(v_ego, lead_msg, clusters)
+  else:
+    cluster = None
+
+  lead_dict = {'status': False}
+  if cluster is not None:
+    lead_dict = cluster.get_RadarState(lead_msg.prob)
+  elif (cluster is None) and ready and (lead_msg.prob > .5):
+    lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
+
+  if low_speed_override:
+    low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
+    if len(low_speed_clusters) > 0:
+      closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel)
+
+      # Only choose new cluster if it is actually closer than the previous one
+      if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']):
+        lead_dict = closest_cluster.get_RadarState()
+
+  return lead_dict
+
+
+class RadarD():
+  def __init__(self, radar_ts, delay=0):
+    self.current_time = 0
+
+    self.tracks = defaultdict(dict)
+    self.kalman_params = KalmanParams(radar_ts)
+
+    self.last_md_ts = 0
+    self.last_controls_state_ts = 0
+
+    self.active = 0
+
+    # v_ego
+    self.v_ego = 0.
+    self.v_ego_hist = deque([0], maxlen=delay+1)
+
+    self.ready = False
+
+  def update(self, frame, sm, rr, has_radar):
+    self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
+
+    if sm.updated['controlsState']:
+      self.active = sm['controlsState'].active
+      self.v_ego = sm['controlsState'].vEgo
+      self.v_ego_hist.append(self.v_ego)
+    if sm.updated['model']:
+      self.ready = True
+
+    ar_pts = {}
+    for pt in rr.points:
+      ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
+
+    # *** remove missing points from meta data ***
+    for ids in list(self.tracks.keys()):
+      if ids not in ar_pts:
+        self.tracks.pop(ids, None)
+
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      rpt = ar_pts[ids]
+
+      # align v_ego by a fixed time to align it with the radar measurement
+      v_lead = rpt[2] + self.v_ego_hist[0]
+
+      # create the track if it doesn't exist or it's a new track
+      if ids not in self.tracks:
+        self.tracks[ids] = Track(v_lead, self.kalman_params)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
+
+    idens = list(sorted(self.tracks.keys()))
+    track_pts = list([self.tracks[iden].get_key_for_cluster() for iden in idens])
+
+
+    # If we have multiple points, cluster them
+    if len(track_pts) > 1:
+      cluster_idxs = cluster_points_centroid(track_pts, 2.5)
+      clusters = [None] * (max(cluster_idxs) + 1)
+
+      for idx in range(len(track_pts)):
+        cluster_i = cluster_idxs[idx]
+        if clusters[cluster_i] is None:
+          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 range(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', 'model'])
+    dat.radarState.mdMonoTime = self.last_md_ts
+    dat.radarState.canMonoTimes = list(rr.canMonoTimes)
+    dat.radarState.radarErrors = list(rr.errors)
+    dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
+
+    if has_radar:
+      dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
+      dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
+    return dat
+
+
+# fuses camera and radar data for best lead detection
+def radard_thread(sm=None, pm=None, can_sock=None):
+  set_realtime_priority(2)
+
+  # 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))
+  cloudlog.info("radard got CarParams")
+
+  # import the radar from the fingerprint
+  cloudlog.info("radard is importing %s", CP.carName)
+  RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
+
+  if can_sock is None:
+    can_sock = messaging.sub_sock('can')
+
+  if sm is None:
+    sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
+
+  # *** publish radarState and liveTracks
+  if pm is None:
+    pm = messaging.PubMaster(['radarState', 'liveTracks'])
+
+  RI = RadarInterface(CP)
+
+  rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
+  RD = RadarD(CP.radarTimeStep, RI.delay)
+
+  has_radar = not CP.radarOffCan
+
+  while 1:
+    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+    rr = RI.update(can_strings)
+
+    if rr is None:
+      continue
+
+    sm.update(0)
+
+    dat = RD.update(rk.frame, sm, rr, has_radar)
+    dat.radarState.cumLagMs = -rk.remaining*1000.
+
+    pm.send('radarState', dat)
+
+    # *** publish tracks for UI debugging (keep last) ***
+    tracks = RD.tracks
+    dat = messaging.new_message()
+    dat.init('liveTracks', len(tracks))
+
+    for cnt, ids in enumerate(sorted(tracks.keys())):
+      dat.liveTracks[cnt] = {
+        "trackId": ids,
+        "dRel": float(tracks[ids].dRel),
+        "yRel": float(tracks[ids].yRel),
+        "vRel": float(tracks[ids].vRel),
+      }
+    pm.send('liveTracks', dat)
+
+    rk.monitor_time()
+
+
+def main(sm=None, pm=None, can_sock=None):
+  radard_thread(sm, pm, can_sock)
+
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/tests/test_clustering.py

@@ -0,0 +1,138 @@
+import time
+import unittest
+import numpy as np
+from fastcluster import linkage_vector
+from scipy.cluster import _hierarchy
+from scipy.spatial.distance import pdist
+
+from selfdrive.controls.lib.cluster.fastcluster_py import hclust, ffi
+from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
+
+
+def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
+  # supersimplified function to get fast clustering. Got it from scipy
+  Z = np.asarray(Z, order='c')
+  n = Z.shape[0] + 1
+  T = np.zeros((n,), dtype='i')
+  _hierarchy.cluster_dist(Z, T, float(t), int(n))
+  return T
+
+
+TRACK_PTS = np.array([[59.26000137, -9.35999966, -5.42500019],
+                      [91.61999817, -0.31999999, -2.75],
+                      [31.38000031, 0.40000001, -0.2],
+                      [89.57999725, -8.07999992, -18.04999924],
+                      [53.42000122, 0.63999999, -0.175],
+                      [31.38000031, 0.47999999, -0.2],
+                      [36.33999939, 0.16, -0.2],
+                      [53.33999939, 0.95999998, -0.175],
+                      [59.26000137, -9.76000023, -5.44999981],
+                      [33.93999977, 0.40000001, -0.22499999],
+                      [106.74000092, -5.76000023, -18.04999924]])
+
+CORRECT_LINK = np.array([[2., 5., 0.07999998, 2.],
+                         [4., 7., 0.32984889, 2.],
+                         [0., 8., 0.40078104, 2.],
+                         [6., 9., 2.41209933, 2.],
+                         [11., 14., 3.76342275, 4.],
+                         [12., 13., 13.02297651, 4.],
+                         [1., 3., 17.27626057, 2.],
+                         [10., 17., 17.92918845, 3.],
+                         [15., 16., 23.68525366, 8.],
+                         [18., 19., 52.52351319, 11.]])
+
+CORRECT_LABELS = np.array([7, 1, 4, 2, 6, 4, 5, 6, 7, 5, 3], dtype=np.int32)
+
+
+def plot_cluster(pts, idx_old, idx_new):
+    import matplotlib.pyplot as plt
+    m = 'Set1'
+
+    plt.figure()
+    plt.subplot(1, 2, 1)
+    plt.scatter(pts[:, 0], pts[:, 1], c=idx_old, cmap=m)
+    plt.title("Old")
+    plt.colorbar()
+    plt.subplot(1, 2, 2)
+    plt.scatter(pts[:, 0], pts[:, 1], c=idx_new, cmap=m)
+    plt.title("New")
+    plt.colorbar()
+
+    plt.show()
+
+
+def same_clusters(correct, other):
+  correct = np.asarray(correct)
+  other = np.asarray(other)
+  if len(correct) != len(other):
+    return False
+
+  for i in range(len(correct)):
+    c = np.where(correct == correct[i])
+    o = np.where(other == other[i])
+    if not np.array_equal(c, o):
+      return False
+  return True
+
+
+class TestClustering(unittest.TestCase):
+  def test_scipy_clustering(self):
+    old_link = linkage_vector(TRACK_PTS, method='centroid')
+    old_cluster_idxs = fcluster(old_link, 2.5, criterion='distance')
+
+    np.testing.assert_allclose(old_link, CORRECT_LINK)
+    np.testing.assert_allclose(old_cluster_idxs, CORRECT_LABELS)
+
+  def test_pdist(self):
+    pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64)
+    pts_ptr = ffi.cast("double *", pts.ctypes.data)
+
+    n, m = pts.shape
+    out = np.zeros((n * (n - 1) // 2, ), dtype=np.float64)
+    out_ptr = ffi.cast("double *", out.ctypes.data)
+    hclust.hclust_pdist(n, m, pts_ptr, out_ptr)
+
+    np.testing.assert_allclose(out, np.power(pdist(TRACK_PTS), 2))
+
+  def test_cpp_clustering(self):
+    pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64)
+    pts_ptr = ffi.cast("double *", pts.ctypes.data)
+    n, m = pts.shape
+
+    labels = np.zeros((n, ), dtype=np.int32)
+    labels_ptr = ffi.cast("int *", labels.ctypes.data)
+    hclust.cluster_points_centroid(n, m, pts_ptr, 2.5**2, labels_ptr)
+    self.assertTrue(same_clusters(CORRECT_LABELS, labels))
+
+  def test_cpp_wrapper_clustering(self):
+    labels = cluster_points_centroid(TRACK_PTS, 2.5)
+    self.assertTrue(same_clusters(CORRECT_LABELS, labels))
+
+  def test_random_cluster(self):
+    np.random.seed(1337)
+    N = 1000
+
+    t_old = 0.
+    t_new = 0.
+
+    for _ in range(N):
+      n = int(np.random.uniform(2, 32))
+      x = np.random.uniform(-10, 50, (n, 1))
+      y = np.random.uniform(-5, 5, (n, 1))
+      vrel = np.random.uniform(-5, 5, (n, 1))
+      pts = np.hstack([x, y, vrel])
+
+      t = time.time()
+      old_link = linkage_vector(pts, method='centroid')
+      old_cluster_idx = fcluster(old_link, 2.5, criterion='distance')
+      t_old += time.time() - t
+
+      t = time.time()
+      cluster_idx = cluster_points_centroid(pts, 2.5)
+      t_new += time.time() - t
+
+      self.assertTrue(same_clusters(old_cluster_idx, cluster_idx))
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_following_distance.py

@@ -0,0 +1,91 @@
+import unittest
+import numpy as np
+
+from cereal import log
+import cereal.messaging as messaging
+from selfdrive.config import Conversions as CV
+from selfdrive.controls.lib.planner import calc_cruise_accel_limits
+from selfdrive.controls.lib.speed_smoother import speed_smoother
+from selfdrive.controls.lib.long_mpc import LongitudinalMpc
+
+
+def RW(v_ego, v_l):
+  TR = 1.8
+  G = 9.81
+  return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
+
+
+class FakePubMaster():
+  def send(self, s, data):
+    assert data
+
+
+def run_following_distance_simulation(v_lead, t_end=200.0):
+  dt = 0.2
+  t = 0.
+
+  x_lead = 200.0
+
+  x_ego = 0.0
+  v_ego = v_lead
+  a_ego = 0.0
+
+  v_cruise_setpoint = v_lead + 10.
+
+  pm = FakePubMaster()
+  mpc = LongitudinalMpc(1)
+
+  first = True
+  while t < t_end:
+    # Run cruise control
+    accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)]
+    jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
+    v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
+                                        accel_limits[1], accel_limits[0],
+                                        jerk_limits[1], jerk_limits[0],
+                                        dt)
+
+    # Setup CarState
+    CS = messaging.new_message()
+    CS.init('carState')
+    CS.carState.vEgo = v_ego
+    CS.carState.aEgo = a_ego
+
+    # Setup lead packet
+    lead = log.RadarState.LeadData.new_message()
+    lead.status = True
+    lead.dRel = x_lead - x_ego
+    lead.vLead = v_lead
+    lead.aLeadK = 0.0
+
+    # Run MPC
+    mpc.set_cur_state(v_ego, a_ego)
+    if first:  # Make sure MPC is converged on first timestep
+      for _ in range(20):
+        mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
+    mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
+
+    # Choose slowest of two solutions
+    if v_cruise < mpc.v_mpc:
+      v_ego, a_ego = v_cruise, a_cruise
+    else:
+      v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
+
+    # Update state
+    x_lead += v_lead * dt
+    x_ego += v_ego * dt
+    t += dt
+    first = False
+
+  return x_lead - x_ego
+
+
+class TestFollowingDistance(unittest.TestCase):
+  def test_following_distanc(self):
+    for speed_mph in np.linspace(10, 100, num=10):
+      v_lead = float(speed_mph * CV.MPH_TO_MS)
+
+      simulation_steady_state = run_following_distance_simulation(v_lead)
+      correct_steady_state = RW(v_lead, v_lead) + 4.0
+
+      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.1)

selfdrive/controls/tests/test_lateral_mpc.py

@@ -0,0 +1,129 @@
+import unittest
+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.,
+            l_prob=1., r_prob=1., p_prob=1.,
+            poly_l=np.array([0., 0., 0., 1.8]), poly_r=np.array([0., 0., 0., -1.8]), poly_p=np.array([0., 0., 0., 0.]),
+            lane_width=3.6, poly_shift=0.):
+
+  libmpc = libmpc_py.libmpc
+  libmpc.init(1.0, 3.0, 1.0, 1.0)
+
+  mpc_solution = libmpc_py.ffi.new("log_t *")
+
+  p_l = poly_l.copy()
+  p_l[3] += poly_shift
+
+  p_r = poly_r.copy()
+  p_r[3] += poly_shift
+
+  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)
+
+  v_ref = v_ref
+  curvature_factor = VM.curvature_factor(v_ref)
+
+  l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l)))
+  r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r)))
+  d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly)))
+
+  cur_state = libmpc_py.ffi.new("state_t *")
+  cur_state[0].x = x_init
+  cur_state[0].y = y_init
+  cur_state[0].psi = psi_init
+  cur_state[0].delta = delta_init
+
+  # converge in no more than 20 iterations
+  for _ in range(20):
+    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
+
+
+class TestLateralMpc(unittest.TestCase):
+
+  def _assert_null(self, sol, delta=1e-6):
+    for i in range(len(sol[0].y)):
+      self.assertAlmostEqual(sol[0].y[i], 0., delta=delta)
+      self.assertAlmostEqual(sol[0].psi[i], 0., delta=delta)
+      self.assertAlmostEqual(sol[0].delta[i], 0., delta=delta)
+
+  def _assert_simmetry(self, sol, delta=1e-6):
+    for i in range(len(sol[0][0].y)):
+      self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].delta[i], -sol[1][0].delta[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta)
+
+  def _assert_identity(self, sol, ignore_y=False, delta=1e-6):
+    for i in range(len(sol[0][0].y)):
+      self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].delta[i], sol[1][0].delta[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta)
+      if not ignore_y:
+        self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=delta)
+
+  def test_straight(self):
+    sol = run_mpc()
+    self._assert_null(sol)
+
+  def test_y_symmetry(self):
+    sol = []
+    for y_init in [-0.5, 0.5]:
+      sol.append(run_mpc(y_init=y_init))
+    self._assert_simmetry(sol)
+
+  def test_poly_symmetry(self):
+    sol = []
+    for poly_shift in [-1., 1.]:
+      sol.append(run_mpc(poly_shift=poly_shift))
+    self._assert_simmetry(sol)
+
+  def test_delta_symmetry(self):
+    sol = []
+    for delta_init in [-0.1, 0.1]:
+      sol.append(run_mpc(delta_init=delta_init))
+    self._assert_simmetry(sol)
+
+  def test_psi_symmetry(self):
+    sol = []
+    for psi_init in [-0.1, 0.1]:
+      sol.append(run_mpc(psi_init=psi_init))
+    self._assert_simmetry(sol)
+
+  def test_prob_symmetry(self):
+    sol = []
+    lane_width = 3.
+    for r_prob in [0., 1.]:
+      sol.append(run_mpc(r_prob=r_prob, l_prob=1.-r_prob, lane_width=lane_width))
+    self._assert_simmetry(sol)
+
+  def test_y_shift_vs_poly_shift(self):
+    shift = 1.
+    sol = []
+    sol.append(run_mpc(y_init=shift))
+    sol.append(run_mpc(poly_shift=-shift))
+    # need larger delta than standard, otherwise it false triggers.
+    # this is acceptable because the 2 cases are very different from the optimizer standpoint
+    self._assert_identity(sol, ignore_y=True, delta=1e-5)
+
+  def test_no_overshoot(self):
+    y_init = 1.
+    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()

selfdrive/controls/tests/test_monitoring.py

@@ -0,0 +1,234 @@
+import unittest
+import numpy as np
+from common.realtime import DT_CTRL, DT_DMON
+from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \
+                                  _AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \
+                                  _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \
+                                  _DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL, \
+                                  _POSESTD_THRESHOLD, _HI_STD_TIMEOUT
+from selfdrive.controls.lib.gps_helpers import is_rhd_region
+
+_TEST_TIMESPAN = 120 # seconds
+_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
+_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1
+_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
+_INVISIBLE_SECONDS_TO_RED = _AWARENESS_TIME + 1
+_UNCERTAIN_SECONDS_TO_GREEN = _HI_STD_TIMEOUT + 0.5
+
+class fake_DM_msg():
+  def __init__(self, is_face_detected, is_distracted=False, is_model_uncertain=False):
+    self.faceOrientation = [0.,0.,0.]
+    self.facePosition = [0.,0.]
+    self.faceProb = 1. * is_face_detected
+    self.leftEyeProb = 1.
+    self.rightEyeProb = 1.
+    self.leftBlinkProb = 1. * is_distracted
+    self.rightBlinkProb = 1. * is_distracted
+    self.faceOrientationStd = [1.*is_model_uncertain,1.*is_model_uncertain,1.*is_model_uncertain]
+    self.facePositionStd = [1.*is_model_uncertain,1.*is_model_uncertain]
+
+
+# driver state from neural net, 10Hz
+msg_NO_FACE_DETECTED = fake_DM_msg(is_face_detected=False)
+msg_ATTENTIVE = fake_DM_msg(is_face_detected=True)
+msg_DISTRACTED = fake_DM_msg(is_face_detected=True, is_distracted=True)
+msg_ATTENTIVE_UNCERTAIN = fake_DM_msg(is_face_detected=True, is_model_uncertain=True)
+msg_DISTRACTED_UNCERTAIN = fake_DM_msg(is_face_detected=True, is_distracted=True, is_model_uncertain=True)
+msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = fake_DM_msg(is_face_detected=True, is_distracted=True, is_model_uncertain=_POSESTD_THRESHOLD*1.5)
+
+# driver interaction with car
+car_interaction_DETECTED = True
+car_interaction_NOT_DETECTED = False
+
+# openpilot state
+openpilot_ENGAGED = True
+openpilot_NOT_ENGAGED = False
+
+# car standstill state
+car_STANDSTILL = True
+car_NOT_STANDSTILL = False
+
+# some common state vectors
+always_no_face = [msg_NO_FACE_DETECTED] * int(_TEST_TIMESPAN/DT_DMON)
+always_attentive = [msg_ATTENTIVE] * int(_TEST_TIMESPAN/DT_DMON)
+always_distracted = [msg_DISTRACTED] * int(_TEST_TIMESPAN/DT_DMON)
+always_true = [True] * int(_TEST_TIMESPAN/DT_DMON)
+always_false = [False] * int(_TEST_TIMESPAN/DT_DMON)
+
+def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status):
+  # inputs are all 10Hz
+  DS = DriverStatus()
+  events_from_DM = []
+  for idx in range(len(driver_state_msgs)):
+    DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx])
+                # cal_rpy and car_speed don't matter here
+
+    # to match frequency of controlsd (100Hz)
+    for _ in range(int(DT_DMON/DT_CTRL)):
+      event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx])
+    events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests
+
+  assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong'
+  return events_from_DM
+
+class TestMonitoring(unittest.TestCase):
+  # -1. rhd parser sanity check
+  def test_rhd_parser(self):
+    cities = [[32.7, -117.1, 0],\
+              [51.5, 0.129, 1],\
+              [35.7, 139.7, 1],\
+              [-37.8, 144.9, 1],\
+              [32.1, 41.74, 0],\
+              [55.7, 12.69, 0]]
+    result = []
+    for city in cities:
+      result.append(int(is_rhd_region(city[0],city[1])))
+    self.assertEqual(result,[int(city[2]) for city in cities])
+
+  # 0. op engaged, driver is doing fine all the time
+  def test_fully_aware_driver(self):
+    events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)
+    self.assertTrue(np.sum([len(event) for event in events_output])==0)
+
+  # 1. op engaged, driver is distracted and does nothing
+  def test_fully_distracted_driver(self):
+    events_output = run_DState_seq(always_distracted, always_false, always_true, always_false)
+    self.assertTrue(len(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+\
+                      ((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\
+                      ((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_TIME+\
+                      ((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)][0].name, 'driverDistracted')
+
+  # 2. op engaged, no face detected the whole time, no action
+  def test_fully_invisible_driver(self):
+    events_output = run_DState_seq(always_no_face, always_false, always_true, always_false)
+    self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL+\
+                      ((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverUnresponsive')
+    self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\
+                      ((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverUnresponsive')
+    self.assertEqual(events_output[int((_AWARENESS_TIME+\
+                      ((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)][0].name, 'driverUnresponsive')
+
+  # 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
+  #  - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch
+  def test_normal_driver(self):
+    ds_vector = [msg_DISTRACTED] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_DISTRACTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON))
+    interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
+                        [car_interaction_DETECTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
+    events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)
+    self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0)
+
+  # 4. op engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
+  #                          driver dodges, and then touches wheel to no avail, disengages and reengages
+  #  - orange/red alert should remain after disappearance, and only disengaging clears red
+  def test_biggest_comma_fan(self):
+    _invisible_time = 2 # seconds
+    ds_vector = always_distracted[:]
+    interaction_vector = always_false[:]
+    op_vector = always_true[:]
+    ds_vector[int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((_DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
+    ds_vector[int((_DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
+    interaction_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON)
+    op_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON)
+    events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)][0].name, 'promptDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)][0].name, 'driverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)][0].name, 'driverDistracted')
+    self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0)
+
+  # 5. op engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
+  #  - both actions should clear the alert, but momentary appearence should not
+  def test_sometimes_transparent_commuter(self):
+      _visible_time = np.random.choice([1,10]) # seconds
+      # print _visible_time
+      ds_vector = always_no_face[:]*2
+      interaction_vector = always_false[:]*2
+      ds_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
+      interaction_vector[int((_INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
+      events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
+      self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
+      self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+      self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)])==0)
+      if _visible_time == 1:
+        self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+        self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)][0].name, 'preDriverUnresponsive')
+      elif _visible_time == 10:
+        self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+        self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)])==0)
+      else:
+        pass
+
+  # 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
+  #  - only disengage will clear the alert
+  def test_last_second_responder(self):
+    _visible_time = 2 # seconds
+    ds_vector = always_no_face[:]
+    interaction_vector = always_false[:]
+    op_vector = always_true[:]
+    ds_vector[int(_INVISIBLE_SECONDS_TO_RED/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
+    interaction_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
+    op_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
+    events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
+    self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)][0].name, 'driverUnresponsive')
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)][0].name, 'driverUnresponsive')
+    self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)][0].name, 'driverUnresponsive')
+    self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0)
+
+  # 7. op not engaged, always distracted driver
+  #  - dm should stay quiet when not engaged
+  def test_pure_dashcam_user(self):
+    events_output = run_DState_seq(always_distracted, always_false, always_false, always_false)
+    self.assertTrue(np.sum([len(event) for event in events_output])==0)
+
+  # 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving
+  #  - should only reach green when stopped, but continues counting down on launch
+  def test_long_traffic_light_victim(self):
+    _redlight_time = 60 # seconds
+    standstill_vector = always_true[:]
+    standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON)
+    events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector)
+    self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)][0].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((_redlight_time-0.1)/DT_DMON)][0].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted')
+
+  # 9. op engaged, model is extremely uncertain. driver first attentive, then distracted
+  #  - should only pop the green alert about model uncertainty
+  #  - (note: this's just for sanity check, std output should never be this high)
+  def test_one_indecisive_model(self):
+    ds_vector = [msg_ATTENTIVE_UNCERTAIN] * int(_UNCERTAIN_SECONDS_TO_GREEN/DT_DMON) + \
+                [msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_DISTRACTED_UNCERTAIN] * (int(_TEST_TIMESPAN/DT_DMON)-int((_DISTRACTED_SECONDS_TO_ORANGE+_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON))
+    interaction_vector = always_false[:]
+    events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)
+    self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN-0.1)/DT_DMON)][0].name, 'driverMonitorLowAcc')
+    self.assertTrue(len(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN+_DISTRACTED_SECONDS_TO_ORANGE-0.5)/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_TEST_TIMESPAN-5.)/DT_DMON)][0].name, 'driverMonitorLowAcc')
+
+  # 10. op engaged, model is somehow uncertain and driver is distracted
+  #  - should slow down the alert countdown but it still gets there
+  def test_somehow_indecisive_model(self):
+    ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(_TEST_TIMESPAN/DT_DMON)
+    interaction_vector = always_false[:]
+    events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)
+    self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)])==0)
+    self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON)][0].name, 'driverMonitorLowAcc')
+    self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'preDriverDistracted')
+    self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'promptDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_TIME+1)/DT_DMON)][1].name, 'promptDriverDistracted')
+    self.assertEqual(events_output[int((_DISTRACTED_TIME*2.5)/DT_DMON)][1].name, 'driverDistracted')
+
+if __name__ == "__main__":
+  print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS)
+  unittest.main()