Exemple #1
0
def cycle_alerts(duration=200, is_metric=False):
    alerts = list(EVENTS.keys())
    print(alerts)

    alerts = [
        EventName.preDriverDistracted, EventName.promptDriverDistracted,
        EventName.driverDistracted
    ]

    CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
    sm = messaging.SubMaster([
        'deviceState', 'pandaState', 'roadCameraState', 'modelV2',
        'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
        'lateralPlan', 'liveLocationKalman'
    ])

    controls_state = messaging.pub_sock('controlsState')
    deviceState = messaging.pub_sock('deviceState')

    idx, last_alert_millis = 0, 0

    events = Events()
    AM = AlertManager()

    frame = 0

    while 1:
        if frame % duration == 0:
            idx = (idx + 1) % len(alerts)
            events.clear()
            events.add(alerts[idx])

        current_alert_types = [
            ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
            ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING
        ]
        a = events.create_alerts(current_alert_types, [CP, sm, is_metric])
        AM.add_many(frame, a)
        AM.process_alerts(frame)

        dat = messaging.new_message()
        dat.init('controlsState')

        dat.controlsState.alertText1 = AM.alert_text_1
        dat.controlsState.alertText2 = AM.alert_text_2
        dat.controlsState.alertSize = AM.alert_size
        dat.controlsState.alertStatus = AM.alert_status
        dat.controlsState.alertBlinkingRate = AM.alert_rate
        dat.controlsState.alertType = AM.alert_type
        dat.controlsState.alertSound = AM.audible_alert
        controls_state.send(dat.to_bytes())

        dat = messaging.new_message()
        dat.init('deviceState')
        dat.deviceState.started = True
        deviceState.send(dat.to_bytes())

        frame += 1
        time.sleep(0.01)
Exemple #2
0
class Controls:
  def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
    config_realtime_process(4, Priority.CTRL_HIGH)

    # Setup sockets
    self.pm = pm
    if self.pm is None:
      self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
                                     'carControl', 'carEvents', 'carParams'])

    self.camera_packets = ["roadCameraState", "driverCameraState"]
    if TICI:
      self.camera_packets.append("wideRoadCameraState")

    self.can_sock = can_sock
    if can_sock is None:
      can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20
      self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

    if TICI:
      self.log_sock = messaging.sub_sock('androidLog')

    if CI is None:
      # wait for one pandaState and one CAN packet
      print("Waiting for CAN messages...")
      get_one_can(self.can_sock)

      self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
    else:
      self.CI, self.CP = CI, CI.CP

    params = Params()
    self.joystick_mode = params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None)
    joystick_packet = ['testJoystick'] if self.joystick_mode else []

    self.sm = sm
    if self.sm is None:
      ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
      self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                     'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
                                     'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
                                    ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])

    # set alternative experiences from parameters
    self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator")
    self.CP.alternativeExperience = 0
    if not self.disengage_on_accelerator:
      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS

    # read params
    self.is_metric = params.get_bool("IsMetric")
    self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
    openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
    passive = params.get_bool("Passive") or not openpilot_enabled_toggle

    # detect sound card presence and ensure successful init
    sounds_available = HARDWARE.get_sound_card_online()

    car_recognized = self.CP.carName != 'mock'

    controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
    self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly
    if self.read_only:
      safety_config = car.CarParams.SafetyConfig.new_message()
      safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
      self.CP.safetyConfigs = [safety_config]

    # Write CarParams for radard
    cp_bytes = self.CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)

    self.CC = car.CarControl.new_message()
    self.CS_prev = car.CarState.new_message()
    self.AM = AlertManager()
    self.events = Events()

    self.LoC = LongControl(self.CP)
    self.VM = VehicleModel(self.CP)

    self.LaC: LatControl
    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
      self.LaC = LatControlAngle(self.CP, self.CI)
    elif self.CP.lateralTuning.which() == 'pid':
      self.LaC = LatControlPID(self.CP, self.CI)
    elif self.CP.lateralTuning.which() == 'indi':
      self.LaC = LatControlINDI(self.CP, self.CI)
    elif self.CP.lateralTuning.which() == 'torque':
      self.LaC = LatControlTorque(self.CP, self.CI)

    self.initialized = False
    self.state = State.disabled
    self.enabled = False
    self.active = False
    self.can_rcv_error = False
    self.soft_disable_timer = 0
    self.v_cruise_kph = 255
    self.v_cruise_kph_last = 0
    self.mismatch_counter = 0
    self.cruise_mismatch_counter = 0
    self.can_rcv_error_counter = 0
    self.last_blinker_frame = 0
    self.distance_traveled = 0
    self.last_functional_fan_frame = 0
    self.events_prev = []
    self.current_alert_types = [ET.PERMANENT]
    self.logged_comm_issue = None
    self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
    self.last_actuators = car.CarControl.Actuators.new_message()
    self.desired_curvature = 0.0
    self.desired_curvature_rate = 0.0

    # TODO: no longer necessary, aside from process replay
    self.sm['liveParameters'].valid = True

    self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0)

    if not sounds_available:
      self.events.add(EventName.soundsUnavailable, static=True)
    if not car_recognized:
      self.events.add(EventName.carUnrecognized, static=True)
      if len(self.CP.carFw) > 0:
        set_offroad_alert("Offroad_CarUnrecognized", True)
      else:
        set_offroad_alert("Offroad_NoFirmware", True)
    elif self.read_only:
      self.events.add(EventName.dashcamMode, static=True)
    elif self.joystick_mode:
      self.events.add(EventName.joystickDebug, static=True)
      self.startup_event = None

    # controlsd is driven by can recv, expected at 100Hz
    self.rk = Ratekeeper(100, print_delay_threshold=None)
    self.prof = Profiler(False)  # off by default

  def update_events(self, CS):
    """Compute carEvents from carState"""

    self.events.clear()

    # Add startup event
    if self.startup_event is not None:
      self.events.add(self.startup_event)
      self.startup_event = None

    # Don't add any more events if not initialized
    if not self.initialized:
      self.events.add(EventName.controlsInitializing)
      return

    # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
    if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
      (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)):
      self.events.add(EventName.pedalPressed)

    if CS.gasPressed:
      self.events.add(EventName.pedalPressedPreEnable if self.disengage_on_accelerator else
                      EventName.gasPressedOverride)

    if not self.CP.notCar:
      self.events.add_from_msg(self.sm['driverMonitoringState'].events)

    # Handle car events. Ignore when CAN is invalid
    if CS.canTimeout:
      self.events.add(EventName.canBusMissing)
    elif not CS.canValid:
      self.events.add(EventName.canError)
    else:
      self.events.add_from_msg(CS.events)

    # Create events for temperature, disk space, and memory
    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
      self.events.add(EventName.overheat)
    if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
      # under 7% of space free no enable allowed
      self.events.add(EventName.outOfSpace)
    # TODO: make tici threshold the same
    if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
      self.events.add(EventName.lowMemory)

    # TODO: enable this once loggerd CPU usage is more reasonable
    #cpus = list(self.sm['deviceState'].cpuUsagePercent)
    #if max(cpus, default=0) > 95 and not SIMULATION:
    #  self.events.add(EventName.highCpuUsage)

    # Alert if fan isn't spinning for 5 seconds
    if self.sm['peripheralState'].pandaType == PandaType.dos:
      if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
        if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
          self.events.add(EventName.fanMalfunction)
      else:
        self.last_functional_fan_frame = self.sm.frame

    # Handle calibration status
    cal_status = self.sm['liveCalibration'].calStatus
    if cal_status != Calibration.CALIBRATED:
      if cal_status == Calibration.UNCALIBRATED:
        self.events.add(EventName.calibrationIncomplete)
      else:
        self.events.add(EventName.calibrationInvalid)

    # Handle lane change
    if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
      direction = self.sm['lateralPlan'].laneChangeDirection
      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
         (CS.rightBlindspot and direction == LaneChangeDirection.right):
        self.events.add(EventName.laneChangeBlocked)
      else:
        if direction == LaneChangeDirection.left:
          self.events.add(EventName.preLaneChangeLeft)
        else:
          self.events.add(EventName.preLaneChangeRight)
    elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting,
                                                    LaneChangeState.laneChangeFinishing):
      self.events.add(EventName.laneChange)

    for i, pandaState in enumerate(self.sm['pandaStates']):
      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
      if i < len(self.CP.safetyConfigs):
        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
                          pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
                          pandaState.alternativeExperience != self.CP.alternativeExperience
      else:
        safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES

      if safety_mismatch or self.mismatch_counter >= 200:
        self.events.add(EventName.controlsMismatch)

      if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
        self.events.add(EventName.relayMalfunction)

    # Handle HW and system malfunctions
    # Order is very intentional here. Be careful when modifying this.
    # All events here should at least have NO_ENTRY and SOFT_DISABLE.
    num_events = len(self.events)

    not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
    if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
      self.events.add(EventName.processNotRunning)
    else:
      if not SIMULATION and not self.rk.lagging:
        if not self.sm.all_alive(self.camera_packets):
          self.events.add(EventName.cameraMalfunction)
        elif not self.sm.all_freq_ok(self.camera_packets):
          self.events.add(EventName.cameraFrameRate)
    if self.rk.lagging:
      self.events.add(EventName.controlsdLagging)
    if len(self.sm['radarState'].radarErrors):
      self.events.add(EventName.radarFault)
    if not self.sm.valid['pandaStates']:
      self.events.add(EventName.usbError)

    # generic catch-all. ideally, a more specific event should be added above instead
    no_system_errors = len(self.events) != num_events
    if (not self.sm.all_checks() or self.can_rcv_error) and no_system_errors and CS.canValid and not CS.canTimeout:
      if not self.sm.all_alive():
        self.events.add(EventName.commIssue)
      elif not self.sm.all_freq_ok():
        self.events.add(EventName.commIssueAvgFreq)
      else: # invalid or can_rcv_error.
        self.events.add(EventName.commIssue)

      logs = {
        'invalid': [s for s, valid in self.sm.valid.items() if not valid],
        'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
        'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
        'can_error': self.can_rcv_error,
      }
      if logs != self.logged_comm_issue:
        cloudlog.event("commIssue", error=True, **logs)
        self.logged_comm_issue = logs
    else:
      self.logged_comm_issue = None

    if not self.sm['liveParameters'].valid:
      self.events.add(EventName.vehicleModelInvalid)
    if not self.sm['lateralPlan'].mpcSolutionValid:
      self.events.add(EventName.plannerError)
    if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
      if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
        self.events.add(EventName.sensorDataInvalid)
    if not self.sm['liveLocationKalman'].posenetOK:
      self.events.add(EventName.posenetInvalid)
    if not self.sm['liveLocationKalman'].deviceStable:
      self.events.add(EventName.deviceFalling)

    if not REPLAY:
      # Check for mismatch between openpilot and car's PCM
      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
      self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
      if self.cruise_mismatch_counter > int(6. / DT_CTRL):
        self.events.add(EventName.cruiseMismatch)

    # Check for FCW
    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
    model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
    planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
    if planner_fcw or model_fcw:
      self.events.add(EventName.fcw)

    if TICI:
      for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
        try:
          msg = m.androidLog.message
          if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
            csid = msg.split("CSID:")[-1].split(" ")[0]
            evt = CSID_MAP.get(csid, None)
            if evt is not None:
              self.events.add(evt)
        except UnicodeDecodeError:
          pass

    # TODO: fix simulator
    if not SIMULATION:
      if not NOSENSOR:
        if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
          # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
          self.events.add(EventName.noGps)

      if self.sm['modelV2'].frameDropPerc > 20:
        self.events.add(EventName.modeldLagging)
      if self.sm['liveLocationKalman'].excessiveResets:
        self.events.add(EventName.localizerMalfunction)

    # Only allow engagement with brake pressed when stopped behind another stopped car
    speeds = self.sm['longitudinalPlan'].speeds
    if len(speeds) > 1:
      v_future = speeds[-1]
    else:
      v_future = 100.0
    if CS.brakePressed and v_future >= self.CP.vEgoStarting \
      and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
      self.events.add(EventName.noTarget)

  def data_sample(self):
    """Receive data from sockets and update carState"""

    # Update carState from CAN
    can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
    CS = self.CI.update(self.CC, can_strs)

    self.sm.update(0)

    if not self.initialized:
      all_valid = CS.canValid and self.sm.all_checks()
      if all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION:
        if not self.read_only:
          self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
        self.initialized = True

        if REPLAY and self.sm['pandaStates'][0].controlsAllowed:
          self.state = State.enabled

        Params().put_bool("ControlsReady", True)

    # Check for CAN timeout
    if not can_strs:
      self.can_rcv_error_counter += 1
      self.can_rcv_error = True
    else:
      self.can_rcv_error = False

    # 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 self.enabled:
      self.mismatch_counter = 0

    # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
    if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
           if ps.safetyModel not in IGNORED_SAFETY_MODES):
      self.mismatch_counter += 1

    self.distance_traveled += CS.vEgo * DT_CTRL

    return CS

  def state_transition(self, CS):
    """Compute conditional state transitions and execute actions on state transitions"""

    self.v_cruise_kph_last = self.v_cruise_kph

    # if stock cruise is completely disabled, then we can use our own set speed logic
    if not self.CP.pcmCruise:
      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents,
                                          self.button_timers, self.enabled, self.is_metric)
    else:
      if CS.cruiseState.available:
        self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
      else:
        self.v_cruise_kph = 0

    # decrement the soft disable timer at every step, as it's reset on
    # entrance in SOFT_DISABLING state
    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)

    self.current_alert_types = [ET.PERMANENT]

    # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
    if self.state != State.disabled:
      # user and immediate disable always have priority in a non-disabled state
      if self.events.any(ET.USER_DISABLE):
        self.state = State.disabled
        self.current_alert_types.append(ET.USER_DISABLE)

      elif self.events.any(ET.IMMEDIATE_DISABLE):
        self.state = State.disabled
        self.current_alert_types.append(ET.IMMEDIATE_DISABLE)

      else:
        # ENABLED
        if self.state == State.enabled:
          if self.events.any(ET.SOFT_DISABLE):
            self.state = State.softDisabling
            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
            self.current_alert_types.append(ET.SOFT_DISABLE)

          elif self.events.any(ET.OVERRIDE):
            self.state = State.overriding
            self.current_alert_types.append(ET.OVERRIDE)

        # SOFT DISABLING
        elif self.state == State.softDisabling:
          if not self.events.any(ET.SOFT_DISABLE):
            # no more soft disabling condition, so go back to ENABLED
            self.state = State.enabled

          elif self.soft_disable_timer > 0:
            self.current_alert_types.append(ET.SOFT_DISABLE)

          elif self.soft_disable_timer <= 0:
            self.state = State.disabled

        # PRE ENABLING
        elif self.state == State.preEnabled:
          if self.events.any(ET.NO_ENTRY):
            self.state = State.disabled
            self.current_alert_types.append(ET.NO_ENTRY)
          elif not self.events.any(ET.PRE_ENABLE):
            self.state = State.enabled
          else:
            self.current_alert_types.append(ET.PRE_ENABLE)

        # OVERRIDING
        elif self.state == State.overriding:
          if self.events.any(ET.SOFT_DISABLE):
            self.state = State.softDisabling
            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
            self.current_alert_types.append(ET.SOFT_DISABLE)
          elif not self.events.any(ET.OVERRIDE):
            self.state = State.enabled
          else:
            self.current_alert_types.append(ET.OVERRIDE)

    # DISABLED
    elif self.state == State.disabled:
      if self.events.any(ET.ENABLE):
        if self.events.any(ET.NO_ENTRY):
          self.current_alert_types.append(ET.NO_ENTRY)

        else:
          if self.events.any(ET.PRE_ENABLE):
            self.state = State.preEnabled
          elif self.events.any(ET.OVERRIDE):
            self.state = State.overriding
          else:
            self.state = State.enabled
          self.current_alert_types.append(ET.ENABLE)
          if not self.CP.pcmCruise:
            self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)

    # Check if openpilot is engaged and actuators are enabled
    self.enabled = self.state in ENABLED_STATES
    self.active = self.state in ACTIVE_STATES
    if self.active:
      self.current_alert_types.append(ET.WARNING)

  def state_control(self, CS):
    """Given the state, this function returns a CarControl packet"""

    # Update VehicleModel
    params = self.sm['liveParameters']
    x = max(params.stiffnessFactor, 0.1)
    sr = max(params.steerRatio, 0.1)
    self.VM.update_params(x, sr)

    lat_plan = self.sm['lateralPlan']
    long_plan = self.sm['longitudinalPlan']

    CC = car.CarControl.new_message()
    CC.enabled = self.enabled
    # Check which actuators can be enabled
    CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                     CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
    CC.longActive = self.active and not self.events.any(ET.OVERRIDE) and self.CP.openpilotLongitudinalControl

    actuators = CC.actuators
    actuators.longControlState = self.LoC.long_control_state

    if CS.leftBlinker or CS.rightBlinker:
      self.last_blinker_frame = self.sm.frame

    # State specific actions

    if not CC.latActive:
      self.LaC.reset()
    if not CC.longActive:
      self.LoC.reset(v_pid=CS.vEgo)

    if not self.joystick_mode:
      # accel PID loop
      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
      t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL
      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)

      # Steering PID loop and lateral MPC
      self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
                                                                                       lat_plan.psis,
                                                                                       lat_plan.curvatures,
                                                                                       lat_plan.curvatureRates)
      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params,
                                                                             self.last_actuators, self.desired_curvature,
                                                                             self.desired_curvature_rate, self.sm['liveLocationKalman'])
    else:
      lac_log = log.ControlsState.LateralDebugState.new_message()
      if self.sm.rcv_frame['testJoystick'] > 0:
        if CC.longActive:
          actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)

        if CC.latActive:
          steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
          # max angle is 45 for angle-based cars
          actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.

        lac_log.active = self.active
        lac_log.steeringAngleDeg = CS.steeringAngleDeg
        lac_log.output = actuators.steer
        lac_log.saturated = abs(actuators.steer) >= 0.9

    # Send a "steering required alert" if saturation count has reached the limit
    if lac_log.active and lac_log.saturated and not CS.steeringPressed:
      dpath_points = lat_plan.dPathPoints
      if len(dpath_points):
        # Check if we deviated from the path
        # TODO use desired vs actual curvature
        left_deviation = actuators.steer > 0 and dpath_points[0] < -0.20
        right_deviation = actuators.steer < 0 and dpath_points[0] > 0.20

        if left_deviation or right_deviation:
          self.events.add(EventName.steerSaturated)

    # Ensure no NaNs/Infs
    for p in ACTUATOR_FIELDS:
      attr = getattr(actuators, p)
      if not isinstance(attr, SupportsFloat):
        continue

      if not math.isfinite(attr):
        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
        setattr(actuators, p, 0.0)

    return CC, lac_log

  def update_button_timers(self, buttonEvents):
    # increment timer for buttons still pressed
    for k in self.button_timers:
      if self.button_timers[k] > 0:
        self.button_timers[k] += 1

    for b in buttonEvents:
      if b.type.raw in self.button_timers:
        self.button_timers[b.type.raw] = 1 if b.pressed else 0

  def publish_logs(self, CS, start_time, CC, lac_log):
    """Send actuators and hud commands to the car, send controlsstate and MPC logging"""

    # Orientation and angle rates can be useful for carcontroller
    # Only calibrated (car) frame is relevant for the carcontroller
    orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
    if len(orientation_value) > 2:
      CC.orientationNED = orientation_value
    angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
    if len(angular_rate_value) > 2:
      CC.angularVelocity = angular_rate_value

    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
    if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
      CC.cruiseControl.cancel = True

    hudControl = CC.hudControl
    hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
    hudControl.speedVisible = self.enabled
    hudControl.lanesVisible = self.enabled
    hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead

    hudControl.rightLaneVisible = True
    hudControl.leftLaneVisible = True

    recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
    ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
                    and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

    model_v2 = self.sm['modelV2']
    desire_prediction = model_v2.meta.desirePrediction
    if len(desire_prediction) and ldw_allowed:
      right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
      left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
      l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
      r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]

      lane_lines = model_v2.laneLines
      l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
      r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))

      hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
      hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)

    if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
      self.events.add(EventName.ldw)

    clear_event_types = set()
    if ET.WARNING not in self.current_alert_types:
      clear_event_types.add(ET.WARNING)
    if self.enabled:
      clear_event_types.add(ET.NO_ENTRY)

    alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer])
    self.AM.add_many(self.sm.frame, alerts)
    current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
    if current_alert:
      hudControl.visualAlert = current_alert.visual_alert

    if not self.read_only and self.initialized:
      # send car controls over can
      self.last_actuators, can_sends = self.CI.apply(CC)
      self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
      CC.actuatorsOutput = self.last_actuators

    force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
                  (self.state == State.softDisabling)

    # Curvature & Steering angle
    params = self.sm['liveParameters']

    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg)
    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll)

    # controlsState
    dat = messaging.new_message('controlsState')
    dat.valid = CS.canValid
    controlsState = dat.controlsState
    if current_alert:
      controlsState.alertText1 = current_alert.alert_text_1
      controlsState.alertText2 = current_alert.alert_text_2
      controlsState.alertSize = current_alert.alert_size
      controlsState.alertStatus = current_alert.alert_status
      controlsState.alertBlinkingRate = current_alert.alert_rate
      controlsState.alertType = current_alert.alert_type
      controlsState.alertSound = current_alert.audible_alert

    controlsState.canMonoTimes = list(CS.canMonoTimes)
    controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
    controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
    controlsState.enabled = self.enabled
    controlsState.active = self.active
    controlsState.curvature = curvature
    controlsState.desiredCurvature = self.desired_curvature
    controlsState.desiredCurvatureRate = self.desired_curvature_rate
    controlsState.state = self.state
    controlsState.engageable = not self.events.any(ET.NO_ENTRY)
    controlsState.longControlState = self.LoC.long_control_state
    controlsState.vPid = float(self.LoC.v_pid)
    controlsState.vCruise = float(self.v_cruise_kph)
    controlsState.upAccelCmd = float(self.LoC.pid.p)
    controlsState.uiAccelCmd = float(self.LoC.pid.i)
    controlsState.ufAccelCmd = float(self.LoC.pid.f)
    controlsState.cumLagMs = -self.rk.remaining * 1000.
    controlsState.startMonoTime = int(start_time * 1e9)
    controlsState.forceDecel = bool(force_decel)
    controlsState.canErrorCounter = self.can_rcv_error_counter

    lat_tuning = self.CP.lateralTuning.which()
    if self.joystick_mode:
      controlsState.lateralControlState.debugState = lac_log
    elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
      controlsState.lateralControlState.angleState = lac_log
    elif lat_tuning == 'pid':
      controlsState.lateralControlState.pidState = lac_log
    elif lat_tuning == 'torque':
      controlsState.lateralControlState.torqueState = lac_log
    elif lat_tuning == 'indi':
      controlsState.lateralControlState.indiState = lac_log

    self.pm.send('controlsState', dat)

    # carState
    car_events = self.events.to_msg()
    cs_send = messaging.new_message('carState')
    cs_send.valid = CS.canValid
    cs_send.carState = CS
    cs_send.carState.events = car_events
    self.pm.send('carState', cs_send)

    # carEvents - logged every second or on change
    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
      ce_send = messaging.new_message('carEvents', len(self.events))
      ce_send.carEvents = car_events
      self.pm.send('carEvents', ce_send)
    self.events_prev = self.events.names.copy()

    # carParams - logged every 50 seconds (> 1 per segment)
    if (self.sm.frame % int(50. / DT_CTRL) == 0):
      cp_send = messaging.new_message('carParams')
      cp_send.carParams = self.CP
      self.pm.send('carParams', cp_send)

    # carControl
    cc_send = messaging.new_message('carControl')
    cc_send.valid = CS.canValid
    cc_send.carControl = CC
    self.pm.send('carControl', cc_send)

    # copy CarControl to pass to CarInterface on the next iteration
    self.CC = CC

  def step(self):
    start_time = sec_since_boot()
    self.prof.checkpoint("Ratekeeper", ignore=True)

    # Sample data from sockets and get a carState
    CS = self.data_sample()
    cloudlog.timestamp("Data sampled")
    self.prof.checkpoint("Sample")

    self.update_events(CS)
    cloudlog.timestamp("Events updated")

    if not self.read_only and self.initialized:
      # Update control state
      self.state_transition(CS)
      self.prof.checkpoint("State transition")

    # Compute actuators (runs PID loops and lateral MPC)
    CC, lac_log = self.state_control(CS)

    self.prof.checkpoint("State Control")

    # Publish data
    self.publish_logs(CS, start_time, CC, lac_log)
    self.prof.checkpoint("Sent")

    self.update_button_timers(CS.buttonEvents)
    self.CS_prev = CS

  def controlsd_thread(self):
    while True:
      self.step()
      self.rk.monitor_time()
      self.prof.display()
Exemple #3
0
class Controls:
  def __init__(self, sm=None, pm=None, can_sock=None):
    config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

    # Setup sockets
    self.pm = pm
    if self.pm is None:
      self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
                                     'carControl', 'carEvents', 'carParams'])

    self.sm = sm
    if self.sm is None:
      ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
      self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration',
                                     'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
                                     'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'],
                                     ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])

    self.can_sock = can_sock
    if can_sock is None:
      can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
      self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

    # wait for one pandaState and one CAN packet
    print("Waiting for CAN messages...")
    get_one_can(self.can_sock)

    self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])

    # read params
    params = Params()
    self.is_metric = params.get_bool("IsMetric")
    self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
    self.enable_lte_onroad = params.get_bool("EnableLteOnroad")
    community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
    openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
    passive = params.get_bool("Passive") or not openpilot_enabled_toggle
    self.commIssue_ignored = params.get_bool("ComIssueGone")
    self.auto_enabled = params.get_bool("AutoEnable") and params.get_bool("MadModeEnabled")

    # detect sound card presence and ensure successful init
    sounds_available = HARDWARE.get_sound_card_online()

    car_recognized = self.CP.carName != 'mock'
    fuzzy_fingerprint = self.CP.fuzzyFingerprint

    # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
    controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
    community_feature = self.CP.communityFeature or fuzzy_fingerprint
    community_feature_disallowed = community_feature and (not community_feature_toggle)
    self.read_only = not car_recognized or not controller_available or \
                       self.CP.dashcamOnly or community_feature_disallowed
    if self.read_only:
      self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

    # Write CarParams for radard
    cp_bytes = self.CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)

    self.CC = car.CarControl.new_message()
    self.AM = AlertManager()
    self.events = Events()

    self.LoC = LongControl(self.CP, self.CI.compute_gb)
    self.VM = VehicleModel(self.CP)

    self.lateral_control_method = 0
    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
      self.LaC = LatControlAngle(self.CP)
      self.lateral_control_method = 3
    elif self.CP.lateralTuning.which() == 'pid':
      self.LaC = LatControlPID(self.CP)
      self.lateral_control_method = 0
    elif self.CP.lateralTuning.which() == 'indi':
      self.LaC = LatControlINDI(self.CP)
      self.lateral_control_method = 1
    elif self.CP.lateralTuning.which() == 'lqr':
      self.LaC = LatControlLQR(self.CP)
      self.lateral_control_method = 2

    self.long_plan_source = 0
    self.controlsAllowed = False

    self.initialized = False
    self.state = State.disabled
    self.enabled = False
    self.active = False
    self.can_rcv_error = False
    self.soft_disable_timer = 0
    self.v_cruise_kph = 255
    self.v_cruise_kph_last = 0
    self.mismatch_counter = 0
    self.can_error_counter = 0
    self.last_blinker_frame = 0
    self.saturated_count = 0
    self.distance_traveled = 0
    self.last_functional_fan_frame = 0
    self.events_prev = []
    self.current_alert_types = [ET.PERMANENT]
    self.logged_comm_issue = False

    # TODO: no longer necessary, aside from process replay
    self.sm['liveParameters'].valid = True

    self.startup_event = get_startup_event(car_recognized, controller_available, fuzzy_fingerprint)

    if not sounds_available:
      self.events.add(EventName.soundsUnavailable, static=True)
    if community_feature_disallowed:
      self.events.add(EventName.communityFeatureDisallowed, static=True)
    if not car_recognized:
      self.events.add(EventName.carUnrecognized, static=True)
    #elif self.read_only:
    #  self.events.add(EventName.dashcamMode, static=True)

    # controlsd is driven by can recv, expected at 100Hz
    self.rk = Ratekeeper(100, print_delay_threshold=None)
    self.prof = Profiler(False)  # off by default

    self.hyundai_lkas = self.read_only  #read_only
    
    self.mpc_frame = 0

    self.steerRatio_Max = float(int(Params().get("SteerRatioMaxAdj")) * 0.1)
    self.angle_differ_range = [0, 15]
    self.steerRatio_range = [self.CP.steerRatio, self.steerRatio_Max]
    self.new_steerRatio = self.CP.steerRatio
    self.new_steerRatio_prev = self.CP.steerRatio
    self.steerRatio_to_send = 0
    
    self.model_long_alert_prev = True
    self.delayed_comm_issue_timer = 0

  def auto_enable(self, CS):
    if self.state != State.enabled and CS.vEgo >= 3 * CV.KPH_TO_MS and CS.gearShifter == 2 and self.sm['liveCalibration'].calStatus != Calibration.UNCALIBRATED:
      if self.sm.all_alive_and_valid() and self.enabled != self.controlsAllowed:
        self.events.add( EventName.pcmEnable )

  def update_events(self, CS):
    """Compute carEvents from carState"""

    self.events.clear()
    self.events.add_from_msg(CS.events)
    self.events.add_from_msg(self.sm['driverMonitoringState'].events)

    # Handle startup event
    if self.startup_event is not None:
      self.events.add(self.startup_event)
      self.startup_event = None

    # Don't add any more events if not initialized
    if not self.initialized:
      self.events.add(EventName.controlsInitializing)
      return

    # Create events for battery, temperature, disk space, and memory
    if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
      # at zero percent battery, while discharging, OP should not allowed
      self.events.add(EventName.lowBattery)
    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
      self.events.add(EventName.overheat)
    if self.sm['deviceState'].freeSpacePercent < 7:
      # under 7% of space free no enable allowed
      self.events.add(EventName.outOfSpace)
    if self.sm['deviceState'].memoryUsagePercent  > 90:
      self.events.add(EventName.lowMemory)

    # Alert if fan isn't spinning for 5 seconds
    if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
      if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
        if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
          self.events.add(EventName.fanMalfunction)
      else:
        self.last_functional_fan_frame = self.sm.frame

    # Handle calibration status
    cal_status = self.sm['liveCalibration'].calStatus
    if cal_status != Calibration.CALIBRATED:
      if cal_status == Calibration.UNCALIBRATED:
        self.events.add(EventName.calibrationIncomplete)
      else:
        self.events.add(EventName.calibrationInvalid)

    # Handle lane change
    if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
      direction = self.sm['lateralPlan'].laneChangeDirection
      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
         (CS.rightBlindspot and direction == LaneChangeDirection.right):
        self.events.add(EventName.laneChangeBlocked)
      else:
        if direction == LaneChangeDirection.left:
          self.events.add(EventName.preLaneChangeLeft)
        else:
          self.events.add(EventName.preLaneChangeRight)
    elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
                                                 LaneChangeState.laneChangeFinishing]:
      self.events.add(EventName.laneChange)

    if self.can_rcv_error or not CS.canValid:
      self.events.add(EventName.canError)

    safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel or self.sm['pandaState'].safetyParam != self.CP.safetyParam
    if safety_mismatch or self.mismatch_counter >= 200:
      self.events.add(EventName.controlsMismatch)

    if not self.sm['liveParameters'].valid:
      self.events.add(EventName.vehicleModelInvalid)

    if len(self.sm['radarState'].radarErrors):
      self.events.add(EventName.radarFault)
    elif not self.sm.all_alive_and_valid() and self.sm['pandaState'].pandaType != PandaType.whitePanda and not self.commIssue_ignored:
      self.delayed_comm_issue_timer += 1
      if self.delayed_comm_issue_timer > 100:
        self.events.add(EventName.commIssue)
      if not self.logged_comm_issue:
        cloudlog.error(f"commIssue - valid: {self.sm.valid} - alive: {self.sm.alive}")
        self.logged_comm_issue = True
    else:
      self.logged_comm_issue = False
      self.delayed_comm_issue_timer = 0

    if not self.sm['lateralPlan'].mpcSolutionValid and not (EventName.laneChangeManual in self.events.names) and CS.steeringAngleDeg < 15:
      self.events.add(EventName.plannerError)
    if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
      if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
        self.events.add(EventName.sensorDataInvalid)
    if not self.sm['liveLocationKalman'].posenetOK:
      self.events.add(EventName.posenetInvalid)
    if not self.sm['liveLocationKalman'].deviceStable:
      self.events.add(EventName.deviceFalling)
    if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults:
      self.events.add(EventName.relayMalfunction)
    if self.sm['longitudinalPlan'].fcw:
      self.events.add(EventName.fcw)

    # TODO: fix simulator
    if not SIMULATION:
      #if not NOSENSOR:
      #  if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and \
      #    (not TICI or self.enable_lte_onroad):
      #    # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
      #    self.events.add(EventName.noGps)
      if not self.sm.all_alive(['roadCameraState', 'driverCameraState']):
        self.events.add(EventName.cameraMalfunction)
      if self.sm['modelV2'].frameDropPerc > 20:
        self.events.add(EventName.modeldLagging)

      # Check if all manager processes are running
      not_running = set(p.name for p in self.sm['managerState'].processes if not p.running)
      if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
        self.events.add(EventName.processNotRunning)

    # Only allow engagement with brake pressed when stopped behind another stopped car
    #if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
    #  and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
    #  self.events.add(EventName.noTarget)

    # ModelLongAlert
    if Params().get_bool("ModelLongEnabled") and self.model_long_alert_prev:
      self.events.add(EventName.modelLongAlert)
      self.model_long_alert_prev = not self.model_long_alert_prev
    elif not Params().get_bool("ModelLongEnabled"):
      self.model_long_alert_prev = True
      

    # atom
    if self.auto_enabled:
      self.auto_enable( CS )

  def data_sample(self):
    """Receive data from sockets and update carState"""

    # Update carState from CAN
    can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
    CS = self.CI.update(self.CC, can_strs)

    self.sm.update(0)

    all_valid = CS.canValid and self.sm.all_alive_and_valid()
    if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 2.0):
      self.initialized = True
      Params().put_bool("ControlsReady", True)

    # Check for CAN timeout
    if not can_strs:
      self.can_error_counter += 1
      self.can_rcv_error = True
    else:
      self.can_rcv_error = False

    # 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.
    self.controlsAllowed = self.sm['pandaState'].controlsAllowed
    if not self.enabled:
      self.mismatch_counter = 0
    elif not self.controlsAllowed and self.enabled:
      self.mismatch_counter += 1

    self.distance_traveled += CS.vEgo * DT_CTRL

    return CS

  def state_transition(self, CS):
    """Compute conditional state transitions and execute actions on state transitions"""

    # if stock cruise is completely disabled, then we can use our own set speed logic
    # self.CP.enableCruise is true
    self.CP.enableCruise = self.CI.CP.enableCruise
    if not self.CP.enableCruise:
      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
      if int(CS.vSetDis)-1 > self.v_cruise_kph:
        self.v_cruise_kph = int(CS.vSetDis)
    elif self.CP.enableCruise and CS.cruiseState.enabled:
      if Params().get_bool('OpkrVariableCruise') and CS.cruiseState.modeSel != 0 and self.CP.vCruisekph > 30:
        self.v_cruise_kph = self.CP.vCruisekph
        self.v_cruise_kph_last = self.v_cruise_kph
      elif CS.cruiseButtons == Buttons.RES_ACCEL and Params().get_bool('OpkrVariableCruise') and CS.cruiseState.modeSel != 0 and CS.vSetDis < (self.v_cruise_kph_last - 1):
        self.v_cruise_kph = self.v_cruise_kph_last
        if int(CS.vSetDis)-1 > self.v_cruise_kph:
          self.v_cruise_kph = int(CS.vSetDis)
      elif CS.cruiseButtons == Buttons.RES_ACCEL and Params().get_bool('OpkrVariableCruise') and CS.cruiseState.modeSel != 0 and 30 <= self.v_cruise_kph_last <= round(CS.vEgo*CV.MS_TO_KPH):
        self.v_cruise_kph = round(CS.vEgo*CV.MS_TO_KPH)
        if int(CS.vSetDis)-1 > self.v_cruise_kph:
          self.v_cruise_kph = int(CS.vSetDis)
        self.v_cruise_kph_last = self.v_cruise_kph
      elif CS.cruiseButtons == Buttons.RES_ACCEL or CS.cruiseButtons == Buttons.SET_DECEL:
        self.v_cruise_kph = round(CS.cruiseState.speed * CV.MS_TO_KPH)
        self.v_cruise_kph_last = self.v_cruise_kph
      elif CS.driverAcc and Params().get_bool('OpkrVariableCruise') and Params().get_bool('CruiseOverMaxSpeed') and 30 <= self.v_cruise_kph < int(round(CS.vEgo*CV.MS_TO_KPH)):
        self.v_cruise_kph = int(round(CS.vEgo*CV.MS_TO_KPH))
        self.v_cruise_kph_last = self.v_cruise_kph

    # decrease the soft disable timer at every step, as it's reset on
    # entrance in SOFT_DISABLING state
    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)

    self.current_alert_types = [ET.PERMANENT]

    # ENABLED, PRE ENABLING, SOFT DISABLING
    if self.state != State.disabled:
      # user and immediate disable always have priority in a non-disabled state
      if self.events.any(ET.USER_DISABLE):
        self.state = State.disabled
        self.current_alert_types.append(ET.USER_DISABLE)

      elif self.events.any(ET.IMMEDIATE_DISABLE):
        self.state = State.disabled
        self.current_alert_types.append(ET.IMMEDIATE_DISABLE)

      else:
        # ENABLED
        if self.state == State.enabled:
          if self.events.any(ET.SOFT_DISABLE):
            self.state = State.softDisabling
            self.soft_disable_timer = 300   # 3s
            self.current_alert_types.append(ET.SOFT_DISABLE)

        # SOFT DISABLING
        elif self.state == State.softDisabling:
          if not self.events.any(ET.SOFT_DISABLE):
            # no more soft disabling condition, so go back to ENABLED
            self.state = State.enabled

          elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
            self.current_alert_types.append(ET.SOFT_DISABLE)

          elif self.soft_disable_timer <= 0:
            self.state = State.disabled

        # PRE ENABLING
        elif self.state == State.preEnabled:
          if not self.events.any(ET.PRE_ENABLE):
            self.state = State.enabled
          else:
            self.current_alert_types.append(ET.PRE_ENABLE)

    # DISABLED
    elif self.state == State.disabled:
      if self.events.any(ET.ENABLE):
        if self.events.any(ET.NO_ENTRY):
          self.current_alert_types.append(ET.NO_ENTRY)

        else:
          if self.events.any(ET.PRE_ENABLE):
            self.state = State.preEnabled
          else:
            self.state = State.enabled
          self.current_alert_types.append(ET.ENABLE)
          #self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
          self.v_cruise_kph = 0
          self.v_cruise_kph_last = 0

    # Check if actuators are enabled
    self.active = self.state == State.enabled or self.state == State.softDisabling
    if self.active:
      self.current_alert_types.append(ET.WARNING)

    # Check if openpilot is engaged
    self.enabled = self.active or self.state == State.preEnabled

  def state_control(self, CS):
    """Given the state, this function returns an actuators packet"""
    lat_plan = self.sm['lateralPlan']
    long_plan = self.sm['longitudinalPlan']

    anglesteer_current = CS.steeringAngleDeg
    anglesteer_desire = lat_plan.steerAngleDesireDeg   
    output_scale = lat_plan.outputScale

    live_sr = Params().get_bool('OpkrLiveSteerRatio')

    if not live_sr:
      angle_diff = abs(anglesteer_desire) - abs(anglesteer_current)
      if abs(output_scale) >= self.CP.steerMaxV[0] and CS.vEgo > 8:
        self.new_steerRatio_prev = interp(angle_diff, self.angle_differ_range, self.steerRatio_range)
        if self.new_steerRatio_prev > self.new_steerRatio:
          self.new_steerRatio = self.new_steerRatio_prev
      else:
        self.mpc_frame += 1
        if self.mpc_frame % 100 == 0:
          self.new_steerRatio -= 0.1
          if self.new_steerRatio <= self.CP.steerRatio:
            self.new_steerRatio = self.CP.steerRatio
          self.mpc_frame = 0

    # Update VehicleModel
    params = self.sm['liveParameters']
    x = max(params.stiffnessFactor, 0.1)
    if live_sr:
      sr = max(params.steerRatio, 0.1)
    else:
     sr = max(self.new_steerRatio, 0.1)
    self.VM.update_params(x, sr)
    
    self.steerRatio_to_send = sr

    actuators = car.CarControl.Actuators.new_message()

    if CS.leftBlinker or CS.rightBlinker:
      self.last_blinker_frame = self.sm.frame

    # State specific actions

    if not self.active:
      self.LaC.reset()
      self.LoC.reset(v_pid=CS.vEgo)

    long_plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['longitudinalPlan'])
    # no greater than dt mpc + dt, to prevent too high extraps
    dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL

    a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (long_plan.aTarget - long_plan.aStart)
    v_acc_sol = long_plan.vStart + dt * (a_acc_sol + long_plan.aStart) / 2.0

    extras_loc = {'lead_one': self.sm['radarState'].leadOne, 'has_lead': long_plan.hasLead}
    # Gas/Brake PID loop
    actuators.gas, actuators.brake = self.LoC.update(self.active and CS.cruiseState.speed > 1., CS, v_acc_sol, long_plan.vTargetFuture, long_plan.aTarget, a_acc_sol, self.CP, long_plan.hasLead, self.sm['radarState'], long_plan.longitudinalPlanSource, extras_loc)

    # Steering PID loop and lateral MPC
    actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, lat_plan)

    # Check for difference between desired angle and angle for angle based control
    angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
      abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD

    if angle_control_saturated and not CS.steeringPressed and self.active:
      self.saturated_count += 1
    else:
      self.saturated_count = 0

    # Send a "steering required alert" if saturation count has reached the limit
    if (lac_log.saturated and not CS.steeringPressed) or \
       (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):

      if len(lat_plan.dPathPoints):
        # Check if we deviated from the path
        left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
        right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1

        if left_deviation or right_deviation:
          self.events.add(EventName.steerSaturated)

    return actuators, v_acc_sol, a_acc_sol, lac_log

  def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
    """Send actuators and hud commands to the car, send controlsstate and MPC logging"""

    self.log_alertTextMsg1 = trace1.global_alertTextMsg1
    self.log_alertTextMsg2 = trace1.global_alertTextMsg2

    CC = car.CarControl.new_message()
    CC.enabled = self.enabled
    CC.actuators = actuators

    CC.cruiseControl.override = True
    CC.cruiseControl.cancel = self.CP.enableCruise and not self.enabled and CS.cruiseState.enabled

    # Some override values for Honda
    # brake discount removes a sharp nonlinearity
    brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
    speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
    CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
    CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo, self.sm['longitudinalPlan'].vTarget)

    CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
    CC.hudControl.speedVisible = self.enabled
    CC.hudControl.lanesVisible = self.enabled
    CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
    CC.hudControl.leadDistance = self.sm['radarState'].leadOne.dRel
    CC.hudControl.leadvRel = self.sm['radarState'].leadOne.vRel
    CC.hudControl.leadyRel = self.sm['radarState'].leadOne.yRel

    right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
    left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
    CC.hudControl.rightLaneVisible = bool(right_lane_visible)
    CC.hudControl.leftLaneVisible = bool(left_lane_visible)

    recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
    ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
                    and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

    meta = self.sm['modelV2'].meta
    if len(meta.desirePrediction) and ldw_allowed:
      l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
      r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
      if CS.cruiseState.modeSel == 3:
        l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET_A))
        r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET_A))
      else:
        l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
        r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))

      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:
      self.events.add(EventName.ldw)

    clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
    alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric])
    self.AM.add_many(self.sm.frame, alerts, self.enabled)
    self.AM.process_alerts(self.sm.frame, clear_event)
    CC.hudControl.visualAlert = self.AM.visual_alert

    if not self.hyundai_lkas and self.enabled:
      # send car controls over can
      can_sends = self.CI.apply(CC, self.sm)
      self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))

    force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
                  (self.state == State.softDisabling)

    # Curvature & Steering angle
    params = self.sm['liveParameters']
    lat_plan = self.sm['lateralPlan']

    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
    angle_steers_des = math.degrees(self.VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
    angle_steers_des += params.angleOffsetDeg

    # controlsState
    dat = messaging.new_message('controlsState')
    dat.valid = CS.canValid
    controlsState = dat.controlsState
    controlsState.alertText1 = self.AM.alert_text_1
    controlsState.alertText2 = self.AM.alert_text_2
    controlsState.alertSize = self.AM.alert_size
    controlsState.alertStatus = self.AM.alert_status
    controlsState.alertBlinkingRate = self.AM.alert_rate
    controlsState.alertType = self.AM.alert_type
    controlsState.alertSound = self.AM.audible_alert
    controlsState.canMonoTimes = list(CS.canMonoTimes)
    controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
    controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
    controlsState.enabled = self.enabled
    controlsState.active = self.active
    controlsState.curvature = curvature
    controlsState.steeringAngleDesiredDeg = angle_steers_des
    controlsState.state = self.state
    controlsState.engageable = not self.events.any(ET.NO_ENTRY)
    controlsState.longControlState = self.LoC.long_control_state
    controlsState.vPid = float(self.LoC.v_pid)
    controlsState.vCruise = float(self.v_cruise_kph)
    controlsState.upAccelCmd = float(self.LoC.pid.p)
    controlsState.uiAccelCmd = float(self.LoC.pid.id)
    controlsState.ufAccelCmd = float(self.LoC.pid.f)
    controlsState.vTargetLead = float(v_acc)
    controlsState.aTarget = float(a_acc)
    controlsState.cumLagMs = -self.rk.remaining * 1000.
    controlsState.startMonoTime = int(start_time * 1e9)
    controlsState.forceDecel = bool(force_decel)
    controlsState.canErrorCounter = self.can_error_counter
    controlsState.alertTextMsg1 = self.log_alertTextMsg1
    controlsState.alertTextMsg2 = self.log_alertTextMsg2
    controlsState.limitSpeedCamera = float(self.sm['longitudinalPlan'].targetSpeedCamera)
    controlsState.limitSpeedCameraDist = float(self.sm['longitudinalPlan'].targetSpeedCameraDist)
    controlsState.lateralControlMethod = int(self.lateral_control_method)
    controlsState.steerRatio = float(self.steerRatio_to_send)

    if self.sm['longitudinalPlan'].longitudinalPlanSource == LongitudinalPlanSource.cruise:
      self.long_plan_source = 1
    elif self.sm['longitudinalPlan'].longitudinalPlanSource == LongitudinalPlanSource.mpc1:
      self.long_plan_source = 2
    elif self.sm['longitudinalPlan'].longitudinalPlanSource == LongitudinalPlanSource.mpc2:
      self.long_plan_source = 3
    elif self.sm['longitudinalPlan'].longitudinalPlanSource == LongitudinalPlanSource.mpc3:
      self.long_plan_source = 4
    elif self.sm['longitudinalPlan'].longitudinalPlanSource == LongitudinalPlanSource.model:
      self.long_plan_source = 5
    else:
      self.long_plan_source = 0
    controlsState.longPlanSource = self.long_plan_source

    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
      controlsState.lateralControlState.angleState = lac_log
    elif self.CP.lateralTuning.which() == 'pid':
      controlsState.lateralControlState.pidState = lac_log
    elif self.CP.lateralTuning.which() == 'lqr':
      controlsState.lateralControlState.lqrState = lac_log
    elif self.CP.lateralTuning.which() == 'indi':
      controlsState.lateralControlState.indiState = lac_log
    self.pm.send('controlsState', dat)

    # carState
    car_events = self.events.to_msg()
    cs_send = messaging.new_message('carState')
    cs_send.valid = CS.canValid
    cs_send.carState = CS
    cs_send.carState.events = car_events
    self.pm.send('carState', cs_send)

    # carEvents - logged every second or on change
    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
      ce_send = messaging.new_message('carEvents', len(self.events))
      ce_send.carEvents = car_events
      self.pm.send('carEvents', ce_send)
    self.events_prev = self.events.names.copy()

    # carParams - logged every 50 seconds (> 1 per segment)
    if (self.sm.frame % int(50. / DT_CTRL) == 0):
      cp_send = messaging.new_message('carParams')
      cp_send.carParams = self.CP
      self.pm.send('carParams', cp_send)

    # carControl
    cc_send = messaging.new_message('carControl')
    cc_send.valid = CS.canValid
    cc_send.carControl = CC
    self.pm.send('carControl', cc_send)

    # copy CarControl to pass to CarInterface on the next iteration
    self.CC = CC

  def step(self):
    start_time = sec_since_boot()
    self.prof.checkpoint("Ratekeeper", ignore=True)

    # Sample data from sockets and get a carState
    CS = self.data_sample()
    self.prof.checkpoint("Sample")

    if self.read_only:
      self.hyundai_lkas = self.read_only
    elif CS.cruiseState.enabled and self.hyundai_lkas:
      self.hyundai_lkas = False

    self.update_events(CS)

    if not self.hyundai_lkas:
      # Update control state
      self.state_transition(CS)
      self.prof.checkpoint("State transition")

    # Compute actuators (runs PID loops and lateral MPC)
    actuators, v_acc, a_acc, lac_log = self.state_control(CS)

    self.prof.checkpoint("State Control")

    # Publish data
    self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
    self.prof.checkpoint("Sent")

    if not CS.cruiseState.enabled and not self.hyundai_lkas:
      self.hyundai_lkas = True

  def controlsd_thread(self):
    while True:
      self.step()
      self.rk.monitor_time()
      self.prof.display()
Exemple #4
0
class Controls:
    def __init__(self, sm=None, pm=None, can_sock=None):
        config_realtime_process(3, Priority.CTRL_HIGH)
        self.op_params = opParams()

        # Setup sockets
        self.pm = pm
        if self.pm is None:
            self.pm = messaging.PubMaster([
                'sendcan', 'controlsState', 'carState', 'carControl',
                'carEvents', 'carParams'
            ])

        self.sm = sm
        if self.sm is None:
            ignore = ['driverCameraState', 'managerState'
                      ] if SIMULATION else None
            self.sm = messaging.SubMaster([
                'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
                'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
                'liveLocationKalman', 'roadCameraState', 'driverCameraState',
                'managerState', 'liveParameters', 'radarState'
            ],
                                          ignore_alive=ignore)

        self.sm_smiskol = messaging.SubMaster([
            'radarState', 'dynamicFollowData', 'liveTracks',
            'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset',
            'modelLongButton'
        ])

        self.op_params = opParams()
        self.df_manager = dfManager()
        self.support_white_panda = self.op_params.get('support_white_panda')
        self.last_model_long = False

        self.can_sock = can_sock
        if can_sock is None:
            can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
                                                 False) else 100
            self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

        # wait for one pandaState and one CAN packet
        panda_type = messaging.recv_one(
            self.sm.sock['pandaState']).pandaState.pandaType
        has_relay = panda_type in [
            PandaType.blackPanda, PandaType.uno, PandaType.dos
        ]
        print("Waiting for CAN messages...")
        get_one_can(self.can_sock)

        self.CI, self.CP, candidate = get_car(self.can_sock,
                                              self.pm.sock['sendcan'],
                                              has_relay)
        threading.Thread(target=log_fingerprint, args=[candidate]).start()

        # read params
        params = Params()
        self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
        self.is_ldw_enabled = params.get("IsLdwEnabled",
                                         encoding='utf8') == "1"
        community_feature_toggle = params.get("CommunityFeaturesToggle",
                                              encoding='utf8') == "1"
        openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
                                              encoding='utf8') == "1"
        passive = params.get(
            "Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle

        # detect sound card presence and ensure successful init
        sounds_available = HARDWARE.get_sound_card_online()

        car_recognized = self.CP.carName != 'mock'
        # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
        controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
        community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
        self.read_only = not car_recognized or not controller_available or \
                           self.CP.dashcamOnly or community_feature_disallowed
        if self.read_only:
            self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

        # Write CarParams for radard and boardd safety mode
        cp_bytes = self.CP.to_bytes()
        params.put("CarParams", cp_bytes)
        put_nonblocking("CarParamsCache", cp_bytes)

        self.CC = car.CarControl.new_message()
        self.AM = AlertManager()
        self.events = Events()

        self.LoC = LongControl(self.CP, self.CI.compute_gb, candidate)
        self.VM = VehicleModel(self.CP)

        if self.CP.lateralTuning.which() == 'pid':
            self.LaC = LatControlPID(self.CP)
        elif self.CP.lateralTuning.which() == 'indi':
            self.LaC = LatControlINDI(self.CP)
        elif self.CP.lateralTuning.which() == 'lqr':
            self.LaC = LatControlLQR(self.CP)

        self.state = State.disabled
        self.enabled = False
        self.active = False
        self.can_rcv_error = False
        self.soft_disable_timer = 0
        self.v_cruise_kph = 255
        self.v_cruise_kph_last = 0
        self.mismatch_counter = 0
        self.can_error_counter = 0
        self.last_blinker_frame = 0
        self.saturated_count = 0
        self.distance_traveled = 0
        self.last_functional_fan_frame = 0
        self.events_prev = []
        self.current_alert_types = [ET.PERMANENT]
        self.logged_comm_issue = False

        self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
        self.sm['deviceState'].freeSpacePercent = 100
        self.sm['driverMonitoringState'].events = []
        self.sm['driverMonitoringState'].awarenessStatus = 1.
        self.sm['driverMonitoringState'].faceDetected = False

        self.startup_event = get_startup_event(car_recognized,
                                               controller_available)

        if not sounds_available:
            self.events.add(EventName.soundsUnavailable, static=True)
        if community_feature_disallowed:
            self.events.add(EventName.communityFeatureDisallowed, static=True)
        if not car_recognized:
            self.events.add(EventName.carUnrecognized, static=True)

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default

        self.lead_rel_speed = 255
        self.lead_long_dist = 255

    def update_events(self, CS):
        """Compute carEvents from carState"""

        self.events.clear()
        self.events.add_from_msg(CS.events)
        self.events.add_from_msg(self.sm['driverMonitoringState'].events)

        # Handle startup event
        if self.startup_event is not None:
            self.events.add(self.startup_event)
            self.startup_event = None

        # Create events for battery, temperature, disk space, and memory
        if self.sm['deviceState'].batteryPercent < 1 and self.sm[
                'deviceState'].chargingError:
            # at zero percent battery, while discharging, OP should not allowed
            self.events.add(EventName.lowBattery)
        if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
            self.events.add(EventName.overheat)
        if self.sm['deviceState'].freeSpacePercent < 7:
            # under 7% of space free no enable allowed
            self.events.add(EventName.outOfSpace)
        if self.sm['deviceState'].memoryUsagePercent > 90:
            self.events.add(EventName.lowMemory)

        # Alert if fan isn't spinning for 5 seconds
        if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
            if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm[
                    'deviceState'].fanSpeedPercentDesired > 50:
                if (self.sm.frame -
                        self.last_functional_fan_frame) * DT_CTRL > 5.0:
                    self.events.add(EventName.fanMalfunction)
            else:
                self.last_functional_fan_frame = self.sm.frame

        # Handle calibration status
        cal_status = self.sm['liveCalibration'].calStatus
        if cal_status != Calibration.CALIBRATED:
            if cal_status == Calibration.UNCALIBRATED:
                self.events.add(EventName.calibrationIncomplete)
            else:
                self.events.add(EventName.calibrationInvalid)

        # Handle lane change
        if self.sm[
                'lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
            direction = self.sm['lateralPlan'].laneChangeDirection
            if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
               (CS.rightBlindspot and direction == LaneChangeDirection.right):
                self.events.add(EventName.laneChangeBlocked)
            else:
                if direction == LaneChangeDirection.left:
                    self.events.add(EventName.preLaneChangeLeft)
                else:
                    self.events.add(EventName.preLaneChangeRight)
        elif self.sm['lateralPlan'].laneChangeState in [
                LaneChangeState.laneChangeStarting,
                LaneChangeState.laneChangeFinishing
        ]:
            self.events.add(EventName.laneChange)

        if self.can_rcv_error or (not CS.canValid
                                  and self.sm.frame > 5 / DT_CTRL):
            self.events.add(EventName.canError)
        if (self.sm['pandaState'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \
          self.mismatch_counter >= 200:
            self.events.add(EventName.controlsMismatch)

        if len(self.sm['radarState'].radarErrors):
            self.events.add(EventName.radarFault)
        elif not self.sm.valid['liveParameters']:
            self.events.add(EventName.vehicleModelInvalid)
        elif not self.sm.all_alive_and_valid():
            self.events.add(EventName.commIssue)
            if not self.logged_comm_issue:
                cloudlog.error(
                    f"commIssue - valid: {self.sm.valid} - alive: {self.sm.alive}"
                )
                self.logged_comm_issue = True
        else:
            self.logged_comm_issue = False

        if not self.sm['lateralPlan'].mpcSolutionValid:
            self.events.add(EventName.plannerError)
        if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
            if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
                self.events.add(EventName.sensorDataInvalid)
        if not self.sm['liveLocationKalman'].posenetOK:
            self.events.add(EventName.posenetInvalid)
        if not self.sm['liveLocationKalman'].deviceStable:
            self.events.add(EventName.deviceFalling)
        if log.PandaState.FaultType.relayMalfunction in self.sm[
                'pandaState'].faults:
            self.events.add(EventName.relayMalfunction)
        if self.sm['longitudinalPlan'].fcw:
            self.events.add(EventName.fcw)

            # TODO: fix simulator
            # if not SIMULATION:
            #   if not NOSENSOR and not self.support_white_panda:
            #     if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI:
            #       # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
            #       self.events.add(EventName.noGps)
            if not self.sm.all_alive(['roadCameraState', 'driverCameraState'
                                      ]) and (self.sm.frame > 5 / DT_CTRL):
                self.events.add(EventName.cameraMalfunction)
            if self.sm['modelV2'].frameDropPerc > 20:
                self.events.add(EventName.modeldLagging)

            # Check if all manager processes are running
            not_running = set(p.name for p in self.sm['managerState'].processes
                              if not p.running)
            if self.sm.rcv_frame['managerState'] and (not_running -
                                                      IGNORE_PROCESSES):
                self.events.add(EventName.processNotRunning)

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
          and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3 and not self.last_model_long:
            self.events.add(EventName.noTarget)

        self.add_stock_additions_alerts(CS)

        # vision-only fcw, can be disabled if radar is present
        if self.sm.updated['radarState']:
            self.lead_rel_speed = self.sm['radarState'].leadOne.vRel
            self.lead_long_dist = self.sm['radarState'].leadOne.dRel
        #if CS.cruiseState.enabled and self.lead_long_dist > 5 and self.lead_long_dist < 100 and self.lead_rel_speed <= -0.5 and CS.vEgo >= 5 and \
        #        ((self.lead_long_dist/abs(self.lead_rel_speed) < 2.) or (self.lead_long_dist/abs(self.lead_rel_speed) < 4. and self.lead_rel_speed < -10) or \
        #         (self.lead_long_dist/abs(self.lead_rel_speed) < 5. and self.lead_long_dist/CS.vEgo < 1.5)):
        #  self.events.add(EventName.fcw)

    def add_stock_additions_alerts(self, CS):
        self.AM.SA_set_frame(self.sm.frame)
        self.AM.SA_set_enabled(self.enabled)
        # alert priority is defined by code location, keeping is highest, then lane speed alert, then auto-df alert
        if self.sm_smiskol['modelLongButton'].enabled != self.last_model_long:
            extra_text_1 = 'disabled!' if self.last_model_long else 'enabled!'
            extra_text_2 = '' if self.last_model_long else ', model may behave unexpectedly'
            self.AM.SA_add('modelLongAlert',
                           extra_text_1=extra_text_1,
                           extra_text_2=extra_text_2)
            return

        if self.sm_smiskol['dynamicCameraOffset'].keepingLeft:
            self.AM.SA_add('laneSpeedKeeping',
                           extra_text_1='LEFT',
                           extra_text_2='Oncoming traffic in right lane')
            return
        elif self.sm_smiskol['dynamicCameraOffset'].keepingRight:
            self.AM.SA_add('laneSpeedKeeping',
                           extra_text_1='RIGHT',
                           extra_text_2='Oncoming traffic in left lane')
            return

        ls_state = self.sm_smiskol['laneSpeed'].state
        if ls_state != '':
            self.AM.SA_add('lsButtonAlert', extra_text_1=ls_state)
            return

        faster_lane = self.sm_smiskol['laneSpeed'].fastestLane
        if faster_lane in ['left', 'right']:
            ls_alert = 'laneSpeedAlert'
            if not self.sm_smiskol['laneSpeed'].new:
                ls_alert += 'Silent'
            self.AM.SA_add(
                ls_alert,
                extra_text_1='{} lane faster'.format(faster_lane).upper(),
                extra_text_2='Change lanes to faster {} lane'.format(
                    faster_lane))
            return

        df_out = self.df_manager.update()
        if df_out.changed:
            df_alert = 'dfButtonAlert'
            if df_out.is_auto and df_out.last_is_auto:
                # only show auto alert if engaged, not hiding auto, and time since lane speed alert not showing
                if CS.cruiseState.enabled and not self.op_params.get(
                        'hide_auto_df_alerts'):
                    df_alert += 'Silent'
                    self.AM.SA_add(df_alert,
                                   extra_text_1=df_out.model_profile_text +
                                   ' (auto)')
                    return
            else:
                self.AM.SA_add(
                    df_alert,
                    extra_text_1=df_out.user_profile_text,
                    extra_text_2='Dynamic follow: {} profile active'.format(
                        df_out.user_profile_text))
                return

    def data_sample(self):
        """Receive data from sockets and update carState"""

        # Update carState from CAN
        can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
        CS = self.CI.update(self.CC, can_strs)

        self.sm.update(0)
        self.sm_smiskol.update(0)

        # Check for CAN timeout
        if not can_strs:
            self.can_error_counter += 1
            self.can_rcv_error = True
        else:
            self.can_rcv_error = False

        # 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 self.enabled:
            self.mismatch_counter = 0

        if not self.sm['pandaState'].controlsAllowed and self.enabled:
            self.mismatch_counter += 1

        self.distance_traveled += CS.vEgo * DT_CTRL

        return CS

    def state_transition(self, CS):
        """Compute conditional state transitions and execute actions on state transitions"""

        self.v_cruise_kph_last = self.v_cruise_kph

        # if stock cruise is completely disabled, then we can use our own set speed logic
        if not self.CP.enableCruise:
            self.v_cruise_kph = update_v_cruise(self.v_cruise_kph,
                                                CS.buttonEvents, self.enabled)
        elif self.CP.enableCruise and CS.cruiseState.enabled:
            self.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
        self.soft_disable_timer = max(0, self.soft_disable_timer - 1)

        self.current_alert_types = [ET.PERMANENT]

        # ENABLED, PRE ENABLING, SOFT DISABLING
        if self.state != State.disabled:
            # user and immediate disable always have priority in a non-disabled state
            if self.events.any(ET.USER_DISABLE):
                self.state = State.disabled
                self.current_alert_types.append(ET.USER_DISABLE)

            elif self.events.any(ET.IMMEDIATE_DISABLE):
                self.state = State.disabled
                self.current_alert_types.append(ET.IMMEDIATE_DISABLE)

            else:
                # ENABLED
                if self.state == State.enabled:
                    if self.events.any(ET.SOFT_DISABLE):
                        self.state = State.softDisabling
                        self.soft_disable_timer = 300  # 3s
                        self.current_alert_types.append(ET.SOFT_DISABLE)

                # SOFT DISABLING
                elif self.state == State.softDisabling:
                    if not self.events.any(ET.SOFT_DISABLE):
                        # no more soft disabling condition, so go back to ENABLED
                        self.state = State.enabled

                    elif self.events.any(
                            ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
                        self.current_alert_types.append(ET.SOFT_DISABLE)

                    elif self.soft_disable_timer <= 0:
                        self.state = State.disabled

                # PRE ENABLING
                elif self.state == State.preEnabled:
                    if not self.events.any(ET.PRE_ENABLE):
                        self.state = State.enabled
                    else:
                        self.current_alert_types.append(ET.PRE_ENABLE)

        # DISABLED
        elif self.state == State.disabled:
            if self.events.any(ET.ENABLE):
                if self.events.any(ET.NO_ENTRY):
                    self.current_alert_types.append(ET.NO_ENTRY)

                else:
                    if self.events.any(ET.PRE_ENABLE):
                        self.state = State.preEnabled
                    else:
                        self.state = State.enabled
                    self.current_alert_types.append(ET.ENABLE)
                    self.v_cruise_kph = initialize_v_cruise(
                        CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)

        # Check if actuators are enabled
        self.active = self.state == State.enabled or self.state == State.softDisabling
        if self.active:
            self.current_alert_types.append(ET.WARNING)

        # Check if openpilot is engaged
        self.enabled = self.active or self.state == State.preEnabled

    def state_control(self, CS):
        """Given the state, this function returns an actuators packet"""

        lat_plan = self.sm['lateralPlan']
        long_plan = self.sm['longitudinalPlan']

        actuators = car.CarControl.Actuators.new_message()

        if CS.leftBlinker or CS.rightBlinker:
            self.last_blinker_frame = self.sm.frame

        # State specific actions

        if not self.active:
            self.LaC.reset()
            self.LoC.reset(v_pid=CS.vEgo)

        long_plan_age = DT_CTRL * (self.sm.frame -
                                   self.sm.rcv_frame['longitudinalPlan'])
        # no greater than dt mpc + dt, to prevent too high extraps
        dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL

        a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (
            long_plan.aTarget - long_plan.aStart)
        v_acc_sol = long_plan.vStart + dt * (a_acc_sol +
                                             long_plan.aStart) / 2.0

        extras_loc = {
            'lead_one': self.sm_smiskol['radarState'].leadOne,
            'mpc_TR': self.sm_smiskol['dynamicFollowData'].mpcTR,
            'live_tracks': self.sm_smiskol['liveTracks'],
            'has_lead': long_plan.hasLead
        }

        # Gas/Brake PID loop
        actuators.gas, actuators.brake = self.LoC.update(
            self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol,
            self.CP, extras_loc)
        # Steering PID loop and lateral MPC
        actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
            self.active, CS, self.CP, lat_plan)

        # Check for difference between desired angle and angle for angle based control
        angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
          abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD

        if angle_control_saturated and not CS.steeringPressed and self.active:
            self.saturated_count += 1
        else:
            self.saturated_count = 0

        # Send a "steering required alert" if saturation count has reached the limit
        if (lac_log.saturated and not CS.steeringPressed) or \
           (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
            # Check if we deviated from the path
            left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
                0] < -0.1
            right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
                0] > 0.1

            if left_deviation or right_deviation:
                self.events.add(EventName.steerSaturated)

        return actuators, v_acc_sol, a_acc_sol, lac_log

    def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
        """Send actuators and hud commands to the car, send controlsstate and MPC logging"""

        CC = car.CarControl.new_message()
        CC.enabled = self.enabled
        CC.actuators = actuators

        CC.cruiseControl.override = True
        CC.cruiseControl.cancel = not self.CP.enableCruise or (
            not self.enabled and CS.cruiseState.enabled)

        # Some override values for Honda
        # brake discount removes a sharp nonlinearity
        brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
        speed_override = max(0.0,
                             (self.LoC.v_pid + CS.cruiseState.speedOffset) *
                             brake_discount)
        CC.cruiseControl.speedOverride = float(
            speed_override if self.CP.enableCruise else 0.0)
        CC.cruiseControl.accelOverride = self.CI.calc_accel_override(
            CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo,
            self.sm['longitudinalPlan'].vTarget)

        CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
        CC.hudControl.speedVisible = self.enabled
        CC.hudControl.lanesVisible = self.enabled
        CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead

        right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
        left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
        CC.hudControl.rightLaneVisible = bool(right_lane_visible)
        CC.hudControl.leftLaneVisible = bool(left_lane_visible)

        recent_blinker = (self.sm.frame - self.last_blinker_frame
                          ) * DT_CTRL < 5.0  # 5s blinker cooldown
        ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
                        and (not self.active or CS.epsDisabled == True) and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

        meta = self.sm['modelV2'].meta
        if len(meta.desirePrediction) and ldw_allowed:
            l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
                                                       1]
            r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
                                                       1]
            CAMERA_OFFSET = self.sm['lateralPlan'].cameraOffset
            ldw_average_car_width = 1.750483672001016  # from sedans, suvs, and minivans (todo: find from all openpilot Toyotas instead)
            ldw_m_from_wheel = 0.15
            ldw_threshold = ldw_average_car_width / 2 + ldw_m_from_wheel
            l_lane_close = left_lane_visible and (
                self.sm['modelV2'].laneLines[1].y[0] >
                -(ldw_threshold + CAMERA_OFFSET))
            r_lane_close = right_lane_visible and (
                self.sm['modelV2'].laneLines[2].y[0] <
                (ldw_threshold - CAMERA_OFFSET))

            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:
            self.events.add(EventName.ldw)

        clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
        alerts = self.events.create_alerts(self.current_alert_types,
                                           [self.CP, self.sm, self.is_metric])
        self.AM.add_many(self.sm.frame, alerts, self.enabled)

        self.last_model_long = self.sm_smiskol['modelLongButton'].enabled

        self.AM.process_alerts(self.sm.frame, clear_event)
        CC.hudControl.visualAlert = self.AM.visual_alert

        if not self.read_only:
            # send car controls over can
            can_sends = self.CI.apply(CC)
            self.pm.send(
                'sendcan',
                can_list_to_can_capnp(can_sends,
                                      msgtype='sendcan',
                                      valid=CS.canValid))

        force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
                      (self.state == State.softDisabling)

        steer_angle_rad = (
            CS.steeringAngleDeg -
            self.sm['lateralPlan'].angleOffsetDeg) * CV.DEG_TO_RAD

        # controlsState
        dat = messaging.new_message('controlsState')
        dat.valid = CS.canValid
        controlsState = dat.controlsState
        controlsState.alertText1 = self.AM.alert_text_1
        controlsState.alertText2 = self.AM.alert_text_2
        controlsState.alertSize = self.AM.alert_size
        controlsState.alertStatus = self.AM.alert_status
        controlsState.alertBlinkingRate = self.AM.alert_rate
        controlsState.alertType = self.AM.alert_type
        controlsState.alertSound = self.AM.audible_alert
        controlsState.canMonoTimes = list(CS.canMonoTimes)
        controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
            'longitudinalPlan']
        controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
        controlsState.enabled = self.enabled
        controlsState.active = self.active
        controlsState.curvature = self.VM.calc_curvature(
            steer_angle_rad, CS.vEgo)
        controlsState.state = self.state
        controlsState.engageable = not self.events.any(ET.NO_ENTRY)
        controlsState.longControlState = self.LoC.long_control_state
        controlsState.vPid = float(self.LoC.v_pid)
        controlsState.vCruise = float(self.v_cruise_kph)
        controlsState.upAccelCmd = float(self.LoC.pid.p)
        controlsState.uiAccelCmd = float(self.LoC.pid.id)
        controlsState.ufAccelCmd = float(self.LoC.pid.f)
        controlsState.steeringAngleDesiredDeg = float(
            self.LaC.angle_steers_des)
        controlsState.vTargetLead = float(v_acc)
        controlsState.aTarget = float(a_acc)
        controlsState.cumLagMs = -self.rk.remaining * 1000.
        controlsState.startMonoTime = int(start_time * 1e9)
        controlsState.forceDecel = bool(force_decel)
        controlsState.canErrorCounter = self.can_error_counter

        if self.CP.lateralTuning.which() == 'pid':
            controlsState.lateralControlState.pidState = lac_log
        elif self.CP.lateralTuning.which() == 'lqr':
            controlsState.lateralControlState.lqrState = lac_log
        elif self.CP.lateralTuning.which() == 'indi':
            controlsState.lateralControlState.indiState = lac_log
        self.pm.send('controlsState', dat)

        # carState
        car_events = self.events.to_msg()
        cs_send = messaging.new_message('carState')
        cs_send.valid = CS.canValid
        cs_send.carState = CS
        cs_send.carState.events = car_events
        self.pm.send('carState', cs_send)

        # carEvents - logged every second or on change
        if (self.sm.frame % int(1. / DT_CTRL)
                == 0) or (self.events.names != self.events_prev):
            ce_send = messaging.new_message('carEvents', len(self.events))
            ce_send.carEvents = car_events
            self.pm.send('carEvents', ce_send)
        self.events_prev = self.events.names.copy()

        # carParams - logged every 50 seconds (> 1 per segment)
        if (self.sm.frame % int(50. / DT_CTRL) == 0):
            cp_send = messaging.new_message('carParams')
            cp_send.carParams = self.CP
            self.pm.send('carParams', cp_send)

        # carControl
        cc_send = messaging.new_message('carControl')
        cc_send.valid = CS.canValid
        cc_send.carControl = CC
        self.pm.send('carControl', cc_send)

        # copy CarControl to pass to CarInterface on the next iteration
        self.CC = CC

    def step(self):
        start_time = sec_since_boot()
        self.prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data from sockets and get a carState
        CS = self.data_sample()
        self.prof.checkpoint("Sample")

        self.update_events(CS)

        if not self.read_only:
            # Update control state
            self.state_transition(CS)
            self.prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_acc, a_acc, lac_log = self.state_control(CS)

        self.prof.checkpoint("State Control")

        # Publish data
        self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
        self.prof.checkpoint("Sent")

    def controlsd_thread(self):
        while True:
            self.step()
            self.rk.monitor_time()
            self.prof.display()
Exemple #5
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=None,
                             gas_resume_speed=-1,
                             pcm_enable=True):
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and (
                extra_gears is None or cs_out.gearShifter not in extra_gears):
            events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        if cs_out.gasPressed:
            events.add(EventName.gasPressed)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)
        if cs_out.cruiseState.nonAdaptive:
            events.add(EventName.wrongCruiseMode)

        self.steer_warning = self.steer_warning + 1 if cs_out.steerWarning else 0
        self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1

        # Handle permanent and temporary steering faults
        if cs_out.steerError:
            events.add(EventName.steerUnavailable)
        elif cs_out.steerWarning:
            # only escalate to the harsher alert after the condition has
            # persisted for 0.5s and we're certain that the user isn't overriding
            if not cs_out.standstill and self.steering_unpressed > int(0.5 / DT_CTRL) and \
               self.steer_warning > int(0.5 / DT_CTRL):
                events.add(EventName.steerTempUnavailable)
            else:
                events.add(EventName.steerTempUnavailableSilent)

        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        # Optionally allow to press gas at zero speed to resume.
        # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME!
        if (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \
           (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
            events.add(EventName.pedalPressed)

        # we engage when pcm is active (rising edge)
        if pcm_enable:
            if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
                events.add(EventName.pcmEnable)
            elif not cs_out.cruiseState.enabled:
                events.add(EventName.pcmDisable)

        return events
Exemple #6
0
class Controls:
  def __init__(self, sm=None, pm=None, can_sock=None):
    config_realtime_process(3, Priority.CTRL_HIGH)

    # Setup sockets
    self.pm = pm
    if self.pm is None:
      self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
                                     'carControl', 'carEvents', 'carParams'])

    self.sm = sm
    if self.sm is None:
      self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration',
                                     'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])

    self.can_sock = can_sock
    if can_sock is None:
      can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
      self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

    # wait for one health and one CAN packet
    hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
    has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
    print("Waiting for CAN messages...")
    get_one_can(self.can_sock)

    self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)

    # read params
    params = Params()
    self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
    self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
    internet_needed = (params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and (params.get("DisableUpdates") != b"1")
    community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
    openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
    passive = params.get("Passive", encoding='utf8') == "1" or \
              internet_needed or not openpilot_enabled_toggle

    # detect sound card presence and ensure successful init
    sounds_available = HARDWARE.get_sound_card_online()

    car_recognized = self.CP.carName != 'mock'
    # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
    controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
    community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
    self.read_only = not car_recognized or not controller_available or \
                       self.CP.dashcamOnly or community_feature_disallowed
    if self.read_only:
      self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

    # Write CarParams for radard and boardd safety mode
    cp_bytes = self.CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)

    self.CC = car.CarControl.new_message()
    self.AM = AlertManager()
    self.events = Events()

    self.LoC = LongControl(self.CP, self.CI.compute_gb)
    self.VM = VehicleModel(self.CP)

    if self.CP.lateralTuning.which() == 'pid':
      self.LaC = LatControlPID(self.CP)
    elif self.CP.lateralTuning.which() == 'indi':
      self.LaC = LatControlINDI(self.CP)
    elif self.CP.lateralTuning.which() == 'lqr':
      self.LaC = LatControlLQR(self.CP)

    self.state = State.disabled
    self.enabled = False
    self.active = False
    self.can_rcv_error = False
    self.soft_disable_timer = 0
    self.v_cruise_kph = 255
    self.v_cruise_kph_last = 0
    self.mismatch_counter = 0
    self.can_error_counter = 0
    self.last_blinker_frame = 0
    self.saturated_count = 0
    self.distance_traveled = 0
    self.last_functional_fan_frame = 0
    self.events_prev = []
    self.current_alert_types = [ET.PERMANENT]

    self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
    self.sm['thermal'].freeSpace = 1.
    self.sm['dMonitoringState'].events = []
    self.sm['dMonitoringState'].awarenessStatus = 1.
    self.sm['dMonitoringState'].faceDetected = False

    self.startup_event = get_startup_event(car_recognized, controller_available, hw_type)

    if not sounds_available:
      self.events.add(EventName.soundsUnavailable, static=True)
    if internet_needed:
      self.events.add(EventName.internetConnectivityNeeded, static=True)
    if community_feature_disallowed:
      self.events.add(EventName.communityFeatureDisallowed, static=True)
    if not car_recognized:
      self.events.add(EventName.carUnrecognized, static=True)
    if hw_type == HwType.whitePanda:
      self.events.add(EventName.whitePandaUnsupported, static=True)

    # controlsd is driven by can recv, expected at 100Hz
    self.rk = Ratekeeper(100, print_delay_threshold=None)
    self.prof = Profiler(False)  # off by default

  def update_events(self, CS):
    """Compute carEvents from carState"""

    self.events.clear()
    self.events.add_from_msg(CS.events)
    self.events.add_from_msg(self.sm['dMonitoringState'].events)

    # Handle startup event
    if self.startup_event is not None:
      self.events.add(self.startup_event)
      self.startup_event = None

    # Create events for battery, temperature, disk space, and memory
    if self.sm['thermal'].batteryPercent < 1 and self.sm['thermal'].chargingError:
      # at zero percent battery, while discharging, OP should not allowed
      self.events.add(EventName.lowBattery)
    if self.sm['thermal'].thermalStatus >= ThermalStatus.red:
      self.events.add(EventName.overheat)
    if self.sm['thermal'].freeSpace < 0.07:
      # under 7% of space free no enable allowed
      self.events.add(EventName.outOfSpace)
    if self.sm['thermal'].memUsedPercent > 90:
      self.events.add(EventName.lowMemory)

    # Alert if fan isn't spinning for 5 seconds
    if self.sm['health'].hwType in [HwType.uno, HwType.dos]:
      if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeed > 50:
        if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
          self.events.add(EventName.fanMalfunction)
      else:
        self.last_functional_fan_frame = self.sm.frame

    # Handle calibration status
    cal_status = self.sm['liveCalibration'].calStatus
    if cal_status != Calibration.CALIBRATED:
      if cal_status == Calibration.UNCALIBRATED:
        self.events.add(EventName.calibrationIncomplete)
      else:
        self.events.add(EventName.calibrationInvalid)

    # Handle lane change
    if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange:
      direction = self.sm['pathPlan'].laneChangeDirection
      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
         (CS.rightBlindspot and direction == LaneChangeDirection.right):
        self.events.add(EventName.laneChangeBlocked)
      else:
        if direction == LaneChangeDirection.left:
          self.events.add(EventName.preLaneChangeLeft)
        else:
          self.events.add(EventName.preLaneChangeRight)
    elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
                                                 LaneChangeState.laneChangeFinishing]:
      self.events.add(EventName.laneChange)

    if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
      self.events.add(EventName.canError)
    if self.mismatch_counter >= 200:
      self.events.add(EventName.controlsMismatch)
    if not self.sm.alive['plan'] and self.sm.alive['pathPlan']:
      # only plan not being received: radar not communicating
      self.events.add(EventName.radarCommIssue)
    elif not self.sm.all_alive_and_valid():
      self.events.add(EventName.commIssue)
    if not self.sm['pathPlan'].mpcSolutionValid:
      self.events.add(EventName.plannerError)
    if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
      if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
        self.events.add(EventName.sensorDataInvalid)
    if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
      # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
      if not (SIMULATION or NOSENSOR):  # TODO: send GPS in carla
        self.events.add(EventName.noGps)
    if not self.sm['pathPlan'].paramsValid:
      self.events.add(EventName.vehicleModelInvalid)
    if not self.sm['liveLocationKalman'].posenetOK:
      self.events.add(EventName.posenetInvalid)
    if not self.sm['liveLocationKalman'].deviceStable:
      self.events.add(EventName.deviceFalling)
    if not self.sm['plan'].radarValid:
      self.events.add(EventName.radarFault)
    if self.sm['plan'].radarCanError:
      self.events.add(EventName.radarCanError)
    if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults:
      self.events.add(EventName.relayMalfunction)
    if self.sm['plan'].fcw:
      self.events.add(EventName.fcw)
    if self.sm['model'].frameDropPerc > 1 and (not SIMULATION):
        self.events.add(EventName.modeldLagging)

    # Only allow engagement with brake pressed when stopped behind another stopped car
    if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
      and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
      self.events.add(EventName.noTarget)

  def data_sample(self):
    """Receive data from sockets and update carState"""

    # Update carState from CAN
    can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
    CS = self.CI.update(self.CC, can_strs)

    self.sm.update(0)

    # Check for CAN timeout
    if not can_strs:
      self.can_error_counter += 1
      self.can_rcv_error = True
    else:
      self.can_rcv_error = False

    # 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 self.enabled:
      self.mismatch_counter = 0

    if not self.sm['health'].controlsAllowed and self.enabled:
      self.mismatch_counter += 1

    self.distance_traveled += CS.vEgo * DT_CTRL

    return CS

  def state_transition(self, CS):
    """Compute conditional state transitions and execute actions on state transitions"""

    self.v_cruise_kph_last = self.v_cruise_kph

    # if stock cruise is completely disabled, then we can use our own set speed logic
    if not self.CP.enableCruise:
      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
    elif self.CP.enableCruise and CS.cruiseState.enabled:
      self.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
    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)

    self.current_alert_types = [ET.PERMANENT]

    # ENABLED, PRE ENABLING, SOFT DISABLING
    if self.state != State.disabled:
      # user and immediate disable always have priority in a non-disabled state
      if self.events.any(ET.USER_DISABLE):
        self.state = State.disabled
        self.current_alert_types.append(ET.USER_DISABLE)

      elif self.events.any(ET.IMMEDIATE_DISABLE):
        self.state = State.disabled
        self.current_alert_types.append(ET.IMMEDIATE_DISABLE)

      else:
        # ENABLED
        if self.state == State.enabled:
          if self.events.any(ET.SOFT_DISABLE):
            self.state = State.softDisabling
            self.soft_disable_timer = 300   # 3s
            self.current_alert_types.append(ET.SOFT_DISABLE)

        # SOFT DISABLING
        elif self.state == State.softDisabling:
          if not self.events.any(ET.SOFT_DISABLE):
            # no more soft disabling condition, so go back to ENABLED
            self.state = State.enabled

          elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
            self.current_alert_types.append(ET.SOFT_DISABLE)

          elif self.soft_disable_timer <= 0:
            self.state = State.disabled

        # PRE ENABLING
        elif self.state == State.preEnabled:
          if not self.events.any(ET.PRE_ENABLE):
            self.state = State.enabled
          else:
            self.current_alert_types.append(ET.PRE_ENABLE)

    # DISABLED
    elif self.state == State.disabled:
      if self.events.any(ET.ENABLE):
        if self.events.any(ET.NO_ENTRY):
          self.current_alert_types.append(ET.NO_ENTRY)

        else:
          if self.events.any(ET.PRE_ENABLE):
            self.state = State.preEnabled
          else:
            self.state = State.enabled
          self.current_alert_types.append(ET.ENABLE)
          self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)

    # Check if actuators are enabled
    self.active = self.state == State.enabled or self.state == State.softDisabling
    if self.active:
      self.current_alert_types.append(ET.WARNING)

    # Check if openpilot is engaged
    self.enabled = self.active or self.state == State.preEnabled

  def state_control(self, CS):
    """Given the state, this function returns an actuators packet"""

    plan = self.sm['plan']
    path_plan = self.sm['pathPlan']

    actuators = car.CarControl.Actuators.new_message()

    if CS.leftBlinker or CS.rightBlinker:
      self.last_blinker_frame = self.sm.frame

    # State specific actions

    if not self.active:
      self.LaC.reset()
      self.LoC.reset(v_pid=CS.vEgo)

    plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan'])
    # no greater than dt mpc + dt, to prevent too high extraps
    dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL

    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 = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
    # Steering PID loop and lateral MPC
    actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)

    # Check for difference between desired angle and angle for angle based control
    angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
      abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD

    if angle_control_saturated and not CS.steeringPressed and self.active:
      self.saturated_count += 1
    else:
      self.saturated_count = 0

    # Send a "steering required alert" if saturation count has reached the limit
    if (lac_log.saturated and not CS.steeringPressed) or \
       (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
      # 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:
        self.events.add(EventName.steerSaturated)

    return actuators, v_acc_sol, a_acc_sol, lac_log

  def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
    """Send actuators and hud commands to the car, send controlsstate and MPC logging"""

    CC = car.CarControl.new_message()
    CC.enabled = self.enabled
    CC.actuators = actuators

    CC.cruiseControl.override = True
    CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled)

    # Some override values for Honda
    # brake discount removes a sharp nonlinearity
    brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
    speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
    CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
    CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget)

    CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
    CC.hudControl.speedVisible = self.enabled
    CC.hudControl.lanesVisible = self.enabled
    CC.hudControl.leadVisible = self.sm['plan'].hasLead

    right_lane_visible = self.sm['pathPlan'].rProb > 0.5
    left_lane_visible = self.sm['pathPlan'].lProb > 0.5
    CC.hudControl.rightLaneVisible = bool(right_lane_visible)
    CC.hudControl.leftLaneVisible = bool(left_lane_visible)

    recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
    ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
                    and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

    meta = self.sm['model'].meta
    if len(meta.desirePrediction) and ldw_allowed:
      l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
      r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
      l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
      r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))

      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:
      self.events.add(EventName.ldw)

    clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
    alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric])
    self.AM.add_many(self.sm.frame, alerts, self.enabled)
    self.AM.process_alerts(self.sm.frame, clear_event)
    CC.hudControl.visualAlert = self.AM.visual_alert

    if not self.read_only:
      # send car controls over can
      can_sends = self.CI.apply(CC)
      self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))

    force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
                    (self.state == State.softDisabling)

    steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD

    # controlsState
    dat = messaging.new_message('controlsState')
    dat.valid = CS.canValid
    controlsState = dat.controlsState
    controlsState.alertText1 = self.AM.alert_text_1
    controlsState.alertText2 = self.AM.alert_text_2
    controlsState.alertSize = self.AM.alert_size
    controlsState.alertStatus = self.AM.alert_status
    controlsState.alertBlinkingRate = self.AM.alert_rate
    controlsState.alertType = self.AM.alert_type
    controlsState.alertSound = self.AM.audible_alert
    controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected
    controlsState.canMonoTimes = list(CS.canMonoTimes)
    controlsState.planMonoTime = self.sm.logMonoTime['plan']
    controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan']
    controlsState.enabled = self.enabled
    controlsState.active = self.active
    controlsState.vEgo = CS.vEgo
    controlsState.vEgoRaw = CS.vEgoRaw
    controlsState.angleSteers = CS.steeringAngle
    controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
    controlsState.steerOverride = CS.steeringPressed
    controlsState.state = self.state
    controlsState.engageable = not self.events.any(ET.NO_ENTRY)
    controlsState.longControlState = self.LoC.long_control_state
    controlsState.vPid = float(self.LoC.v_pid)
    controlsState.vCruise = float(self.v_cruise_kph)
    controlsState.upAccelCmd = float(self.LoC.pid.p)
    controlsState.uiAccelCmd = float(self.LoC.pid.i)
    controlsState.ufAccelCmd = float(self.LoC.pid.f)
    controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
    controlsState.vTargetLead = float(v_acc)
    controlsState.aTarget = float(a_acc)
    controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
    controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
    controlsState.vCurvature = self.sm['plan'].vCurvature
    controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model
    controlsState.cumLagMs = -self.rk.remaining * 1000.
    controlsState.startMonoTime = int(start_time * 1e9)
    controlsState.mapValid = self.sm['plan'].mapValid
    controlsState.forceDecel = bool(force_decel)
    controlsState.canErrorCounter = self.can_error_counter

    if self.CP.lateralTuning.which() == 'pid':
      controlsState.lateralControlState.pidState = lac_log
    elif self.CP.lateralTuning.which() == 'lqr':
      controlsState.lateralControlState.lqrState = lac_log
    elif self.CP.lateralTuning.which() == 'indi':
      controlsState.lateralControlState.indiState = lac_log
    self.pm.send('controlsState', dat)

    # carState
    car_events = self.events.to_msg()
    cs_send = messaging.new_message('carState')
    cs_send.valid = CS.canValid
    cs_send.carState = CS
    cs_send.carState.events = car_events
    self.pm.send('carState', cs_send)

    # carEvents - logged every second or on change
    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
      ce_send = messaging.new_message('carEvents', len(self.events))
      ce_send.carEvents = car_events
      self.pm.send('carEvents', ce_send)
    self.events_prev = self.events.names.copy()

    # carParams - logged every 50 seconds (> 1 per segment)
    if (self.sm.frame % int(50. / DT_CTRL) == 0):
      cp_send = messaging.new_message('carParams')
      cp_send.carParams = self.CP
      self.pm.send('carParams', cp_send)

    # carControl
    cc_send = messaging.new_message('carControl')
    cc_send.valid = CS.canValid
    cc_send.carControl = CC
    self.pm.send('carControl', cc_send)

    # copy CarControl to pass to CarInterface on the next iteration
    self.CC = CC

  def step(self):
    start_time = sec_since_boot()
    self.prof.checkpoint("Ratekeeper", ignore=True)

    # Sample data from sockets and get a carState
    CS = self.data_sample()
    self.prof.checkpoint("Sample")

    self.update_events(CS)

    if not self.read_only:
      # Update control state
      self.state_transition(CS)
      self.prof.checkpoint("State transition")

    # Compute actuators (runs PID loops and lateral MPC)
    actuators, v_acc, a_acc, lac_log = self.state_control(CS)

    self.prof.checkpoint("State Control")

    # Publish data
    self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
    self.prof.checkpoint("Sent")

  def controlsd_thread(self):
    while True:
      self.step()
      self.rk.monitor_time()
      self.prof.display()
Exemple #7
0
def dmonitoringd_thread(sm=None, pm=None):
    gc.disable()
    set_realtime_priority(2)

    if pm is None:
        pm = messaging.PubMaster(['driverMonitoringState'])

    if sm is None:
        sm = messaging.SubMaster([
            'driverStateV2', 'liveCalibration', 'carState', 'controlsState',
            'modelV2'
        ],
                                 poll=['driverStateV2'])

    driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected"))

    sm['liveCalibration'].calStatus = Calibration.INVALID
    sm['liveCalibration'].rpyCalib = [0, 0, 0]
    sm['carState'].buttonEvents = []
    sm['carState'].standstill = True

    v_cruise_last = 0
    driver_engaged = False

    # 10Hz <- dmonitoringmodeld
    while True:
        sm.update()

        if not sm.updated['driverStateV2']:
            continue

        # Get interaction
        if sm.updated['carState']:
            v_cruise = sm['carState'].cruiseState.speed
            driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
                              v_cruise != v_cruise_last or \
                              sm['carState'].steeringPressed or \
                              sm['carState'].gasPressed
            v_cruise_last = v_cruise

        if sm.updated['modelV2']:
            driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo)

        # Get data from dmonitoringmodeld
        events = Events()
        driver_status.update_states(sm['driverStateV2'],
                                    sm['liveCalibration'].rpyCalib,
                                    sm['carState'].vEgo,
                                    sm['controlsState'].enabled)

        # Block engaging after max number of distrations
        if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
           driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION:
            events.add(car.CarEvent.EventName.tooDistracted)

        # Update events from driver state
        driver_status.update_events(events, driver_engaged,
                                    sm['controlsState'].enabled,
                                    sm['carState'].standstill)

        # build driverMonitoringState packet
        dat = messaging.new_message('driverMonitoringState')
        dat.driverMonitoringState = {
            "events":
            events.to_msg(),
            "faceDetected":
            driver_status.face_detected,
            "isDistracted":
            driver_status.driver_distracted,
            "distractedType":
            sum(driver_status.distracted_types),
            "awarenessStatus":
            driver_status.awareness,
            "posePitchOffset":
            driver_status.pose.pitch_offseter.filtered_stat.mean(),
            "posePitchValidCount":
            driver_status.pose.pitch_offseter.filtered_stat.n,
            "poseYawOffset":
            driver_status.pose.yaw_offseter.filtered_stat.mean(),
            "poseYawValidCount":
            driver_status.pose.yaw_offseter.filtered_stat.n,
            "stepChange":
            driver_status.step_change,
            "awarenessActive":
            driver_status.awareness_active,
            "awarenessPassive":
            driver_status.awareness_passive,
            "isLowStd":
            driver_status.pose.low_std,
            "hiStdCount":
            driver_status.hi_stds,
            "isActiveMode":
            driver_status.active_monitoring_mode,
            "isRHD":
            driver_status.wheel_on_right,
        }
        pm.send('driverMonitoringState', dat)

        # save rhd virtual toggle every 5 mins
        if (sm['driverStateV2'].frameId % 6000 == 0
                and driver_status.wheelpos_learner.filtered_stat.n >
                driver_status.settings._WHEELPOS_FILTER_MIN_COUNT
                and driver_status.wheel_on_right
                == (driver_status.wheelpos_learner.filtered_stat.M >
                    driver_status.settings._WHEELPOS_THRESHOLD)):
            put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right)
Exemple #8
0
def dmonitoringd_thread(sm=None, pm=None):
    if pm is None:
        pm = messaging.PubMaster(['driverMonitoringState'])

    if sm is None:
        sm = messaging.SubMaster([
            'driverState', 'liveCalibration', 'carState', 'controlsState',
            'modelV2'
        ],
                                 poll=['driverState'])

    driver_status = DriverStatus(rhd=Params().get_bool("IsRHD"))

    sm['liveCalibration'].calStatus = Calibration.INVALID
    sm['liveCalibration'].rpyCalib = [0, 0, 0]
    sm['carState'].vEgo = 0.
    sm['carState'].buttonEvents = []
    sm['carState'].standstill = True

    v_cruise_last = 0
    driver_engaged = False

    # 10Hz <- dmonitoringmodeld
    while True:
        sm.update()

        if not sm.updated['driverState']:
            continue

        # Get interaction
        if sm.updated['carState']:
            v_cruise = sm['carState'].cruiseState.speed
            driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
                              v_cruise != v_cruise_last or \
                              sm['carState'].steeringPressed or \
                              sm['carState'].gasPressed
            if driver_engaged:
                driver_status.update(Events(), True,
                                     sm['controlsState'].enabled,
                                     sm['carState'].standstill,
                                     sm['carState'].vEgo)
            v_cruise_last = v_cruise

        if sm.updated['modelV2']:
            driver_status.set_policy(sm['modelV2'])

        # Get data from dmonitoringmodeld
        events = Events()
        driver_status.get_pose(sm['driverState'],
                               sm['liveCalibration'].rpyCalib,
                               sm['carState'].vEgo,
                               sm['controlsState'].enabled)

        # Block engaging after max number of distrations
        if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
            events.add(car.CarEvent.EventName.tooDistracted)

        # Update events from driver state
        driver_status.update(events, driver_engaged,
                             sm['controlsState'].enabled,
                             sm['carState'].standstill, sm['carState'].vEgo)

        # build driverMonitoringState packet
        dat = messaging.new_message('driverMonitoringState')
        dat.driverMonitoringState = {
            "events":
            events.to_msg(),
            "faceDetected":
            driver_status.face_detected,
            "isDistracted":
            driver_status.driver_distracted,
            "awarenessStatus":
            driver_status.awareness,
            "posePitchOffset":
            driver_status.pose.pitch_offseter.filtered_stat.mean(),
            "posePitchValidCount":
            driver_status.pose.pitch_offseter.filtered_stat.n,
            "poseYawOffset":
            driver_status.pose.yaw_offseter.filtered_stat.mean(),
            "poseYawValidCount":
            driver_status.pose.yaw_offseter.filtered_stat.n,
            "stepChange":
            driver_status.step_change,
            "awarenessActive":
            driver_status.awareness_active,
            "awarenessPassive":
            driver_status.awareness_passive,
            "isLowStd":
            driver_status.pose.low_std,
            "hiStdCount":
            driver_status.hi_stds,
            "isActiveMode":
            driver_status.active_monitoring_mode,
        }
        pm.send('driverMonitoringState', dat)
Exemple #9
0
def dmonitoringd_thread(sm=None, pm=None):
    if pm is None:
        pm = messaging.PubMaster(['dMonitoringState'])

    if sm is None:
        sm = messaging.SubMaster([
            'driverState', 'liveCalibration', 'carState', 'model', 'dragonConf'
        ],
                                 poll=['driverState'])

    driver_status = DriverStatus()
    driver_status.is_rhd_region = Params().get("IsRHD") == b"1"

    offroad = Params().get("IsOffroad") == b"1"

    sm['liveCalibration'].calStatus = Calibration.INVALID
    sm['liveCalibration'].rpyCalib = [0, 0, 0]
    sm['carState'].vEgo = 0.
    sm['carState'].cruiseState.enabled = False
    sm['carState'].cruiseState.speed = 0.
    sm['carState'].buttonEvents = []
    sm['carState'].steeringPressed = False
    sm['carState'].gasPressed = False
    sm['carState'].brakePressed = False
    sm['carState'].standstill = True

    v_cruise_last = 0
    driver_engaged = False

    # dp
    sm['dragonConf'].dpDriverMonitor = True
    sm['dragonConf'].dpSteeringMonitor = True
    sm['dragonConf'].dpSteeringMonitorTimer = 70

    # 10Hz <- dmonitoringmodeld
    while True:
        start_time = sec_since_boot()
        sm.update()

        # dp
        if not sm['dragonConf'].dpDriverMonitor:
            driver_status.active_monitoring_mode = False
            driver_status.face_detected = False
            driver_status.threshold_pre = 15. / sm[
                'dragonConf'].dpSteeringMonitorTimer
            driver_status.threshold_prompt = 6. / sm[
                'dragonConf'].dpSteeringMonitorTimer
            driver_status.step_change = DT_DMON / sm[
                'dragonConf'].dpSteeringMonitorTimer
            if not sm['dragonConf'].dpSteeringMonitor:
                driver_status.awareness = 1.
                driver_status.awareness_active = 1.
                driver_status.awareness_passive = 1.
                driver_status.terminal_alert_cnt = 0
                driver_status.terminal_time = 0

        if not sm.updated['driverState']:
            continue

        # Get interaction
        if sm.updated['carState']:
            v_cruise = sm['carState'].cruiseState.speed
            driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
                              v_cruise != v_cruise_last or \
                              sm['carState'].steeringPressed or \
                              sm['carState'].gasPressed or \
                              sm['carState'].brakePressed
            if driver_engaged:
                driver_status.update(Events(), True,
                                     sm['carState'].cruiseState.enabled,
                                     sm['carState'].standstill)
            v_cruise_last = v_cruise

        if sm.updated['model']:
            driver_status.set_policy(sm['model'])

        # Get data from dmonitoringmodeld
        events = Events()
        if sm['dragonConf'].dpDriverMonitor:
            driver_status.get_pose(sm['driverState'],
                                   sm['liveCalibration'].rpyCalib,
                                   sm['carState'].vEgo,
                                   sm['carState'].cruiseState.enabled)

        # Block engaging after max number of distrations
        if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
            events.add(car.CarEvent.EventName.tooDistracted)

        # Update events from driver state
        driver_status.update(events, driver_engaged,
                             sm['carState'].cruiseState.enabled,
                             sm['carState'].standstill)

        # build dMonitoringState packet
        dat = messaging.new_message('dMonitoringState')
        dat.dMonitoringState = {
            "events":
            events.to_msg(),
            "faceDetected":
            driver_status.face_detected,
            "isDistracted":
            driver_status.driver_distracted,
            "awarenessStatus":
            driver_status.awareness,
            "isRHD":
            driver_status.is_rhd_region,
            "posePitchOffset":
            driver_status.pose.pitch_offseter.filtered_stat.mean(),
            "posePitchValidCount":
            driver_status.pose.pitch_offseter.filtered_stat.n,
            "poseYawOffset":
            driver_status.pose.yaw_offseter.filtered_stat.mean(),
            "poseYawValidCount":
            driver_status.pose.yaw_offseter.filtered_stat.n,
            "stepChange":
            driver_status.step_change,
            "awarenessActive":
            driver_status.awareness_active,
            "awarenessPassive":
            driver_status.awareness_passive,
            "isLowStd":
            driver_status.pose.low_std,
            "hiStdCount":
            driver_status.hi_stds,
            "isPreview":
            offroad,
        }
        pm.send('dMonitoringState', dat)
        diff = sec_since_boot() - start_time
        if not sm['dragonConf'].dpDriverMonitor and diff < 0.1:
            time.sleep(0.1 - diff)
Exemple #10
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=None,
                             gas_resume_speed=-1,
                             pcm_enable=True):
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and (
                extra_gears is None or cs_out.gearShifter not in extra_gears):
            events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)

        self.steer_warning = self.steer_warning + 1 if cs_out.steerWarning else 0
        self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1

        # Handle permanent and temporary steering faults
        if cs_out.steerError:
            events.add(EventName.steerUnavailable)
        elif cs_out.steerWarning:
            # only escalate to the harsher alert after the condition has
            # persisted for 0.5s and we're certain that the user isn't overriding
            if self.steering_unpressed > int(
                    0.5 / DT_CTRL) and self.steer_warning > int(0.5 / DT_CTRL):
                events.add(EventName.steerTempUnavailable)
            else:
                events.add(EventName.steerTempUnavailableSilent)

        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        if cs_out.brakePressed and (not self.CS.out.brakePressed
                                    or not cs_out.standstill):
            pass
            #events.add(EventName.pedalPressed)

        if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
            events.add(EventName.pcmEnable)
        elif not cs_out.cruiseState.enabled:
            events.add(EventName.pcmDisable)

        return events
Exemple #11
0
def cycle_alerts(duration=200, is_metric=False):
    # all alerts
    #alerts = list(EVENTS.keys())

    # this plays each type of audible alert
    alerts = [
        (EventName.buttonEnable, ET.ENABLE),
        (EventName.buttonCancel, ET.USER_DISABLE),
        (EventName.wrongGear, ET.NO_ENTRY),
        (EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
        (EventName.accFaulted, ET.IMMEDIATE_DISABLE),

        # DM sequence
        (EventName.preDriverDistracted, ET.WARNING),
        (EventName.promptDriverDistracted, ET.WARNING),
        (EventName.driverDistracted, ET.WARNING),
    ]

    # debug alerts
    alerts = [
        (EventName.highCpuUsage, ET.NO_ENTRY),
        (EventName.lowMemory, ET.PERMANENT),
        (EventName.overheat, ET.PERMANENT),
        (EventName.outOfSpace, ET.PERMANENT),
        (EventName.modeldLagging, ET.PERMANENT),
    ]

    CP = CarInterface.get_params("HONDA CIVIC 2016")
    sm = messaging.SubMaster([
        'deviceState', 'pandaStates', 'roadCameraState', 'modelV2',
        'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
        'lateralPlan', 'liveLocationKalman'
    ])

    sm['deviceState'].freeSpacePercent = 55
    sm['deviceState'].memoryUsagePercent = 55
    sm['deviceState'].cpuTempC = [1, 2, 100]
    sm['deviceState'].gpuTempC = [211, 2, 100]
    sm['deviceState'].cpuUsagePercent = [23, 54]
    sm['modelV2'].frameDropPerc = 20

    pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])

    events = Events()
    AM = AlertManager()

    frame = 0
    while True:
        for alert, et in alerts:
            events.clear()
            events.add(alert)

            a = events.create_alerts([
                et,
            ], [CP, sm, is_metric, 0])
            AM.add_many(frame, a)
            alert = AM.process_alerts(frame, [])
            print(alert)
            for _ in range(duration):
                dat = messaging.new_message()
                dat.init('controlsState')
                dat.controlsState.enabled = False

                if alert:
                    dat.controlsState.alertText1 = alert.alert_text_1
                    dat.controlsState.alertText2 = alert.alert_text_2
                    dat.controlsState.alertSize = alert.alert_size
                    dat.controlsState.alertStatus = alert.alert_status
                    dat.controlsState.alertBlinkingRate = alert.alert_rate
                    dat.controlsState.alertType = alert.alert_type
                    dat.controlsState.alertSound = alert.audible_alert
                pm.send('controlsState', dat)

                dat = messaging.new_message()
                dat.init('deviceState')
                dat.deviceState.started = True
                pm.send('deviceState', dat)

                dat = messaging.new_message('pandaStates', 1)
                dat.pandaStates[0].ignitionLine = True
                dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
                pm.send('pandaStates', dat)

                frame += 1
                time.sleep(DT_CTRL)
def cycle_alerts(duration=200, is_metric=False):
    # all alerts
    #alerts = list(EVENTS.keys())

    # this plays each type of audible alert
    alerts = [
        (EventName.buttonEnable, ET.ENABLE),
        (EventName.buttonCancel, ET.USER_DISABLE),
        (EventName.wrongGear, ET.NO_ENTRY),
        (EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
        (EventName.accFaulted, ET.IMMEDIATE_DISABLE),

        # DM sequence
        (EventName.preDriverDistracted, ET.WARNING),
        (EventName.promptDriverDistracted, ET.WARNING),
        (EventName.driverDistracted, ET.WARNING),
    ]

    CP = CarInterface.get_params("HONDA CIVIC 2016")
    sm = messaging.SubMaster([
        'deviceState', 'pandaStates', 'roadCameraState', 'modelV2',
        'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
        'lateralPlan', 'liveLocationKalman'
    ])

    pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])

    events = Events()
    AM = AlertManager()

    frame = 0
    while True:
        current_alert_types = [
            ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
            ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING
        ]

        for alert, et in alerts:
            events.clear()
            events.add(alert)

            a = events.create_alerts([
                et,
            ], [CP, sm, is_metric, 0])
            AM.add_many(frame, a)
            AM.process_alerts(frame)
            print(AM.alert)
            for _ in range(duration):
                dat = messaging.new_message()
                dat.init('controlsState')
                dat.controlsState.enabled = True

                dat.controlsState.alertText1 = AM.alert_text_1
                dat.controlsState.alertText2 = AM.alert_text_2
                dat.controlsState.alertSize = AM.alert_size
                dat.controlsState.alertStatus = AM.alert_status
                dat.controlsState.alertBlinkingRate = AM.alert_rate
                dat.controlsState.alertType = AM.alert_type
                dat.controlsState.alertSound = AM.audible_alert
                pm.send('controlsState', dat)

                dat = messaging.new_message()
                dat.init('deviceState')
                dat.deviceState.started = True
                pm.send('deviceState', dat)

                dat = messaging.new_message('pandaStates', 1)
                dat.pandaStates[0].ignitionLine = True
                dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
                pm.send('pandaStates', dat)

                frame += 1
                time.sleep(DT_CTRL)
Exemple #13
0
    def update(self, c, can_strings):
        # ******************* do can recv *******************
        canMonoTimes = []

        self.cp.update_strings(can_strings)
        ch_can_valid = self.cp.can_valid
        self.epas_cp.update_strings(can_strings)
        epas_can_valid = self.epas_cp.can_valid
        self.pedal_cp.update_strings(can_strings)
        pedal_can_valid = self.pedal_cp.can_valid

        can_rcv_error = not (ch_can_valid and epas_can_valid
                             and pedal_can_valid)

        self.CS.update(self.cp, self.epas_cp, self.pedal_cp)

        # create message
        ret = car.CarState.new_message()
        ret.canValid = ch_can_valid  # and epas_can_valid #and pedal_can_valid
        # speeds
        ret.vEgo = self.CS.v_ego
        ret.aEgo = self.CS.a_ego
        ret.vEgoRaw = self.CS.v_ego_raw
        ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
                                       self.CS.v_ego)
        ret.standstill = self.CS.standstill
        ret.wheelSpeeds.fl = self.CS.v_wheel_fl
        ret.wheelSpeeds.fr = self.CS.v_wheel_fr
        ret.wheelSpeeds.rl = self.CS.v_wheel_rl
        ret.wheelSpeeds.rr = self.CS.v_wheel_rr

        # gas pedal, we don't use with with interceptor so it's always 0/False
        ret.gas = self.CS.user_gas
        if not self.CP.enableGasInterceptor:
            ret.gasPressed = self.CS.user_gas_pressed
        else:
            ret.gasPressed = self.CS.user_gas_pressed

        # brake pedal
        ret.brakePressed = False  # (self.CS.brake_pressed != 0) and (self.CS.cstm_btns.get_button_status("brake") == 0)
        # FIXME: read sendcan for brakelights
        brakelights_threshold = 0.1
        ret.brakeLights = bool(self.CS.brake_switch
                               or c.actuators.brake > brakelights_threshold)

        # steering wheel
        ret.steeringAngle = self.CS.angle_steers
        ret.steeringRate = self.CS.angle_steers_rate

        # gear shifter lever
        ret.gearShifter = self.CS.gear_shifter

        ret.steeringTorque = self.CS.steer_torque_driver
        ret.steeringPressed = self.CS.steer_override

        # cruise state
        ret.cruiseState.enabled = True  # self.CS.pcm_acc_status != 0
        ret.cruiseState.speed = (
            self.CS.v_cruise_pcm * CV.KPH_TO_MS *
            (CV.MPH_TO_KPH if self.CS.imperial_speed_units else 1.0))
        ret.cruiseState.available = bool(self.CS.main_on)
        ret.cruiseState.speedOffset = 0.0
        ret.cruiseState.standstill = False

        # TODO: button presses
        buttonEvents = []
        ret.leftBlinker = bool(self.CS.turn_signal_state_left == 1)
        ret.rightBlinker = bool(self.CS.turn_signal_state_right == 1)

        ret.doorOpen = not self.CS.door_all_closed
        ret.seatbeltUnlatched = not self.CS.seatbelt

        if self.CS.prev_turn_signal_stalk_state != self.CS.turn_signal_stalk_state:
            if (self.CS.turn_signal_stalk_state == 1
                    or self.CS.prev_turn_signal_stalk_state == 1):
                be = car.CarState.ButtonEvent.new_message()
                be.type = "leftBlinker"
                be.pressed = self.CS.turn_signal_stalk_state == 1
                buttonEvents.append(be)
            if (self.CS.turn_signal_stalk_state == 2
                    or self.CS.prev_turn_signal_stalk_state == 2):
                be = car.CarState.ButtonEvent.new_message()
                be.type = "rightBlinker"
                be.pressed = self.CS.turn_signal_stalk_state == 2
                buttonEvents.append(be)

        if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
            be = car.CarState.ButtonEvent.new_message()
            be.type = "unknown"
            if self.CS.cruise_buttons != 0:
                be.pressed = True
                but = self.CS.cruise_buttons
            else:
                be.pressed = False
                but = self.CS.prev_cruise_buttons
            if but == CruiseButtons.RES_ACCEL:
                be.type = "accelCruise"
            elif but == CruiseButtons.DECEL_SET:
                be.type = "decelCruise"
            elif but == CruiseButtons.CANCEL:
                be.type = "cancel"
            elif but == CruiseButtons.MAIN:
                be.type = "altButton3"
            buttonEvents.append(be)

        if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
            be = car.CarState.ButtonEvent.new_message()
            be.type = "unknown"
            be.pressed = bool(self.CS.cruise_buttons)
            buttonEvents.append(be)
        ret.buttonEvents = buttonEvents

        # events
        events = Events()

        # notification messages for DAS
        if (not c.enabled) and (self.CC.opState == 2):
            self.CC.opState = 0
        if c.enabled and (self.CC.opState == 0):
            self.CC.opState = 1
        if can_rcv_error:
            self.can_invalid_count += 1
            if (
                    self.can_invalid_count >= 100
            ):  # BB increased to 100 to see if we still get the can error messages
                events.add(EventName.invalidGiraffeHonda)
                self.CS.DAS_canErrors = 1
                if self.CC.opState == 1:
                    self.CC.opState = 2
        else:
            self.can_invalid_count = 0
        if self.CS.steer_error:
            if not self.CS.enableHSO:
                events.add(EventName.steerUnavailable)
        elif self.CS.steer_warning:
            if not self.CS.enableHSO:
                events.add(EventName.steerTempUnavailable)
                if self.CC.opState == 1:
                    self.CC.opState = 2
        if self.CS.brake_error:
            events.add(EventName.brakeUnavailable)
            if self.CC.opState == 1:
                self.CC.opState = 2
        if not ret.gearShifter == "drive":
            events.add(EventName.wrongGear)
            if c.enabled:
                self.CC.DAS_222_accCameraBlind = 1
                self.CC.warningCounter = 300
                self.CC.warningNeeded = 1
        if ret.doorOpen:
            events.add(EventName.doorOpen)
            self.CS.DAS_doorOpen = 1
            if self.CC.opState == 1:
                self.CC.opState = 0
        if ret.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
            if c.enabled:
                self.CC.DAS_211_accNoSeatBelt = 1
                self.CC.warningCounter = 300
                self.CC.warningNeeded = 1
            if self.CC.opState == 1:
                self.CC.opState = 2
        if self.CS.esp_disabled:
            events.add(EventName.espDisabled)
            if self.CC.opState == 1:
                self.CC.opState = 2
        if not self.CS.main_on:
            events.add(EventName.wrongCarMode)
            if self.CC.opState == 1:
                self.CC.opState = 0
        if ret.gearShifter == "reverse":
            events.add(EventName.reverseGear)
            self.CS.DAS_notInDrive = 1
            if self.CC.opState == 1:
                self.CC.opState = 0
        if ret.gearShifter == "drive":
            self.CS.DAS_notInDrive = 0
        if self.CS.brake_hold:
            events.add(EventName.brakeHold)
            if self.CC.opState == 1:
                self.CC.opState = 0
        if self.CS.park_brake:
            events.add(EventName.parkBrake)
            if self.CC.opState == 1:
                self.CC.opState = 0
        if (not c.enabled) and (self.CC.opState == 1):
            self.CC.opState = 0

        if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
            events.add(EventName.speedTooLow)

        # Standard OP method to disengage:
        # disable on pedals rising edge or when brake is pressed and speed isn't zero
        #    if (ret.gasPressed and not self.gas_pressed_prev) or \
        #       (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
        #       events.add(EventName.steerTempUnavailable)

        # if (self.CS.cstm_btns.get_button_status("brake")>0):
        #  if ((self.CS.brake_pressed !=0) != self.brake_pressed_prev): #break not canceling when pressed
        #  self.CS.cstm_btns.set_button_status("brake", 2 if self.CS.brake_pressed != 0 else 1)
        # else:
        #  if ret.brakePressed:i
        #    events.add(EventName.pedalPressed)
        # if ret.gasPressed:
        #   events.add(EventName.pedalPressed)

        # it can happen that car cruise disables while comma system is enabled: need to
        # keep braking if needed or if the speed is very low
        if (self.CP.enableCruise and not ret.cruiseState.enabled
                and c.actuators.brake <= 0.0):
            # non loud alert if cruise disbales below 25mph as expected (+ a little margin)
            if ret.vEgo < self.CP.minEnableSpeed + 2.0:
                events.add(EventName.speedTooLow)
            else:
                events.add(EventName.cruiseDisabled)
        if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
            events.add(EventName.manualRestart)

        cur_time = self.frame * DT_CTRL
        enable_pressed = False
        # handle button presses
        for b in ret.buttonEvents:

            # do enable on both accel and decel buttons
            if b.type == "altButton3" and not b.pressed:
                print("enabled pressed at", cur_time)
                self.last_enable_pressed = cur_time
                enable_pressed = True

            # do disable on button down
            if b.type == "cancel" and b.pressed:
                events.add(EventName.buttonCancel)

        if self.CP.enableCruise:
            # KEEP THIS EVENT LAST! send enable event if button is pressed and there are
            # NO_ENTRY events, so controlsd will display alerts. Also not send enable events
            # too close in time, so a no_entry will not be followed by another one.
            # TODO: button press should be the only thing that triggers enble
            if ((cur_time - self.last_enable_pressed) < 0.2 and (  # pylint: disable=chained-comparison
                    cur_time - self.last_enable_sent) > 0.2
                    and ret.cruiseState.enabled) or (enable_pressed and
                                                     events.any(ET.NO_ENTRY)):
                if ret.seatbeltUnlatched:
                    self.CC.DAS_211_accNoSeatBelt = 1
                    self.CC.warningCounter = 300
                    self.CC.warningNeeded = 1
                elif not ret.gearShifter == "drive":
                    self.CC.DAS_222_accCameraBlind = 1
                    self.CC.warningCounter = 300
                    self.CC.warningNeeded = 1
                elif not self.CS.apEnabled:
                    self.CC.DAS_206_apUnavailable = 1
                    self.CC.warningCounter = 300
                    self.CC.warningNeeded = 1
                else:
                    events.add(EventName.buttonEnable)
                self.last_enable_sent = cur_time
        elif enable_pressed:
            if ret.seatbeltUnlatched:
                self.CC.DAS_211_accNoSeatBelt = 1
                self.CC.warningCounter = 300
                self.CC.warningNeeded = 1
            elif not ret.gearShifter == "drive":
                self.CC.DAS_222_accCameraBlind = 1
                self.CC.warningCounter = 300
                self.CC.warningNeeded = 1
            elif not self.CS.apEnabled:
                self.CC.DAS_206_apUnavailable = 1
                self.CC.warningCounter = 300
                self.CC.warningNeeded = 1
            else:
                events.add(EventName.buttonEnable)

        ret.events = events.to_msg()
        ret.canMonoTimes = canMonoTimes

        # update previous brake/gas pressed
        self.gas_pressed_prev = ret.gasPressed
        self.brake_pressed_prev = self.CS.brake_pressed != 0

        # cast to reader so it can't be modified
        return ret.as_reader()
Exemple #14
0
    def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True):
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and (
                extra_gears is None or cs_out.gearShifter not in extra_gears):
            events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        #if cs_out.gasPressed:
        #  events.add(EventName.gasPressed)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)
        if cs_out.cruiseState.nonAdaptive:
            events.add(EventName.wrongCruiseMode)
        #if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
        #  events.add(EventName.brakeHold)

        # Handle permanent and temporary steering faults
        self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
        if cs_out.steerWarning:
            # if the user overrode recently, show a less harsh alert
            if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(
                    1.5 / DT_CTRL):
                self.silent_steer_warning = True
                events.add(EventName.steerTempUnavailableSilent)
            else:
                events.add(EventName.steerTempUnavailable)
        else:
            self.silent_steer_warning = False
        if cs_out.steerError:
            events.add(EventName.steerUnavailable)

        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        if (cs_out.gasPressed and not self.CS.out.gasPressed) or \
           (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
            events.add(EventName.pedalPressed)

        # we engage when pcm is active (rising edge)
        if pcm_enable:
            if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
                events.add(EventName.pcmEnable)
            elif not cs_out.cruiseState.enabled:
                events.add(EventName.pcmDisable)

        return events
Exemple #15
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=[],
                             gas_resume_speed=-1):  # pylint: disable=dangerous-default-value
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears:
            events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)

        if cs_out.steerError:
            events.add(EventName.steerUnavailable)
        elif cs_out.steerWarning:
            events.add(EventName.steerTempUnavailable)

        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        if cs_out.brakePressed and (not self.CS.out.brakePressed
                                    or not cs_out.standstill):
            pass
            #events.add(EventName.pedalPressed)

        if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
            events.add(EventName.pcmEnable)
        elif not cs_out.cruiseState.enabled:
            events.add(EventName.pcmDisable)

        return events
Exemple #16
0
def dmonitoringd_thread(sm=None, pm=None):
    gc.disable()

    # start the loop
    set_realtime_priority(53)

    params = Params()

    # Pub/Sub Sockets
    if pm is None:
        pm = messaging.PubMaster(['dMonitoringState'])

    if sm is None:
        sm = messaging.SubMaster(
            ['driverState', 'liveCalibration', 'carState', 'model'])

    driver_status = DriverStatus()
    is_rhd = params.get("IsRHD")
    if is_rhd is not None:
        driver_status.is_rhd_region = bool(int(is_rhd))
        driver_status.is_rhd_region_checked = True

    sm['liveCalibration'].calStatus = Calibration.INVALID
    sm['carState'].vEgo = 0.
    sm['carState'].cruiseState.enabled = False
    sm['carState'].cruiseState.speed = 0.
    sm['carState'].buttonEvents = []
    sm['carState'].steeringPressed = False
    sm['carState'].gasPressed = False
    sm['carState'].standstill = True

    cal_rpy = [0, 0, 0]
    v_cruise_last = 0
    driver_engaged = False

    # 10Hz <- dmonitoringmodeld
    while True:
        sm.update()

        # Handle calibration
        if sm.updated['liveCalibration']:
            if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
                if len(sm['liveCalibration'].rpyCalib) == 3:
                    cal_rpy = sm['liveCalibration'].rpyCalib

        # Get interaction
        if sm.updated['carState']:
            v_cruise = sm['carState'].cruiseState.speed
            driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
                              v_cruise != v_cruise_last or \
                              sm['carState'].steeringPressed or \
                              sm['carState'].gasPressed
            if driver_engaged:
                driver_status.update(Events(), True,
                                     sm['carState'].cruiseState.enabled,
                                     sm['carState'].standstill)
            v_cruise_last = v_cruise

        # Get model meta
        if sm.updated['model']:
            driver_status.set_policy(sm['model'])

        # Get data from dmonitoringmodeld
        if sm.updated['driverState']:
            events = Events()
            driver_status.get_pose(sm['driverState'], cal_rpy,
                                   sm['carState'].vEgo,
                                   sm['carState'].cruiseState.enabled)
            # Block any engage after certain distrations
            if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
                events.add(car.CarEvent.EventName.tooDistracted)
            # Update events from driver state
            driver_status.update(events, driver_engaged,
                                 sm['carState'].cruiseState.enabled,
                                 sm['carState'].standstill)

            # dMonitoringState packet
            dat = messaging.new_message('dMonitoringState')
            dat.dMonitoringState = {
                "events":
                events.to_msg(),
                "faceDetected":
                driver_status.face_detected,
                "isDistracted":
                driver_status.driver_distracted,
                "awarenessStatus":
                driver_status.awareness,
                "isRHD":
                driver_status.is_rhd_region,
                "rhdChecked":
                driver_status.is_rhd_region_checked,
                "posePitchOffset":
                driver_status.pose.pitch_offseter.filtered_stat.mean(),
                "posePitchValidCount":
                driver_status.pose.pitch_offseter.filtered_stat.n,
                "poseYawOffset":
                driver_status.pose.yaw_offseter.filtered_stat.mean(),
                "poseYawValidCount":
                driver_status.pose.yaw_offseter.filtered_stat.n,
                "stepChange":
                driver_status.step_change,
                "awarenessActive":
                driver_status.awareness_active,
                "awarenessPassive":
                driver_status.awareness_passive,
                "isLowStd":
                driver_status.pose.low_std,
                "hiStdCount":
                driver_status.hi_stds,
                "isPreview":
                False,
            }
            pm.send('dMonitoringState', dat)
Exemple #17
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=[],
                             gas_resume_speed=-1,
                             pcm_enable=True):
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        elif cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        elif cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears:
            events.add(EventName.wrongGear)
        elif cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        elif not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        elif cs_out.espDisabled:
            events.add(EventName.espDisabled)
        elif cs_out.gasPressed and self.CP.longcontrolEnabled:
            events.add(EventName.gasPressed)
        elif cs_out.stockFcw:
            events.add(EventName.stockFcw)
        elif cs_out.stockAeb:
            events.add(EventName.stockAeb)

        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        # Optionally allow to press gas at zero speed to resume.
        # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME!
        if self.CP.longcontrolEnabled:
            if (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \
              (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
                events.add(EventName.pedalPressed)

        # we engage when pcm is active (rising edge)
        #if pcm_enable:
        #  if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
        #    events.add( EventName.pcmEnable )
        #  elif not cs_out.cruiseState.enabled:
        #    events.add( EventName.pcmDisable )

        if not pcm_enable:
            pass
        elif cs_out.gearShifter != GearShifter.drive:
            self.cruise_enabled_prev = cs_out.cruiseState.enabled
            if self.pcm_enable_prev:
                self.pcm_enable_cmd = False
        elif cs_out.cruiseState.enabled != self.cruise_enabled_prev:
            self.cruise_enabled_prev = cs_out.cruiseState.enabled
            if cs_out.cruiseState.enabled:
                self.pcm_enable_cmd = True
            else:
                self.pcm_enable_cmd = False

        if self.pcm_enable_prev != self.pcm_enable_cmd:
            self.pcm_enable_prev = self.pcm_enable_cmd
            if self.pcm_enable_cmd:
                events.add(EventName.pcmEnable)
            else:
                events.add(EventName.pcmDisable)

        return events
Exemple #18
0
class Controls:
    def __init__(self, sm=None, pm=None, can_sock=None):
        config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

        # Setup sockets
        self.pm = pm
        if self.pm is None:
            self.pm = messaging.PubMaster([
                'sendcan', 'controlsState', 'carState', 'carControl',
                'carEvents', 'carParams'
            ])

        self.camera_packets = ["roadCameraState", "driverCameraState"]
        if TICI:
            self.camera_packets.append("wideRoadCameraState")

        params = Params()
        self.joystick_mode = params.get_bool("JoystickDebugMode")
        joystick_packet = ['testJoystick'] if self.joystick_mode else []

        self.sm = sm
        if self.sm is None:
            ignore = ['driverCameraState', 'managerState'
                      ] if SIMULATION else None
            self.sm = messaging.SubMaster(
                [
                    'deviceState', 'pandaStates', 'peripheralState', 'modelV2',
                    'liveCalibration', 'driverMonitoringState',
                    'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
                    'managerState', 'liveParameters', 'radarState'
                ] + self.camera_packets + joystick_packet,
                ignore_alive=ignore,
                ignore_avg_freq=['radarState', 'longitudinalPlan'])

        self.can_sock = can_sock
        if can_sock is None:
            can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
                                                 False) else 100
            self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

        if TICI:
            self.log_sock = messaging.sub_sock('androidLog')

        # wait for one pandaState and one CAN packet
        print("Waiting for CAN messages...")
        get_one_can(self.can_sock)

        self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])

        # read params
        self.is_metric = params.get_bool("IsMetric")
        self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
        community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
        openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
        passive = params.get_bool("Passive") or not openpilot_enabled_toggle

        # detect sound card presence and ensure successful init
        sounds_available = HARDWARE.get_sound_card_online()

        car_recognized = self.CP.carName != 'mock'

        controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
        community_feature = self.CP.communityFeature or \
                            self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
        community_feature_disallowed = community_feature and (
            not community_feature_toggle)
        self.read_only = not car_recognized or not controller_available or \
                           self.CP.dashcamOnly or community_feature_disallowed
        if self.read_only:
            safety_config = car.CarParams.SafetyConfig.new_message()
            safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
            self.CP.safetyConfigs = [safety_config]

        # Write CarParams for radard
        cp_bytes = self.CP.to_bytes()
        params.put("CarParams", cp_bytes)
        put_nonblocking("CarParamsCache", cp_bytes)

        self.CC = car.CarControl.new_message()
        self.AM = AlertManager()
        self.events = Events()

        self.LoC = LongControl(self.CP)
        self.VM = VehicleModel(self.CP)

        if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
            self.LaC = LatControlAngle(self.CP)
        elif self.CP.lateralTuning.which() == 'pid':
            self.LaC = LatControlPID(self.CP, self.CI)
        elif self.CP.lateralTuning.which() == 'indi':
            self.LaC = LatControlINDI(self.CP)
        elif self.CP.lateralTuning.which() == 'lqr':
            self.LaC = LatControlLQR(self.CP)

        self.initialized = False
        self.state = State.disabled
        self.enabled = False
        self.active = False
        self.can_rcv_error = False
        self.soft_disable_timer = 0
        self.v_cruise_kph = 255
        self.v_cruise_kph_last = 0
        self.mismatch_counter = 0
        self.can_error_counter = 0
        self.last_blinker_frame = 0
        self.saturated_count = 0
        self.distance_traveled = 0
        self.last_functional_fan_frame = 0
        self.events_prev = []
        self.current_alert_types = [ET.PERMANENT]
        self.logged_comm_issue = False
        self.button_timers = {
            ButtonEvent.Type.decelCruise: 0,
            ButtonEvent.Type.accelCruise: 0
        }

        # TODO: no longer necessary, aside from process replay
        self.sm['liveParameters'].valid = True

        self.startup_event = get_startup_event(car_recognized,
                                               controller_available,
                                               len(self.CP.carFw) > 0)

        if not sounds_available:
            self.events.add(EventName.soundsUnavailable, static=True)
        if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
            self.events.add(EventName.communityFeatureDisallowed, static=True)
        if not car_recognized:
            self.events.add(EventName.carUnrecognized, static=True)
        elif self.read_only:
            self.events.add(EventName.dashcamMode, static=True)
        elif self.joystick_mode:
            self.events.add(EventName.joystickDebug, static=True)
            self.startup_event = None

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default

    def update_events(self, CS):
        """Compute carEvents from carState"""

        self.events.clear()
        self.events.add_from_msg(CS.events)
        self.events.add_from_msg(self.sm['driverMonitoringState'].events)

        # Handle startup event
        if self.startup_event is not None:
            self.events.add(self.startup_event)
            self.startup_event = None

        # Don't add any more events if not initialized
        if not self.initialized:
            self.events.add(EventName.controlsInitializing)
            return

        # Create events for battery, temperature, disk space, and memory
        if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
           self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
            # at zero percent battery, while discharging, OP should not allowed
            self.events.add(EventName.lowBattery)
        if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
            self.events.add(EventName.overheat)
        if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
            # under 7% of space free no enable allowed
            self.events.add(EventName.outOfSpace)
        # TODO: make tici threshold the same
        if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else
                                                        65) and not SIMULATION:
            self.events.add(EventName.lowMemory)
        cpus = list(
            self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
        if max(cpus, default=0) > 95 and not SIMULATION:
            self.events.add(EventName.highCpuUsage)

        # Alert if fan isn't spinning for 5 seconds
        if self.sm['peripheralState'].pandaType in [
                PandaType.uno, PandaType.dos
        ]:
            if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm[
                    'deviceState'].fanSpeedPercentDesired > 50:
                if (self.sm.frame -
                        self.last_functional_fan_frame) * DT_CTRL > 5.0:
                    self.events.add(EventName.fanMalfunction)
            else:
                self.last_functional_fan_frame = self.sm.frame

        # Handle calibration status
        cal_status = self.sm['liveCalibration'].calStatus
        if cal_status != Calibration.CALIBRATED:
            if cal_status == Calibration.UNCALIBRATED:
                self.events.add(EventName.calibrationIncomplete)
            else:
                self.events.add(EventName.calibrationInvalid)

        # Handle lane change
        if self.sm[
                'lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
            direction = self.sm['lateralPlan'].laneChangeDirection
            if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
               (CS.rightBlindspot and direction == LaneChangeDirection.right):
                self.events.add(EventName.laneChangeBlocked)
            else:
                if direction == LaneChangeDirection.left:
                    self.events.add(EventName.preLaneChangeLeft)
                else:
                    self.events.add(EventName.preLaneChangeRight)
        elif self.sm['lateralPlan'].laneChangeState in [
                LaneChangeState.laneChangeStarting,
                LaneChangeState.laneChangeFinishing
        ]:
            self.events.add(EventName.laneChange)

        if self.can_rcv_error or not CS.canValid:
            self.events.add(EventName.canError)

        for i, pandaState in enumerate(self.sm['pandaStates']):
            # All pandas must match the list of safetyConfigs, and if outside this list, must be silent
            if i < len(self.CP.safetyConfigs):
                safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[
                    i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[
                        i].safetyParam
            else:
                safety_mismatch = pandaState.safetyModel != SafetyModel.silent
            if safety_mismatch or self.mismatch_counter >= 200:
                self.events.add(EventName.controlsMismatch)

        if not self.sm['liveParameters'].valid:
            self.events.add(EventName.vehicleModelInvalid)

        if len(self.sm['radarState'].radarErrors):
            self.events.add(EventName.radarFault)
        elif not self.sm.valid["pandaStates"]:
            self.events.add(EventName.usbError)
        elif not self.sm.all_alive_and_valid():
            self.events.add(EventName.commIssue)
            if not self.logged_comm_issue:
                invalid = [
                    s for s, valid in self.sm.valid.items() if not valid
                ]
                not_alive = [
                    s for s, alive in self.sm.alive.items() if not alive
                ]
                cloudlog.event("commIssue",
                               invalid=invalid,
                               not_alive=not_alive)
                self.logged_comm_issue = True
        else:
            self.logged_comm_issue = False

        if not self.sm['lateralPlan'].mpcSolutionValid:
            self.events.add(EventName.plannerError)
        if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
            if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
                self.events.add(EventName.sensorDataInvalid)
        if not self.sm['liveLocationKalman'].posenetOK:
            self.events.add(EventName.posenetInvalid)
        if not self.sm['liveLocationKalman'].deviceStable:
            self.events.add(EventName.deviceFalling)
        for pandaState in self.sm['pandaStates']:
            if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
                self.events.add(EventName.relayMalfunction)
        planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
        model_fcw = self.sm[
            'modelV2'].meta.hardBrakePredicted and not CS.brakePressed
        if planner_fcw or model_fcw:
            self.events.add(EventName.fcw)

        if TICI:
            logs = messaging.drain_sock(self.log_sock, wait_for_one=False)
            messages = []
            for m in logs:
                try:
                    messages.append(m.androidLog.message)
                except UnicodeDecodeError:
                    pass

            for err in [
                    "ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW",
                    "APPLY FAILED"
            ]:
                for m in messages:
                    if err not in m:
                        continue

                    csid = m.split("CSID:")[-1].split(" ")[0]
                    evt = {
                        "0": EventName.roadCameraError,
                        "1": EventName.wideRoadCameraError,
                        "2": EventName.driverCameraError
                    }.get(csid, None)
                    if evt is not None:
                        self.events.add(evt)

        # TODO: fix simulator
        if not SIMULATION:
            if not NOSENSOR:
                if not self.sm['liveLocationKalman'].gpsOK and (
                        self.distance_traveled > 1000):
                    # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
                    self.events.add(EventName.noGps)
            if not self.sm.all_alive(self.camera_packets):
                self.events.add(EventName.cameraMalfunction)
            if self.sm['modelV2'].frameDropPerc > 20:
                self.events.add(EventName.modeldLagging)
            if self.sm['liveLocationKalman'].excessiveResets:
                self.events.add(EventName.localizerMalfunction)

            # Check if all manager processes are running
            not_running = set(p.name for p in self.sm['managerState'].processes
                              if not p.running)
            if self.sm.rcv_frame['managerState'] and (not_running -
                                                      IGNORE_PROCESSES):
                self.events.add(EventName.processNotRunning)

        # Only allow engagement with brake pressed when stopped behind another stopped car
        speeds = self.sm['longitudinalPlan'].speeds
        if len(speeds) > 1:
            v_future = speeds[-1]
        else:
            v_future = 100.0
        if CS.brakePressed and v_future >= self.CP.vEgoStarting \
          and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
            self.events.add(EventName.noTarget)

    def data_sample(self):
        """Receive data from sockets and update carState"""

        # Update carState from CAN
        can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
        CS = self.CI.update(self.CC, can_strs)

        self.sm.update(0)

        all_valid = CS.canValid and self.sm.all_alive_and_valid()
        if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5
                                     or SIMULATION):
            if not self.read_only:
                self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
            self.initialized = True
            Params().put_bool("ControlsReady", True)

        # Check for CAN timeout
        if not can_strs:
            self.can_error_counter += 1
            self.can_rcv_error = True
        else:
            self.can_rcv_error = False

        # 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 self.enabled:
            self.mismatch_counter = 0

        # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
        for pandaState in self.sm['pandaStates']:
            if pandaState.safetyModel != SafetyModel.silent and not pandaState.controlsAllowed and self.enabled:
                self.mismatch_counter += 1

        self.distance_traveled += CS.vEgo * DT_CTRL

        return CS

    def state_transition(self, CS):
        """Compute conditional state transitions and execute actions on state transitions"""

        self.v_cruise_kph_last = self.v_cruise_kph

        # if stock cruise is completely disabled, then we can use our own set speed logic
        if not self.CP.pcmCruise:
            self.v_cruise_kph = update_v_cruise(self.v_cruise_kph,
                                                CS.buttonEvents,
                                                self.button_timers,
                                                self.enabled, self.is_metric)
        elif self.CP.pcmCruise and CS.cruiseState.enabled:
            self.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
        self.soft_disable_timer = max(0, self.soft_disable_timer - 1)

        self.current_alert_types = [ET.PERMANENT]

        # ENABLED, PRE ENABLING, SOFT DISABLING
        if self.state != State.disabled:
            # user and immediate disable always have priority in a non-disabled state
            if self.events.any(ET.USER_DISABLE):
                self.state = State.disabled
                self.current_alert_types.append(ET.USER_DISABLE)

            elif self.events.any(ET.IMMEDIATE_DISABLE):
                self.state = State.disabled
                self.current_alert_types.append(ET.IMMEDIATE_DISABLE)

            else:
                # ENABLED
                if self.state == State.enabled:
                    if self.events.any(ET.SOFT_DISABLE):
                        self.state = State.softDisabling
                        self.soft_disable_timer = 300  # 3s
                        self.current_alert_types.append(ET.SOFT_DISABLE)

                # SOFT DISABLING
                elif self.state == State.softDisabling:
                    if not self.events.any(ET.SOFT_DISABLE):
                        # no more soft disabling condition, so go back to ENABLED
                        self.state = State.enabled

                    elif self.events.any(
                            ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
                        self.current_alert_types.append(ET.SOFT_DISABLE)

                    elif self.soft_disable_timer <= 0:
                        self.state = State.disabled

                # PRE ENABLING
                elif self.state == State.preEnabled:
                    if not self.events.any(ET.PRE_ENABLE):
                        self.state = State.enabled
                    else:
                        self.current_alert_types.append(ET.PRE_ENABLE)

        # DISABLED
        elif self.state == State.disabled:
            if self.events.any(ET.ENABLE):
                if self.events.any(ET.NO_ENTRY):
                    self.current_alert_types.append(ET.NO_ENTRY)

                else:
                    if self.events.any(ET.PRE_ENABLE):
                        self.state = State.preEnabled
                    else:
                        self.state = State.enabled
                    self.current_alert_types.append(ET.ENABLE)
                    self.v_cruise_kph = initialize_v_cruise(
                        CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)

        # Check if actuators are enabled
        self.active = self.state == State.enabled or self.state == State.softDisabling
        if self.active:
            self.current_alert_types.append(ET.WARNING)

        # Check if openpilot is engaged
        self.enabled = self.active or self.state == State.preEnabled

    def state_control(self, CS):
        """Given the state, this function returns an actuators packet"""

        # Update VehicleModel
        params = self.sm['liveParameters']
        x = max(params.stiffnessFactor, 0.1)
        sr = max(params.steerRatio, 0.1)
        self.VM.update_params(x, sr)

        lat_plan = self.sm['lateralPlan']
        long_plan = self.sm['longitudinalPlan']

        actuators = car.CarControl.Actuators.new_message()
        actuators.longControlState = self.LoC.long_control_state

        if CS.leftBlinker or CS.rightBlinker:
            self.last_blinker_frame = self.sm.frame

        # State specific actions

        if not self.active:
            self.LaC.reset()
            self.LoC.reset(v_pid=CS.vEgo)

        if not self.joystick_mode:
            # accel PID loop
            pid_accel_limits = self.CI.get_pid_accel_limits(
                self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
            actuators.accel = self.LoC.update(self.active, CS, self.CP,
                                              long_plan, pid_accel_limits)

            # Steering PID loop and lateral MPC
            desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(
                self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures,
                lat_plan.curvatureRates)
            actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
                self.active, CS, self.CP, self.VM, params, desired_curvature,
                desired_curvature_rate)
        else:
            lac_log = log.ControlsState.LateralDebugState.new_message()
            if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
                actuators.accel = 4.0 * clip(self.sm['testJoystick'].axes[0],
                                             -1, 1)

                steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
                # max angle is 45 for angle-based cars
                actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.

                lac_log.active = True
                lac_log.steeringAngleDeg = CS.steeringAngleDeg
                lac_log.output = steer
                lac_log.saturated = abs(steer) >= 0.9

        # Check for difference between desired angle and angle for angle based control
        angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
          abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD

        if angle_control_saturated and not CS.steeringPressed and self.active:
            self.saturated_count += 1
        else:
            self.saturated_count = 0

        # Send a "steering required alert" if saturation count has reached the limit
        if (lac_log.saturated and not CS.steeringPressed) or \
           (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):

            if len(lat_plan.dPathPoints):
                # Check if we deviated from the path
                left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
                    0] < -0.1
                right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
                    0] > 0.1

                if left_deviation or right_deviation:
                    self.events.add(EventName.steerSaturated)

        # Ensure no NaNs/Infs
        for p in ACTUATOR_FIELDS:
            attr = getattr(actuators, p)
            if not isinstance(attr, Number):
                continue

            if not math.isfinite(attr):
                cloudlog.error(
                    f"actuators.{p} not finite {actuators.to_dict()}")
                setattr(actuators, p, 0.0)

        return actuators, lac_log

    def update_button_timers(self, buttonEvents):
        # increment timer for buttons still pressed
        for k in self.button_timers.keys():
            if self.button_timers[k] > 0:
                self.button_timers[k] += 1

        for b in buttonEvents:
            if b.type.raw in self.button_timers:
                self.button_timers[b.type.raw] = 1 if b.pressed else 0

    def publish_logs(self, CS, start_time, actuators, lac_log):
        """Send actuators and hud commands to the car, send controlsstate and MPC logging"""

        CC = car.CarControl.new_message()
        CC.enabled = self.enabled
        CC.active = self.active
        CC.actuators = actuators

        CC.cruiseControl.cancel = CS.cruiseState.enabled and (
            not self.enabled or not self.CP.pcmCruise)
        if self.joystick_mode and self.sm.rcv_frame[
                'testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
            CC.cruiseControl.cancel = True

        CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
        CC.hudControl.speedVisible = self.enabled
        CC.hudControl.lanesVisible = self.enabled
        CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead

        right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
        left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
        CC.hudControl.rightLaneVisible = bool(right_lane_visible)
        CC.hudControl.leftLaneVisible = bool(left_lane_visible)

        recent_blinker = (self.sm.frame - self.last_blinker_frame
                          ) * DT_CTRL < 5.0  # 5s blinker cooldown
        ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
                        and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

        meta = self.sm['modelV2'].meta
        if len(meta.desirePrediction) and ldw_allowed:
            l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
                                                       1]
            r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
                                                       1]
            l_lane_close = left_lane_visible and (
                self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
            r_lane_close = right_lane_visible and (
                self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))

            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:
            self.events.add(EventName.ldw)

        clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
        alerts = self.events.create_alerts(self.current_alert_types,
                                           [self.CP, self.sm, self.is_metric])
        self.AM.add_many(self.sm.frame, alerts, self.enabled)
        self.AM.process_alerts(self.sm.frame, clear_event)
        CC.hudControl.visualAlert = self.AM.visual_alert

        if not self.read_only and self.initialized:
            # send car controls over can
            can_sends = self.CI.apply(CC)
            self.pm.send(
                'sendcan',
                can_list_to_can_capnp(can_sends,
                                      msgtype='sendcan',
                                      valid=CS.canValid))

        force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
                      (self.state == State.softDisabling)

        # Curvature & Steering angle
        params = self.sm['liveParameters']
        steer_angle_without_offset = math.radians(CS.steeringAngleDeg -
                                                  params.angleOffsetAverageDeg)
        curvature = -self.VM.calc_curvature(steer_angle_without_offset,
                                            CS.vEgo)

        # controlsState
        dat = messaging.new_message('controlsState')
        dat.valid = CS.canValid
        controlsState = dat.controlsState
        controlsState.alertText1 = self.AM.alert_text_1
        controlsState.alertText2 = self.AM.alert_text_2
        controlsState.alertSize = self.AM.alert_size
        controlsState.alertStatus = self.AM.alert_status
        controlsState.alertBlinkingRate = self.AM.alert_rate
        controlsState.alertType = self.AM.alert_type
        controlsState.alertSound = self.AM.audible_alert
        controlsState.canMonoTimes = list(CS.canMonoTimes)
        controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
            'longitudinalPlan']
        controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
        controlsState.enabled = self.enabled
        controlsState.active = self.active
        controlsState.curvature = curvature
        controlsState.state = self.state
        controlsState.engageable = not self.events.any(ET.NO_ENTRY)
        controlsState.longControlState = self.LoC.long_control_state
        controlsState.vPid = float(self.LoC.v_pid)
        controlsState.vCruise = float(self.v_cruise_kph)
        controlsState.upAccelCmd = float(self.LoC.pid.p)
        controlsState.uiAccelCmd = float(self.LoC.pid.i)
        controlsState.ufAccelCmd = float(self.LoC.pid.f)
        controlsState.cumLagMs = -self.rk.remaining * 1000.
        controlsState.startMonoTime = int(start_time * 1e9)
        controlsState.forceDecel = bool(force_decel)
        controlsState.canErrorCounter = self.can_error_counter

        if self.joystick_mode:
            controlsState.lateralControlState.debugState = lac_log
        elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
            controlsState.lateralControlState.angleState = lac_log
        elif self.CP.lateralTuning.which() == 'pid':
            controlsState.lateralControlState.pidState = lac_log
        elif self.CP.lateralTuning.which() == 'lqr':
            controlsState.lateralControlState.lqrState = lac_log
        elif self.CP.lateralTuning.which() == 'indi':
            controlsState.lateralControlState.indiState = lac_log
        self.pm.send('controlsState', dat)

        # carState
        car_events = self.events.to_msg()
        cs_send = messaging.new_message('carState')
        cs_send.valid = CS.canValid
        cs_send.carState = CS
        cs_send.carState.events = car_events
        self.pm.send('carState', cs_send)

        # carEvents - logged every second or on change
        if (self.sm.frame % int(1. / DT_CTRL)
                == 0) or (self.events.names != self.events_prev):
            ce_send = messaging.new_message('carEvents', len(self.events))
            ce_send.carEvents = car_events
            self.pm.send('carEvents', ce_send)
        self.events_prev = self.events.names.copy()

        # carParams - logged every 50 seconds (> 1 per segment)
        if (self.sm.frame % int(50. / DT_CTRL) == 0):
            cp_send = messaging.new_message('carParams')
            cp_send.carParams = self.CP
            self.pm.send('carParams', cp_send)

        # carControl
        cc_send = messaging.new_message('carControl')
        cc_send.valid = CS.canValid
        cc_send.carControl = CC
        self.pm.send('carControl', cc_send)

        # copy CarControl to pass to CarInterface on the next iteration
        self.CC = CC

    def step(self):
        start_time = sec_since_boot()
        self.prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data from sockets and get a carState
        CS = self.data_sample()
        self.prof.checkpoint("Sample")

        self.update_events(CS)

        if not self.read_only and self.initialized:
            # Update control state
            self.state_transition(CS)
            self.prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, lac_log = self.state_control(CS)

        self.prof.checkpoint("State Control")

        # Publish data
        self.publish_logs(CS, start_time, actuators, lac_log)
        self.prof.checkpoint("Sent")

        self.update_button_timers(CS.buttonEvents)

    def controlsd_thread(self):
        while True:
            self.step()
            self.rk.monitor_time()
            self.prof.display()
Exemple #19
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=None,
                             pcm_enable=True,
                             allow_enable=True):
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and (
                extra_gears is None or cs_out.gearShifter not in extra_gears):
            events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)
        if cs_out.cruiseState.nonAdaptive:
            events.add(EventName.wrongCruiseMode)
        if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
            events.add(EventName.brakeHold)
        if cs_out.parkingBrake:
            events.add(EventName.parkBrake)
        if cs_out.accFaulted:
            events.add(EventName.accFaulted)

        # Handle permanent and temporary steering faults
        self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
        if cs_out.steerFaultTemporary:
            # if the user overrode recently, show a less harsh alert
            if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(
                    1.5 / DT_CTRL):
                self.silent_steer_warning = True
                events.add(EventName.steerTempUnavailableSilent)
            else:
                events.add(EventName.steerTempUnavailable)
        else:
            self.silent_steer_warning = False
        if cs_out.steerFaultPermanent:
            events.add(EventName.steerUnavailable)

        # we engage when pcm is active (rising edge)
        # enabling can optionally be blocked by the car interface
        if pcm_enable:
            if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
                events.add(EventName.pcmEnable)
            elif not cs_out.cruiseState.enabled:
                events.add(EventName.pcmDisable)

        return events
Exemple #20
0
def cycle_alerts(duration=200, is_metric=False):
    # all alerts
    #alerts = list(EVENTS.keys())

    # this plays each type of audible alert
    alerts = [
        (EventName.buttonEnable, ET.ENABLE),
        (EventName.buttonCancel, ET.USER_DISABLE),
        (EventName.wrongGear, ET.NO_ENTRY),
        (EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
        (EventName.accFaulted, ET.IMMEDIATE_DISABLE),

        # DM sequence
        (EventName.preDriverDistracted, ET.WARNING),
        (EventName.promptDriverDistracted, ET.WARNING),
        (EventName.driverDistracted, ET.WARNING),
    ]

    # debug alerts
    alerts = [
        #(EventName.highCpuUsage, ET.NO_ENTRY),
        #(EventName.lowMemory, ET.PERMANENT),
        #(EventName.overheat, ET.PERMANENT),
        #(EventName.outOfSpace, ET.PERMANENT),
        #(EventName.modeldLagging, ET.PERMANENT),
        #(EventName.processNotRunning, ET.NO_ENTRY),
        #(EventName.commIssue, ET.NO_ENTRY),
        #(EventName.calibrationInvalid, ET.PERMANENT),
        (EventName.cameraMalfunction, ET.PERMANENT),
        (EventName.cameraFrameRate, ET.PERMANENT),
    ]

    cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']

    CS = car.CarState.new_message()
    CP = CarInterface.get_params("HONDA CIVIC 2016")
    sm = messaging.SubMaster([
        'deviceState', 'pandaStates', 'roadCameraState', 'modelV2',
        'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
        'lateralPlan', 'liveLocationKalman', 'managerState'
    ] + cameras)

    pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])

    events = Events()
    AM = AlertManager()

    frame = 0
    while True:
        for alert, et in alerts:
            events.clear()
            events.add(alert)

            sm['deviceState'].freeSpacePercent = randperc()
            sm['deviceState'].memoryUsagePercent = int(randperc())
            sm['deviceState'].cpuTempC = [randperc() for _ in range(3)]
            sm['deviceState'].gpuTempC = [randperc() for _ in range(3)]
            sm['deviceState'].cpuUsagePercent = [
                int(randperc()) for _ in range(8)
            ]
            sm['modelV2'].frameDropPerc = randperc()

            if random.random() > 0.25:
                sm['modelV2'].velocity.x = [
                    random.random(),
                ]
            if random.random() > 0.25:
                CS.vEgo = random.random()

            procs = [
                p.get_process_state_msg() for p in managed_processes.values()
            ]
            random.shuffle(procs)
            for i in range(random.randint(0, 10)):
                procs[i].shouldBeRunning = True
            sm['managerState'].processes = procs

            sm['liveCalibration'].rpyCalib = [
                -1 * random.random() for _ in range(random.randint(0, 3))
            ]

            for s in sm.data.keys():
                prob = 0.3 if s in cameras else 0.08
                sm.alive[s] = random.random() > prob
                sm.valid[s] = random.random() > prob
                sm.freq_ok[s] = random.random() > prob

            a = events.create_alerts([
                et,
            ], [CP, CS, sm, is_metric, 0])
            AM.add_many(frame, a)
            alert = AM.process_alerts(frame, [])
            print(alert)
            for _ in range(duration):
                dat = messaging.new_message()
                dat.init('controlsState')
                dat.controlsState.enabled = False

                if alert:
                    dat.controlsState.alertText1 = alert.alert_text_1
                    dat.controlsState.alertText2 = alert.alert_text_2
                    dat.controlsState.alertSize = alert.alert_size
                    dat.controlsState.alertStatus = alert.alert_status
                    dat.controlsState.alertBlinkingRate = alert.alert_rate
                    dat.controlsState.alertType = alert.alert_type
                    dat.controlsState.alertSound = alert.audible_alert
                pm.send('controlsState', dat)

                dat = messaging.new_message()
                dat.init('deviceState')
                dat.deviceState.started = True
                pm.send('deviceState', dat)

                dat = messaging.new_message('pandaStates', 1)
                dat.pandaStates[0].ignitionLine = True
                dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
                pm.send('pandaStates', dat)

                frame += 1
                time.sleep(DT_CTRL)
Exemple #21
0
class Controls:
    def __init__(self, sm=None, pm=None, can_sock=None):
        config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

        # Setup sockets
        self.pm = pm
        if self.pm is None:
            self.pm = messaging.PubMaster([
                'sendcan', 'controlsState', 'carState', 'carControl',
                'carEvents', 'carParams'
            ])

        self.sm = sm
        if self.sm is None:
            ignore = ['driverCameraState', 'managerState'
                      ] if SIMULATION else None
            self.sm = messaging.SubMaster([
                'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
                'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
                'liveLocationKalman', 'roadCameraState', 'driverCameraState',
                'managerState', 'liveParameters', 'radarState'
            ],
                                          ignore_alive=ignore)

        self.can_sock = can_sock
        if can_sock is None:
            can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
                                                 False) else 100
            self.can_sock = messaging.sub_sock('can', timeout=can_timeout)

        # wait for one pandaState and one CAN packet
        hw_type = messaging.recv_one(
            self.sm.sock['pandaState']).pandaState.pandaType
        has_relay = hw_type in [
            PandaType.blackPanda, PandaType.uno, PandaType.dos
        ]
        print("Waiting for CAN messages...")
        get_one_can(self.can_sock)

        self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
                                   has_relay)

        # read params
        params = Params()
        self.is_metric = params.get_bool("IsMetric")
        self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
        self.enable_lte_onroad = params.get_bool("EnableLteOnroad")
        community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
        openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
        passive = params.get_bool("Passive") or not openpilot_enabled_toggle

        # detect sound card presence and ensure successful init
        sounds_available = HARDWARE.get_sound_card_online()

        car_recognized = self.CP.carName != 'mock'
        # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
        controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
        community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
        self.read_only = not car_recognized or not controller_available or \
                           self.CP.dashcamOnly or community_feature_disallowed
        if self.read_only:
            self.CP.safetyModel = car.CarParams.SafetyModel.noOutput

        # Write CarParams for radard and boardd safety mode
        cp_bytes = self.CP.to_bytes()
        params.put("CarParams", cp_bytes)
        put_nonblocking("CarParamsCache", cp_bytes)

        self.CC = car.CarControl.new_message()
        self.AM = AlertManager()
        self.events = Events()

        self.LoC = LongControl(self.CP, self.CI.compute_gb)
        self.VM = VehicleModel(self.CP)

        if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
            self.LaC = LatControlAngle(self.CP)
        elif self.CP.lateralTuning.which() == 'pid':
            self.LaC = LatControlPID(self.CP)
        elif self.CP.lateralTuning.which() == 'indi':
            self.LaC = LatControlINDI(self.CP)
        elif self.CP.lateralTuning.which() == 'lqr':
            self.LaC = LatControlLQR(self.CP)

        self.state = State.disabled
        self.enabled = False
        self.active = False
        self.can_rcv_error = False
        self.soft_disable_timer = 0
        self.v_cruise_kph = 255
        self.v_cruise_kph_last = 0
        self.mismatch_counter = 0
        self.can_error_counter = 0
        self.last_blinker_frame = 0
        self.saturated_count = 0
        self.distance_traveled = 0
        self.last_functional_fan_frame = 0
        self.events_prev = []
        self.current_alert_types = [ET.PERMANENT]
        self.logged_comm_issue = False
        self.angle_steers_des = 0.
        self.road_limit_speed = 0
        self.road_limit_left_dist = 0
        self.v_cruise_kph_limit = 0
        self.curve_speed_ms = 255.

        self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
        self.sm['deviceState'].freeSpacePercent = 100
        self.sm['driverMonitoringState'].events = []
        self.sm['driverMonitoringState'].awarenessStatus = 1.
        self.sm['driverMonitoringState'].faceDetected = False
        self.sm['liveParameters'].valid = True

        self.startup_event = get_startup_event(car_recognized,
                                               controller_available, hw_type)

        if not sounds_available:
            self.events.add(EventName.soundsUnavailable, static=True)
        if community_feature_disallowed:
            self.events.add(EventName.communityFeatureDisallowed, static=True)
        if not car_recognized:
            self.events.add(EventName.carUnrecognized, static=True)
#    if hw_type == PandaType.greyPanda:
#      self.events.add(EventName.startupGreyPanda, static=True)
        elif self.read_only:
            self.events.add(EventName.dashcamMode, static=True)

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default

    def update_events(self, CS):
        """Compute carEvents from carState"""

        self.events.clear()
        self.events.add_from_msg(CS.events)
        self.events.add_from_msg(self.sm['driverMonitoringState'].events)

        # Handle startup event
        if self.startup_event is not None:
            self.events.add(self.startup_event)
            self.startup_event = None

        # Create events for battery, temperature, disk space, and memory
        if self.sm['deviceState'].batteryPercent < 1 and self.sm[
                'deviceState'].chargingError:
            # at zero percent battery, while discharging, OP should not allowed
            self.events.add(EventName.lowBattery)
        if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
            self.events.add(EventName.overheat)
        if self.sm['deviceState'].freeSpacePercent < 7:
            # under 7% of space free no enable allowed
            self.events.add(EventName.outOfSpace)
        if self.sm['deviceState'].memoryUsagePercent > 90:
            self.events.add(EventName.lowMemory)

        # Alert if fan isn't spinning for 5 seconds
        if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
            if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm[
                    'deviceState'].fanSpeedPercentDesired > 50:
                if (self.sm.frame -
                        self.last_functional_fan_frame) * DT_CTRL > 5.0:
                    self.events.add(EventName.fanMalfunction)
            else:
                self.last_functional_fan_frame = self.sm.frame

        # Handle calibration status
        cal_status = self.sm['liveCalibration'].calStatus
        if cal_status != Calibration.CALIBRATED:
            if cal_status == Calibration.UNCALIBRATED:
                self.events.add(EventName.calibrationIncomplete)
            else:
                self.events.add(EventName.calibrationInvalid)

        # Handle lane change
        if self.sm[
                'lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
            direction = self.sm['lateralPlan'].laneChangeDirection
            if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
               (CS.rightBlindspot and direction == LaneChangeDirection.right):
                self.events.add(EventName.laneChangeBlocked)
            else:
                if direction == LaneChangeDirection.left:
                    self.events.add(EventName.preLaneChangeLeft)
                else:
                    self.events.add(EventName.preLaneChangeRight)
        elif self.sm['lateralPlan'].laneChangeState in [
                LaneChangeState.laneChangeStarting,
                LaneChangeState.laneChangeFinishing
        ]:
            self.events.add(EventName.laneChange)

        if self.can_rcv_error or (not CS.canValid
                                  and self.sm.frame > 5 / DT_CTRL):
            self.events.add(EventName.canError)

        safety_mismatch = self.sm[
            'pandaState'].safetyModel != self.CP.safetyModel
        safety_mismatch = safety_mismatch or self.sm[
            'pandaState'].safetyParam != self.CP.safetyParam
        if (safety_mismatch and
                self.sm.frame > 2 / DT_CTRL) or self.mismatch_counter >= 200:
            self.events.add(EventName.controlsMismatch)

        if not self.sm['liveParameters'].valid:
            self.events.add(EventName.vehicleModelInvalid)

        if len(self.sm['radarState'].radarErrors):
            self.events.add(EventName.radarFault)
        #elif not self.sm.all_alive_and_valid():
        #self.events.add(EventName.commIssue)
        #if not self.logged_comm_issue:
        #cloudlog.error(f"commIssue - valid: {self.sm.valid} - alive: {self.sm.alive}")
        #self.logged_comm_issue = True
        else:
            self.logged_comm_issue = False

        if not self.sm['lateralPlan'].mpcSolutionValid:
            self.events.add(EventName.plannerError)
        if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
            if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
                self.events.add(EventName.sensorDataInvalid)
        if not self.sm['liveLocationKalman'].posenetOK:
            self.events.add(EventName.posenetInvalid)
        if not self.sm['liveLocationKalman'].deviceStable:
            self.events.add(EventName.deviceFalling)
        if log.PandaState.FaultType.relayMalfunction in self.sm[
                'pandaState'].faults:
            self.events.add(EventName.relayMalfunction)
        if self.sm['longitudinalPlan'].fcw:
            self.events.add(EventName.fcw)

        # TODO: fix simulator
        if not SIMULATION:
            #if not NOSENSOR:
            #if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and \
            #(not TICI or self.enable_lte_onroad):
            # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
            #self.events.add(EventName.noGps)
            if not self.sm.all_alive(['roadCameraState', 'driverCameraState'
                                      ]) and (self.sm.frame > 5 / DT_CTRL):
                self.events.add(EventName.cameraMalfunction)
            if self.sm['modelV2'].frameDropPerc > 20:
                self.events.add(EventName.modeldLagging)

            # Check if all manager processes are running
            not_running = set(p.name for p in self.sm['managerState'].processes
                              if not p.running)
            if self.sm.rcv_frame['managerState'] and (not_running -
                                                      IGNORE_PROCESSES):
                self.events.add(EventName.processNotRunning)

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
          and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
            self.events.add(EventName.noTarget)

    def data_sample(self):
        """Receive data from sockets and update carState"""

        # Update carState from CAN
        can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
        CS = self.CI.update(self.CC, can_strs)

        self.sm.update(0)

        # Check for CAN timeout
        if not can_strs:
            self.can_error_counter += 1
            self.can_rcv_error = True
        else:
            self.can_rcv_error = False

        # 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 self.enabled:
            self.mismatch_counter = 0

        if not self.sm['pandaState'].controlsAllowed and self.enabled:
            self.mismatch_counter += 1

        self.distance_traveled += CS.vEgo * DT_CTRL

        return CS

    def cal_curve_speed(self, sm, v_ego, frame):

        if frame % 10 == 0:
            md = sm['modelV2']
            if md is not None and len(
                    md.position.x) == TRAJECTORY_SIZE and len(
                        md.position.y) == TRAJECTORY_SIZE:
                x = md.position.x
                y = md.position.y
                dy = np.gradient(y, x)
                d2y = np.gradient(dy, x)
                curv = d2y / (1 + dy**2)**1.5
                curv = curv[5:TRAJECTORY_SIZE - 10]
                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.mean(v_curvature) * 0.9

                if model_speed < v_ego:
                    self.curve_speed_ms = float(
                        max(model_speed, 32. * CV.KPH_TO_MS))
                else:
                    self.curve_speed_ms = 255.

                if np.isnan(self.curve_speed_ms):
                    self.curve_speed_ms = 255.
            else:
                self.curve_speed_ms = 255.

        return self.curve_speed_ms

    def state_transition(self, CS):
        """Compute conditional state transitions and execute actions on state transitions"""

        self.v_cruise_kph_last = self.v_cruise_kph

        # if stock cruise is completely disabled, then we can use our own set speed logic
        if not self.CP.enableCruise:
            self.v_cruise_kph = update_v_cruise(self.v_cruise_kph,
                                                CS.buttonEvents, self.enabled)
        elif self.CP.enableCruise and CS.cruiseState.enabled:
            self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH

        curv_speed_ms = self.cal_curve_speed(self.sm, CS.vEgo, self.sm.frame)
        self.v_cruise_kph = min(self.v_cruise_kph,
                                curv_speed_ms * CV.MS_TO_KPH)

        limit_speed, self.road_limit_speed, self.road_limit_left_dist, first_started, log = road_speed_limiter_get_max_speed(
            CS, self.v_cruise_kph)

        if limit_speed > 20:
            self.v_cruise_kph_limit = min(limit_speed, self.v_cruise_kph)

            if limit_speed < CS.vEgo * CV.MS_TO_KPH:
                self.events.add(EventName.slowingDownSpeed)

        else:
            self.v_cruise_kph_limit = self.v_cruise_kph

        # decrease the soft disable timer at every step, as it's reset on
        # entrance in SOFT_DISABLING state
        self.soft_disable_timer = max(0, self.soft_disable_timer - 1)

        self.current_alert_types = [ET.PERMANENT]

        # ENABLED, PRE ENABLING, SOFT DISABLING
        if self.state != State.disabled:
            # user and immediate disable always have priority in a non-disabled state
            if self.events.any(ET.USER_DISABLE):
                self.state = State.disabled
                self.current_alert_types.append(ET.USER_DISABLE)

            elif self.events.any(ET.IMMEDIATE_DISABLE):
                self.state = State.disabled
                self.current_alert_types.append(ET.IMMEDIATE_DISABLE)

            else:
                # ENABLED
                if self.state == State.enabled:
                    if self.events.any(ET.SOFT_DISABLE):
                        self.state = State.softDisabling
                        self.soft_disable_timer = 300  # 3s
                        self.current_alert_types.append(ET.SOFT_DISABLE)

                # SOFT DISABLING
                elif self.state == State.softDisabling:
                    if not self.events.any(ET.SOFT_DISABLE):
                        # no more soft disabling condition, so go back to ENABLED
                        self.state = State.enabled

                    elif self.events.any(
                            ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
                        self.current_alert_types.append(ET.SOFT_DISABLE)

                    elif self.soft_disable_timer <= 0:
                        self.state = State.disabled

                # PRE ENABLING
                elif self.state == State.preEnabled:
                    if not self.events.any(ET.PRE_ENABLE):
                        self.state = State.enabled
                    else:
                        self.current_alert_types.append(ET.PRE_ENABLE)

        # DISABLED
        elif self.state == State.disabled:
            if self.events.any(ET.ENABLE):
                if self.events.any(ET.NO_ENTRY):
                    self.current_alert_types.append(ET.NO_ENTRY)

                else:
                    if self.events.any(ET.PRE_ENABLE):
                        self.state = State.preEnabled
                    else:
                        self.state = State.enabled
                    self.current_alert_types.append(ET.ENABLE)
                    self.v_cruise_kph = initialize_v_cruise(
                        CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)

        # Check if actuators are enabled
        self.active = self.state == State.enabled or self.state == State.softDisabling
        if self.active:
            self.current_alert_types.append(ET.WARNING)

        # Check if openpilot is engaged
        self.enabled = self.active or self.state == State.preEnabled

    def state_control(self, CS):
        """Given the state, this function returns an actuators packet"""

        # Neokii's live tune
        # Update VehicleModel
        params = self.sm['liveParameters']
        x = max(params.stiffnessFactor, 0.1)
        #sr = max(params.steerRatio, 0.1)

        if ntune_isEnabled('useLiveSteerRatio'):
            sr = max(self.sm['liveParameters'].steerRatio, 0.1)
        else:
            if self.CP.carName in [CAR.VOLT]:
                sr = interp(abs(self.angle_steers_des), [5., 35.],
                            [13.5, 17.7])
            else:
                sr = max(ntune_get('steerRatio'), 0.1)

        self.VM.update_params(x, sr)

        lat_plan = self.sm['lateralPlan']
        long_plan = self.sm['longitudinalPlan']

        actuators = car.CarControl.Actuators.new_message()

        if CS.leftBlinker or CS.rightBlinker:
            self.last_blinker_frame = self.sm.frame

        # State specific actions

        if not self.active:
            self.LaC.reset()
            self.LoC.reset(v_pid=CS.vEgo)

        long_plan_age = DT_CTRL * (self.sm.frame -
                                   self.sm.rcv_frame['longitudinalPlan'])
        # no greater than dt mpc + dt, to prevent too high extraps
        dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL

        a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (
            long_plan.aTarget - long_plan.aStart)
        v_acc_sol = long_plan.vStart + dt * (a_acc_sol +
                                             long_plan.aStart) / 2.0

        # Gas/Brake PID loop
        actuators.gas, actuators.brake = self.LoC.update(
            self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol,
            self.CP)

        # Steering PID loop and lateral MPC
        actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
            self.active, CS, self.CP, self.VM, params, lat_plan)

        # Check for difference between desired angle and angle for angle based control
        angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
          abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD

        if angle_control_saturated and not CS.steeringPressed and self.active:
            self.saturated_count += 1
        else:
            self.saturated_count = 0

        # Send a "steering required alert" if saturation count has reached the limit
        if (lac_log.saturated and not CS.steeringPressed) or \
           (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):

            if len(lat_plan.dPathPoints):
                # Check if we deviated from the path
                left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
                    0] < -0.1
                right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
                    0] > 0.1


#  Bellow 2Lines' notations are for disable Alerts
#        if left_deviation or right_deviation:
#          self.events.add(EventName.steerSaturated)

        return actuators, v_acc_sol, a_acc_sol, lac_log

    def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
        """Send actuators and hud commands to the car, send controlsstate and MPC logging"""

        CC = car.CarControl.new_message()
        CC.enabled = self.enabled
        CC.actuators = actuators

        CC.cruiseControl.override = True
        CC.cruiseControl.cancel = not self.CP.enableCruise or (
            not self.enabled and CS.cruiseState.enabled)

        # Some override values for Honda
        # brake discount removes a sharp nonlinearity
        brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
        speed_override = max(0.0,
                             (self.LoC.v_pid + CS.cruiseState.speedOffset) *
                             brake_discount)
        CC.cruiseControl.speedOverride = float(
            speed_override if self.CP.enableCruise else 0.0)
        CC.cruiseControl.accelOverride = self.CI.calc_accel_override(
            CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo,
            self.sm['longitudinalPlan'].vTarget)

        CC.hudControl.setSpeed = float(self.v_cruise_kph_limit * CV.KPH_TO_MS)
        CC.hudControl.speedVisible = self.enabled
        CC.hudControl.lanesVisible = self.enabled
        CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead

        right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
        left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
        CC.hudControl.rightLaneVisible = bool(right_lane_visible)
        CC.hudControl.leftLaneVisible = bool(left_lane_visible)

        recent_blinker = (self.sm.frame - self.last_blinker_frame
                          ) * DT_CTRL < 5.0  # 5s blinker cooldown
        ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
                        and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

        meta = self.sm['modelV2'].meta
        if len(meta.desirePrediction) and ldw_allowed:
            l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
                                                       1]
            r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
                                                       1]

            cameraOffset = ntune_get("cameraOffset")
            l_lane_close = left_lane_visible and (
                self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + cameraOffset))
            r_lane_close = right_lane_visible and (
                self.sm['modelV2'].laneLines[2].y[0] < (1.08 - cameraOffset))

            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:
            self.events.add(EventName.ldw)

        clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
        alerts = self.events.create_alerts(self.current_alert_types,
                                           [self.CP, self.sm, self.is_metric])
        self.AM.add_many(self.sm.frame, alerts, self.enabled)
        self.AM.process_alerts(self.sm.frame, clear_event)
        CC.hudControl.visualAlert = self.AM.visual_alert

        if not self.read_only:
            # send car controls over can
            can_sends = self.CI.apply(CC)
            self.pm.send(
                'sendcan',
                can_list_to_can_capnp(can_sends,
                                      msgtype='sendcan',
                                      valid=CS.canValid))

        force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
                      (self.state == State.softDisabling)

        # Curvature & Steering angle
        params = self.sm['liveParameters']
        lat_plan = self.sm['lateralPlan']

        steer_angle_without_offset = math.radians(CS.steeringAngleDeg -
                                                  params.angleOffsetAverageDeg)
        curvature = -self.VM.calc_curvature(steer_angle_without_offset,
                                            CS.vEgo)
        self.angle_steers_des = math.degrees(
            self.VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
        self.angle_steers_des += params.angleOffsetDeg

        # controlsState
        dat = messaging.new_message('controlsState')
        dat.valid = CS.canValid
        controlsState = dat.controlsState
        controlsState.alertText1 = self.AM.alert_text_1
        controlsState.alertText2 = self.AM.alert_text_2
        controlsState.alertSize = self.AM.alert_size
        controlsState.alertStatus = self.AM.alert_status
        controlsState.alertBlinkingRate = self.AM.alert_rate
        controlsState.alertType = self.AM.alert_type
        controlsState.alertSound = self.AM.audible_alert
        controlsState.canMonoTimes = list(CS.canMonoTimes)
        controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime[
            'longitudinalPlan']
        controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
        controlsState.enabled = self.enabled
        controlsState.active = self.active

        #   bellow 3Lines are for Wheel Rotation
        controlsState.vEgo = CS.vEgo
        controlsState.vEgoRaw = CS.vEgoRaw
        controlsState.steerOverride = CS.steeringPressed

        #   controlsState.angleSteers = CS.steeringAngleDeg
        controlsState.curvature = curvature
        controlsState.steeringAngleDesiredDeg = self.angle_steers_des
        controlsState.state = self.state
        controlsState.engageable = not self.events.any(ET.NO_ENTRY)
        controlsState.longControlState = self.LoC.long_control_state
        controlsState.vPid = float(self.LoC.v_pid)
        controlsState.vCruise = float(self.v_cruise_kph_limit)
        controlsState.upAccelCmd = float(self.LoC.pid.p)
        controlsState.uiAccelCmd = float(self.LoC.pid.i)
        controlsState.ufAccelCmd = float(self.LoC.pid.f)
        controlsState.vTargetLead = float(v_acc)
        controlsState.aTarget = float(a_acc)
        controlsState.cumLagMs = -self.rk.remaining * 1000.
        controlsState.startMonoTime = int(start_time * 1e9)
        controlsState.forceDecel = bool(force_decel)
        controlsState.canErrorCounter = self.can_error_counter
        controlsState.angleSteers = steer_angle_without_offset * CV.RAD_TO_DEG
        controlsState.roadLimitSpeed = self.road_limit_speed
        controlsState.roadLimitSpeedLeftDist = self.road_limit_left_dist

        # display SR/SRC/SAD on Ui
        controlsState.steerRatio = self.VM.sR
        controlsState.steerRateCost = ntune_get('steerRateCost')
        controlsState.steerActuatorDelay = ntune_get('steerActuatorDelay')

        if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
            controlsState.lateralControlState.angleState = lac_log
        elif self.CP.lateralTuning.which() == 'pid':
            controlsState.lateralControlState.pidState = lac_log
        elif self.CP.lateralTuning.which() == 'lqr':
            controlsState.lateralControlState.lqrState = lac_log
        elif self.CP.lateralTuning.which() == 'indi':
            controlsState.lateralControlState.indiState = lac_log
        self.pm.send('controlsState', dat)

        # carState
        car_events = self.events.to_msg()
        cs_send = messaging.new_message('carState')
        cs_send.valid = CS.canValid
        cs_send.carState = CS
        cs_send.carState.events = car_events
        self.pm.send('carState', cs_send)

        # carEvents - logged every second or on change
        if (self.sm.frame % int(1. / DT_CTRL)
                == 0) or (self.events.names != self.events_prev):
            ce_send = messaging.new_message('carEvents', len(self.events))
            ce_send.carEvents = car_events
            self.pm.send('carEvents', ce_send)
        self.events_prev = self.events.names.copy()

        # carParams - logged every 50 seconds (> 1 per segment)
        if (self.sm.frame % int(50. / DT_CTRL) == 0):
            cp_send = messaging.new_message('carParams')
            cp_send.carParams = self.CP
            self.pm.send('carParams', cp_send)

        # carControl
        cc_send = messaging.new_message('carControl')
        cc_send.valid = CS.canValid
        cc_send.carControl = CC
        self.pm.send('carControl', cc_send)

        # copy CarControl to pass to CarInterface on the next iteration
        self.CC = CC

    def step(self):
        start_time = sec_since_boot()
        self.prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data from sockets and get a carState
        CS = self.data_sample()
        self.prof.checkpoint("Sample")

        self.update_events(CS)

        if not self.read_only:
            # Update control state
            self.state_transition(CS)
            self.prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_acc, a_acc, lac_log = self.state_control(CS)

        self.prof.checkpoint("State Control")

        # Publish data
        self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
        self.prof.checkpoint("Sent")

    def controlsd_thread(self):
        while True:
            self.step()
            self.rk.monitor_time()
            self.prof.display()
Exemple #22
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=[],
                             gas_resume_speed=-1,
                             pcm_enable=True):  # pylint: disable=dangerous-default-value
        events = Events()

        if cs_out.doorOpen:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears:
            events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        if cs_out.gasPressed and self.disengage_on_gas:
            events.add(EventName.gasPressed)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)
        if cs_out.cruiseState.nonAdaptive:
            events.add(EventName.wrongCruiseMode)

        if cs_out.steerError:
            events.add(EventName.steerUnavailable)
        elif cs_out.steerWarning:
            events.add(EventName.steerTempUnavailable)

        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        # Optionally allow to press gas at zero speed to resume.
        # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME!
        if (self.disengage_on_gas and cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \
           (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
            events.add(EventName.pedalPressed)

        # we engage when pcm is active (rising edge)
        if pcm_enable:
            if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
                events.add(EventName.pcmEnable)
            elif not cs_out.cruiseState.enabled:
                events.add(EventName.pcmDisable)

        return events
Exemple #23
0
    def create_common_events(self,
                             cs_out,
                             extra_gears=[],
                             gas_resume_speed=-1,
                             pcm_enable=True):  # pylint: disable=dangerous-default-value
        if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:  # this lets us modularize which checks we want to turn off op if cc was engaged previoiusly or not
            disengage_event = True
        else:
            if travis:
                disengage_event = True
            else:
                disengage_event = False

        events = Events()
        eventsArne182 = Events_arne182()

        if cs_out.doorOpen and disengage_event:
            events.add(EventName.doorOpen)
        if cs_out.seatbeltUnlatched and disengage_event:
            events.add(EventName.seatbeltNotLatched)
        if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears:
            if cs_out.vEgo < 5:
                eventsArne182.add(EventNameArne182.wrongGearArne)
            else:
                events.add(EventName.wrongGear)
        if cs_out.gearShifter == GearShifter.reverse:
            if cs_out.vEgo < 5:
                eventsArne182.add(EventNameArne182.reverseGearArne)
            else:
                events.add(EventName.reverseGear)
        if not cs_out.cruiseState.available:
            events.add(EventName.wrongCarMode)
        if cs_out.espDisabled:
            events.add(EventName.espDisabled)
        if cs_out.gasPressed and disengage_event:
            events.add(EventName.gasPressed)
        if cs_out.stockFcw:
            events.add(EventName.stockFcw)
        if cs_out.stockAeb:
            events.add(EventName.stockAeb)
        if cs_out.vEgo > MAX_CTRL_SPEED:
            events.add(EventName.speedTooHigh)
        if cs_out.cruiseState.nonAdaptive:
            events.add(EventName.wrongCruiseMode)

        # TODO: move this stuff to the capnp strut
        if cs_out.steerError:
            events.add(EventName.steerUnavailable)
        elif cs_out.steerWarning:
            events.add(EventName.steerTempUnavailable)
        # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
        # Optionally allow to press gas at zero speed to resume.
        # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME!
        if disengage_event and ((cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \
           (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill))):
            events.add(EventName.pedalPressed)

        # we engage when pcm is active (rising edge)
        if pcm_enable:
            if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
                if 0.8 < cs_out.vEgo < 1.2:
                    eventsArne182.add(EventNameArne182.pcmEnable)
                else:
                    events.add(EventName.pcmEnable)
            elif not cs_out.cruiseState.enabled:
                if 0.8 < cs_out.vEgo < 1.2:
                    eventsArne182.add(EventNameArne182.pcmDisable)
                else:
                    events.add(EventName.pcmDisable)

        return events, eventsArne182