Example #1
0
    def test_correctness(self):
        # Test all commands, randomize the params.
        for _ in xrange(1000):
            # Toyota
            steer = random.randint(-1, 1)
            enabled = (random.randint(0, 2) % 2 == 0)
            apgs_enabled = (random.randint(0, 2) % 2 == 0)
            m_old = create_ipas_steer_command(self.cp_old, steer, enabled,
                                              apgs_enabled)
            m = create_ipas_steer_command(self.cp, steer, enabled,
                                          apgs_enabled)
            self.assertEqual(m_old, m)

            steer = (random.randint(0, 2) % 2 == 0)
            steer_req = (random.randint(0, 2) % 2 == 0)
            raw_cnt = random.randint(1, 65536)
            m_old = create_steer_command(self.cp_old, steer, steer_req,
                                         raw_cnt)
            m = create_steer_command(self.cp, steer, steer_req, raw_cnt)
            self.assertEqual(m_old, m)

            accel = (random.randint(0, 2) % 2 == 0)
            pcm_cancel = (random.randint(0, 2) % 2 == 0)
            standstill_req = (random.randint(0, 2) % 2 == 0)
            lead = (random.randint(0, 2) % 2 == 0)
            m_old = create_accel_command(self.cp_old, accel, pcm_cancel,
                                         standstill_req, lead)
            m = create_accel_command(self.cp, accel, pcm_cancel,
                                     standstill_req, lead)
            self.assertEqual(m_old, m)

            fcw = random.randint(1, 65536)
            m_old = create_fcw_command(self.cp_old, fcw)
            m = create_fcw_command(self.cp, fcw)
            self.assertEqual(m_old, m)

            steer = (random.randint(0, 2) % 2 == 0)
            sound1 = random.randint(1, 65536)
            sound2 = random.randint(1, 65536)
            left_line = (random.randint(0, 2) % 2 == 0)
            right_line = (random.randint(0, 2) % 2 == 0)
            left_lane_depart = (random.randint(0, 2) % 2 == 0)
            right_lane_depart = (random.randint(0, 2) % 2 == 0)
            m_old = create_ui_command(self.cp_old, steer, sound1, sound2,
                                      left_line, right_line, left_lane_depart,
                                      right_lane_depart)
            m = create_ui_command(self.cp, steer, sound1, sound2, left_line,
                                  right_line, left_lane_depart,
                                  right_lane_depart)
            self.assertEqual(m_old, m)
Example #2
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
               left_line, right_line, lead, left_lane_depart,
               right_lane_depart):

        # *** compute control surfaces ***

        # gas and brake
        interceptor_gas_cmd = 0.
        pcm_accel_cmd = actuators.gas - actuators.brake

        if CS.CP.enableGasInterceptor:
            # handle hysteresis when around the minimum acc speed
            if CS.out.vEgo < MIN_ACC_SPEED:
                self.use_interceptor = True
            elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
                self.use_interceptor = False

            if self.use_interceptor and enabled:
                # only send negative accel when using interceptor. gas handles acceleration
                # +0.06 offset to reduce ABS pump usage when OP is engaged
                interceptor_gas_cmd = clip(actuators.gas, 0., 1.)
                pcm_accel_cmd = 0.06 - actuators.brake

        pcm_accel_cmd, self.accel_steady = accel_hysteresis(
            pcm_accel_cmd, self.accel_steady, enabled)
        pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE,
                             CarControllerParams.ACCEL_MIN,
                             CarControllerParams.ACCEL_MAX)

        # steer torque
        new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
        apply_steer = apply_toyota_steer_torque_limits(
            new_steer, self.last_steer, CS.out.steeringTorqueEps,
            CarControllerParams)
        self.steer_rate_limited = new_steer != apply_steer

        # Cut steering while we're in a known fault state (2s)
        if not enabled or CS.steer_state in [9, 25]:
            apply_steer = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        if not enabled and CS.pcm_acc_status:
            # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
            pcm_cancel_cmd = 1

        # on entering standstill, send standstill request
        if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
            self.standstill_req = True
        if CS.pcm_acc_status != 8:
            # pcm entered standstill or it's disabled
            self.standstill_req = False

        self.last_steer = apply_steer
        self.last_accel = pcm_accel_cmd
        self.last_standstill = CS.out.standstill

        can_sends = []

        #*** control msgs ***
        #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

        # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
        # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
        # on consecutive messages
        can_sends.append(
            create_steer_command(self.packer, apply_steer, apply_steer_req,
                                 frame))
        if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
            can_sends.append(
                create_lta_steer_command(self.packer, 0, 0, frame // 2))

        # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
        # if frame % 2 == 0:
        #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
        #   can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))

        # we can spam can to cancel the system even if we are using lat only control
        if (frame % 3 == 0
                and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
            lead = lead or CS.out.vEgo < 12.  # at low speed we always assume the lead is present do ACC can be engaged

            # Lexus IS uses a different cancellation message
            if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
                can_sends.append(create_acc_cancel_command(self.packer))
            elif CS.CP.openpilotLongitudinalControl:
                can_sends.append(
                    create_accel_command(self.packer, pcm_accel_cmd,
                                         pcm_cancel_cmd, self.standstill_req,
                                         lead, CS.acc_type))
            else:
                can_sends.append(
                    create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
                                         lead, CS.acc_type))

        if frame % 2 == 0 and CS.CP.enableGasInterceptor:
            # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
            # This prevents unexpected pedal range rescaling
            can_sends.append(
                create_gas_command(self.packer, interceptor_gas_cmd,
                                   frame // 2))

        # ui mesg is at 100Hz but we send asap if:
        # - there is something to display
        # - there is something to stop displaying
        fcw_alert = hud_alert == VisualAlert.fcw
        steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]

        send_ui = False
        if ((fcw_alert or steer_alert) and not self.alert_active) or \
           (not (fcw_alert or steer_alert) and self.alert_active):
            send_ui = True
            self.alert_active = not self.alert_active
        elif pcm_cancel_cmd:
            # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
            send_ui = True

        if (frame % 100 == 0 or send_ui):
            can_sends.append(
                create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
                                  left_line, right_line, left_lane_depart,
                                  right_lane_depart))

        if frame % 100 == 0 and CS.CP.enableDsu:
            can_sends.append(create_fcw_command(self.packer, fcw_alert))

        #*** static msgs ***

        for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
            if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
                can_sends.append(make_can_msg(addr, vl, bus))

        return can_sends
Example #3
0
  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # *** compute control surfaces ***

    # gas and brake
    if CS.CP.enableGasInterceptor and enabled:
      MAX_INTERCEPTOR_GAS = 0.5
      # RAV4 has very sensitive gas pedal
      if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
      elif CS.CP.carFingerprint in [CAR.COROLLA]:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
      else:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
      # offset for creep and windbrake
      pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
      pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
      interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
    else:
      interceptor_gas_cmd = 0.
    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

    # steer torque
    new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
    self.steer_rate_limited = new_steer != apply_steer

    # Cut steering while we're in a known fault state (2s)
    if not enabled or CS.steer_state in [9, 25]:
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
    # than CS.cruiseState.enabled. confirm they're not meaningfully different
    if not enabled and CS.pcm_acc_status:
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_standstill = CS.out.standstill

    can_sends = []

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
    if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
      can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))

    # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
    # if frame % 2 == 0:
    #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
    #   can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))

    # we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
      lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present do ACC can be engaged

      # Lexus IS uses a different cancellation message
      if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
        can_sends.append(create_acc_cancel_command(self.packer))
      elif CS.CP.openpilotLongitudinalControl:
        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))

    if frame % 2 == 0 and CS.CP.enableGasInterceptor:
      # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
      # This prevents unexpected pedal range rescaling
      can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    fcw_alert = hud_alert == VisualAlert.fcw
    steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]

    send_ui = False
    if ((fcw_alert or steer_alert) and not self.alert_active) or \
       (not (fcw_alert or steer_alert) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    elif pcm_cancel_cmd:
      # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
      send_ui = True

    if (frame % 100 == 0 or send_ui):
      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))

    if frame % 100 == 0 and CS.CP.enableDsu:
      can_sends.append(create_fcw_command(self.packer, fcw_alert))

    #*** static msgs ***

    for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
      if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus))

    return can_sends
Example #4
0
  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # *** compute control surfaces ***

    # gas and brake

    apply_gas = clip(actuators.gas, 0., 1.)

    if CS.CP.enableGasInterceptor:
      # send only negative accel if interceptor is detected. otherwise, send the regular value
      # +0.06 offset to reduce ABS pump usage when OP is engaged
      apply_accel = 0.06 - actuators.brake
    else:
      apply_accel = actuators.gas - actuators.brake

    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)

    # steer torque
    apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))

    apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)

    # only cut torque when steer state is a known fault
    if CS.steer_state in [9, 25]:
      self.last_fault_frame = frame

    # Cut steering for 2s after fault
    if not enabled or (frame - self.last_fault_frame < 200):
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    self.steer_angle_enabled, self.ipas_reset_counter = \
      ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
    #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))

    # steer angle
    if self.steer_angle_enabled and CS.ipas_active:
      apply_angle = actuators.steerAngle
      angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
      apply_angle = clip(apply_angle, -angle_lim, angle_lim)

      # windup slower
      if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
        angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
      else:
        angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)

      apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
    else:
      apply_angle = CS.angle_steers

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.standstill and not self.last_standstill:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_angle = apply_angle
    self.last_accel = apply_accel
    self.last_standstill = CS.standstill

    can_sends = []

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if ECU.CAM in self.fake_ecus:
      if self.angle_control:
        can_sends.append(create_steer_command(self.packer, 0., 0, frame))
      else:
        can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))

    if self.angle_control:
      can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
                                                 ECU.APGS in self.fake_ecus))
    elif ECU.APGS in self.fake_ecus:
      can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))

    if CS.cstm_btns_tr > 0:
      distance = 1 
    else:
      distance = 0 

    # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
      lead = lead or CS.v_ego < 12.    # at low speed we always assume the lead is present do ACC can be engaged

      # Lexus IS uses a different cancellation message
      if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
        can_sends.append(create_acc_cancel_command(self.packer))
      elif ECU.DSU in self.fake_ecus:
        can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, distance))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, distance))

    if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
        # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
        # This prevents unexpected pedal range rescaling
        can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    alert_out = process_hud_alert(hud_alert)
    steer, fcw = alert_out

    if (any(alert_out) and not self.alert_active) or \
       (not any(alert_out) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    else:
      send_ui = False

    # disengage msg causes a bad fault sound so play a good sound instead
    if pcm_cancel_cmd:
      send_ui = True

    if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
      can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))

    if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
      can_sends.append(create_fcw_command(self.packer, fcw))

    #*** static msgs ***

    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
      if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus, False))

    return can_sends
Example #5
0
  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # dp
    if frame % 500 == 0:
      modified = get_last_modified()
      if self.dp_last_modified != modified:
        self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True
        self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False

        self.dragon_lat_ctrl, \
        self.lane_change_enabled, \
        self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)

        self.dp_last_modified = modified

    # *** compute control surfaces ***

    # gas and brake

    apply_gas = clip(actuators.gas, 0., 1.)

    if CS.CP.enableGasInterceptor:
      # send only negative accel if interceptor is detected. otherwise, send the regular value
      # +0.06 offset to reduce ABS pump usage when OP is engaged
      apply_accel = 0.06 - actuators.brake
    else:
      apply_accel = actuators.gas - actuators.brake

    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)

    # steer torque
    new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
    self.steer_rate_limited = new_steer != apply_steer

    # only cut torque when steer state is a known fault
    if CS.steer_state in [9, 25]:
      self.last_fault_frame = frame

    # # Cut steering for 2s after fault
    if not enabled: # or (frame - self.last_fault_frame < 200):
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if not self.dragon_toyota_sng_mod and CS.out.standstill and not self.last_standstill:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_accel = apply_accel
    self.last_standstill = CS.out.standstill

    # dp
    apply_steer_req = common_controller_ctrl(enabled,
                                             self.dragon_lat_ctrl,
                                             self.dragon_enable_steering_on_signal,
                                             CS.out.leftBlinker,
                                             CS.out.rightBlinker,
                                             apply_steer_req)

    can_sends = []

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if Ecu.fwdCamera in self.fake_ecus:
      can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))

    # we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
      lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present do ACC can be engaged

      # Lexus IS uses a different cancellation message
      if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
        can_sends.append(create_acc_cancel_command(self.packer))
      elif CS.CP.openpilotLongitudinalControl:
        can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))

    if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
      # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
      # This prevents unexpected pedal range rescaling
      can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    fcw_alert = hud_alert == VisualAlert.fcw
    steer_alert = hud_alert == VisualAlert.steerRequired

    send_ui = False
    if ((fcw_alert or steer_alert) and not self.alert_active) or \
       (not (fcw_alert or steer_alert) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    elif pcm_cancel_cmd:
      # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
      send_ui = True

    # dp
    if self.dragon_lane_departure_warning:
      dragon_left_lane_depart = left_lane_depart
      dragon_right_lane_depart = right_lane_depart
    else:
      dragon_left_lane_depart = False
      dragon_right_lane_depart = False

    if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))

    if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
      can_sends.append(create_fcw_command(self.packer, fcw_alert))

    #*** static msgs ***

    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
      if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus))

    return can_sends
    def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
               hud_alert, audible_alert, forwarding_camera, left_line,
               right_line, lead):
        #update custom UI buttons and alerts
        CS.UE.update_custom_ui()
        if (frame % 1000 == 0):
            CS.cstm_btns.send_button_info()
            CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)

        # *** compute control surfaces ***

        # gas and brake

        apply_gas = clip(actuators.gas, 0., 1.)

        if CS.CP.enableGasInterceptor:
            # send only negative accel if interceptor is detected. otherwise, send the regular value
            # +0.06 offset to reduce ABS pump usage when OP is engaged
            apply_accel = 0.06 - actuators.brake
        else:
            apply_accel = actuators.gas - actuators.brake

        apply_accel, self.accel_steady = accel_hysteresis(
            apply_accel, self.accel_steady, enabled)
        apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
        # Get the angle from ALCA.
        alca_enabled = False
        alca_steer = 0.
        alca_angle = 0.
        turn_signal_needed = 0
        # Update ALCA status and custom button every 0.1 sec.
        if self.ALCA.pid == None:
            self.ALCA.set_pid(CS)
        if (frame % 10 == 0):
            self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
        # steer torque
        alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
            enabled, CS, frame, actuators)
        apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))

        apply_steer = apply_toyota_steer_torque_limits(apply_steer,
                                                       self.last_steer,
                                                       CS.steer_torque_motor,
                                                       SteerLimitParams)

        # only cut torque when steer state is a known fault
        if CS.steer_state in [9, 25]:
            self.last_fault_frame = frame

        # Cut steering for 2s after fault
        if not enabled or (frame - self.last_fault_frame < 200):
            apply_steer = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        self.steer_angle_enabled, self.ipas_reset_counter = \
          ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
        #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active

        # steer angle
        if self.steer_angle_enabled and CS.ipas_active:

            apply_angle = alca_angle
            angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
            apply_angle = clip(apply_angle, -angle_lim, angle_lim)

            # windup slower
            if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
                    self.last_angle):
                angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
                                        ANGLE_DELTA_V)
            else:
                angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
                                        ANGLE_DELTA_VU)

            apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
                               self.last_angle + angle_rate_lim)
        else:
            apply_angle = CS.angle_steers

        if not enabled and CS.pcm_acc_status:
            # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
            pcm_cancel_cmd = 1

        # on entering standstill, send standstill request
        if CS.standstill and not self.last_standstill:
            self.standstill_req = True
        if CS.pcm_acc_status != 8:
            # pcm entered standstill or it's disabled
            self.standstill_req = False

        self.last_steer = apply_steer
        self.last_angle = apply_angle
        self.last_accel = apply_accel
        self.last_standstill = CS.standstill

        can_sends = []

        #*** control msgs ***
        #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor

        # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
        # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
        # on consecutive messages
        if ECU.CAM in self.fake_ecus:
            if self.angle_control:
                can_sends.append(
                    create_steer_command(self.packer, 0., 0, frame))
            else:
                can_sends.append(
                    create_steer_command(self.packer, apply_steer,
                                         apply_steer_req, frame))

        if self.angle_control:
            can_sends.append(
                create_ipas_steer_command(self.packer, apply_angle,
                                          self.steer_angle_enabled, ECU.APGS
                                          in self.fake_ecus))
        elif ECU.APGS in self.fake_ecus:
            can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
                                                       True))

        # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
        if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
                pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
            lead = lead or CS.v_ego < 12.  # at low speed we always assume the lead is present do ACC can be engaged
            if ECU.DSU in self.fake_ecus:
                can_sends.append(
                    create_accel_command(self.packer, apply_accel,
                                         pcm_cancel_cmd, self.standstill_req,
                                         lead))
            else:
                can_sends.append(
                    create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
                                         lead))

        if CS.CP.enableGasInterceptor:
            # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
            # This prevents unexpected pedal range rescaling
            can_sends.append(create_gas_command(self.packer, apply_gas))

        if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
            for addr in TARGET_IDS:
                can_sends.append(create_video_target(frame / 10, addr))

        # ui mesg is at 100Hz but we send asap if:
        # - there is something to display
        # - there is something to stop displaying
        alert_out = process_hud_alert(hud_alert, audible_alert)
        steer, fcw, sound1, sound2 = alert_out

        if (any(alert_out) and not self.alert_active) or \
           (not any(alert_out) and self.alert_active):
            send_ui = True
            self.alert_active = not self.alert_active
        else:
            send_ui = False

        if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
            can_sends.append(
                create_ui_command(self.packer, steer, sound1, sound2,
                                  left_line, right_line))

        if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
            can_sends.append(create_fcw_command(self.packer, fcw))

        #*** static msgs ***

        for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
            if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
                    ecu == ECU.CAM and forwarding_camera):
                # special cases
                if fr_step == 5 and ecu == ECU.CAM and bus == 1:
                    cnt = (((frame / 5) % 7) + 1) << 5
                    vl = chr(cnt) + vl
                elif addr in (0x489, 0x48a) and bus == 0:
                    # add counter for those 2 messages (last 4 bits)
                    cnt = ((frame / 100) % 0xf) + 1
                    if addr == 0x48a:
                        # 0x48a has a 8 preceding the counter
                        cnt += 1 << 7
                    vl += chr(cnt)

                can_sends.append(make_can_msg(addr, vl, bus, False))

        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
Example #7
0
  def update(self, sendcan, enabled, CS, frame, actuators,
             pcm_cancel_cmd, hud_alert, audible_alert):

    # *** compute control surfaces ***

    # gas and brake
    apply_accel = actuators.gas - actuators.brake
    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)

    # steer torque
    apply_steer = int(round(actuators.steer * STEER_MAX))

    max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
    min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)

    apply_steer = clip(apply_steer, min_lim, max_lim)

    # slow rate if steer torque increases in magnitude
    if self.last_steer > 0:
      apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP)
    else:
      apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))

    # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
    # cuts steer torque immediately anyway TODO: monitor if this is a real issue
    # only cut torque when steer state is a known fault
    if not enabled or CS.steer_state in [9, 25]:
      apply_steer = 0

    self.steer_angle_enabled, self.ipas_reset_counter = \
      ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
    #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active

    # steer angle
    if self.steer_angle_enabled and CS.ipas_active:
      apply_angle = actuators.steerAngle
      angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
      apply_angle = clip(apply_angle, -angle_lim, angle_lim)

      # windup slower
      if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
        angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
      else:
        angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)

      apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
    else:
      apply_angle = CS.angle_steers

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.standstill and not self.last_standstill:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_angle = apply_angle
    self.last_accel = apply_accel
    self.last_standstill = CS.standstill

    can_sends = []

    #*** control msgs ***
    #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if ECU.CAM in self.fake_ecus:
      if self.angle_control:
        can_sends.append(create_steer_command(self.packer, 0., frame))
      else:
        can_sends.append(create_steer_command(self.packer, apply_steer, frame))

    if self.angle_control:
      can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, 
                                                 ECU.APGS in self.fake_ecus))
    elif ECU.APGS in self.fake_ecus:
      can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))

    # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
      if ECU.DSU in self.fake_ecus:
        can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False))

    if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
      for addr in TARGET_IDS:
        can_sends.append(create_video_target(frame/10, addr))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    alert_out = process_hud_alert(hud_alert, audible_alert)
    steer, fcw, sound1, sound2 = alert_out

    if (any(alert_out) and not self.alert_active) or \
       (not any(alert_out) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    else:
      send_ui = False

    if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
      can_sends.append(create_ui_command(self.packer, steer, sound1, sound2))
      can_sends.append(create_fcw_command(self.packer, fcw))

    #*** static msgs ***

    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
      if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
        # special cases
        if fr_step == 5 and ecu == ECU.CAM and bus == 1:
          cnt = (((frame / 5) % 7) + 1) << 5
          vl = chr(cnt) + vl
        elif addr in (0x489, 0x48a) and bus == 0:
          # add counter for those 2 messages (last 4 bits)
          cnt = ((frame/100)%0xf) + 1
          if addr == 0x48a:
            # 0x48a has a 8 preceding the counter
            cnt += 1 << 7
          vl += chr(cnt)

        can_sends.append(make_can_msg(addr, vl, bus, False))


    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
Example #8
0
    def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
               hud_alert, audible_alert, forwarding_camera, left_line,
               right_line, lead, left_lane_depart, right_lane_depart):

        #update custom UI buttons and alerts
        CS.UE.update_custom_ui()
        if (frame % 1000 == 0):
            CS.cstm_btns.send_button_info()
            CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)

        # *** compute control surfaces ***

        # gas and brake

        apply_gas = clip(actuators.gas, 0., 1.)

        if CS.CP.enableGasInterceptor:
            # send only negative accel if interceptor is detected. otherwise, send the regular value
            # +0.06 offset to reduce ABS pump usage when OP is engaged
            apply_accel = 0.06 - actuators.brake
        else:
            apply_accel = actuators.gas - actuators.brake

        apply_accel, self.accel_steady = accel_hysteresis(
            apply_accel, self.accel_steady, enabled)
        apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
        # Get the angle from ALCA.
        alca_enabled = False
        alca_steer = 0.
        alca_angle = 0.
        turn_signal_needed = 0
        # Update ALCA status and custom button every 0.1 sec.
        if self.ALCA.pid == None and not CS.indi_toggle:
            self.ALCA.set_pid(CS)
        if (frame % 10 == 0):
            self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
        # steer torque
        alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
            enabled, CS, frame, actuators)
        #apply_steer = int(round(alca_steer * STEER_MAX))

        self.phantom.update()
        # steer torque
        if self.phantom.data["status"]:
            apply_steer = int(round(self.phantom.data["angle"]))
            if abs(CS.angle_steers) > 400:
                apply_steer = 0
        else:
            apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
            if abs(CS.angle_steers) > 100:
                apply_steer = 0
        if not CS.lane_departure_toggle_on:
            apply_steer = 0

        # only cut torque when steer state is a known fault
        if CS.steer_state in [3, 7, 9, 11, 25]:
            self.last_fault_frame = frame

        # Cut steering for 2s after fault
        cutout_time = 100 if self.phantom.data["status"] else 200

        if not enabled or (frame - self.last_fault_frame < cutout_time):
            apply_steer = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        apply_steer = apply_toyota_steer_torque_limits(apply_steer,
                                                       self.last_steer,
                                                       CS.steer_torque_motor,
                                                       SteerLimitParams)
        if apply_steer == 0 and self.last_steer == 0:
            apply_steer_req = 0

        if not enabled and right_lane_depart and CS.v_ego > 12.5 and not CS.right_blinker_on:
            apply_steer = self.last_steer + 3
            apply_steer = min(apply_steer, 800)
            #print "right"
            #print apply_steer
            apply_steer_req = 1

        if not enabled and left_lane_depart and CS.v_ego > 12.5 and not CS.left_blinker_on:
            apply_steer = self.last_steer - 3
            apply_steer = max(apply_steer, -800)
            #print "left"
            #print apply_steer
            apply_steer_req = 1

        self.steer_angle_enabled, self.ipas_reset_counter = \
          ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
        #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))

        # steer angle
        if self.steer_angle_enabled and CS.ipas_active:

            apply_angle = alca_angle
            #apply_angle = actuators.steerAngle
            angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
            apply_angle = clip(apply_angle, -angle_lim, angle_lim)

            # windup slower
            if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
                    self.last_angle):
                angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
                                        ANGLE_DELTA_V)
            else:
                angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
                                        ANGLE_DELTA_VU)

            apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
                               self.last_angle + angle_rate_lim)
        else:
            apply_angle = CS.angle_steers

        if not enabled and CS.pcm_acc_status:
            # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
            pcm_cancel_cmd = 1

        # on entering standstill, send standstill request
        #if CS.standstill and not self.last_standstill:
        #  self.standstill_req = True
        if CS.pcm_acc_status != 8:
            # pcm entered standstill or it's disabled
            self.standstill_req = False

        self.last_steer = apply_steer
        self.last_angle = apply_angle
        self.last_accel = apply_accel
        self.last_standstill = CS.standstill

        can_sends = []

        # Enable blindspot debug mode once
        if BLINDSPOTDEBUG:
            self.blindspot_poll_counter += 1
        if self.blindspot_poll_counter > 1000:  # 10 seconds after start
            if CS.left_blinker_on:
                self.blindspot_blink_counter_left += 1
                #print "debug Left Blinker on"
            elif CS.right_blinker_on:
                self.blindspot_blink_counter_right += 1
            else:
                self.blindspot_blink_counter_left = 0
                self.blindspot_blink_counter_right = 0
                #print "debug Left Blinker off"
                if self.blindspot_debug_enabled_left:
                    can_sends.append(
                        set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
                    self.blindspot_debug_enabled_left = False
                    #print "debug Left blindspot debug disabled"
                if self.blindspot_debug_enabled_right:
                    can_sends.append(
                        set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
                    self.blindspot_debug_enabled_right = False
                    #print "debug Right blindspot debug disabled"
            if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left:  #check blinds
                can_sends.append(set_blindspot_debug_mode(
                    LEFT_BLINDSPOT, True))
                #print "debug Left blindspot debug enabled"
                self.blindspot_debug_enabled_left = True
            if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right:  #enable blindspot debug mode
                if CS.v_ego > 6:  #polling at low speeds switches camera off
                    can_sends.append(
                        set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
                    #print "debug Right blindspot debug enabled"
                    self.blindspot_debug_enabled_right = True
        if self.blindspot_debug_enabled_left:
            if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001:  # Poll blindspots at 5 Hz
                can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
        if self.blindspot_debug_enabled_right:
            if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005:  # Poll blindspots at 5 Hz
                can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))

        #*** control msgs ***
        #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

        # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
        # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
        # on consecutive messages
        if ECU.CAM in self.fake_ecus:
            if self.angle_control:
                can_sends.append(
                    create_steer_command(self.packer, 0., 0, frame))
            else:
                can_sends.append(
                    create_steer_command(self.packer, apply_steer,
                                         apply_steer_req, frame))

        if self.angle_control:
            can_sends.append(
                create_ipas_steer_command(self.packer, apply_angle,
                                          self.steer_angle_enabled, ECU.APGS
                                          in self.fake_ecus))
        elif ECU.APGS in self.fake_ecus:
            can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
                                                       True))

        if CS.cstm_btns.get_button_status("tr") > 0:
            distance = 1
        else:
            distance = 0

        # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
        if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
                pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
            lead = lead or CS.v_ego < 12.  # at low speed we always assume the lead is present do ACC can be engaged
            if ECU.DSU in self.fake_ecus:
                can_sends.append(
                    create_accel_command(self.packer, apply_accel,
                                         pcm_cancel_cmd, self.standstill_req,
                                         lead, distance))
            else:
                can_sends.append(
                    create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
                                         lead, distance))

        if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
            # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
            # This prevents unexpected pedal range rescaling
            can_sends.append(
                create_gas_command(self.packer, apply_gas, frame // 2))

        if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
            for addr in TARGET_IDS:
                can_sends.append(create_video_target(frame // 10, addr))

        # ui mesg is at 100Hz but we send asap if:
        # - there is something to display
        # - there is something to stop displaying
        alert_out = process_hud_alert(hud_alert, audible_alert)
        steer, fcw, sound1, sound2 = alert_out

        if (any(alert_out) and not self.alert_active) or \
           (not any(alert_out) and self.alert_active):
            send_ui = True
            self.alert_active = not self.alert_active
        else:
            send_ui = False
        if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
            can_sends.append(
                create_ui_command(self.packer, steer, sound1, sound2,
                                  left_line, right_line, left_lane_depart,
                                  right_lane_depart))

        if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR:
            can_sends.append(create_fcw_command(self.packer, fcw))

        #*** static msgs ***

        for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
            if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
                    ecu == ECU.CAM and forwarding_camera):
                # special cases
                if fr_step == 5 and ecu == ECU.CAM and bus == 1:
                    cnt = (((frame // 5) % 7) + 1) << 5
                    vl = chr(cnt) + vl
                elif addr in (0x489, 0x48a) and bus == 0:
                    # add counter for those 2 messages (last 4 bits)
                    cnt = ((frame // 100) % 0xf) + 1
                    if addr == 0x48a:
                        # 0x48a has a 8 preceding the counter
                        cnt += 1 << 7
                    vl += chr(cnt)

                can_sends.append(make_can_msg(addr, vl, bus, False))

        sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
Example #9
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
               left_line, right_line, lead, left_lane_depart,
               right_lane_depart):
        #if not enabled:
        #  self.sm.update(0)
        #if self.sm.updated['pathPlan']:
        #  blinker = CS.out.leftBlinker or CS.out.rightBlinker
        #  ldw_allowed = CS.out.vEgo > 12.5 and not blinker
        #  CAMERA_OFFSET = op_params.get('camera_offset', 0.06)
        #  right_lane_visible = self.sm['pathPlan'].rProb > 0.5
        #  left_lane_visible = self.sm['pathPlan'].lProb > 0.5
        #  self.rightLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].rPoly[3] > -(0.93 + CAMERA_OFFSET) and right_lane_visible)
        #  self.leftLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].lPoly[3] < (0.93 - CAMERA_OFFSET) and left_lane_visible)
        #  print("blinker")
        #  print(blinker)
        #  print("ldw_allowed")
        #  print(ldw_allowed)
        #  print("CAMERA_OFFSET")
        #  print(CAMERA_OFFSET)
        #  print("right_lane_visible")
        #  print(right_lane_visible)
        #  print("left_lane_visible")
        #  print(left_lane_visible)
        #  print("self.rightLaneDepart")
        #  print(self.rightLaneDepart)
        #  print("self.leftLaneDepart")
        #  print(self.leftLaneDepart)
        # *** compute control surfaces ***

        # gas and brake

        apply_gas = clip(actuators.gas, 0., 1.)

        if CS.CP.enableGasInterceptor:
            # send only negative accel if interceptor is detected. otherwise, send the regular value
            # +0.06 offset to reduce ABS pump usage when OP is engaged
            apply_accel = 0.06 - actuators.brake
        else:
            apply_accel = actuators.gas - actuators.brake

        apply_accel, self.accel_steady = accel_hysteresis(
            apply_accel, self.accel_steady, enabled)
        apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)

        if CS.CP.enableGasInterceptor:
            if CS.out.gasPressed:
                apply_accel = max(apply_accel, 0.06)
            if CS.out.brakePressed:
                apply_gas = 0.0
                apply_accel = min(apply_accel, 0.00)
        else:
            if CS.out.gasPressed:
                apply_accel = max(apply_accel, 0.0)
            if CS.out.brakePressed and CS.out.vEgo > 1:
                apply_accel = min(apply_accel, 0.0)

        # steer torque
        new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))

        # only cut torque when steer state is a known fault
        if CS.steer_state in [9, 25]:
            self.last_fault_frame = frame

        # Cut steering for 1s after fault
        if (frame - self.last_fault_frame < 100) or abs(
                CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 100
                                               and CS.CP.carFingerprint
                                               in [CAR.RAV4H, CAR.PRIUS]):
            new_steer = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        if not enabled and right_lane_depart and CS.out.vEgo > 12.5 and not CS.out.rightBlinker:
            new_steer = self.last_steer + 3
            new_steer = min(new_steer, 800)
            print("right")
            print(new_steer)
            apply_steer_req = 1

        if not enabled and left_lane_depart and CS.out.vEgo > 12.5 and not CS.out.leftBlinker:
            new_steer = self.last_steer - 3
            new_steer = max(new_steer, -800)
            print("left")
            print(new_steer)
            apply_steer_req = 1

        apply_steer = apply_toyota_steer_torque_limits(
            new_steer, self.last_steer, CS.out.steeringTorqueEps,
            SteerLimitParams)
        self.steer_rate_limited = new_steer != apply_steer

        if not enabled and abs(apply_steer) > 800 and not (right_lane_depart or
                                                           left_lane_depart):
            apply_steer = 0
            apply_steer_req = 0

        if not enabled and CS.pcm_acc_status:
            # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
            pcm_cancel_cmd = 1

        # on entering standstill, send standstill request
        if CS.out.standstill and not self.last_standstill:
            self.standstill_req = True
        if CS.pcm_acc_status != 8:
            # pcm entered standstill or it's disabled
            self.standstill_req = False

        self.last_steer = apply_steer
        self.last_accel = apply_accel
        self.last_standstill = CS.out.standstill

        can_sends = []

        #*** control msgs ***
        #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

        # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
        # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
        # on consecutive messages
        if Ecu.fwdCamera in self.fake_ecus:
            can_sends.append(
                create_steer_command(self.packer, apply_steer, apply_steer_req,
                                     frame))

        # we can spam can to cancel the system even if we are using lat only control
        if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (
                pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
            lead = lead or CS.out.vEgo < 12.  # at low speed we always assume the lead is present do ACC can be engaged

            # Lexus IS uses a different cancellation message
            if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
                can_sends.append(create_acc_cancel_command(self.packer))
            elif CS.CP.openpilotLongitudinalControl:
                can_sends.append(
                    create_accel_command(self.packer, apply_accel,
                                         pcm_cancel_cmd, self.standstill_req,
                                         lead))
            else:
                can_sends.append(
                    create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
                                         lead))

        if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
            # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
            # This prevents unexpected pedal range rescaling
            can_sends.append(
                create_gas_command(self.packer, apply_gas, frame // 2))

        # ui mesg is at 100Hz but we send asap if:
        # - there is something to display
        # - there is something to stop displaying
        fcw_alert = hud_alert == VisualAlert.fcw
        steer_alert = hud_alert == VisualAlert.steerRequired

        send_ui = False
        if ((fcw_alert or steer_alert) and not self.alert_active) or \
           (not (fcw_alert or steer_alert) and self.alert_active):
            send_ui = True
            self.alert_active = not self.alert_active
        elif pcm_cancel_cmd:
            # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
            send_ui = True

        if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
            can_sends.append(
                create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
                                  left_line, right_line, left_lane_depart,
                                  right_lane_depart))

        if frame % 100 == 0 and (Ecu.dsu in self.fake_ecus
                                 or Ecu.unknown in self.fake_ecus):
            can_sends.append(create_fcw_command(self.packer, fcw_alert))

        #*** static msgs ***

        for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
            if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
                can_sends.append(make_can_msg(addr, vl, bus))

        # Enable blindspot debug mode once
        if frame > 1000 and not (
                CS.CP.carFingerprint in TSS2_CAR or CS.CP.carFingerprint
                == CAR.CAMRY):  # 10 seconds after start and not a tss2 car
            if BLINDSPOTALWAYSON:
                self.blindspot_blink_counter_left += 1
                self.blindspot_blink_counter_right += 1
                #print("debug blindspot alwayson!")
            elif CS.out.leftBlinker:
                self.blindspot_blink_counter_left += 1
                #print("debug Left Blinker on")
            elif CS.out.rightBlinker:
                self.blindspot_blink_counter_right += 1
                #print("debug Right Blinker on")
            else:
                self.blindspot_blink_counter_left = 0
                self.blindspot_blink_counter_right = 0
                if self.blindspot_debug_enabled_left:
                    can_sends.append(
                        set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
                    #can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x01\x00\x00\x00\x00', 0))
                    self.blindspot_debug_enabled_left = False
                    #print ("debug Left blindspot debug disabled")
                if self.blindspot_debug_enabled_right:
                    can_sends.append(
                        set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
                    #can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
                    self.blindspot_debug_enabled_right = False
                    #print("debug Right blindspot debug disabled")
            if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left:  #check blinds
                can_sends.append(set_blindspot_debug_mode(
                    LEFT_BLINDSPOT, True))
                #can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x60\x00\x00\x00\x00', 0))
                #print("debug Left blindspot debug enabled")
                self.blindspot_debug_enabled_left = True
            if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right:  #enable blindspot debug mode
                if CS.out.vEgo > 6:  #polling at low speeds switches camera off
                    #can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x60\x00\x00\x00\x00', 0))
                    can_sends.append(
                        set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
                    #print("debug Right blindspot debug enabled")
                    self.blindspot_debug_enabled_right = True
            if CS.out.vEgo < 6 and self.blindspot_debug_enabled_right:  # if enabled and speed falls below 6m/s
                #can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
                can_sends.append(
                    set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
                self.blindspot_debug_enabled_right = False
                #print("debug Right blindspot debug disabled")
        if self.blindspot_debug_enabled_left:
            if frame % 20 == 0 and frame > 1001:  # Poll blindspots at 5 Hz
                can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
                #can_sends.append(make_can_msg(0x750, b'\x41\x02\x21\x69\x00\x00\x00\x00', 0))
                #print("debug Left blindspot poll")
        if self.blindspot_debug_enabled_right:
            if frame % 20 == 10 and frame > 1005:  # Poll blindspots at 5 Hz
                #can_sends.append(make_can_msg(0x750, b'\x42\x02\x21\x69\x00\x00\x00\x00', 0))
                can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
                #print("debug Right blindspot poll")

        return can_sends
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
               left_line, right_line, lead, left_lane_depart,
               right_lane_depart, dragonconf):

        # *** compute control surfaces ***

        # gas and brake
        interceptor_gas_cmd = 0.
        pcm_accel_cmd = actuators.accel

        if CS.CP.enableGasInterceptor:
            # handle hysteresis when around the minimum acc speed
            if CS.out.vEgo < MIN_ACC_SPEED:
                self.use_interceptor = True
            elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
                self.use_interceptor = False

            if self.use_interceptor and enabled:
                # only send negative accel when using interceptor. gas handles acceleration
                # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
                MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED],
                                             [0.2, 0.5])
                interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0.,
                                           MAX_INTERCEPTOR_GAS)
                pcm_accel_cmd = 0.18 - max(0, -actuators.accel)

        pcm_accel_cmd, self.accel_steady = accel_hysteresis(
            pcm_accel_cmd, self.accel_steady, enabled)
        pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN,
                             CarControllerParams.ACCEL_MAX)

        # steer torque
        new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
        apply_steer = apply_toyota_steer_torque_limits(
            new_steer, self.last_steer, CS.out.steeringTorqueEps,
            CarControllerParams)
        self.steer_rate_limited = new_steer != apply_steer

        # Cut steering while we're in a known fault state (2s)
        if not enabled or CS.steer_state in [
                9, 25
        ] or abs(CS.out.steeringRateDeg) > 100 or (
                abs(CS.out.steeringAngleDeg) > 150
                and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
            apply_steer = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
        # than CS.cruiseState.enabled. confirm they're not meaningfully different
        if not enabled and CS.pcm_acc_status:
            pcm_cancel_cmd = 1

        # on entering standstill, send standstill request
        if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
            self.standstill_req = True
        if CS.pcm_acc_status != 8:
            # pcm entered standstill or it's disabled
            self.standstill_req = False

        # dp
        blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
        if not enabled:
            self.blinker_end_frame = 0
        if self.last_blinker_on and not blinker_on:
            self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
        apply_steer = common_controller_ctrl(
            enabled, dragonconf, blinker_on or frame < self.blinker_end_frame,
            apply_steer, CS.out.vEgo)
        self.last_blinker_on = blinker_on

        self.last_steer = apply_steer
        self.last_accel = pcm_accel_cmd
        self.last_standstill = CS.out.standstill

        can_sends = []

        #*** control msgs ***
        #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

        # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
        # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
        # on consecutive messages
        can_sends.append(
            create_steer_command(self.packer, apply_steer, apply_steer_req,
                                 frame))
        if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
            can_sends.append(
                create_lta_steer_command(self.packer, 0, 0, frame // 2))

        # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
        # if frame % 2 == 0:
        #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
        #   can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))

        if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled:
            pcm_accel_cmd = 0.
            if CS.CP.enableGasInterceptor:
                interceptor_gas_cmd = 0.

        # we can spam can to cancel the system even if we are using lat only control
        if (frame % 3 == 0
                and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
            lead = lead or CS.out.vEgo < 12.  # at low speed we always assume the lead is present do ACC can be engaged

            if dragonconf.dpAtl and not dragonconf.dpAtlOpLong:
                pass
            # Lexus IS uses a different cancellation message
            elif pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
                can_sends.append(create_acc_cancel_command(self.packer))
            elif CS.CP.openpilotLongitudinalControl:
                can_sends.append(
                    create_accel_command(self.packer, pcm_accel_cmd,
                                         pcm_cancel_cmd, self.standstill_req,
                                         lead, CS.acc_type, CS.distance))
            else:
                can_sends.append(
                    create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
                                         lead, CS.acc_type, CS.distance))

        if frame % 2 == 0 and CS.CP.enableGasInterceptor:
            # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
            # This prevents unexpected pedal range rescaling
            can_sends.append(
                create_gas_command(self.packer, interceptor_gas_cmd,
                                   frame // 2))

        # ui mesg is at 100Hz but we send asap if:
        # - there is something to display
        # - there is something to stop displaying
        fcw_alert = hud_alert == VisualAlert.fcw
        steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]

        send_ui = False
        if ((fcw_alert or steer_alert) and not self.alert_active) or \
           (not (fcw_alert or steer_alert) and self.alert_active):
            send_ui = True
            self.alert_active = not self.alert_active
        elif pcm_cancel_cmd:
            # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
            send_ui = True

        # dp
        if not dragonconf.dpToyotaLdw:
            left_lane_depart = False
            right_lane_depart = False

        if (frame % 100 == 0 or send_ui):
            can_sends.append(
                create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
                                  left_line, right_line, left_lane_depart,
                                  right_lane_depart, enabled))

        if frame % 100 == 0 and CS.CP.enableDsu:
            can_sends.append(create_fcw_command(self.packer, fcw_alert))

        #*** static msgs ***

        for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
            if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
                can_sends.append(make_can_msg(addr, vl, bus))

        return can_sends
  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart, dragonconf):

    # *** compute control surfaces ***

    # gas and brake

    apply_gas = clip(actuators.gas, 0., 1.)

    if CS.CP.enableGasInterceptor:
      # send only negative accel if interceptor is detected. otherwise, send the regular value
      # +0.06 offset to reduce ABS pump usage when OP is engaged
      apply_accel = 0.06 - actuators.brake
    else:
      apply_accel = actuators.gas - actuators.brake

    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)

    # steer torque
    new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
    self.steer_rate_limited = new_steer != apply_steer

    # Cut steering while we're in a known fault state (2s)
    if not enabled or CS.steer_state in [9, 25] or abs(CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    # dp
    blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
    if not enabled:
      self.blinker_end_frame = 0
    if self.last_blinker_on and not blinker_on:
      self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
    apply_steer = common_controller_ctrl(enabled,
                                         dragonconf,
                                         blinker_on or frame < self.blinker_end_frame,
                                         apply_steer, CS.out.vEgo)
    self.last_blinker_on = blinker_on

    self.last_steer = apply_steer
    self.last_accel = apply_accel
    self.last_standstill = CS.out.standstill

    can_sends = []

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if Ecu.fwdCamera in self.fake_ecus:
      can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))

      # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
      # if frame % 2 == 0:
      #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
      #   can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2))

    # we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
      lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present do ACC can be engaged

      # Lexus IS uses a different cancellation message
      if not dragonconf.dpAtl:
        if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
          can_sends.append(create_acc_cancel_command(self.packer))
        elif CS.CP.openpilotLongitudinalControl:
          can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
        else:
          can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))

    if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
      # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
      # This prevents unexpected pedal range rescaling
      can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    fcw_alert = hud_alert == VisualAlert.fcw
    steer_alert = hud_alert == VisualAlert.steerRequired

    send_ui = False
    if ((fcw_alert or steer_alert) and not self.alert_active) or \
       (not (fcw_alert or steer_alert) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    elif pcm_cancel_cmd:
      # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
      send_ui = True

    # dp
    if dragonconf.dpToyotaLdw:
      dragon_left_lane_depart = left_lane_depart
      dragon_right_lane_depart = right_lane_depart
    else:
      dragon_left_lane_depart = False
      dragon_right_lane_depart = False


    if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))

    if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
      can_sends.append(create_fcw_command(self.packer, fcw_alert))

    #*** static msgs ***

    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
      if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus))

    return can_sends
Example #12
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
               left_line, right_line, lead, left_lane_depart,
               right_lane_depart):

        # *** compute control surfaces ***
        self.sm.update(0)
        if self.sm.updated['radarState']:
            self.lead_v = self.sm['radarState'].leadOne.vRel
            self.lead_a = self.sm['radarState'].leadOne.aRel
            self.lead_d = self.sm['radarState'].leadOne.dRel
        if self.sm.updated['controlsState']:
            self.LCS = self.sm['controlsState'].longControlState
            if frame % 500 == 0:  # *** 5 sec interval? ***
                print(self.LCS)

        # gas and brake


#    apply_gas = 0.
#    apply_accel = actuators.gas - actuators.brake

#    if CS.CP.enableGasInterceptor and enabled and CS.out.vEgo < MIN_ACC_SPEED:
#      apply_gas = clip(actuators.gas, 0., 1.)
#      # converts desired acceleration to gas percentage for pedal
#      # +0.06 offset to reduce ABS pump usage when applying very small gas
#      if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo):
#        apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.)
#      apply_accel += 0.06

        apply_gas = clip(actuators.gas, 0., 1.)

        if CS.CP.enableGasInterceptor:
            # send only negative accel if interceptor is detected. otherwise, send the regular value
            # +0.06 offset to reduce ABS pump usage when OP is engaged
            #apply_accel = 0.06 - actuators.brake    # Original
            if lead:
                apply_accel = 0.06 - actuators.brake
                #apply_accel = 0.06
            else:
                apply_accel = 0.06
            # End new
        else:
            apply_accel = actuators.gas - actuators.brake

        apply_accel, self.accel_steady = accel_hysteresis(
            apply_accel, self.accel_steady, enabled)
        apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE,
                           CarControllerParams.ACCEL_MIN,
                           CarControllerParams.ACCEL_MAX)

        # steer torque
        new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
        apply_steer = apply_toyota_steer_torque_limits(
            new_steer, self.last_steer, CS.out.steeringTorqueEps,
            CarControllerParams)
        self.steer_rate_limited = new_steer != apply_steer

        # Cut steering while we're in a known fault state (2s)
        if not enabled or abs(CS.out.steeringRateDeg) > 100:
            #if not enabled or CS.steer_state in [9, 25] or CS.out.epsDisabled==1 or abs(CS.out.steeringRateDeg) > 100:    #Original statement
            apply_steer = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        #if not enabled and CS.pcm_acc_status:    # Original
        if not enabled:
            # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
            pcm_cancel_cmd = 1

        # on entering standstill, send standstill request
        if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR and not self.standstill_hack:
            self.standstill_req = True
        if CS.pcm_acc_status != 8:
            # pcm entered standstill or it's disabled
            self.standstill_req = False

        self.last_steer = apply_steer
        self.last_accel = apply_accel
        self.last_standstill = CS.out.standstill

        can_sends = []

        if (frame % 2 == 0):
            can_sends.append(
                create_lead_command(self.packer, self.lead_v, self.lead_a,
                                    self.lead_d))

        #*** control msgs ***
        #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

        # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
        # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
        # on consecutive messages
        if Ecu.fwdCamera in self.fake_ecus:
            can_sends.append(
                create_steer_command(self.packer, apply_steer, apply_steer_req,
                                     frame))
            if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
                can_sends.append(
                    create_lta_steer_command(self.packer, 0, 0, frame // 2))

            # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
            # if frame % 2 == 0:
            #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
            #   can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))

        # we can spam can to cancel the system even if we are using lat only control
        if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (
                pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
            lead = lead or CS.out.vEgo < 12.  # at low speed we always assume the lead is present do ACC can be engaged

            # Lexus IS uses a different cancellation message
            if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
                can_sends.append(create_acc_cancel_command(self.packer))
            elif CS.CP.openpilotLongitudinalControl:
                can_sends.append(
                    create_accel_command(self.packer, apply_accel,
                                         pcm_cancel_cmd, self.standstill_req,
                                         lead))
            else:
                #can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))   # Original value
                can_sends.append(
                    create_accel_command(self.packer, 500, pcm_cancel_cmd,
                                         False, lead))

        if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
            # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
            # This prevents unexpected pedal range rescaling
            can_sends.append(
                create_gas_command(self.packer, apply_gas, frame // 2))

        # ui mesg is at 100Hz but we send asap if:
        # - there is something to display
        # - there is something to stop displaying
        fcw_alert = hud_alert == VisualAlert.fcw
        steer_alert = hud_alert == VisualAlert.steerRequired

        send_ui = False
        if ((fcw_alert or steer_alert) and not self.alert_active) or \
           (not (fcw_alert or steer_alert) and self.alert_active):
            send_ui = True
            self.alert_active = not self.alert_active
        elif pcm_cancel_cmd:
            # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
            send_ui = True

        if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
            can_sends.append(
                create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
                                  left_line, right_line, left_lane_depart,
                                  right_lane_depart))

        if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
            can_sends.append(create_fcw_command(self.packer, fcw_alert))

        #*** static msgs ***

        for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
            if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
                can_sends.append(make_can_msg(addr, vl, bus))

        return can_sends
Example #13
0
  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # gas and brake
    if CS.CP.enableGasInterceptor and active:
      MAX_INTERCEPTOR_GAS = 0.5
      # RAV4 has very sensitive gas pedal
      if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
      elif CS.CP.carFingerprint in (CAR.COROLLA,):
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
      else:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
      # offset for creep and windbrake
      pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
      pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
      interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
    else:
      interceptor_gas_cmd = 0.
    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

    # steer torque
    new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
    self.steer_rate_limited = new_steer != apply_steer

    # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
    # than CS.cruiseState.enabled. confirm they're not meaningfully different
    if not enabled and CS.pcm_acc_status:
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_standstill = CS.out.standstill

    can_sends = []

    if CS.CP.steerControlType == car.CarParams.SteerControlType.angle:
      can_sends.append(create_steer_command(self.packer, 0, 0, frame))
      # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
      # at 0 degrees, so we need to offset the commanded angle as well.
      # Also need to pulse steer_req to prevent 5 second steering lockout in some cases
      steer_req = active  # TODO(thinh): Check and cut out steering while we are in a known fault state
      percentage = interp(abs(CS.out.steeringTorque), [50, 100], [100, 0])
      commanded_angle = interp(percentage, [-10, 100], [CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
                    actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg])
      can_sends.append(create_lta_steer_command(self.packer, commanded_angle, steer_req, frame))
    else:
      # Cut steering while we're in a known fault state (2s)
      if not active or CS.steer_state in (9, 25):
        apply_steer = 0
        apply_steer_req = 0
      else:
        apply_steer_req = 1
      # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
      # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
      # on consecutive messages
      can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
      if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
        can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))

    # we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
      lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present so ACC can be engaged

      # Lexus IS uses a different cancellation message
      if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
        can_sends.append(create_acc_cancel_command(self.packer))
      elif CS.CP.openpilotLongitudinalControl:
        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance_btn))
        self.accel = pcm_accel_cmd
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance_btn))

    if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
      # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
      # This prevents unexpected pedal range rescaling
      can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
      self.gas = interceptor_gas_cmd

    # ui mesg is at 1Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    fcw_alert = hud_alert == VisualAlert.fcw
    steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw)

    send_ui = False
    if ((fcw_alert or steer_alert) and not self.alert_active) or \
       (not (fcw_alert or steer_alert) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    elif pcm_cancel_cmd:
      # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
      send_ui = True

    if (frame % 100 == 0 or send_ui):
      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))

    if frame % 100 == 0 and CS.CP.enableDsu:
      can_sends.append(create_fcw_command(self.packer, fcw_alert))

    # *** static msgs ***
    for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
      if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus))

    new_actuators = actuators.copy()
    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
    new_actuators.accel = self.accel
    new_actuators.gas = self.gas

    return new_actuators, can_sends
Example #14
0
  def update(self, sendcan, enabled, CS, frame, actuators,
             pcm_cancel_cmd, hud_alert, audible_alert):

    # *** compute control surfaces ***
    tt = sec_since_boot()

    # steer torque is converted back to CAN reference (positive when steering right)
    apply_accel = actuators.gas - actuators.brake
    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = int(round(clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)))

    # steer torque is converted back to CAN reference (positive when steering right)
    apply_steer = int(round(actuators.steer * STEER_MAX))

    max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
    min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)

    apply_steer = clip(apply_steer, min_lim, max_lim)

    # slow rate if steer torque increases in magnitude
    if self.last_steer > 0:
      apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP)
    else:
      apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
    # cuts steer torque immediately anyway TODO: monitor if this is a real issue
    if not enabled or CS.steer_error:
      apply_steer = 0

    self.last_steer = apply_steer
    self.last_accel = apply_accel

    can_sends = []

    #*** control msgs ***
    #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if ECU.CAM in self.fake_ecus:
      can_sends.append(create_steer_command(apply_steer, frame))

    if ECU.APGS in self.fake_ecus:
      can_sends.append(create_ipas_steer_command(apply_steer))

    # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
      if ECU.DSU in self.fake_ecus:
        can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd))
      else:
        can_sends.append(create_accel_command(0, pcm_cancel_cmd))

    if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
      for addr in TARGET_IDS:
        can_sends.append(create_video_target(frame/10, addr))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    hud1, hud2 = process_hud_alert(hud_alert, audible_alert)
    if ((hud1 or hud2) and not self.alert_active) or \
       (not (hud1 or hud2) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    else:
      send_ui = False

    if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
      can_sends.append(create_ui_command(hud1, hud2))

    #*** static msgs ***

    for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems():
      if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
        # send msg!
        # special cases

        if fr_step == 5 and ecu == ECU.CAM and bus == 1:
          cnt = (((frame / 5) % 7) + 1) << 5
          vl = chr(cnt) + vl
        elif addr in (0x489, 0x48a) and bus == 0:
          # add counter for those 2 messages (last 4 bits)
          cnt = ((frame/100)%0xf) + 1
          if addr == 0x48a:
            # 0x48a has a 8 preceding the counter
            cnt += 1 << 7
          vl += chr(cnt)

        can_sends.append(make_can_msg(addr, vl, bus, False))


    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
Example #15
0
  def update(self, enabled, CS, frame, actuators,
             pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # *** compute control surfaces ***

    # gas and brake

    apply_gas = clip(actuators.gas, 0., 1.)

    if CS.CP.enableGasInterceptor:
      # send only negative accel if interceptor is detected. otherwise, send the regular value
      # +0.06 offset to reduce ABS pump usage when OP is engaged
      apply_accel = 0.06 - actuators.brake
    else:
      apply_accel = actuators.gas - actuators.brake

    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)

    # steer torque
    apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))

    apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)

    # only cut torque when steer state is a known fault
    if CS.steer_state in [9, 25]:
      self.last_fault_frame = frame

    # Cut steering for 2s after fault
    if not enabled or (frame - self.last_fault_frame < 200):
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    self.steer_angle_enabled, self.ipas_reset_counter = \
      ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
    #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))

    # steer angle
    if self.steer_angle_enabled and CS.ipas_active:
      apply_angle = actuators.steerAngle
      angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
      apply_angle = clip(apply_angle, -angle_lim, angle_lim)

      # windup slower
      if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
        angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
      else:
        angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)

      apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
    else:
      apply_angle = CS.angle_steers

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.standstill and not self.last_standstill:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_angle = apply_angle
    self.last_accel = apply_accel
    self.last_standstill = CS.standstill

    can_sends = []

    # dragonpilot
    if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1":
      self.turning_signal_timer = 100

    if self.turning_signal_timer > 0:
      self.turning_signal_timer -= 1
      apply_steer_req = 0

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if ECU.CAM in self.fake_ecus:
      if self.angle_control:
        can_sends.append(create_steer_command(self.packer, 0., 0, frame))
      else:
        can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))

    if self.angle_control:
      can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
                                                 ECU.APGS in self.fake_ecus))
    elif ECU.APGS in self.fake_ecus:
      can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))

    # DragonAllowGas
    # if we detect gas pedal pressed, we do not want OP to apply gas or brake
    # gasPressed code from interface.py
    if CS.CP.enableGasInterceptor:
      # use interceptor values to disengage on pedal press
      gasPressed = CS.pedal_gas > 15
    else:
      gasPressed = CS.pedal_gas > 0

    if params.get("DragonAllowGas") == "1" and gasPressed:
      apply_accel = 0
      apply_gas = 0

    # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
      lead = lead or CS.v_ego < 12.    # at low speed we always assume the lead is present do ACC can be engaged
      if ECU.DSU in self.fake_ecus:
        can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))

    if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
        # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
        # This prevents unexpected pedal range rescaling
        can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))

    if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
      for addr in TARGET_IDS:
        can_sends.append(create_video_target(frame//10, addr))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    alert_out = process_hud_alert(hud_alert, audible_alert)
    steer, fcw, sound1, sound2 = alert_out

    if (any(alert_out) and not self.alert_active) or \
       (not any(alert_out) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    else:
      send_ui = False

    if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
      can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))

    if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
      can_sends.append(create_fcw_command(self.packer, fcw))

    #*** static msgs ***

    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
      if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
        # special cases
        if fr_step == 5 and ecu == ECU.CAM and bus == 1:
          cnt = (((frame // 5) % 7) + 1) << 5
          vl = chr(cnt) + vl
        elif addr in (0x489, 0x48a) and bus == 0:
          # add counter for those 2 messages (last 4 bits)
          cnt = ((frame // 100) % 0xf) + 1
          if addr == 0x48a:
            # 0x48a has a 8 preceding the counter
            cnt += 1 << 7
          vl += chr(cnt)

        can_sends.append(make_can_msg(addr, vl, bus, False))

    return can_sends
Example #16
0
  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # *** compute control surfaces ***

    # gas and brake

    apply_gas = clip(actuators.gas, 0., 1.)

    if CS.CP.enableGasInterceptor:
      # send only negative accel if interceptor is detected. otherwise, send the regular value
      # +0.06 offset to reduce ABS pump usage when OP is engaged
      apply_accel = 0.06 - actuators.brake
    else:
      apply_accel = actuators.gas - actuators.brake

    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
    # Get the angle from ALCA.
    alca_enabled = False
    alca_steer = 0.
    alca_angle = 0.
    turn_signal_needed = 0
    # Update ALCA status and custom button every 0.1 sec.
    if self.ALCA.pid == None:
      self.ALCA.set_pid(CS)
    if (frame % 10 == 0):
      self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
    # steer torque
    new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
    self.steer_rate_limited = new_steer != apply_steer

    # only cut torque when steer state is a known fault
    if CS.steer_state in [9, 25]:
      self.last_fault_frame = frame

    # Cut steering for 2s after fault
    if not enabled or (frame - self.last_fault_frame < 200):
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    if not enabled and CS.pcm_acc_status:
      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.out.standstill and not self.last_standstill:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_accel = apply_accel
    self.last_standstill = CS.out.standstill

    can_sends = []

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    if Ecu.fwdCamera in self.fake_ecus:
      can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))

    # we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
      lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present do ACC can be engaged

      # Lexus IS uses a different cancellation message
      if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
        can_sends.append(create_acc_cancel_command(self.packer))
      elif CS.CP.openpilotLongitudinalControl:
        can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))

    if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
        # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
        # This prevents unexpected pedal range rescaling
        can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    alert_out = process_hud_alert(hud_alert)
    steer, fcw = alert_out

    if (any(alert_out) and not self.alert_active) or \
       (not any(alert_out) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    else:
      send_ui = False

    # disengage msg causes a bad fault sound so play a good sound instead
    if pcm_cancel_cmd:
      send_ui = True

    if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
      can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))

    if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
      can_sends.append(create_fcw_command(self.packer, fcw))

    #*** static msgs ***

    for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
      if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus))

    return can_sends