openpilot_comma/selfdrive/controls/controlsd.py

#!/usr/bin/env python
import zmq
import numpy as np

from common.services import service_list
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper

from selfdrive.config import CruiseButtons
from selfdrive.config import Conversions as CV

from selfdrive.controls.lib.drive_helpers import learn_angle_offset
from selfdrive.controls.lib.alert_database import process_alert, AI

import selfdrive.messaging as messaging

from selfdrive.controls.lib.carstate import CarState
from selfdrive.controls.lib.carcontroller import CarController
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.latcontrol import LatControl

from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise

def controlsd_thread(gctx, rate=100):  #rate in Hz
  # *** log ***
  context = zmq.Context()
  live100 = messaging.pub_sock(context, service_list['live100'].port)
  thermal = messaging.sub_sock(context, service_list['thermal'].port)
  live20 = messaging.sub_sock(context, service_list['live20'].port)
  model = messaging.sub_sock(context, service_list['model'].port)

  logcan = messaging.sub_sock(context, service_list['can'].port)
  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)

  # *** init the major players ***
  CS = CarState(logcan)
  CC = CarController()

  PP = PathPlanner(model)
  AC = AdaptiveCruise(live20)

  LoC = LongControl()
  LaC = LatControl()

  # *** control initial values ***
  apply_brake = 0
  enabled = False

  # *** time values ***
  last_enable_pressed = 0

  # *** controls initial values ***
  # *** display stuff
  soft_disable_start = 0
  sounding = False
  no_mismatch_pcm_last, no_mismatch_ctrl_last = 0, 0

  # car state
  alert, sound_exp, hud_exp, text_exp, alert_p = None, 0, 0, 0, 0
  rear_view_cam, rear_view_toggle = False, False

  v_cruise = 255           # this means no display
  v_cruise_max = 144
  v_cruise_min = 8
  v_cruise_delta = 8

  # on activation target at least 25mph. With 5mph you need too much tapping
  v_cruise_enable_min = 40

  hud_v_cruise = 255

  angle_offset = 0

  max_enable_speed = 57.   # ~91 mph

  pcm_threshold = 25.*CV.MPH_TO_MS    # below this speed pcm cancels

  overtemp = True

  # 0.0 - 1.0
  awareness_status = 0.0

  # start the loop
  set_realtime_priority(2)

  rk = Ratekeeper(rate)
  while 1:
    cur_time = sec_since_boot()

    # read CAN
    canMonoTimes = CS.update(logcan)

    # **** rearview mirror management ***
    if CS.cruise_setting == 1 and CS.prev_cruise_setting == 0:
      rear_view_toggle = not rear_view_toggle

    # show rear view camera on phone if in reverse gear or when lkas button is pressed
    rear_view_cam = (CS.gear_shifter == 2) or rear_view_toggle or CS.blinker_on

    # *** thermal checking logic ***

    # thermal data, checked every second
    td = messaging.recv_sock(thermal)
    if td is not None:
      cpu_temps = [td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
                   td.thermal.cpu3, td.thermal.mem, td.thermal.gpu]
      # check overtemp
      overtemp = any(t > 950 for t in cpu_temps)

    # *** getting model logic ***
    PP.update(cur_time, CS.v_ego)

    if rk.frame % 5 == 2:
      # *** run this at 20hz again ***
      angle_offset = learn_angle_offset(enabled, CS.v_ego, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steer_override)

    # to avoid race conditions, check if control has been disabled for at least 0.2s
    mismatch_ctrl = not CC.controls_allowed and enabled
    mismatch_pcm = (not CS.pcm_acc_status and (not apply_brake or CS.v_ego < 0.1)) and enabled

    # keep resetting start timer if mismatch isn't true
    if not mismatch_ctrl:
      no_mismatch_ctrl_last = cur_time
    if not mismatch_pcm or not CS.brake_only:
      no_mismatch_pcm_last = cur_time

    #*** v_cruise logic ***
    if CS.brake_only:
      v_cruise = int(CS.v_cruise_pcm) # TODO: why sometimes v_cruise_pcm is long type?
    else:
      if CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.RES_ACCEL and enabled:
        v_cruise = v_cruise - (v_cruise % v_cruise_delta) + v_cruise_delta
      elif CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.DECEL_SET and enabled:
        v_cruise = v_cruise + (v_cruise % v_cruise_delta) - v_cruise_delta

    # *** enabling/disabling logic ***
    enable_pressed = (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) \
                     and CS.cruise_buttons == 0

    if enable_pressed:
      print "enabled pressed at", cur_time
      last_enable_pressed = cur_time

    # if pcm does speed control than we need to wait on pcm to enable
    if CS.brake_only:
      enable_condition = (cur_time - last_enable_pressed) < 0.2 and CS.pcm_acc_status
    else:
      enable_condition = enable_pressed

    # always clear the alert at every cycle
    alert_id = []

    # check for PCM not enabling
    if CS.brake_only and (cur_time - last_enable_pressed) < 0.2 and not CS.pcm_acc_status:
      print "waiting for PCM to enable"

    # check for denied enabling
    if enable_pressed and not enabled:
      deny_enable = \
        [(AI.SEATBELT, not CS.seatbelt),
         (AI.DOOR_OPEN, not CS.door_all_closed),
         (AI.ESP_OFF, CS.esp_disabled),
         (AI.STEER_ERROR, CS.steer_error),
         (AI.BRAKE_ERROR, CS.brake_error),
         (AI.GEAR_NOT_D, not CS.gear_shifter_valid),
         (AI.MAIN_OFF, not CS.main_on),
         (AI.PEDAL_PRESSED, CS.user_gas_pressed or CS.brake_pressed or (CS.pedal_gas > 0 and CS.brake_only)),
         (AI.HIGH_SPEED, CS.v_ego > max_enable_speed),
         (AI.OVERHEAT, overtemp),
         (AI.COMM_ISSUE, PP.dead or AC.dead),
         (AI.CONTROLSD_LAG, rk.remaining < -0.2)]
      for alertn, cond in deny_enable:
        if cond:
          alert_id += [alertn]

    # check for soft disables
    if enabled:
      soft_disable = \
        [(AI.SEATBELT_SD, not CS.seatbelt),
         (AI.DOOR_OPEN_SD, not CS.door_all_closed),
         (AI.ESP_OFF_SD, CS.esp_disabled),
         (AI.OVERHEAT_SD, overtemp),
         (AI.COMM_ISSUE_SD, PP.dead or AC.dead),
         (AI.CONTROLSD_LAG_SD, rk.remaining < -0.2)]
      sounding = False
      for alertn, cond in soft_disable:
        if cond:
          alert_id += [alertn]
          sounding = True
          # soft disengagement expired, user need to take control
          if (cur_time - soft_disable_start) > 3.:
            enabled = False
            v_cruise = 255
      if not sounding:
        soft_disable_start = cur_time

    # check for immediate disables
    if enabled:
      immediate_disable = \
        [(AI.PCM_LOW_SPEED, (cur_time > no_mismatch_pcm_last > 0.2) and CS.v_ego < pcm_threshold),
         (AI.STEER_ERROR_ID, CS.steer_error),
         (AI.BRAKE_ERROR_ID, CS.brake_error),
         (AI.CTRL_MISMATCH_ID, (cur_time - no_mismatch_ctrl_last) > 0.2),
         (AI.PCM_MISMATCH_ID, (cur_time - no_mismatch_pcm_last) > 0.2)]
      for alertn, cond in immediate_disable:
        if cond:
          alert_id += [alertn]
          # immediate turn off control
          enabled = False
          v_cruise = 255

    # user disabling
    if enabled and (CS.user_gas_pressed or CS.brake_pressed or not CS.gear_shifter_valid or \
         (CS.cruise_buttons == CruiseButtons.CANCEL and CS.prev_cruise_buttons == 0) or \
         not CS.main_on or (CS.pedal_gas > 0 and CS.brake_only)):
      enabled = False
      v_cruise = 255
      alert_id += [AI.DISABLE]

    # enabling
    if enable_condition and not enabled and len(alert_id) == 0:
      print "*** enabling controls"

      #enable both lateral and longitudinal controls
      enabled = True
      counter_pcm_enabled = CS.counter_pcm
      # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
      # what we want to be displayed in mph
      v_cruise_mph = round(CS.v_ego * CV.MS_TO_MPH * CS.ui_speed_fudge)
      # what we need to send to have that displayed
      v_cruise = int(round(np.maximum(v_cruise_mph * CV.MPH_TO_KPH, v_cruise_enable_min)))

      # 6 minutes driver you're on
      awareness_status = 1.0

      # reset the PID loops
      LaC.reset()
      # start long control at actual speed
      LoC.reset(v_pid = CS.v_ego)

      alert_id += [AI.ENABLE]

    if v_cruise != 255 and not CS.brake_only:
      v_cruise = np.clip(v_cruise, v_cruise_min, v_cruise_max)

    # **** awareness status manager ****
    if enabled:
      # gives the user 6 minutes
      awareness_status -= 1.0/(100*60*6)
      # reset on steering, blinker, or cruise buttons
      if CS.steer_override or CS.blinker_on or CS.cruise_buttons or CS.cruise_setting:
        awareness_status = 1.0
      if awareness_status <= 0.:
        alert_id += [AI.DRIVER_DISTRACTED]

    # ****** initial actuators commands ***
    # *** gas/brake PID loop ***
    AC.update(cur_time, CS.v_ego, CS.angle_steers, LoC.v_pid, awareness_status, CS.VP)
    final_gas, final_brake = LoC.update(enabled, CS, v_cruise, AC.v_target_lead, AC.a_target, AC.jerk_factor)
    pcm_accel = int(np.clip(AC.a_pcm/1.4,0,1)*0xc6)   # TODO: perc of max accel in ACC?

    # *** steering PID loop ***
    final_steer, sat_flag = LaC.update(enabled, CS, PP.d_poly, angle_offset)

    # this needs to stay before hysteresis logic to avoid pcm staying on control during brake hysteresis
    pcm_override = True   # this is always True
    pcm_cancel_cmd = False
    if CS.brake_only and final_brake == 0.:
      pcm_speed = LoC.v_pid - .3  # FIXME: just for exp
    else:
      pcm_speed = 0

    # ***** handle alerts ****
    # send a "steering required alert" if saturation count has reached the limit
    if sat_flag:
      alert_id += [AI.STEER_SATURATED]

    # process the alert, based on id
    alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p = \
      process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p)

    # alerts pub
    if len(alert_id) != 0:
      print alert_id, alert_text

    # *** process for hud display ***
    if not enabled or (hud_v_cruise == 255 and CS.counter_pcm == counter_pcm_enabled):
      hud_v_cruise = 255
    else:
      hud_v_cruise = v_cruise

    # *** actually do can sends ***
    CC.update(sendcan, enabled, CS, rk.frame, \
      final_gas, final_brake, final_steer, \
      pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
      hud_v_cruise, hud_show_lanes = enabled, \
      hud_show_car = AC.has_lead, \
      hud_alert = hud_alert, \
      snd_beep = beep, snd_chime = chime)

    # ***** publish state to logger *****

    # publish controls state at 100Hz
    dat = messaging.new_message()
    dat.init('live100')

    # move liveUI into live100
    dat.live100.rearViewCam = bool(rear_view_cam)
    dat.live100.alertText1 = alert_text[0]
    dat.live100.alertText2 = alert_text[1]
    dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0

    # what packets were used to process
    dat.live100.canMonoTimes = canMonoTimes
    dat.live100.mdMonoTime = PP.logMonoTime
    dat.live100.l20MonoTime = AC.logMonoTime

    # if controls is enabled
    dat.live100.enabled = enabled

    # car state
    dat.live100.vEgo = float(CS.v_ego)
    dat.live100.aEgo = float(CS.a_ego)
    dat.live100.angleSteers = float(CS.angle_steers)
    dat.live100.hudLead = CS.hud_lead
    dat.live100.steerOverride = CS.steer_override

    # longitudinal control state
    dat.live100.vPid = float(LoC.v_pid)
    dat.live100.vCruise = float(v_cruise)
    dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
    dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)

    # lateral control state
    dat.live100.yActual = float(LaC.y_actual)
    dat.live100.yDes = float(LaC.y_des)
    dat.live100.upSteer = float(LaC.Up_steer)
    dat.live100.uiSteer = float(LaC.Ui_steer)

    # processed radar state, should add a_pcm?
    dat.live100.vTargetLead = float(AC.v_target_lead)
    dat.live100.aTargetMin = float(AC.a_target[0])
    dat.live100.aTargetMax = float(AC.a_target[1])
    dat.live100.jerkFactor = float(AC.jerk_factor)

    # lag
    dat.live100.cumLagMs = -rk.remaining*1000.

    live100.send(dat.to_bytes())

    # *** run loop at fixed rate ***
    rk.keep_time()

def main(gctx=None):
  controlsd_thread(gctx, 100)

if __name__ == "__main__":
  main()