예제 #1
0
class CarController(object):
    def __init__(self, dbc_name, car_fingerprint, enable_camera):
        self.apply_steer_last = 0
        self.turning_signal_timer = 0
        self.car_fingerprint = car_fingerprint
        self.lkas11_cnt = 0
        self.mdps12_cnt = 0
        self.cnt = 0
        self.last_resume_cnt = 0
        self.map_speed = 0
        self.enable_camera = enable_camera
        # True when camera present, and we need to replace all the camera messages
        # otherwise we forward the camera msgs and we just replace the lkas cmd signals
        self.camera_disconnected = False
        self.packer = CANPacker(dbc_name)
        context = zmq.Context()
        self.params = Params()
        self.map_data_sock = messaging.sub_sock(
            context, service_list['liveMapData'].port, conflate=True)
        self.speed_conv = 3.6
        self.speed_adjusted = False
        self.checksum = "NONE"
        self.checksum_learn_cnt = 0
        self.en_cnt = 0
        self.apply_steer_ang = 0.0
        self.en_spas = 3
        self.mdps11_stat_last = 0
        self.lkas = False
        self.spas_present = True  # TODO Make Automatic

        self.ALCA = ALCAController(
            self, True, False)  # Enabled True and SteerByAngle only False

    def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd,
               hud_alert):

        if not self.enable_camera:
            return

        if CS.camcan > 0:
            if self.checksum == "NONE":
                self.checksum = learn_checksum(self.packer, CS.lkas11)
                print("Discovered Checksum", self.checksum)
                if self.checksum == "NONE":
                    return
        elif CS.steer_error == 1:
            if self.checksum_learn_cnt > 200:
                self.checksum_learn_cnt = 0
                if self.checksum == "NONE":
                    print("Testing 6B Checksum")
                    self.checksum == "6B"
                elif self.checksum == "6B":
                    print("Testing 7B Checksum")
                    self.checksum == "7B"
                elif self.checksum == "7B":
                    print("Testing CRC8 Checksum")
                    self.checksum == "crc8"
                else:
                    self.checksum == "NONE"
            else:
                self.checksum_learn_cnt += 1

        force_enable = False

        # I don't care about your opinion, deal with it!
        if (CS.cstm_btns.get_button_status("alwon") > 0) and CS.acc_active:
            enabled = True
            force_enable = True

        if (self.car_fingerprint in FEATURES["soft_disable"]
                and CS.v_wheel < 16.8):
            enabled = False
            force_enable = False

        if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1):
            self.turning_signal_timer = 100  # Disable for 1.0 Seconds after blinker turned off

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

        # 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)
        self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)

        alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
            enabled, CS, self.cnt, actuators)
        if force_enable and not CS.acc_active:
            apply_steer = int(
                round(actuators.steer * SteerLimitParams.STEER_MAX))
        else:
            apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))

        # SPAS limit angle extremes for safety
        apply_steer_ang_req = np.clip(actuators.steerAngle,
                                      -1 * (SteerLimitParams.STEER_ANG_MAX),
                                      SteerLimitParams.STEER_ANG_MAX)
        # SPAS limit angle rate for safety
        if abs(self.apply_steer_ang - apply_steer_ang_req) > 0.6:
            if apply_steer_ang_req > self.apply_steer_ang:
                self.apply_steer_ang += 0.5
            else:
                self.apply_steer_ang -= 0.5
        else:
            self.apply_steer_ang = apply_steer_ang_req

        # Limit steer rate for safety
        apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last,
                                       SteerLimitParams,
                                       CS.steer_torque_driver)

        if alca_enabled:
            self.turning_signal_timer = 0

        if self.turning_signal_timer > 0:
            self.turning_signal_timer = self.turning_signal_timer - 1
            turning_signal = 1
        else:
            turning_signal = 0

        # Use LKAS or SPAS
        if CS.mdps11_stat == 7 or CS.v_wheel > 2.7:
            self.lkas = True
        elif CS.v_wheel < 0.1:
            self.lkas = False
        if self.spas_present:
            self.lkas = True

        # If ALCA is disabled, and turning indicators are turned on, we do not want OP to steer,
        if not enabled or (turning_signal and not alca_enabled):
            if self.lkas:
                apply_steer = 0
            else:
                self.apply_steer_ang = 0.0
                self.en_cnt = 0

        steer_req = 1 if enabled and self.lkas else 0

        self.apply_steer_last = apply_steer

        can_sends = []

        self.lkas11_cnt = self.cnt % 0x10
        self.clu11_cnt = self.cnt % 0x10
        self.mdps12_cnt = self.cnt % 0x100
        self.spas_cnt = self.cnt % 0x200

        can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, \
                                      enabled if self.lkas else False, False if CS.camcan == 0 else CS.lkas11, hud_alert, (CS.cstm_btns.get_button_status("cam") > 0), \
                                      (False if CS.camcan == 0 else True), self.checksum))

        if CS.camcan > 0:
            can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \
                                          CS.camcan, self.checksum))

        # SPAS11 50hz
        if (self.cnt % 2) == 0 and not self.spas_present:
            if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
                self.en_spas == 7
                self.en_cnt = 0

            if self.en_spas == 7 and self.en_cnt >= 8:
                self.en_spas = 3
                self.en_cnt = 0

            if self.en_cnt < 8 and enabled and not self.lkas:
                self.en_spas = 4
            elif self.en_cnt >= 8 and enabled and not self.lkas:
                self.en_spas = 5

            if self.lkas or not enabled:
                self.apply_steer_ang = CS.mdps11_strang
                self.en_spas = 3
                self.en_cnt = 0

            self.mdps11_stat_last = CS.mdps11_stat
            self.en_cnt += 1
            can_sends.append(
                create_spas11(self.packer, (self.spas_cnt / 2), self.en_spas,
                              self.apply_steer_ang, self.checksum))

        # SPAS12 20Hz
        if (self.cnt % 5) == 0 and not self.spas_present:
            can_sends.append(create_spas12(self.packer))

        # Force Disable
        if pcm_cancel_cmd and (not force_enable):
            can_sends.append(
                create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0))
        elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
            self.last_resume_cnt = self.cnt
            can_sends.append(
                create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0))

        # Speed Limit Related Stuff  Lot's of comments for others to understand!
        # Run this twice a second
        if (self.cnt % 50) == 0:
            if self.params.get("LimitSetSpeed") == "1" and self.params.get(
                    "SpeedLimitOffset") is not None:
                # If Not Enabled, or cruise not set, allow auto speed adjustment again
                if not (enabled and CS.acc_active_real):
                    self.speed_adjusted = False
                # Attempt to read the speed limit from zmq
                map_data = messaging.recv_one_or_none(self.map_data_sock)
                # If we got a message
                if map_data != None:
                    # See if we use Metric or dead kings extremeties for measurements, and set a variable to the conversion value
                    if bool(self.params.get("IsMetric")):
                        self.speed_conv = CV.MS_TO_KPH
                    else:
                        self.speed_conv = CV.MS_TO_MPH

                    # If the speed limit is valid
                    if map_data.liveMapData.speedLimitValid:
                        last_speed = self.map_speed
                        # Get the speed limit, and add the offset to it,
                        v_speed = (map_data.liveMapData.speedLimit +
                                   float(self.params.get("SpeedLimitOffset")))
                        ## Stolen curvature code from planner.py, and updated it for us
                        v_curvature = 45.0
                        if map_data.liveMapData.curvatureValid:
                            v_curvature = math.sqrt(
                                1.85 /
                                max(1e-4, abs(map_data.liveMapData.curvature)))
                        # Use the minimum between Speed Limit and Curve Limit, and convert it as needed
                        #self.map_speed = min(v_speed, v_curvature) * self.speed_conv
                        self.map_speed = v_speed * self.speed_conv
                        # Compare it to the last time the speed was read.  If it is different, set the flag to allow it to auto set our speed
                        if last_speed != self.map_speed:
                            self.speed_adjusted = False
                    else:
                        # If it is not valid, set the flag so the cruise speed won't be changed.
                        self.map_speed = 0
                        self.speed_adjusted = True
            else:
                self.speed_adjusted = True

        # Ensure we have cruise IN CONTROL, so we don't do anything dangerous, like turn cruise on
        # Ensure the speed limit is within range of the stock cruise control capabilities
        # Do the spamming 10 times a second, we might get from 0 to 10 successful
        # Only do this if we have not yet set the cruise speed
        if CS.acc_active_real and not self.speed_adjusted and self.map_speed > (
                8.5 * self.speed_conv) and (self.cnt % 9 == 0
                                            or self.cnt % 9 == 1):
            # Use some tolerance because of Floats being what they are...
            if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed *
                                                          1.005):
                can_sends.append(
                    create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL,
                                 (1 if self.cnt % 9 == 1 else 0)))
            elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed /
                                                            1.005):
                can_sends.append(
                    create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL,
                                 (1 if self.cnt % 9 == 1 else 0)))
            # If nothing needed adjusting, then the speed has been set, which will lock out this control
            else:
                self.speed_adjusted = True

        ### If Driver Overrides using accelerator (or gas for the antiquated), cancel auto speed adjustment
        if CS.pedal_gas:
            self.speed_adjusted = True
        ### Send messages to canbus
        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())

        self.cnt += 1
예제 #2
0
class CarController(object):
    def __init__(self, dbc_name, enable_camera=True):
        self.braking = False
        self.brake_steady = 0.
        self.brake_last = 0.
        self.apply_brake_last = 0
        self.last_pump_ts = 0
        self.enable_camera = enable_camera
        self.packer = CANPacker(dbc_name)
        self.new_radar_config = False
        self.ALCA = ALCAController(
            self, True, False)  # Enabled  True and SteerByAngle only False
    def update(self, sendcan, enabled, CS, frame, actuators, \
               pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
               hud_v_cruise, hud_show_lanes, hud_show_car, \
               hud_alert, snd_beep, snd_chime):
        """ Controls thread """

        if not self.enable_camera:
            return

        # *** apply brake hysteresis ***
        brake, self.braking, self.brake_steady = actuator_hystereses(
            actuators.brake, self.braking, self.brake_steady, CS.v_ego,
            CS.CP.carFingerprint)

        # *** no output if not enabled ***
        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 = True

        # *** rate limit after the enable check ***
        self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)

        # vehicle hud display, wait for one update from 10Hz 0x304 msg
        if hud_show_lanes:
            hud_lanes = 1
        else:
            hud_lanes = 0

        if enabled:
            if hud_show_car:
                hud_car = 2
            else:
                hud_car = 1
        else:
            hud_car = 0

        # For lateral control-only, send chimes as a beep since we don't send 0x1fa
        if CS.CP.radarOffCan:
            snd_beep = snd_beep if snd_beep != 0 else snd_chime

        #print chime, alert_id, hud_alert
        fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

        hud = HUDData(int(pcm_accel),
                      int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
                      int(snd_beep), snd_chime, fcw_display, acc_alert,
                      steer_required)

        # **** process the car messages ****

        # *** compute control surfaces ***
        BRAKE_MAX = 1024 / 4
        if CS.CP.carFingerprint in (CAR.ACURA_ILX):
            STEER_MAX = 0xF00
        elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
            STEER_MAX = 0x3e8  # CR-V only uses 12-bits and requires a lower value (max value from energee)
        else:
            STEER_MAX = 0x1000

        #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)

        # 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)

        # steer torque is converted back to CAN reference (positive when steering right)
        apply_gas = clip(actuators.gas, 0., 1.)
        apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
        apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX))

        lkas_active = enabled and not CS.steer_not_allowed

        # Send CAN commands.
        can_sends = []

        # Send steering command.
        idx = frame % 4
        can_sends.append(
            hondacan.create_steering_control(self.packer, apply_steer,
                                             lkas_active, CS.CP.carFingerprint,
                                             idx))

        # Send dashboard UI commands.
        if (frame % 10) == 0:
            idx = (frame / 10) % 4
            can_sends.extend(
                hondacan.create_ui_commands(self.packer, pcm_speed, hud,
                                            CS.CP.carFingerprint, idx))

        if CS.CP.radarOffCan:
            # If using stock ACC, spam cancel command to kill gas when OP disengages.
            if pcm_cancel_cmd:
                can_sends.append(
                    hondacan.spam_buttons_command(self.packer,
                                                  CruiseButtons.CANCEL, idx))
            elif CS.stopped:
                can_sends.append(
                    hondacan.spam_buttons_command(self.packer,
                                                  CruiseButtons.RES_ACCEL,
                                                  idx))

        else:
            # Send gas and brake commands.
            if (frame % 2) == 0:
                idx = (frame / 2) % 4
                pump_on, self.last_pump_ts = brake_pump_hysteresys(
                    apply_brake, self.apply_brake_last, self.last_pump_ts)
                can_sends.append(
                    hondacan.create_brake_command(self.packer, apply_brake,
                                                  pump_on, pcm_override,
                                                  pcm_cancel_cmd, hud.chime,
                                                  hud.fcw, idx))
                self.apply_brake_last = apply_brake

                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(
                        hondacan.create_gas_command(self.packer, apply_gas,
                                                    idx))

        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #3
0
class CarController():
    def __init__(self, dbc_name, CP, VM):
        self.fleet_speed_state = 0
        self.cc_counter = 0
        self.alcaStateData = None
        self.icLeadsData = None
        self.params = Params()
        self.braking = False
        self.brake_steady = 0.
        self.brake_last = 0.
        self.packer = CANPacker(dbc_name)
        self.epas_disabled = True
        self.last_angle = 0.
        self.last_accel = 0.
        self.blinker = Blinker()
        self.ALCA = ALCAController(self, True,
                                   True)  # Enabled and SteerByAngle both True
        self.ACC = ACCController(self)
        self.PCC = PCCController(self)
        self.HSO = HSOController(self)
        self.GYRO = GYROController()
        self.AHB = AHBController(self)
        self.sent_DAS_bootID = False
        self.speedlimit = None
        self.trafficevents = messaging.sub_sock('trafficEvents', conflate=True)
        self.pathPlan = messaging.sub_sock('pathPlan', conflate=True)
        self.radarState = messaging.sub_sock('radarState', conflate=True)
        self.icLeads = messaging.sub_sock('uiIcLeads', conflate=True)
        self.icCarLR = messaging.sub_sock('uiIcCarLR', conflate=True)
        self.alcaState = messaging.sub_sock('alcaState', conflate=True)
        self.gpsLocationExternal = None
        self.opState = 0  # 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
        self.accPitch = 0.
        self.accRoll = 0.
        self.accYaw = 0.
        self.magPitch = 0.
        self.magRoll = 0.
        self.magYaw = 0.
        self.gyroPitch = 0.
        self.gyroRoll = 0.
        self.gyroYaw = 0.
        self.set_speed_limit_active = False
        self.speed_limit_offset = 0.
        self.speed_limit_ms = 0.

        # for warnings
        self.warningCounter = 0
        self.DAS_206_apUnavailable = 0
        self.DAS_222_accCameraBlind = 0  #DAS_206 lkas not ebabled
        self.DAS_219_lcTempUnavailableSpeed = 0
        self.DAS_220_lcTempUnavailableRoad = 0
        self.DAS_221_lcAborting = 0
        self.DAS_211_accNoSeatBelt = 0
        self.DAS_207_lkasUnavailable = 0  #use for manual steer?
        self.DAS_208_rackDetected = 0  #use for low battery?
        self.DAS_202_noisyEnvironment = 0  #use for planner error?
        self.DAS_025_steeringOverride = 0  #another one to use for manual steer?
        self.warningNeeded = 0

        # items for IC integration for Lane and Lead Car
        self.average_over_x_pathplan_values = 1
        self.curv0Matrix = MovingAverage(self.average_over_x_pathplan_values)
        self.curv1Matrix = MovingAverage(self.average_over_x_pathplan_values)
        self.curv2Matrix = MovingAverage(self.average_over_x_pathplan_values)
        self.curv3Matrix = MovingAverage(self.average_over_x_pathplan_values)
        self.leadDxMatrix = MovingAverage(self.average_over_x_pathplan_values)
        self.leadDyMatrix = MovingAverage(self.average_over_x_pathplan_values)
        self.leadDx = 0.
        self.leadDy = 0.
        self.leadClass = 0
        self.leadVx = 0.
        self.leadId = 0
        self.lead2Dx = 0.
        self.lead2Dy = 0.
        self.lead2Class = 0
        self.lead2Vx = 0.
        self.lead2Id = 0
        self.lLine = 0
        self.rLine = 0
        self.curv0 = 0.
        self.curv1 = 0.
        self.curv2 = 0.
        self.curv3 = 0.
        self.visionCurvC0 = 0.
        self.laneRange = 30  #max is 160m but OP has issues with precision beyond 50

        self.laneWidth = 0.

        self.stopSign_visible = False
        self.stopSign_distance = 1000.
        self.stopSign_action = 0
        self.stopSign_resume = False

        self.stopLight_visible = False
        self.stopLight_distance = 1000.
        self.stopLight_action = 0
        self.stopLight_resume = False
        self.stopLight_color = 0.  #0-unknown, 1-red, 2-yellow, 3-green

        self.stopSignWarning = 0
        self.stopLightWarning = 0
        self.stopSignWarning_last = 0
        self.stopLightWarning_last = 0
        self.roadSignType = 0xFF
        self.roadSignStopDist = 1000.
        self.roadSignColor = 0.
        self.roadSignControlActive = 0
        self.roadSignType_last = 0xFF

        self.roadSignDistanceWarning = 50.

        self.alca_enabled = False
        self.ldwStatus = 0
        self.prev_ldwStatus = 0

        self.radarVin_idx = 0
        self.LDW_ENABLE_SPEED = 16
        self.should_ldw = False
        self.ldw_numb_frame_end = 0

        self.isMetric = (self.params.get("IsMetric") == "1")

        self.ahbLead1 = None

    def reset_traffic_events(self):
        self.stopSign_visible = False
        self.stopSign_distance = 1000.
        self.stopSign_action = 0
        self.stopSign_resume = False

        self.stopLight_visible = False
        self.stopLight_distance = 1000.
        self.stopLight_action = 0
        self.stopLight_resume = False
        self.stopLight_color = 0.  #0-unknown, 1-red, 2-yellow, 3-green

    def checkWhichSign(self):
        self.stopSignWarning = 0
        self.stopLightWarning = 0
        self.roadSignType_last = self.roadSignType
        self.roadSignType = 0xFF
        self.roadSignStopDist = 1000.
        self.roadSignColor = 0
        self.roadSignControlActive = 0
        if (self.stopSign_distance < self.stopLight_distance):
            self.roadSignType = 0x00
            self.roadSignStopDist = self.stopSign_distance
            self.roadSignColor = 0
            self.roadSignControlActive = self.stopSign_resume
            if (self.stopSign_distance < self.roadSignDistanceWarning):
                self.stopSignWarning = 1
        elif (self.stopLight_distance < self.stopSign_distance):
            self.roadSignType = 0x01
            self.roadSignStopDist = self.stopLight_distance
            self.roadSignColor = self.stopLight_color
            self.roadSignControlActive = self.stopLight_resume
            if (self.stopLight_distance < self.roadSignDistanceWarning) and (
                    self.roadSignColor == 1):
                self.stopLightWarning = 1

    def update(self, enabled, CS, frame, actuators, \
               pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
               hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
               snd_beep, snd_chime,leftLaneVisible,rightLaneVisible):

        if (not enabled) and (self.ALCA.laneChange_cancelled):
            self.ALCA.laneChange_cancelled = False
            self.ALCA.laneChange_cancelled_counter = 0
            self.warningNeeded = 1
        if self.warningCounter > 0:
            self.warningCounter = self.warningCounter - 1
            if self.warningCounter == 0:
                self.warningNeeded = 1
        if self.warningCounter == 0 or not enabled:
            # when zero reset all warnings
            self.DAS_222_accCameraBlind = 0  #we will see what we can use this for
            self.DAS_219_lcTempUnavailableSpeed = 0
            self.DAS_220_lcTempUnavailableRoad = 0
            self.DAS_221_lcAborting = 0
            self.DAS_211_accNoSeatBelt = 0
            self.DAS_207_lkasUnavailable = 0  #use for manual not in drive?
            self.DAS_208_rackDetected = 0  #use for low battery?
            self.DAS_202_noisyEnvironment = 0  #use for planner error?
            self.DAS_025_steeringOverride = 0  #use for manual steer?
            self.DAS_206_apUnavailable = 0  #Ap disabled from CID

        if CS.keepEonOff:
            if CS.cstm_btns.get_button_status("dsp") != 9:
                CS.cstm_btns.set_button_status("dsp", 9)
        else:
            if CS.cstm_btns.get_button_status("dsp") != 1:
                CS.cstm_btns.set_button_status("dsp", 1)
        # """ Controls thread """

        if not CS.useTeslaMapData:
            if self.speedlimit is None:
                self.speedlimit = messaging.sub_sock('liveMapData',
                                                     conflate=True)

        # *** no output if not enabled ***
        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 = True

        # vehicle hud display, wait for one update from 10Hz 0x304 msg
        if hud_show_lanes:
            hud_lanes = 1
        else:
            hud_lanes = 0

        # TODO: factor this out better
        if enabled:
            if hud_show_car:
                hud_car = 2
            else:
                hud_car = 1
        else:
            hud_car = 0

        # For lateral control-only, send chimes as a beep since we don't send 0x1fa
        #if CS.CP.radarOffCan:

        #print chime, alert_id, hud_alert
        fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

        hud = HUDData(int(pcm_accel),
                      int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
                      int(snd_beep), snd_chime, fcw_display, acc_alert,
                      steer_required)

        if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
            print("INVALID HUD", hud)
            hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)

        # **** process the car messages ****

        # *** compute control surfaces ***

        # Prevent steering while stopped
        MIN_STEERING_VEHICLE_VELOCITY = 0.05  # m/s
        vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)

        #upodate custom UI buttons and alerts
        CS.UE.update_custom_ui()

        if (frame % 100 == 0):
            CS.cstm_btns.send_button_info()
            #read speed limit params
            if CS.hasTeslaIcIntegration:
                self.set_speed_limit_active = True
                self.speed_limit_offset = CS.userSpeedLimitOffsetKph
            else:
                self.set_speed_limit_active = (
                    self.params.get("SpeedLimitOffset")
                    is not None) and (self.params.get("LimitSetSpeed") == "1")
                if self.set_speed_limit_active:
                    self.speed_limit_offset = float(
                        self.params.get("SpeedLimitOffset"))
                    if not self.isMetric:
                        self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS
                else:
                    self.speed_limit_offset = 0.
        if CS.useTeslaGPS and (frame % 10 == 0):
            if self.gpsLocationExternal is None:
                self.gpsLocationExternal = messaging.pub_sock(
                    'gpsLocationExternal')
            sol = gen_solution(CS)
            sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
            self.gpsLocationExternal.send(sol.to_bytes())

        #get pitch/roll/yaw every 0.1 sec
        if (frame % 10 == 0):
            (self.accPitch, self.accRoll,
             self.accYaw), (self.magPitch, self.magRoll,
                            self.magYaw), (self.gyroPitch, self.gyroRoll,
                                           self.gyroYaw) = self.GYRO.update(
                                               CS.v_ego, CS.a_ego,
                                               CS.angle_steers)
            CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,
                                  self.magPitch, self.magRoll, self.magYaw,
                                  self.gyroPitch, self.gyroRoll, self.gyroYaw)

        # Update statuses for custom buttons every 0.1 sec.
        if (frame % 10 == 0):
            self.ALCA.update_status(
                (CS.cstm_btns.get_button_status("alca") > 0)
                and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or
                     (CS.hasTeslaIcIntegration and CS.alcaEnabled)))

        self.blinker.update_state(CS, frame)

        # update PCC module info
        pedal_can_sends = self.PCC.update_stat(CS, frame)
        if self.PCC.pcc_available:
            self.ACC.enable_adaptive_cruise = False
        else:
            # Update ACC module info.
            self.ACC.update_stat(CS, True)
            self.PCC.enable_pedal_cruise = False

        # update CS.v_cruise_pcm based on module selected.
        speed_uom_kph = 1.
        if CS.imperial_speed_units:
            speed_uom_kph = CV.KPH_TO_MPH
        if self.ACC.enable_adaptive_cruise:
            CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph
        elif self.PCC.enable_pedal_cruise:
            CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph
        else:
            CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph
        self.alca_enabled = self.ALCA.update(enabled, CS, actuators,
                                             self.alcaStateData, frame,
                                             self.blinker)
        self.should_ldw = self._should_ldw(CS, frame)
        apply_angle = -actuators.steerAngle  # Tesla is reversed vs OP.
        # Update HSO module info.
        human_control = self.HSO.update_stat(self, CS, enabled, actuators,
                                             frame)
        human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.alca_enabled
        enable_steer_control = (enabled and not human_lane_changing
                                and not human_control and vehicle_moving)

        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)

        #BB disable limits to test 0.5.8
        # apply_angle = clip(apply_angle , self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
        # If human control, send the steering angle as read at steering wheel.
        if human_control:
            apply_angle = CS.angle_steers

        # Send CAN commands.
        can_sends = []

        #if using radar, we need to send the VIN
        if CS.useTeslaRadar and (frame % 100 == 0):
            useRadar = 0
            if CS.useTeslaRadar:
                useRadar = 1
            can_sends.append(
                teslacan.create_radar_VIN_msg(self.radarVin_idx, CS.radarVIN,
                                              1, 0x108, useRadar,
                                              CS.radarPosition,
                                              CS.radarEpasType))
            self.radarVin_idx += 1
            self.radarVin_idx = self.radarVin_idx % 3

        #First we emulate DAS.
        # DAS_longC_enabled (1),DAS_speed_override (1),DAS_apUnavailable (1), DAS_collision_warning (1),  DAS_op_status (4)
        # DAS_speed_kph(8),
        # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3),
        # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
        # DAS_acc_speed_limit (8),
        # DAS_speed_limit_units(8)
        #send fake_das data as 0x553
        # TODO: forward collision warning

        if frame % 10 == 0:
            speedlimitMsg = None
            if self.speedlimit is not None:
                speedlimitMsg = messaging.recv_one_or_none(self.speedlimit)
            icLeadsMsg = self.icLeads.receive(non_blocking=True)
            radarStateMsg = messaging.recv_one_or_none(self.radarState)
            alcaStateMsg = self.alcaState.receive(non_blocking=True)
            pathPlanMsg = messaging.recv_one_or_none(self.pathPlan)
            icCarLRMsg = self.icCarLR.receive(non_blocking=True)
            trafficeventsMsgs = None
            if self.trafficevents is not None:
                trafficeventsMsgs = messaging.recv_sock(self.trafficevents)
            if CS.hasTeslaIcIntegration:
                self.speed_limit_ms = CS.speed_limit_ms
            if (speedlimitMsg is not None) and not CS.useTeslaMapData:
                lmd = speedlimitMsg.liveMapData
                self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0
            if icLeadsMsg is not None:
                self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg)
            if radarStateMsg is not None:
                #to show lead car on IC
                if self.icLeadsData is not None:
                    can_messages = self.showLeadCarOnICCanMessage(
                        radarStateMsg=radarStateMsg)
                    can_sends.extend(can_messages)
            if alcaStateMsg is not None:
                self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg)
            if pathPlanMsg is not None:
                #to show curvature and lanes on IC
                if self.alcaStateData is not None:
                    self.handlePathPlanSocketForCurvatureOnIC(
                        pathPlanMsg=pathPlanMsg,
                        alcaStateData=self.alcaStateData,
                        CS=CS)
            if icCarLRMsg is not None:
                can_messages = self.showLeftAndRightCarsOnICCanMessages(
                    icCarLRMsg=tesla.ICCarsLR.from_bytes(icCarLRMsg))
                can_sends.extend(can_messages)
            if trafficeventsMsgs is not None:
                can_messages = self.handleTrafficEvents(
                    trafficEventsMsgs=trafficeventsMsgs)
                can_sends.extend(can_messages)

        op_status = 0x02
        hands_on_state = 0x00
        forward_collision_warning = 0  #1 if needed
        if hud_alert == AH.FCW:
            forward_collision_warning = hud_alert[1]
            if forward_collision_warning > 1:
                forward_collision_warning = 1
        #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold
        cc_state = 1
        alca_state = 0x00

        speed_override = 0
        collision_warning = 0x00
        speed_control_enabled = 0
        accel_min = -15
        accel_max = 5
        acc_speed_kph = 0
        send_fake_warning = False
        send_fake_msg = False
        if enabled:
            #self.opState  0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
            alca_state = 0x01
            if self.opState == 0:
                op_status = 0x02
            if self.opState == 1:
                op_status = 0x03
            if self.opState == 2:
                op_status = 0x08
            if self.opState == 3:
                op_status = 0x01
            if self.opState == 5:
                op_status = 0x03
            if self.blinker.override_direction > 0:
                alca_state = 0x08 + self.blinker.override_direction
            elif (self.lLine > 1) and (self.rLine > 1):
                alca_state = 0x08
            elif (self.lLine > 1):
                alca_state = 0x06
            elif (self.rLine > 1):
                alca_state = 0x07
            else:
                alca_state = 0x01
            #canceled by user
            if self.ALCA.laneChange_cancelled and (
                    self.ALCA.laneChange_cancelled_counter > 0):
                alca_state = 0x14
            #min speed for ALCA
            if (CS.CL_MIN_V > CS.v_ego):
                alca_state = 0x05
            #max angle for ALCA
            if (abs(actuators.steerAngle) >= CS.CL_MAX_A):
                alca_state = 0x15
            if not enable_steer_control:
                #op_status = 0x08
                hands_on_state = 0x02
            if hud_alert == AH.STEER:
                if snd_chime == CM.MUTE:
                    hands_on_state = 0x03
                else:
                    hands_on_state = 0x05
            if self.PCC.pcc_available:
                acc_speed_kph = self.PCC.pedal_speed_kph
            if hud_alert == AH.FCW:
                collision_warning = hud_alert[1]
                if collision_warning > 1:
                    collision_warning = 1
                #use disabling for alerts/errors to make them aware someting is goin going on
            if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW):
                op_status = 0x08
            if self.ACC.enable_adaptive_cruise:
                acc_speed_kph = self.ACC.new_speed  #pcm_speed * CV.MS_TO_KPH
            if (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or (
                    self.ACC.enable_adaptive_cruise):
                speed_control_enabled = 1
                cc_state = 2
                if not self.ACC.adaptive:
                    cc_state = 3
                CS.speed_control_enabled = 1
            else:
                CS.speed_control_enabled = 0
                if (CS.pcm_acc_status == 4):
                    #car CC enabled but not OP, display the HOLD message
                    cc_state = 3
        else:
            if (CS.pcm_acc_status == 4):
                cc_state = 3
        if enabled:
            if frame % 2 == 0:
                send_fake_msg = True
            if frame % 25 == 0:
                send_fake_warning = True
        else:
            if frame % 23 == 0:
                send_fake_msg = True
            if frame % 60 == 0:
                send_fake_warning = True
        if frame % 10 == 0:
            can_sends.append(
                teslacan.create_fake_DAS_obj_lane_msg(
                    self.leadDx, self.leadDy, self.leadClass, self.rLine,
                    self.lLine, self.curv0, self.curv1, self.curv2, self.curv3,
                    self.laneRange, self.laneWidth))
        speed_override = 0
        if (CS.pedal_interceptor_value > 10) and (cc_state > 1):
            speed_override = 0  #force zero for now
        if (not enable_steer_control) and op_status == 3:
            #hands_on_state = 0x03
            self.DAS_219_lcTempUnavailableSpeed = 1
            self.warningCounter = 100
            self.warningNeeded = 1
        if enabled and self.ALCA.laneChange_cancelled and (
                not CS.steer_override) and (
                    CS.turn_signal_stalk_state
                    == 0) and (self.ALCA.laneChange_cancelled_counter > 0):
            self.DAS_221_lcAborting = 1
            self.warningCounter = 300
            self.warningNeeded = 1
        if CS.hasTeslaIcIntegration:
            highLowBeamStatus, highLowBeamReason, ahbIsEnabled = self.AHB.update(
                CS, frame, self.ahbLead1)
            if frame % 5 == 0:
                self.cc_counter = (
                    self.cc_counter +
                    1) % 40  #use this to change status once a second
                self.fleet_speed_state = 0x00  #fleet speed unavailable
                if FleetSpeed.is_available(CS):
                    if self.ACC.fleet_speed.is_active(
                            frame) or self.PCC.fleet_speed.is_active(frame):
                        self.fleet_speed_state = 0x02  #fleet speed enabled
                    else:
                        self.fleet_speed_state = 0x01  #fleet speed available
                can_sends.append(
                    teslacan.create_fake_DAS_msg2(highLowBeamStatus,
                                                  highLowBeamReason,
                                                  ahbIsEnabled,
                                                  self.fleet_speed_state))
        if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02):
            CS.v_cruise_pcm = CS.v_cruise_pcm + 1
            send_fake_msg = True
        if (self.cc_counter == 3):
            send_fake_msg = True
        if send_fake_msg:
            if enable_steer_control and op_status == 3:
                op_status = 0x5
            park_brake_request = 0  #experimental; disabled for now
            if park_brake_request == 1:
                print("Park Brake Request received")
            adaptive_cruise = 1 if (
                not self.PCC.pcc_available
                and self.ACC.adaptive) or self.PCC.pcc_available else 0
            can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
                  acc_speed_kph, \
                  self.blinker.override_direction,forward_collision_warning, adaptive_cruise,  hands_on_state, \
                  cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \
                  CS.v_cruise_pcm,
                  CS.DAS_fusedSpeedLimit,
                  apply_angle,
                  1 if enable_steer_control else 0,
                  park_brake_request))
        if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (
                self.stopSignWarning != self.stopSignWarning_last) or (
                    self.stopLightWarning != self.stopLightWarning_last) or (
                        self.warningNeeded == 1) or (frame % 100 == 0):
            #if it's time to send OR we have a warning or emergency disable
            can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \
                  self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \
                  self.stopSignWarning, self.stopLightWarning, \
                  self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \
                  self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,CS.useWithoutHarness,CS.usesApillarHarness))
            self.stopLightWarning_last = self.stopLightWarning
            self.stopSignWarning_last = self.stopSignWarning
            self.warningNeeded = 0
        # end of DAS emulation """
        if frame % 100 == 0:  # and CS.hasTeslaIcIntegration:
            #IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake
            can_sends.append(teslacan.create_fake_IC_msg())

        # send enabled ethernet every 0.2 sec
        if frame % 20 == 0:
            can_sends.append(teslacan.create_enabled_eth_msg(1))
        if (not self.PCC.pcc_available
            ) and frame % 5 == 0:  # acc processed at 20Hz
            cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \
                          self.speed_limit_ms * CV.MS_TO_KPH,
                          self.set_speed_limit_active, self.speed_limit_offset)
            if cruise_btn:
                cruise_msg = teslacan.create_cruise_adjust_msg(
                    spdCtrlLvr_stat=cruise_btn,
                    turnIndLvr_Stat=0,
                    real_steering_wheel_stalk=CS.steering_wheel_stalk)
                # Send this CAN msg first because it is racing against the real stalk.
                can_sends.insert(0, cruise_msg)
        apply_accel = 0.
        if self.PCC.pcc_available and frame % 5 == 0:  # pedal processed at 20Hz
            pedalcan = 2
            if CS.useWithoutHarness:
                pedalcan = 0
            apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \
                          self.speed_limit_ms,
                          self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled)
            can_sends.append(
                teslacan.create_pedal_command_msg(apply_accel,
                                                  int(accel_needed), accel_idx,
                                                  pedalcan))
        self.last_angle = apply_angle
        self.last_accel = apply_accel

        return pedal_can_sends + can_sends

    #to show lead car on IC
    def showLeadCarOnICCanMessage(self, radarStateMsg):
        messages = []
        leads = radarStateMsg.radarState
        if leads is None:
            self.ahbLead1 = None
            return messages
        lead_1 = leads.leadOne
        lead_2 = leads.leadTwo
        if (lead_1 is not None) and lead_1.status:
            self.ahbLead1 = lead_1
            self.leadDx = lead_1.dRel
            self.leadDy = self.curv0 - lead_1.yRel
            self.leadId = self.icLeadsData.lead1trackId
            self.leadClass = self.icLeadsData.lead1oClass
            self.leadVx = lead_1.vRel
            if (self.leadId <= 0) or (self.leadId == 63):
                self.leadId = 61
        else:
            self.leadDx = 0.
            self.leadDy = 0.
            self.leadClass = 0
            self.leadId = 0
            self.leadVx = 0xF
        if (lead_2 is not None) and lead_2.status:
            self.lead2Dx = lead_2.dRel
            self.lead2Dy = self.curv0 - lead_2.yRel
            self.lead2Id = self.icLeadsData.lead2trackId
            self.lead2Class = self.icLeadsData.lead2oClass
            self.lead2Vx = lead_2.vRel
            if (self.lead2Id <= 0) or (self.lead2Id == 63):
                self.leadId = 62
        else:
            self.lead2Dx = 0.
            self.lead2Dy = 0.
            self.lead2Class = 0
            self.lead2Id = 0
            self.lead2Vx = 0xF
        messages.append(
            teslacan.create_DAS_LR_object_msg(0, self.leadClass, self.leadId,
                                              self.leadDx, self.leadDy,
                                              self.leadVx, self.lead2Class,
                                              self.lead2Id, self.lead2Dx,
                                              self.lead2Dy, self.lead2Vx))
        return messages

    def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData,
                                             CS):
        pp = pathPlanMsg.pathPlan
        if pp.paramsValid:
            if pp.lProb > 0.75:
                self.lLine = 3
            elif pp.lProb > 0.5:
                self.lLine = 2
            elif pp.lProb > 0.25:
                self.lLine = 1
            else:
                self.lLine = 0
            if pp.rProb > 0.75:
                self.rLine = 3
            elif pp.rProb > 0.5:
                self.rLine = 2
            elif pp.rProb > 0.25:
                self.rLine = 1
            else:
                self.rLine = 0
            #first we clip to the AP limits of the coefficients
            self.curv0 = -clip(
                pp.dPoly[3], -3.5,
                3.5)  #self.curv0Matrix.add(-clip(pp.cPoly[3],-3.5,3.5))
            self.curv1 = -clip(
                pp.dPoly[2], -0.2,
                0.2)  #self.curv1Matrix.add(-clip(pp.cPoly[2],-0.2,0.2))
            self.curv2 = -clip(
                pp.dPoly[1], -0.0025, 0.0025
            )  #self.curv2Matrix.add(-clip(pp.cPoly[1],-0.0025,0.0025))
            self.curv3 = -clip(
                pp.dPoly[0], -0.00003, 0.00003
            )  #self.curv3Matrix.add(-clip(pp.cPoly[0],-0.00003,0.00003))
            self.laneWidth = pp.laneWidth
            self.visionCurvC0 = self.curv0
            self.prev_ldwStatus = self.ldwStatus
            self.ldwStatus = 0
            if alcaStateData.alcaEnabled:
                #exagerate position a little during ALCA to make lane change look smoother on IC
                self.curv1 = 0.0  #straighten the turn for ALCA
                self.curv0 = -self.ALCA.laneChange_direction * alcaStateData.alcaLaneWidth * alcaStateData.alcaStep / alcaStateData.alcaTotalSteps  #animas late change on IC
                self.curv0 = clip(self.curv0, -3.5, 3.5)
                self.lLine = 3
                self.rLine = 3
            else:
                if self.should_ldw:
                    if pp.lProb > LDW_LANE_PROBAB:
                        lLaneC0 = -pp.lPoly[3]
                        if abs(lLaneC0) < LDW_WARNING_2:
                            self.ldwStatus = 3
                        elif abs(lLaneC0) < LDW_WARNING_1:
                            self.ldwStatus = 1
                    if pp.rProb > LDW_LANE_PROBAB:
                        rLaneC0 = -pp.rPoly[3]
                        if abs(rLaneC0) < LDW_WARNING_2:
                            self.ldwStatus = 4
                        elif abs(rLaneC0) < LDW_WARNING_1:
                            self.ldwStatus = 2
            if not (self.prev_ldwStatus == self.ldwStatus):
                self.warningNeeded = 1
                if self.ldwStatus > 0:
                    self.warningCounter = 50
        else:
            self.lLine = 0
            self.rLine = 0
            self.curv0 = self.curv0Matrix.add(0.)
            self.curv1 = self.curv1Matrix.add(0.)
            self.curv2 = self.curv2Matrix.add(0.)
            self.curv3 = self.curv3Matrix.add(0.)

    # Generates IC messages for the Left and Right radar identified cars from radard
    def showLeftAndRightCarsOnICCanMessages(self, icCarLRMsg):
        messages = []
        icCarLR_msg = icCarLRMsg
        if icCarLR_msg is not None:
            #for icCarLR_msg in icCarLR_list:
            messages.append(
                teslacan.create_DAS_LR_object_msg(
                    1, icCarLR_msg.v1Type, icCarLR_msg.v1Id, icCarLR_msg.v1Dx,
                    icCarLR_msg.v1Dy, icCarLR_msg.v1Vrel, icCarLR_msg.v2Type,
                    icCarLR_msg.v2Id, icCarLR_msg.v2Dx, icCarLR_msg.v2Dy,
                    icCarLR_msg.v2Vrel))
            messages.append(
                teslacan.create_DAS_LR_object_msg(
                    2, icCarLR_msg.v3Type, icCarLR_msg.v3Id, icCarLR_msg.v3Dx,
                    icCarLR_msg.v3Dy, icCarLR_msg.v3Vrel, icCarLR_msg.v4Type,
                    icCarLR_msg.v4Id, icCarLR_msg.v4Dx, icCarLR_msg.v4Dy,
                    icCarLR_msg.v4Vrel))
        return messages

    def handleTrafficEvents(self, trafficEventsMsgs):
        messages = []
        self.reset_traffic_events()
        tr_ev_list = trafficEventsMsgs
        if tr_ev_list is not None:
            for tr_ev in tr_ev_list.trafficEvents:
                if tr_ev.type == 0x00:
                    if (tr_ev.distance < self.stopSign_distance):
                        self.stopSign_visible = True
                        self.stopSign_distance = tr_ev.distance
                        self.stopSign_action = tr_ev.action
                        self.stopSign_resume = tr_ev.resuming
                if tr_ev.type == 0x04:
                    if (tr_ev.distance < self.stopLight_distance):
                        self.stopLight_visible = True
                        self.stopLight_distance = tr_ev.distance
                        self.stopLight_action = tr_ev.action
                        self.stopLight_resume = tr_ev.resuming
                        self.stopLight_color = 1.  #0-unknown, 1-red, 2-yellow, 3-green
                if tr_ev.type == 0x01:
                    if (tr_ev.distance < self.stopLight_distance):
                        self.stopLight_visible = True
                        self.stopLight_distance = tr_ev.distance
                        self.stopLight_action = tr_ev.action
                        self.stopLight_resume = tr_ev.resuming
                        self.stopLight_color = 1.  #0-unknown, 1-red, 2-yellow, 3-green
                if tr_ev.type == 0x02:
                    if (tr_ev.distance < self.stopLight_distance):
                        self.stopLight_visible = True
                        self.stopLight_distance = tr_ev.distance
                        self.stopLight_action = tr_ev.action
                        self.stopLight_resume = tr_ev.resuming
                        self.stopLight_color = 2.  #0-unknown, 1-red, 2-yellow, 3-green
                if tr_ev.type == 0x03:
                    if (tr_ev.distance < self.stopLight_distance):
                        self.stopLight_visible = True
                        self.stopLight_distance = tr_ev.distance
                        self.stopLight_action = tr_ev.action
                        self.stopLight_resume = tr_ev.resuming
                        self.stopLight_color = 3.  #0-unknown, 1-red, 2-yellow, 3-green
            self.checkWhichSign()
            if not ((self.roadSignType_last == self.roadSignType) and
                    (self.roadSignType == 0xFF)):
                messages.append(
                    teslacan.create_fake_DAS_sign_msg(
                        self.roadSignType, self.roadSignStopDist,
                        self.roadSignColor, self.roadSignControlActive))
        return messages

    def _should_ldw(self, CS, frame):
        if not CS.enableLdw:
            return False
        if CS.prev_turn_signal_blinking and not CS.turn_signal_blinking:
            self.ldw_numb_frame_end = frame + int(100 * CS.ldwNumbPeriod)

        return CS.v_ego >= self.LDW_ENABLE_SPEED and not CS.turn_signal_blinking and frame > self.ldw_numb_frame_end
예제 #4
0
class CarController(object):
    def __init__(self, canbus, car_fingerprint, allow_controls):
        self.pedal_steady = 0.
        self.start_time = sec_since_boot()
        self.chime = 0
        self.steer_idx = 0
        self.apply_steer_last = 0
        self.car_fingerprint = car_fingerprint
        self.allow_controls = allow_controls
        self.lka_icon_status_last = (False, False)
        self.ALCA = ALCAController(
            self, True, False)  # Enabled  True and SteerByAngle only False

        # Setup detection helper. Routes commands to
        # an appropriate CAN bus number.
        self.canbus = canbus
        self.params = CarControllerParams(car_fingerprint)

        self.packer_pt = CANPacker(DBC[car_fingerprint]['pt'])
        self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis'])

    def update(self, sendcan, enabled, CS, frame, actuators, \
               hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
        """ Controls thread """
        #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)

        # 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)

        # Sanity check.
        if not self.allow_controls:
            return

        P = self.params
        # Send CAN commands.
        can_sends = []
        canbus = self.canbus

        ### STEER ###

        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
            if lkas_enabled:
                apply_steer = alca_steer * P.STEER_MAX
                apply_steer = apply_std_steer_torque_limits(
                    apply_steer, self.apply_steer_last, CS.steer_torque_driver,
                    P)
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame / P.STEER_STEP) % 4
            if not CS.lane_departure_toggle_on:
                apply_steer = 0

            if self.car_fingerprint in SUPERCRUISE_CARS:
                can_sends += gmcan.create_steering_control_ct6(
                    self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
                    lkas_enabled)
            else:
                can_sends.append(
                    gmcan.create_steering_control(self.packer_pt,
                                                  canbus.powertrain,
                                                  apply_steer, idx,
                                                  lkas_enabled))

        ### GAS/BRAKE ###

        if self.car_fingerprint not in SUPERCRUISE_CARS:
            # no output if not enabled, but keep sending keepalive messages
            # treat pedals as one
            final_pedal = actuators.gas - actuators.brake

            # *** apply pedal hysteresis ***
            final_brake, self.brake_steady = actuator_hystereses(
                final_pedal, self.pedal_steady)

            if not enabled:
                # Stock ECU sends max regen when not enabled.
                apply_gas = P.MAX_ACC_REGEN
                apply_brake = 0
            else:
                apply_gas = int(
                    round(interp(final_pedal, P.GAS_LOOKUP_BP,
                                 P.GAS_LOOKUP_V)))
                apply_brake = int(
                    round(
                        interp(final_pedal, P.BRAKE_LOOKUP_BP,
                               P.BRAKE_LOOKUP_V)))

            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                idx = (frame / 4) % 4

                car_stopping = apply_gas < P.ZERO_GAS
                standstill = CS.pcm_acc_status == AccState.STANDSTILL
                at_full_stop = enabled and standstill and car_stopping
                near_stop = enabled and (
                    CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, canbus.chassis, apply_brake, idx,
                        near_stop, at_full_stop))
                acc_enabled = enabled
                if CS.cstm_btns.get_button_status("stop") > 0:
                    # Auto-resume from full stop by resetting ACC control
                    if standstill and not car_stopping:
                        acc_enabled = False

                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   canbus.powertrain,
                                                   apply_gas, idx, acc_enabled,
                                                   at_full_stop))

            # Send dashboard UI commands (ACC status), 25hz
            follow_level = CS.get_follow_level()
            if (frame % 4) == 0:
                can_sends.append(
                    gmcan.create_acc_dashboard_command(
                        self.packer_pt, canbus.powertrain, enabled,
                        hud_v_cruise * CV.MS_TO_KPH, hud_show_car,
                        follow_level))

            # Radar needs to know current speed and yaw rate (50hz),
            # and that ADAS is alive (10hz)
            time_and_headlights_step = 10
            tt = sec_since_boot()

            if frame % time_and_headlights_step == 0:
                idx = (frame / time_and_headlights_step) % 4
                can_sends.append(
                    gmcan.create_adas_time_status(
                        canbus.obstacle, int((tt - self.start_time) * 60),
                        idx))
                can_sends.append(
                    gmcan.create_adas_headlights_status(canbus.obstacle))

            speed_and_accelerometer_step = 2
            if frame % speed_and_accelerometer_step == 0:
                idx = (frame / speed_and_accelerometer_step) % 4
                can_sends.append(
                    gmcan.create_adas_steering_status(canbus.obstacle, idx))
                can_sends.append(
                    gmcan.create_adas_accelerometer_speed_status(
                        canbus.obstacle, CS.v_ego, idx))

            if frame % P.ADAS_KEEPALIVE_STEP == 0:
                can_sends += gmcan.create_adas_keepalive(canbus.powertrain)

            # Show green icon when LKA torque is applied, and
            # alarming orange icon when approaching torque limit.
            # If not sent again, LKA icon disappears in about 5 seconds.
            # Conveniently, sending camera message periodically also works as a keepalive.
            lka_active = CS.lkas_status == 1
            lka_critical = lka_active and abs(actuators.steer) > 0.9
            lka_icon_status = (lka_active, lka_critical)
            if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
                or lka_icon_status != self.lka_icon_status_last:
                can_sends.append(
                    gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
                                                  lka_critical))
                self.lka_icon_status_last = lka_icon_status

        # Send chimes
        if self.chime != chime:
            duration = 0x3c

            # There is no 'repeat forever' chime command
            # TODO: Manage periodic re-issuing of chime command
            # and chime cancellation
            if chime_cnt == -1:
                chime_cnt = 10

            if chime != 0:
                can_sends.append(
                    gmcan.create_chime_command(canbus.sw_gmlan, chime,
                                               duration, chime_cnt))

            # If canceling a repeated chime, cancel command must be
            # issued for the same chime type and duration
            self.chime = chime

        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #5
0
class CarController(object):
    def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
                 enable_apg):
        self.braking = False
        # redundant safety check with the board
        self.controls_allowed = True
        self.last_steer = 0
        self.last_angle = 0
        self.accel_steady = 0.
        self.car_fingerprint = car_fingerprint
        self.alert_active = False
        self.last_standstill = False
        self.standstill_req = False
        self.angle_control = False

        self.steer_angle_enabled = False
        self.ipas_reset_counter = 0
        self.last_fault_frame = -200

        self.fake_ecus = set()
        if enable_camera: self.fake_ecus.add(ECU.CAM)
        if enable_dsu: self.fake_ecus.add(ECU.DSU)
        if enable_apg: self.fake_ecus.add(ECU.APGS)
        self.ALCA = ALCAController(
            self, True, False)  # Enabled True and SteerByAngle only False

        self.packer = CANPacker(dbc_name)

    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())
예제 #6
0
class CarController(object):
    def __init__(self, dbc_name, enable_camera=True):
        self.braking = False
        self.brake_steady = 0.
        self.brake_last = 0.
        self.enable_camera = enable_camera
        self.packer = CANPacker(dbc_name)
        self.epas_disabled = True
        self.last_angle = 0.
        self.last_accel = 0.
        self.ALCA = ALCAController(self, True,
                                   True)  # Enabled and SteerByAngle both True
        self.ACC = ACCController()
        self.PCC = PCCController(self)
        self.HSO = HSOController(self)
        self.sent_DAS_bootID = False
        context = zmq.Context()
        self.poller = zmq.Poller()
        self.speedlimit = messaging.sub_sock(context,
                                             service_list['speedLimit'].port,
                                             conflate=True,
                                             poller=self.poller)
        self.speedlimit_mph = 0


    def update(self, sendcan, enabled, CS, frame, actuators, \
               pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
               hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
               snd_beep, snd_chime):
        """ Controls thread """

        ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
        if not self.enable_camera:
            return

        # *** no output if not enabled ***
        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 = True

        # vehicle hud display, wait for one update from 10Hz 0x304 msg
        if hud_show_lanes:
            hud_lanes = 1
        else:
            hud_lanes = 0

        # TODO: factor this out better
        if enabled:
            if hud_show_car:
                hud_car = 2
            else:
                hud_car = 1
        else:
            hud_car = 0

        # For lateral control-only, send chimes as a beep since we don't send 0x1fa
        #if CS.CP.radarOffCan:

        #print chime, alert_id, hud_alert
        fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

        hud = HUDData(int(pcm_accel),
                      int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
                      int(snd_beep), snd_chime, fcw_display, acc_alert,
                      steer_required)

        if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
            print "INVALID HUD", hud
            hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)

        # **** process the car messages ****

        # *** compute control surfaces ***

        STEER_MAX = 420
        # Prevent steering while stopped
        MIN_STEERING_VEHICLE_VELOCITY = 0.05  # m/s
        vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)

        # Basic highway lane change logic
        changing_lanes = CS.right_blinker_on or CS.left_blinker_on

        #upodate custom UI buttons and alerts
        CS.UE.update_custom_ui()

        if (frame % 1000 == 0):
            CS.cstm_btns.send_button_info()

        # Update statuses for custom buttons 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)
            #print CS.cstm_btns.get_button_status("alca")

        if CS.pedal_interceptor_available:
            #update PCC module info
            self.PCC.update_stat(CS, True, sendcan)
            self.ACC.enable_adaptive_cruise = False
        else:
            # Update ACC module info.
            self.ACC.update_stat(CS, True)
            self.PCC.enable_pedal_cruise = False

        # Update HSO module info.
        human_control = False

        # update CS.v_cruise_pcm based on module selected.
        if self.ACC.enable_adaptive_cruise:
            CS.v_cruise_pcm = self.ACC.acc_speed_kph
        elif self.PCC.enable_pedal_cruise:
            CS.v_cruise_pcm = self.PCC.pedal_speed_kph
        else:
            CS.v_cruise_pcm = CS.v_cruise_actual
        # Get the angle from ALCA.
        alca_enabled = False
        turn_signal_needed = 0
        alca_steer = 0.
        apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
            enabled, CS, frame, actuators)
        apply_angle = -apply_angle  # Tesla is reversed vs OP.
        human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
        human_lane_changing = changing_lanes and not alca_enabled
        enable_steer_control = (enabled and not human_lane_changing
                                and not human_control)

        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)
        # If human control, send the steering angle as read at steering wheel.
        if human_control:
            apply_angle = CS.angle_steers
        # If blinker is on send the actual angle.
        #if (changing_lanes and (CS.laneChange_enabled < 2)):
        #  apply_angle = CS.angle_steers
        # Send CAN commands.
        can_sends = []
        send_step = 5

        if (True):
            #First we emulate DAS.
            #send DAS_bootID
            if not self.sent_DAS_bootID:
                can_sends.append(teslacan.create_DAS_bootID_msg())
                self.sent_DAS_bootID = True
            else:
                #get speed limit
                for socket, _ in self.poller.poll(0):
                    if socket is self.speedlimit:
                        self.speedlimit_mph = ui.SpeedLimitData.from_bytes(
                            socket.recv()).speed * CV.MS_TO_MPH
                #send DAS_info
                if frame % 100 == 0:
                    can_sends.append(
                        teslacan.create_DAS_info_msg(CS.DAS_info_msg))
                    CS.DAS_info_msg += 1
                    CS.DAS_info_msg = CS.DAS_info_msg % 10
                #send DAS_status
                if frame % 50 == 0:
                    op_status = 0x02
                    hands_on_state = 0x00
                    speed_limit_kph = int(self.speedlimit_mph)
                    alca_state = 0x08
                    if enabled:
                        op_status = 0x03
                        alca_state = 0x08 + turn_signal_needed
                        #if not enable_steer_control:
                        #op_status = 0x04
                        #hands_on_state = 0x03
                        if hud_alert == AH.STEER:
                            if snd_chime == CM.MUTE:
                                hands_on_state = 0x03
                            else:
                                hands_on_state = 0x05
                    can_sends.append(
                        teslacan.create_DAS_status_msg(CS.DAS_status_idx,
                                                       op_status,
                                                       speed_limit_kph,
                                                       alca_state,
                                                       hands_on_state))
                    CS.DAS_status_idx += 1
                    CS.DAS_status_idx = CS.DAS_status_idx % 16
                #send DAS_status2
                if frame % 50 == 0:
                    collision_warning = 0x00
                    acc_speed_limit_mph = CS.v_cruise_pcm * CV.KPH_TO_MPH
                    if hud_alert == AH.FCW:
                        collision_warning = 0x01
                    can_sends.append(
                        teslacan.create_DAS_status2_msg(
                            CS.DAS_status2_idx, acc_speed_limit_mph,
                            collision_warning))
                    CS.DAS_status2_idx += 1
                    CS.DAS_status2_idx = CS.DAS_status2_idx % 16
                #send DAS_bodyControl
                if frame % 50 == 0:
                    can_sends.append(
                        teslacan.create_DAS_bodyControls_msg(
                            CS.DAS_bodyControls_idx, turn_signal_needed))
                    CS.DAS_bodyControls_idx += 1
                    CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16
                #send DAS_control
                if frame % 4 == 0:
                    acc_speed_limit_kph = self.ACC.new_speed  #pcm_speed * CV.MS_TO_KPH
                    accel_min = -15
                    accel_max = 5
                    speed_control_enabled = enabled and (acc_speed_limit_kph >
                                                         0)
                    can_sends.append(
                        teslacan.create_DAS_control(CS.DAS_control_idx,
                                                    speed_control_enabled,
                                                    acc_speed_limit_kph,
                                                    accel_min, accel_max))
                    CS.DAS_control_idx += 1
                    CS.DAS_control_idx = CS.DAS_control_idx % 8
                #send DAS_lanes
                if frame % 10 == 0:
                    can_sends.append(
                        teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx))
                    CS.DAS_lanes_idx += 1
                    CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16
                #send DAS_pscControl
                if frame % 4 == 0:
                    can_sends.append(
                        teslacan.create_DAS_pscControl_msg(
                            CS.DAS_pscControl_idx))
                    CS.DAS_pscControl_idx += 1
                    CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16
                #send DAS_telemetryPeriodic
                if frame % 4 == 0:
                    can_sends.append(
                        teslacan.create_DAS_telemetryPeriodic(
                            CS.DAS_telemetryPeriodic1_idx,
                            CS.DAS_telemetryPeriodic2_idx))
                    CS.DAS_telemetryPeriodic2_idx += 1
                    CS.DAS_telemetryPeriodic2_idx = CS.DAS_telemetryPeriodic2_idx % 10
                    if CS.DAS_telemetryPeriodic2_idx == 0:
                        CS.DAS_telemetryPeriodic1_idx += 2
                        CS.DAS_telemetryPeriodic1_idx = CS.DAS_telemetryPeriodic1_idx % 16
                #send DAS_telemetryEvent
                if frame % 10 == 0:
                    #can_sends.append(teslacan.create_DAS_telemetryEvent(CS.DAS_telemetryEvent1_idx,CS.DAS_telemetryEvent2_idx))
                    CS.DAS_telemetryEvent2_idx += 1
                    CS.DAS_telemetryEvent2_idx = CS.DAS_telemetryEvent2_idx % 10
                    if CS.DAS_telemetryEvent2_idx == 0:
                        CS.DAS_telemetryEvent1_idx += 2
                        CS.DAS_telemetryEvent1_idx = CS.DAS_telemetryEvent1_idx % 16
                #send DAS_visualDebug
                if (frame + 1) % 10 == 0:
                    can_sends.append(teslacan.create_DAS_visualDebug_msg())
                #send DAS_chNm
                if (frame + 2) % 10 == 0:
                    can_sends.append(teslacan.create_DAS_chNm())
                #send DAS_objects
                if frame % 3 == 0:
                    can_sends.append(
                        teslacan.create_DAS_objects_msg(CS.DAS_objects_idx))
                    CS.DAS_objects_idx += 1
                    CS.DAS_objects_idx = CS.DAS_objects_idx % 16
                #send DAS_warningMatrix0
                if frame % 6 == 0:
                    can_sends.append(
                        teslacan.create_DAS_warningMatrix0(
                            CS.DAS_warningMatrix0_idx))
                    CS.DAS_warningMatrix0_idx += 1
                    CS.DAS_warningMatrix0_idx = CS.DAS_warningMatrix0_idx % 16
                #send DAS_warningMatrix3
                if (frame + 3) % 6 == 0:
                    driverResumeRequired = 0
                    if enabled and not enable_steer_control:
                        driverResumeRequired = 1
                    can_sends.append(
                        teslacan.create_DAS_warningMatrix3(
                            CS.DAS_warningMatrix3_idx, driverResumeRequired))
                    CS.DAS_warningMatrix3_idx += 1
                    CS.DAS_warningMatrix3_idx = CS.DAS_warningMatrix3_idx % 16
                #send DAS_warningMatrix1
                if frame % 100 == 0:
                    can_sends.append(
                        teslacan.create_DAS_warningMatrix1(
                            CS.DAS_warningMatrix1_idx))
                    CS.DAS_warningMatrix1_idx += 1
                    CS.DAS_warningMatrix1_idx = CS.DAS_warningMatrix1_idx % 16
            # end of DAS emulation """
            idx = frame % 16
            can_sends.append(
                teslacan.create_steering_control(enable_steer_control,
                                                 apply_angle, idx))
            can_sends.append(teslacan.create_epb_enable_signal(idx))
            cruise_btn = None
            if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available:
                cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators,
                                                 pcm_speed)

            #add fake carConfig to trigger IC to display AP
            if frame % 2 == 0:
                carConfig_msg = teslacan.create_GTW_carConfig_msg(
                    real_carConfig_data=CS.real_carConfig,
                    dasHw=1,
                    autoPilot=1,
                    fRadarHw=1)
                #can_sends.append(carConfig_msg)

            if cruise_btn or (turn_signal_needed > 0 and frame % 2 == 0):
                cruise_msg = teslacan.create_cruise_adjust_msg(
                    spdCtrlLvr_stat=cruise_btn,
                    turnIndLvr_Stat=0,  #turn_signal_needed,
                    real_steering_wheel_stalk=CS.steering_wheel_stalk)
                # Send this CAN msg first because it is racing against the real stalk.
                can_sends.insert(0, cruise_msg)
            apply_accel = 0.
            if CS.pedal_interceptor_available and frame % 5 == 0:  # pedal processed at 20Hz
                apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
                    enabled, CS, frame, actuators, pcm_speed)
                can_sends.append(
                    teslacan.create_pedal_command_msg(apply_accel,
                                                      int(accel_needed),
                                                      accel_idx))
            self.last_angle = apply_angle
            self.last_accel = apply_accel
            sendcan.send(
                can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #7
0
class CarController(object):
  def __init__(self, car_fingerprint):
    self.start_time = sec_since_boot()
    self.lkas_active = False
    self.steer_idx = 0
    self.apply_steer_last = 0
    self.car_fingerprint = car_fingerprint
    self.es_distance_cnt = -1
    self.es_lkas_cnt = -1
    self.counter = 0
    
    self.ALCA = ALCAController(self,True,False) # Enabled  True and SteerByAngle only False
    
    # Setup detection helper. Routes commands to
    # an appropriate CAN bus number.
    self.params = CarControllerParams(car_fingerprint)
    print(DBC)
    self.packer = CANPacker(DBC[car_fingerprint]['pt'])

  def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert):
    #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)

    # 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)
    """ Controls thread """

    P = self.params

    # Send CAN commands.
    can_sends = []

    ### STEER ###

    if (frame % P.STEER_STEP) == 0:

      final_steer = alca_steer if enabled else 0.
      apply_steer = int(round(final_steer * P.STEER_MAX))

      # limits due to driver torque

      apply_steer = int(round(apply_steer))
      apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)

      lkas_enabled = enabled and not CS.steer_not_allowed

      if not lkas_enabled:
        apply_steer = 0
        
      if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):

        # add noise to prevent lkas fault from constant torque value over 1s
        if enabled and apply_steer == self.apply_steer_last:
          self.counter =+ 1
          if self.counter == 50:
            apply_steer = round(int(apply_steer * 0.99))
        else:
          self.counter = 0

      can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))

      self.apply_steer_last = apply_steer

    if self.car_fingerprint not in (CAR.OUTBACK, CAR.LEGACY):

      if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
        can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
        self.es_distance_cnt = CS.es_distance_msg["Counter"]

      if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert))
        self.es_lkas_cnt = CS.es_lkas_msg["Counter"]

    if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY) and pcm_cancel_cmd:
      can_sends.append(subarucan.create_door_control(self.packer))

    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
예제 #8
0
class CarController(object):
  def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg):
    self.braking = False
    # redundant safety check with the board
    self.controls_allowed = True
    self.last_steer = 0
    self.last_angle = 0
    self.accel_steady = 0.
    self.car_fingerprint = car_fingerprint
    self.alert_active = False
    self.last_standstill = False
    self.standstill_req = False
    self.angle_control = False
    self.blindspot_poll_counter = 0
    self.blindspot_blink_counter_left = 0
    self.blindspot_blink_counter_right = 0
    self.steer_angle_enabled = False
    self.ipas_reset_counter = 0
    self.last_fault_frame = -200
    self.blindspot_debug_enabled_left = False
    self.blindspot_debug_enabled_right = False

    self.fake_ecus = set()
    if enable_camera: self.fake_ecus.add(ECU.CAM)
    if enable_dsu: self.fake_ecus.add(ECU.DSU)
    if enable_apg: self.fake_ecus.add(ECU.APGS)
    self.ALCA = ALCAController(self,True,False)  # Enabled True and SteerByAngle only False

    self.packer = CANPacker(dbc_name)

  def update(self, sendcan, enabled, CS, frame, actuators,
             pcm_cancel_cmd, hud_alert, audible_alert):
    #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 send brake values 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 * STEER_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 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
      #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", 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:
        if CS.lane_departure_toggle_on:
          can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
        else:
          can_sends.append(create_steer_command(self.packer, 0., 0, frame))
        # rav4h with dsu disconnected

    if self.angle_control:
      if CS.lane_departure_toggle_on:
        can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
                                                   ECU.APGS in self.fake_ecus))
      else:
        can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
    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 = 0b01110011 #x73 comma with toggle - toggle is 5th bit from right
      CS.cstm_btns.set_button_status("tr", 0)
    else:
      distance = 0b01100011 #x63 comma with toggle - toggle is 5th bit from right
    # 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, distance))
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, distance))

    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 self.car_fingerprint not in NO_DSU_CAR:
      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())
예제 #9
0
class CarController(object):
    def __init__(self, dbc_name, enable_camera=True):
        self.braking = False
        self.brake_steady = 0.
        self.brake_last = 0.
        self.enable_camera = enable_camera
        self.packer = CANPacker(dbc_name)
        self.epas_disabled = True
        self.last_angle = 0.
        self.last_accel = 0.
        self.ALCA = ALCAController(self, True,
                                   True)  # Enabled and SteerByAngle both True
        self.ACC = ACCController()
        self.PCC = PCCController(self)
        self.HSO = HSOController(self)


    def update(self, sendcan, enabled, CS, frame, actuators, \
               pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
               hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
               snd_beep, snd_chime):
        """ Controls thread """

        ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
        if not self.enable_camera:
            return

        # *** no output if not enabled ***
        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 = True

        # vehicle hud display, wait for one update from 10Hz 0x304 msg
        if hud_show_lanes:
            hud_lanes = 1
        else:
            hud_lanes = 0

        # TODO: factor this out better
        if enabled:
            if hud_show_car:
                hud_car = 2
            else:
                hud_car = 1
        else:
            hud_car = 0

        # For lateral control-only, send chimes as a beep since we don't send 0x1fa
        #if CS.CP.radarOffCan:

        #print chime, alert_id, hud_alert
        fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

        hud = HUDData(int(pcm_accel),
                      int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
                      int(snd_beep), snd_chime, fcw_display, acc_alert,
                      steer_required)

        if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
            print "INVALID HUD", hud
            hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)

        # **** process the car messages ****

        # *** compute control surfaces ***

        STEER_MAX = 420
        # Prevent steering while stopped
        MIN_STEERING_VEHICLE_VELOCITY = 0.05  # m/s
        vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)

        # Basic highway lane change logic
        changing_lanes = CS.right_blinker_on or CS.left_blinker_on

        #upodate custom UI buttons and alerts
        CS.UE.update_custom_ui()

        if (frame % 1000 == 0):
            CS.cstm_btns.send_button_info()

        # Update statuses for custom buttons 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)
            #print CS.cstm_btns.get_button_status("alca")

        if CS.pedal_hardware_present:
            #update PCC module info
            self.PCC.update_stat(CS, True, sendcan)
            self.ACC.enable_adaptive_cruise = False
        else:
            # Update ACC module info.
            self.ACC.update_stat(CS, True)
            self.PCC.enable_pedal_cruise = False

        # Update HSO module info.
        human_control = False

        # update CS.v_cruise_pcm based on module selected.
        if self.ACC.enable_adaptive_cruise:
            CS.v_cruise_pcm = self.ACC.acc_speed_kph
        elif self.PCC.enable_pedal_cruise:
            CS.v_cruise_pcm = self.PCC.pedal_speed_kph
        else:
            CS.v_cruise_pcm = CS.v_cruise_actual
        # Get the angle from ALCA.
        alca_enabled = False
        turn_signal_needed = 0
        alca_steer = 0.
        apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
            enabled, CS, frame, actuators)
        apply_angle = -apply_angle  # Tesla is reversed vs OP.
        human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
        human_lane_changing = changing_lanes and not alca_enabled
        enable_steer_control = (enabled and not human_lane_changing
                                and not human_control)

        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)
        # If human control, send the steering angle as read at steering wheel.
        if human_control:
            apply_angle = CS.angle_steers
        # If blinker is on send the actual angle.
        #if (changing_lanes and (CS.laneChange_enabled < 2)):
        #  apply_angle = CS.angle_steers
        # Send CAN commands.
        can_sends = []
        send_step = 5

        if (True):
            """#First we emulate DAS.
      if (CS.DAS_info_frm == -1):
        #initialize all frames
        CS.DAS_info_frm = frame # 1.00 s interval
        CS.DAS_status_frm = (frame + 10) % 100 # 0.50 s interval
        CS.DAS_status2_frm = (frame + 35) % 100 # 0.50 s interval in between DAS_status
        CS.DAS_bodyControls_frm = (frame + 40) % 100 # 0.50 s interval
        CS.DAS_lanes_frm = (frame + 5) % 100 # 0.10 s interval 
        CS.DAS_objects_frm = (frame + 2) % 100 # 0.03 s interval
        CS.DAS_pscControl_frm = (frame + 3) % 100 # 0.04 s interval
      if (CS.DAS_info_frm == frame):
        can_sends.append(teslacan.create_DAS_info_msg(CS.DAS_info_msg))
        CS.DAS_info_msg += 1
        CS.DAS_info_msg = CS.DAS_info_msg % 10
      if (CS.DAS_status_frm == frame):
        can_sends.append(teslacan.create_DAS_status_msg(CS.DAS_status_idx))
        CS.DAS_status_idx += 1
        CS.DAS_status_idx = CS.DAS_status_idx % 16
        CS.DAS_status_frm = (CS.DAS_status_frm + 50) % 100
      if (CS.DAS_status2_frm == frame):
        can_sends.append(teslacan.create_DAS_status2_msg(CS.DAS_status2_idx))
        CS.DAS_status2_idx += 1
        CS.DAS_status2_idx = CS.DAS_status2_idx % 16
        CS.DAS_status2_frm = (CS.DAS_status2_frm + 50) % 100
      if (CS.DAS_bodyControls_frm == frame):
        can_sends.append(teslacan.create_DAS_bodyControls_msg(CS.DAS_bodyControls_idx))
        CS.DAS_bodyControls_idx += 1
        CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16
        CS.DAS_bodyControls_frm = (CS.DAS_bodyControls_frm + 50) % 100
      if (CS.DAS_lanes_frm == frame):
        can_sends.append(teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx))
        CS.DAS_lanes_idx += 1
        CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16
        CS.DAS_lanes_frm = (CS.DAS_lanes_frm + 10) % 100
      if (CS.DAS_pscControl_frm == frame):
        can_sends.append(teslacan.create_DAS_pscControl_msg(CS.DAS_pscControl_idx))
        CS.DAS_pscControl_idx += 1
        CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16
        CS.DAS_pscControl_frm = (CS.DAS_pscControl_frm + 4) % 100
      if (CS.DAS_objects_frm == frame):
        can_sends.append(teslacan.create_DAS_objects_msg(CS.DAS_objects_idx))
        CS.DAS_objects_idx += 1
        CS.DAS_objects_idx = CS.DAS_objects_idx % 16
        CS.DAS_objects_frm = (CS.DAS_objects_frm + 3) % 100
      # end of DAS emulation """
            idx = frame % 16
            can_sends.append(
                teslacan.create_steering_control(enable_steer_control,
                                                 apply_angle, idx))
            can_sends.append(teslacan.create_epb_enable_signal(idx))
            cruise_btn = None
            if self.ACC.enable_adaptive_cruise and not self.PCC.pedal_hardware_present:
                cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators,
                                                 pcm_speed)
            if (cruise_btn != None) or ((turn_signal_needed > 0) and
                                        (frame % 2 == 0)):
                cruise_msg = teslacan.create_cruise_adjust_msg(
                    spdCtrlLvr_stat=cruise_btn,
                    turnIndLvr_Stat=turn_signal_needed,
                    real_steering_wheel_stalk=CS.steering_wheel_stalk)
                # Send this CAN msg first because it is racing against the real stalk.
                can_sends.insert(0, cruise_msg)
            apply_accel = 0.
            if self.PCC.pedal_hardware_present:  # and (frame % 10) == 0:
                apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
                    enabled, CS, frame, actuators, pcm_speed)
                can_sends.append(
                    teslacan.create_pedal_command_msg(apply_accel,
                                                      int(accel_needed),
                                                      accel_idx))
            self.last_angle = apply_angle
            self.last_accel = apply_accel
            sendcan.send(
                can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #10
0
class CarController(object):
    def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
                 enable_apg):
        self.braking = False
        # redundant safety check with the board
        self.controls_allowed = True
        self.last_steer = 0
        self.last_angle = 0
        self.accel_steady = 0.
        self.angle_send = 0
        self.car_fingerprint = car_fingerprint
        self.alert_active = False
        self.last_standstill = False
        self.standstill_req = False
        self.angle_control = False

        self.steer_angle_enabled = False
        self.ipas_reset_counter = 0
        self.last_fault_frame = -200

        self.fake_ecus = set()
        if enable_camera: self.fake_ecus.add(ECU.CAM)
        if enable_dsu: self.fake_ecus.add(ECU.DSU)
        if enable_apg: self.fake_ecus.add(ECU.APGS)
        self.ALCA = ALCAController(self, True,
                                   True)  # Enabled True and SteerByAngle True

        self.packer = CANPacker(dbc_name)

    def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
               hud_alert, audible_alert):

        #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 ***

        #Leave this here, will someday use accel...

        # 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 - leave minimum for steer torque incase it's needed
        apply_steer = int(round(actuators.steer * STEER_MAX))

        # steer angle - Currently using steer angle
        if enabled:
            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

        # 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 * STEER_MAX))
        apply_angle = alca_angle

        #Disable if not enabled
        if not enabled:
            apply_angle = 0
            apply_steer_req = 0
        else:
            apply_steer_req = 1

        #Multply by 100 to allow 2 decmals sent over CAN. Arduino will divde by 100.
        angle_send = apply_angle * 100

        #update desired angle for safety loop
        CS.desired_angle = apply_angle

        #Reset enabled time if blinker pressed to not diable during lane change
        if CS.left_blinker_on or CS.right_blinker_on or CS.brake_pressed:
            CS.enabled_time = (
                sec_since_boot() * 1e3
            )  #reset time to not trigger safety after releaaed

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

        can_sends = []

        #Always send CAN steer message
        can_sends.append(
            create_steer_command(self.packer, angle_send, apply_steer_req,
                                 frame))

        # 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))

        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #11
0
class CarController(object):
  def __init__(self, dbc_name, enable_camera=True):
    self.params = Params()
    self.braking = False
    self.brake_steady = 0.
    self.brake_last = 0.
    self.enable_camera = enable_camera
    self.packer = CANPacker(dbc_name)
    self.epas_disabled = True
    self.last_angle = 0.
    self.last_accel = 0.
    self.ALCA = ALCAController(self,True,True)  # Enabled and SteerByAngle both True
    self.ACC = ACCController()
    self.PCC = PCCController(self)
    self.HSO = HSOController(self)
    self.GYRO = GYROController()
    self.sent_DAS_bootID = False
    context = zmq.Context()
    self.poller = zmq.Poller()
    self.speedlimit = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller)
    self.speedlimit_ms = 0
    self.speedlimit_valid = False
    self.speedlimit_units = 0
    self.opState = 0 # 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
    self.accPitch = 0.
    self.accRoll = 0.
    self.accYaw = 0.
    self.magPitch = 0.
    self.magRoll = 0.
    self.magYaw = 0.
    self.gyroPitch = 0.
    self.gyroRoll = 0.
    self.gyroYaw = 0.
    self.set_speed_limit_active = False
    self.speed_limit_offset = 0.

  def update(self, sendcan, enabled, CS, frame, actuators, \
             pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
             hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
             snd_beep, snd_chime):

    """ Controls thread """

    ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars)
    if not self.enable_camera:
      return

    # *** no output if not enabled ***
    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 = True

    # vehicle hud display, wait for one update from 10Hz 0x304 msg
    if hud_show_lanes:
      hud_lanes = 1
    else:
      hud_lanes = 0

    # TODO: factor this out better
    if enabled:
      if hud_show_car:
        hud_car = 2
      else:
        hud_car = 1
    else:
      hud_car = 0
    
    # For lateral control-only, send chimes as a beep since we don't send 0x1fa
    #if CS.CP.radarOffCan:

    #print chime, alert_id, hud_alert
    fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

    hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
 
    if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
      print "INVALID HUD", hud
      hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)

    # **** process the car messages ****

    # *** compute control surfaces ***

    STEER_MAX = 420
    # Prevent steering while stopped
    MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
    vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
    
    # Basic highway lane change logic
    changing_lanes = CS.right_blinker_on or CS.left_blinker_on

    #upodate custom UI buttons and alerts
    CS.UE.update_custom_ui()
      
    if (frame % 1000 == 0):
      CS.cstm_btns.send_button_info()
      #read speed limit params
      self.set_speed_limit_active = self.params.get("SpeedLimitOffset") is not None #self.params.get("LimitSetSpeed") == "1" and 
      if self.set_speed_limit_active:
        self.speed_limit_offset = float(self.params.get("SpeedLimitOffset"))
      else:
        self.speed_limit_offset = 0.

    #get pitch/roll/yaw every 0.1 sec
    if (frame %10 == 0):
      (self.accPitch, self.accRoll, self.accYaw),(self.magPitch, self.magRoll, self.magYaw),(self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update(CS.v_ego,CS.a_ego,CS.angle_steers)
      CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,self.magPitch, self.magRoll, self.magYaw,self.gyroPitch, self.gyroRoll, self.gyroYaw)

    # Update statuses for custom buttons 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 and CS.enableALCA)
      #print CS.cstm_btns.get_button_status("alca")

    
    if CS.pedal_interceptor_available:
      #update PCC module info
      self.PCC.update_stat(CS, True, sendcan)
      self.ACC.enable_adaptive_cruise = False
    else:
      # Update ACC module info.
      self.ACC.update_stat(CS, True)
      self.PCC.enable_pedal_cruise = False
    
    # Update HSO module info.
    human_control = False

    # update CS.v_cruise_pcm based on module selected.
    if self.ACC.enable_adaptive_cruise:
      CS.v_cruise_pcm = self.ACC.acc_speed_kph
    elif self.PCC.enable_pedal_cruise:
      CS.v_cruise_pcm = self.PCC.pedal_speed_kph
    else:
      CS.v_cruise_pcm = CS.v_cruise_actual
    # Get the angle from ALCA.
    alca_enabled = False
    turn_signal_needed = 0
    alca_steer = 0.
    apply_angle, alca_steer,alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators)
    apply_angle = -apply_angle  # Tesla is reversed vs OP.
    human_control = self.HSO.update_stat(CS, enabled, actuators, frame)
    human_lane_changing = changing_lanes and not alca_enabled
    enable_steer_control = (enabled
                            and not human_lane_changing
                            and not human_control 
                            and  vehicle_moving)
    
    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)

    des_angle_factor = interp(CS.v_ego, DES_ANGLE_ADJUST_FACTOR_BP, DES_ANGLE_ADJUST_FACTOR )
    if alca_enabled or not CS.enableSpeedVariableDesAngle:
      des_angle_factor = 1.
    #BB disable limits to test 0.5.8
    # apply_angle = clip(apply_angle * des_angle_factor, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) 
    # If human control, send the steering angle as read at steering wheel.
    if human_control:
      apply_angle = CS.angle_steers

    # Send CAN commands.
    can_sends = []

    #First we emulate DAS.
    # DAS_longC_enabled (1),DAS_gas_to_resume (1),DAS_apUnavailable (1), DAS_collision_warning (1),  DAS_op_status (4)
    # DAS_speed_kph(8), 
    # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (4), 
    # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
    # DAS_acc_speed_limit_mph (8), 
    # DAS_speed_limit_units(8)
    #send fake_das data as 0x553
    # TODO: forward collission warning
    if frame % 10 == 0:
      #get speed limit
      for socket, _ in self.poller.poll(0):
        if socket is self.speedlimit:
          lmd = messaging.recv_one(socket).liveMapData
          self.speedlimit_ms = lmd.speedLimit
          self.speedlimit_valid = lmd.speedLimitValid
          if (self.params.get("IsMetric") == "1"):
            self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_KPH + 0.5
          else:
            self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_MPH + 0.5 
    op_status = 0x02
    hands_on_state = 0x00
    forward_collision_warning = 0 #1 if needed
    if hud_alert == AH.FCW:
      forward_collision_warning = hud_alert[1]
      if forward_collision_warning > 1:
        forward_collision_warning = 1
    #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold
    cc_state = 1 
    speed_limit_to_car = int(self.speedlimit_units)
    alca_state = 0x00 
    apUnavailable = 0
    gas_to_resume = 0
    collision_warning = 0x00
    acc_speed_limit_mph = 0
    speed_control_enabled = 0
    accel_min = -15
    accel_max = 5
    acc_speed_kph = 0
    if enabled:
      #self.opState  0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
      if self.opState == 0:
        op_status = 0x02
      if self.opState == 1:
        op_status = 0x03
      if self.opState == 2:
        op_status = 0x08
      if self.opState == 3:
        op_status = 0x01
      if self.opState == 5:
        op_status = 0x03
      alca_state = 0x08 + turn_signal_needed
      #canceled by user
      if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0):
        alca_state = 0x14
      #min speed for ALCA
      if CS.CL_MIN_V > CS.v_ego:
        alca_state = 0x05
      if not enable_steer_control:
        #op_status = 0x08
        hands_on_state = 0x02
        apUnavailable = 1
      if hud_alert == AH.STEER:
        if snd_chime == CM.MUTE:
          hands_on_state = 0x03
        else:
          hands_on_state = 0x05
      acc_speed_limit_mph = max(self.ACC.acc_speed_kph * CV.KPH_TO_MPH,1)
      if CS.pedal_interceptor_available:
        acc_speed_limit_mph = max(self.PCC.pedal_speed_kph * CV.KPH_TO_MPH,1)
      if hud_alert == AH.FCW:
        collision_warning = hud_alert[1]
        if collision_warning > 1:
          collision_warning = 1
        #use disabling for alerts/errors to make them aware someting is goin going on
      if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW):
        op_status = 0x08
      if self.ACC.enable_adaptive_cruise:
        acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH
      if (CS.pedal_interceptor_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise):
        speed_control_enabled = 1
        cc_state = 2
      else:
        if (CS.pcm_acc_status == 4):
          #car CC enabled but not OP, display the HOLD message
          cc_state = 3
    send_fake_msg = False
    send_fake_warning = False
    if enabled:
      if frame % 2 == 0:
        send_fake_msg = True
      if frame % 25 == 0:
        send_fake_warning = True
    else:
      if frame % 23 == 0:
        send_fake_msg = True
      if frame % 60 == 0:
        send_fake_warning = True
    if send_fake_msg:
      can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,gas_to_resume,apUnavailable, collision_warning, op_status, \
            acc_speed_kph, \
            turn_signal_needed,forward_collision_warning,hands_on_state, \
            cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \
            acc_speed_limit_mph,
            speed_limit_to_car,
            apply_angle,
            1 if enable_steer_control else 0))
    if send_fake_warning or (self.opState == 2) or (self.opState == 5):
      #if it's time to send OR we have a warning or emergency disable
      can_sends.append(teslacan.create_fake_DAS_warning(CS.DAS_noSeatbelt, CS.DAS_canErrors, \
            CS.DAS_plannerErrors, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation))
    # end of DAS emulation """

    idx = frame % 16
    cruise_btn = None
    if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available:
      cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \
                    self.speedlimit_ms * CV.MS_TO_KPH,self.speedlimit_valid, \
                    self.set_speed_limit_active, self.speed_limit_offset)
      if cruise_btn:
          cruise_msg = teslacan.create_cruise_adjust_msg(
            spdCtrlLvr_stat=cruise_btn,
            turnIndLvr_Stat= 0, #turn_signal_needed,
            real_steering_wheel_stalk=CS.steering_wheel_stalk)
          # Send this CAN msg first because it is racing against the real stalk.
          can_sends.insert(0, cruise_msg)
    apply_accel = 0.
    if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz
      apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \
                    self.speedlimit_ms * CV.MS_TO_KPH, self.speedlimit_valid, \
                    self.set_speed_limit_active, self.speed_limit_offset)
      can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx))
    self.last_angle = apply_angle
    self.last_accel = apply_accel
    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #12
0
class CarController():
    def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
                 enable_apg):
        self.braking = False
        # redundant safety check with the board
        self.controls_allowed = True
        self.last_steer = 0
        self.last_angle = 0
        self.accel_steady = 0.
        self.car_fingerprint = car_fingerprint
        self.alert_active = False
        self.last_standstill = False
        self.standstill_req = False
        self.angle_control = False

        self.steer_angle_enabled = False
        self.ipas_reset_counter = 0
        self.last_fault_frame = -200

        self.fake_ecus = set()
        if enable_camera: self.fake_ecus.add(ECU.CAM)
        if enable_dsu: self.fake_ecus.add(ECU.DSU)
        if enable_apg: self.fake_ecus.add(ECU.APGS)
        self.ALCA = ALCAController(
            self, True, False)  # Enabled True and SteerByAngle only False

        self.packer = CANPacker(dbc_name)

    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
        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("{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
            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))

        # 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))
            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.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
예제 #13
0
class CarController(object):
    def __init__(self, dbc_name, car_fingerprint, enable_camera):
        self.braking = False
        # redundant safety check with the board
        self.controls_allowed = True
        self.apply_steer_last = 0
        self.ccframe = 0
        self.prev_frame = -1
        self.hud_count = 0
        self.car_fingerprint = car_fingerprint
        self.alert_active = False
        self.send_chime = False
        self.gone_fast_yet = False

        self.ALCA = ALCAController(
            self, True, False)  # Enabled  True and SteerByAngle only False

        self.fake_ecus = set()
        if enable_camera:
            self.fake_ecus.add(ECU.CAM)

        self.packer = CANPacker(dbc_name)

    def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
               hud_alert, audible_alert):
        #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)

        # 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)

        # this seems needed to avoid steering faults and to force the sync with the EPS counter
        frame = CS.lkas_counter
        if self.prev_frame == frame:
            return

        # *** compute control surfaces ***
        # steer torque
        apply_steer = alca_steer * SteerLimitParams.STEER_MAX
        apply_steer = apply_toyota_steer_torque_limits(apply_steer,
                                                       self.apply_steer_last,
                                                       CS.steer_torque_motor,
                                                       SteerLimitParams)

        moving_fast = CS.v_ego > CS.CP.minSteerSpeed  # for status message
        if CS.v_ego > (CS.CP.minSteerSpeed - 0.5):  # for command high bit
            self.gone_fast_yet = True
        elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
                                      CAR.JEEP_CHEROKEE_2019):
            if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
                self.gone_fast_yet = False  # < 14.5m/s stock turns off this bit, but fine down to 13.5
        lkas_active = moving_fast and enabled

        if not lkas_active:
            apply_steer = 0

        if not CS.lane_departure_toggle_on:
            apply_steer = 0
        self.apply_steer_last = apply_steer

        if audible_alert in LOUD_ALERTS:
            self.send_chime = True

        can_sends = []

        #*** control msgs ***

        if self.send_chime:
            new_msg = create_chimes(AudibleAlert)
            can_sends.append(new_msg)
            if audible_alert not in LOUD_ALERTS:
                self.send_chime = False

        if pcm_cancel_cmd:
            # TODO: would be better to start from frame_2b3
            new_msg = create_wheel_buttons(self.ccframe)
            can_sends.append(new_msg)

        # frame is 100Hz (0.01s period)
        if (self.ccframe % 10 == 0):  # 0.1s period
            new_msg = create_lkas_heartbit(self.packer, CS.lkas_status_ok)
            can_sends.append(new_msg)

        if (self.ccframe % 25 == 0):  # 0.25s period
            if (CS.lkas_car_model != -1):
                new_msg = create_lkas_hud(self.packer, CS.gear_shifter,
                                          lkas_active, hud_alert,
                                          self.hud_count, CS.lkas_car_model)
                can_sends.append(new_msg)
                self.hud_count += 1

        new_msg = create_lkas_command(self.packer, int(apply_steer),
                                      self.gone_fast_yet, frame)
        can_sends.append(new_msg)

        self.ccframe += 1
        self.prev_frame = frame
        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
예제 #14
0
class CarController(object):
    def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu,
                 enable_apg):
        self.braking = False
        # redundant safety check with the board
        self.controls_allowed = True
        self.last_steer = 0
        self.last_angle = 0
        self.accel_steady = 0.
        self.car_fingerprint = car_fingerprint
        self.alert_active = False
        self.last_standstill = False
        self.standstill_req = False
        self.angle_control = False

        self.steer_angle_enabled = False
        self.ipas_reset_counter = 0

        self.fake_ecus = set()
        if enable_camera: self.fake_ecus.add(ECU.CAM)
        if enable_dsu: self.fake_ecus.add(ECU.DSU)
        if enable_apg: self.fake_ecus.add(ECU.APGS)
        self.ALCA = ALCAController(
            self, True, False)  # Enabled True and SteerByAngle only False

        self.packer = CANPacker(dbc_name)

    def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
               hud_alert, audible_alert):
        #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_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 * 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
            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):
            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 and self.car_fingerprint not in NO_DSU_CAR:
            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())