Esempio n. 1
0
def steer_thread():
    context = zmq.Context()
    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
    logcan = messaging.sub_sock(context, service_list['can'].port)

    carstate = messaging.pub_sock(context, service_list['carState'].port)
    carcontrol = messaging.pub_sock(context, service_list['carControl'].port)

    CI, CP = get_car(logcan, sendcan, None)

    print("got car", CP.carName)
    CC = car.CarControl.new_message()

    i = 0
    rate = 0.001
    direction = 1

    while True:

        # send

        CS = CI.update(CC)

        actuators = car.CarControl.Actuators.new_message()

        if i > 0.9 and direction == 1:
            direction = -1
        if i < -0.9 and direction == -1:
            direction = 1

        i += rate * direction

        axis_3 = clip(i * 1.05, -1., 1.)  # -1 to 1
        actuators.steer = axis_3
        actuators.steerAngle = axis_3 * 43.  # deg

        print("steer", actuators.steer)

        CC.actuators.steer = actuators.steer
        CC.actuators.steerAngle = actuators.steerAngle
        CC.enabled = True
        CI.apply(CC)

        # broadcast carState
        cs_send = messaging.new_message()
        cs_send.init('carState')
        cs_send.carState = copy(CS)
        carstate.send(cs_send.to_bytes())

        # broadcast carControl
        cc_send = messaging.new_message()
        cc_send.init('carControl')
        cc_send.carControl = copy(CC)
        carcontrol.send(cc_send.to_bytes())

        # Limit to 100 frames per second
        time.sleep(0.01)
Esempio n. 2
0
def get_car_params(msgs, fsm, can_sock):
    can = FakeSocket(wait=False)
    sendcan = FakeSocket(wait=False)

    canmsgs = [msg for msg in msgs if msg.which() == 'can']
    for m in canmsgs[:300]:
        can.send(m.as_builder().to_bytes())
    _, CP = get_car(can, sendcan)
    Params().put("CarParams", CP.to_bytes())
Esempio n. 3
0
def get_car_params(msgs, fsm, can_sock, fingerprint):
    if fingerprint:
        CarInterface, _, _ = interfaces[fingerprint]
        CP = CarInterface.get_params(fingerprint)
    else:
        can = FakeSocket(wait=False)
        sendcan = FakeSocket(wait=False)

        canmsgs = [msg for msg in msgs if msg.which() == 'can']
        for m in canmsgs[:300]:
            can.send(m.as_builder().to_bytes())
        _, CP = get_car(can, sendcan)
    Params().put("CarParams", CP.to_bytes())
Esempio n. 4
0
def get_car_params(msgs, pub_socks, sub_socks):
    sendcan = pub_socks.get("sendcan", None)
    if sendcan is None:
        sendcan = messaging.pub_sock(service_list["sendcan"].port)
    logcan = sub_socks.get("can", None)
    if logcan is None:
        logcan = messaging.sub_sock(service_list["can"].port)
    can = pub_socks.get("can", None)
    if can is None:
        can = messaging.pub_sock(service_list["can"].port)

    time.sleep(0.5)

    canmsgs = filter(lambda msg: msg.which() == "can", msgs)
    for m in canmsgs[:200]:
        can.send(m.as_builder().to_bytes())
    _, CP = get_car(logcan, sendcan)
    Params().put("CarParams", CP.to_bytes())
    time.sleep(0.5)
    messaging.drain_sock(logcan)
Esempio n. 5
0
def controlsd_thread(gctx=None, rate=100):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    context = zmq.Context()
    params = Params()

    # Pub Sockets
    live100 = messaging.pub_sock(context, service_list['live100'].port)
    carstate = messaging.pub_sock(context, service_list['carState'].port)
    carcontrol = messaging.pub_sock(context, service_list['carControl'].port)

    is_metric = params.get("IsMetric") == "1"
    passive = params.get("Passive") != "0"

    # No sendcan if passive
    if not passive:
        sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
    else:
        sendcan = None

    # Sub sockets
    poller = zmq.Poller()
    thermal = messaging.sub_sock(context,
                                 service_list['thermal'].port,
                                 conflate=True,
                                 poller=poller)
    health = messaging.sub_sock(context,
                                service_list['health'].port,
                                conflate=True,
                                poller=poller)
    cal = messaging.sub_sock(context,
                             service_list['liveCalibration'].port,
                             conflate=True,
                             poller=poller)
    driver_monitor = messaging.sub_sock(context,
                                        service_list['driverMonitoring'].port,
                                        conflate=True,
                                        poller=poller)
    plan_sock = messaging.sub_sock(context,
                                   service_list['plan'].port,
                                   conflate=True,
                                   poller=poller)
    path_plan_sock = messaging.sub_sock(context,
                                        service_list['pathPlan'].port,
                                        conflate=True,
                                        poller=poller)
    logcan = messaging.sub_sock(context, service_list['can'].port)

    CC = car.CarControl.new_message()
    CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)

    if CI is None:
        raise Exception("unsupported car")

    # if stock camera is connected, then force passive behavior
    if not CP.enableCamera:
        passive = True
        sendcan = None

    if passive:
        CP.safetyModel = car.CarParams.SafetyModels.noOutput

    LoC = LongControl(CP, CI.compute_gb)
    VM = VehicleModel(CP)
    LaC = LatControl(CP)
    AM = AlertManager()
    driver_status = DriverStatus()

    if not passive:
        AM.add("startup", False)

    # Write CarParams for radard and boardd safety mode
    params.put("CarParams", CP.to_bytes())
    params.put("LongitudinalControl",
               "1" if CP.openpilotLongitudinalControl else "0")

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    overtemp = False
    free_space = False
    cal_status = Calibration.INVALID
    cal_perc = 0
    mismatch_counter = 0
    low_battery = False

    plan = messaging.new_message()
    plan.init('plan')
    path_plan = messaging.new_message()
    path_plan.init('pathPlan')

    rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
    controls_params = params.get("ControlsParams")
    # Read angle offset from previous drive
    if controls_params is not None:
        controls_params = json.loads(controls_params)
        angle_offset = controls_params['angle_offset']
    else:
        angle_offset = 0.

    prof = Profiler(False)  # off by default

    while True:
        start_time = int(sec_since_boot() * 1e9)
        prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data and compute car events
        CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan  =\
          data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
                      poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
                      state, mismatch_counter, params, plan, path_plan)
        prof.checkpoint("Sample")

        path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
        plan_age = (start_time - plan.logMonoTime) / 1e9
        if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
            events.append(
                create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        events += list(plan.plan.events)

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
            events.append(
                create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        if not passive:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \
          state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
                        v_cruise_kph_last, AM, rk, driver_status,
                        LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)

        prof.checkpoint("State Control")

        # Publish data
        CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events,
                       actuators, v_cruise_kph, rk, carstate, carcontrol,
                       live100, AM, driver_status, LaC, LoC, angle_offset,
                       passive, start_time, params, v_acc, a_acc)
        prof.checkpoint("Sent")

        rk.keep_time()  # Run at 100Hz
        prof.display()
Esempio n. 6
0
def controlsd_thread(sm=None, pm=None, can_sock=None):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    params = Params()

    is_metric = params.get("IsMetric", encoding='utf8') == "1"
    is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
    passive = params.get("Passive", encoding='utf8') == "1"
    openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
                                          encoding='utf8') == "1"
    community_feature_toggle = params.get("CommunityFeaturesToggle",
                                          encoding='utf8') == "1"

    passive = passive or not openpilot_enabled_toggle

    # Passive if internet needed
    internet_needed = params.get("Offroad_ConnectivityNeeded",
                                 encoding='utf8') is not None
    passive = passive or internet_needed

    # Pub/Sub Sockets
    if pm is None:
        pm = messaging.PubMaster([
            'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
            'carParams'
        ])

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

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

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

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

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

    # Write CarParams for radard and boardd safety mode
    cp_bytes = CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)
    put_nonblocking("LongitudinalControl",
                    "1" if CP.openpilotLongitudinalControl else "0")

    CC = car.CarControl.new_message()
    AM = AlertManager()

    startup_alert = get_startup_alert(car_recognized, controller_available)
    AM.add(sm.frame, startup_alert, False)

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

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

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    mismatch_counter = 0
    can_error_counter = 0
    last_blinker_frame = 0
    saturated_count = 0
    events_prev = []

    sm['liveCalibration'].calStatus = Calibration.INVALID
    sm['pathPlan'].sensorValid = True
    sm['pathPlan'].posenetValid = True
    sm['thermal'].freeSpace = 1.
    sm['dMonitoringState'].events = []
    sm['dMonitoringState'].awarenessStatus = 1.
    sm['dMonitoringState'].faceDetected = False

    # detect sound card presence
    sounds_available = not os.path.isfile('/EON') or (
        os.path.isdir('/proc/asound/card0')
        and open('/proc/asound/card0/state').read().strip() == 'ONLINE')

    # controlsd is driven by can recv, expected at 100Hz
    rk = Ratekeeper(100, print_delay_threshold=None)

    prof = Profiler(False)  # off by default

    while True:
        start_time = sec_since_boot()
        prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data and compute car events
        CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(
            CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
            params)
        prof.checkpoint("Sample")

        # Create alerts
        if not sm.alive['plan'] and sm.alive[
                'pathPlan']:  # only plan not being received: radar not communicating
            events.append(
                create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        elif not sm.all_alive_and_valid():
            events.append(
                create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not sm['pathPlan'].mpcSolutionValid:
            events.append(
                create_event('plannerError',
                             [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
        if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
            events.append(
                create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
        if not sm['pathPlan'].paramsValid:
            events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
        if not sm['pathPlan'].posenetValid:
            events.append(
                create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
        if not sm['plan'].radarValid:
            events.append(
                create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if sm['plan'].radarCanError:
            events.append(
                create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not CS.canValid:
            events.append(
                create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
        if not sounds_available:
            events.append(
                create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
        if internet_needed:
            events.append(
                create_event('internetConnectivityNeeded',
                             [ET.NO_ENTRY, ET.PERMANENT]))
        if community_feature_disallowed:
            events.append(
                create_event('communityFeatureDisallowed', [ET.PERMANENT]))
        if read_only and not passive:
            events.append(create_event('carUnrecognized', [ET.PERMANENT]))
        if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
            events.append(
                create_event(
                    'relayMalfunction',
                    [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and sm[
                'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
            events.append(
                create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        if not read_only:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
          state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
                        LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count)

        prof.checkpoint("State Control")

        # Publish data
        CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
                                    actuators, v_cruise_kph, rk, AM, LaC, LoC,
                                    read_only, start_time, v_acc, a_acc,
                                    lac_log, events_prev, last_blinker_frame,
                                    is_ldw_enabled, can_error_counter)
        prof.checkpoint("Sent")

        rk.monitor_time()
        prof.display()
Esempio n. 7
0
    def __init__(self, sm=None, pm=None, can_sock=None):
        config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

        self.startup_event = get_startup_event(car_recognized,
                                               controller_available)

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

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default
Esempio n. 8
0
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    context = zmq.Context()
    params = Params()

    # pub
    live100 = messaging.pub_sock(context, service_list['live100'].port)
    carstate = messaging.pub_sock(context, service_list['carState'].port)
    carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
    livempc = messaging.pub_sock(context, service_list['liveMpc'].port)

    is_metric = params.get("IsMetric") == "1"
    passive = params.get("Passive") != "0"
    if not passive:
        while 1:
            try:
                sendcan = messaging.pub_sock(context,
                                             service_list['sendcan'].port)
                break
            except zmq.error.ZMQError:
                kill_defaultd()
    else:
        sendcan = None

    # sub
    poller = zmq.Poller()
    thermal = messaging.sub_sock(context,
                                 service_list['thermal'].port,
                                 conflate=True,
                                 poller=poller)
    health = messaging.sub_sock(context,
                                service_list['health'].port,
                                conflate=True,
                                poller=poller)
    cal = messaging.sub_sock(context,
                             service_list['liveCalibration'].port,
                             conflate=True,
                             poller=poller)
    driver_monitor = messaging.sub_sock(context,
                                        service_list['driverMonitoring'].port,
                                        conflate=True,
                                        poller=poller)
    gps_location = messaging.sub_sock(context,
                                      service_list['gpsLocationExternal'].port,
                                      conflate=True,
                                      poller=poller)

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

    CC = car.CarControl.new_message()

    CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)

    if CI is None:
        raise Exception("unsupported car")

    # if stock camera is connected, then force passive behavior
    if not CP.enableCamera:
        passive = True
        sendcan = None

    if passive:
        CP.safetyModel = car.CarParams.SafetyModels.noOutput

    fcw_enabled = params.get("IsFcwEnabled") == "1"
    geofence = None

    PL = Planner(CP, fcw_enabled)
    LoC = LongControl(CP, CI.compute_gb)
    VM = VehicleModel(CP)
    LaC = LatControl(VM)
    AM = AlertManager()
    driver_status = DriverStatus()

    if not passive:
        AM.add("startup", False)

    # write CarParams
    params.put("CarParams", CP.to_bytes())

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    overtemp = False
    free_space = False
    cal_status = Calibration.INVALID
    cal_perc = 0
    mismatch_counter = 0
    low_battery = False

    rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)

    # learned angle offset
    angle_offset = default_bias
    calibration_params = params.get("CalibrationParams")
    if calibration_params:
        try:
            calibration_params = json.loads(calibration_params)
            angle_offset = calibration_params["angle_offset2"]
        except (ValueError, KeyError):
            pass

    prof = Profiler(False)  # off by default

    while 1:

        prof.checkpoint("Ratekeeper", ignore=True)

        # sample data and compute car events
        CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(
            CI, CC, thermal, cal, health, driver_monitor, gps_location, poller,
            cal_status, cal_perc, overtemp, free_space, low_battery,
            driver_status, geofence, state, mismatch_counter, params)
        prof.checkpoint("Sample")

        # define plan
        plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph,
                                  driver_status, geofence)
        prof.checkpoint("Plan")

        if not passive:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # compute actuators
        actuators, v_cruise_kph, driver_status, angle_offset = state_control(
            plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM,
            rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive,
            is_metric, cal_perc)
        prof.checkpoint("State Control")

        # publish data
        CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM,
                       state, events, actuators, v_cruise_kph, rk, carstate,
                       carcontrol, live100, livempc, AM, driver_status, LaC,
                       LoC, angle_offset, passive)
        prof.checkpoint("Sent")

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

        prof.display()
Esempio n. 9
0
def controlsd_thread(gctx=None):
    setproctitle('controlsd')
    params = Params()
    print(params)
    # Pub Sockets
    profiler = Profiler(True, 'controls')

    sendcan = messaging.pub_sock(service_list['sendcan'].port)
    controlsstate = messaging.pub_sock(service_list['controlsState'].port)
    carstate = None  #messaging.pub_sock(service_list['carState'].port)
    carcontrol = messaging.pub_sock(service_list['carControl'].port)
    carevents = messaging.pub_sock(service_list['carEvents'].port)
    carparams = messaging.pub_sock(service_list['carParams'].port)

    sm = messaging.SubMaster(['pathPlan', 'health', 'gpsLocationExternal'])
    can_sock = messaging.sub_sock(service_list['can'].port)
    hw_type = messaging.recv_one(sm.sock['health']).health.hwType
    is_panda_black = hw_type == log.HealthData.HwType.blackPanda
    print("panda black: ", is_panda_black)
    wait_for_can(can_sock)
    CI, CP = get_car(can_sock, sendcan, is_panda_black)
    #logcan.close()

    # TODO: Use the logcan socket from above, but that will currenly break the tests
    #can_timeout = None #if os.environ.get('NO_CAN_TIMEOUT', False) else 100
    #can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)

    # Write CarParams for radard and boardd safety mode
    params.put("CarParams", CP.to_bytes())
    params.put("LongitudinalControl",
               "1" if CP.openpilotLongitudinalControl else "0")

    CC = car.CarControl.new_message()
    AM = AlertManager()

    startup_alert = get_startup_alert(True, True)
    AM.add(sm.frame, startup_alert, False)

    LaC = LatControlPID(CP)
    lateral = Lateral(CP)
    lkasMode = int(float(LaC.kegman.conf['lkasMode']))
    #CI.CS.lkasMode = (lkasMode == 0)
    lac_log = None  #car.CarState.lateralControlState.pidState.new_message()

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    events_prev = []
    frame = 0

    sm['pathPlan'].sensorValid = True
    sm['pathPlan'].posenetValid = True

    while True:

        start_time = 0  # time.time()  #sec_since_boot()

        # Sample data and compute car events
        CS, events = data_sample(CI, CC, can_sock, carstate, lac_log, lateral,
                                 sm, profiler)
        profiler.checkpoint('data_sample')

        state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
            state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
        profiler.checkpoint('state_transition')
        # Compute actuators (runs PID loops and lateral MPC)
        sm.update(0)
        profiler.checkpoint('sm_update')

        actuators, lac_log = state_control(sm.frame, lkasMode, sm['pathPlan'],
                                           CS, CP, state, events, AM, LaC,
                                           lac_log, profiler)
        profiler.checkpoint('state_control')

        # Publish data
        CC, events_prev = data_send(sm, CS, CI, CP, state, events, actuators,
                                    carstate, carcontrol, carevents, carparams,
                                    controlsstate, sendcan, AM, LaC,
                                    start_time, lac_log, events_prev, profiler)
        profiler.checkpoint('data_send')
        frame += 1
        if frame % 10000 == 0 and profiler.enabled:
            profiler.display()
            profiler.reset(True)
Esempio n. 10
0
    def __init__(self, sm=None, pm=None, can_sock=None):
        config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

        # controlsd is driven by can recv, expected at 100Hz
        self.rk = Ratekeeper(100, print_delay_threshold=None)
        self.prof = Profiler(False)  # off by default
Esempio n. 11
0
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
  # start the loop
  set_realtime_priority(3)

  context = zmq.Context()

  params = Params()

  # pub
  live100 = messaging.pub_sock(context, service_list['live100'].port)
  carstate = messaging.pub_sock(context, service_list['carState'].port)
  carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
  livempc = messaging.pub_sock(context, service_list['liveMpc'].port)

  passive = params.get("Passive") != "0"
  if not passive:
    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
  else:
    sendcan = None

  # sub
  poller = zmq.Poller()
  thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
  health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
  cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)

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

  CC = car.CarControl.new_message()

  CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)

  if CI is None:
    raise Exception("unsupported car")

  # if stock camera is connected, then force passive behavior
  if not CP.enableCamera:
    passive = True
    sendcan = None

  if passive:
    CP.safetyModel = car.CarParams.SafetyModels.honda

  fcw_enabled = params.get("IsFcwEnabled") == "1"

  PL = Planner(CP, fcw_enabled)
  LoC = LongControl(CP, CI.compute_gb)
  VM = VehicleModel(CP)
  LaC = LatControl(VM)
  AM = AlertManager()

  if not passive:
    AM.add("startup", False)

  # write CarParams
  params.put("CarParams", CP.to_bytes())

  state = State.disabled
  soft_disable_timer = 0
  v_cruise_kph = 255
  overtemp = False
  free_space = False
  cal_status = Calibration.UNCALIBRATED
  rear_view_toggle = False
  rear_view_allowed = params.get("IsRearViewMirror") == "1"

  # 0.0 - 1.0
  awareness_status = 1.
  v_cruise_kph_last = 0

  rk = Ratekeeper(rate, print_delay_threshold=2./1000)

  # learned angle offset
  angle_offset = default_bias
  calibration_params = params.get("CalibrationParams")
  if calibration_params:
    try:
      calibration_params = json.loads(calibration_params)
      angle_offset = calibration_params["angle_offset2"]
    except (ValueError, KeyError):
      pass

  prof = Profiler(False)  # off by default

  while 1:

    prof.checkpoint("Ratekeeper", ignore=True)

    # sample data and compute car events
    CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status,
                                                               overtemp, free_space)
    prof.checkpoint("Sample")

    # define plan
    plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status)
    prof.checkpoint("Plan")

    if not passive:
      # update control state
      state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer,
                                                                                    v_cruise_kph, AM)
      prof.checkpoint("State transition")

    # compute actuators
    actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
                                                                            v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM,
                                                                            angle_offset, rear_view_allowed, rear_view_toggle, passive)
    prof.checkpoint("State Control")

    # publish data
    CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph,
                   rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
                   rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
    prof.checkpoint("Sent")

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

    prof.display()
Esempio n. 12
0
    def __init__(self, sm=None, pm=None, can_sock=None):
        gc.disable()
        set_realtime_priority(3)

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

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

            #self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
            #                               'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])

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

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

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

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

        # detect sound card presence and ensure successful init
        sounds_available = not os.path.isfile('/EON') or (os.path.isfile('/proc/asound/card0/state') \
                                and open('/proc/asound/card0/state').read().strip() == 'ONLINE')

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

        # Write CarParams for radard and boardd safety mode
        cp_bytes = self.CP.to_bytes()
        params.put("CarParams", cp_bytes)
        put_nonblocking("CarParamsCache", cp_bytes)
        put_nonblocking("LongitudinalControl",
                        "1" if self.CP.openpilotLongitudinalControl else "0")

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

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

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

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

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

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

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

        uname = subprocess.check_output(["uname", "-v"],
                                        encoding='utf8').strip()
        if uname == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020":
            self.events.add(EventName.neosUpdateRequired, static=True)

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

        self.hyundai_lkas = self.read_only  #read_only
        self.init_flag = True
Esempio n. 13
0
  def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
    config_realtime_process(4, Priority.CTRL_HIGH)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    # scc smoother
    self.is_cruise_enabled = False
    self.applyMaxSpeed = 0
    self.apply_accel = 0.
    self.fused_accel = 0.
    self.lead_drel = 0.
    self.aReqValue = 0.
    self.aReqValueMin = 0.
    self.aReqValueMax = 0.
    self.sccStockCamStatus = 0
    self.sccStockCamAct = 0

    self.left_lane_visible = False
    self.right_lane_visible = False

    self.wide_camera = TICI and params.get_bool('EnableWideCamera')
    self.disable_op_fcw = params.get_bool('DisableOpFcw')
    self.mad_mode_enabled = Params().get_bool('MadModeEnabled')

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

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

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

    # controlsd is driven by can recv, expected at 100Hz
    self.rk = Ratekeeper(100, print_delay_threshold=None)
    self.prof = Profiler(False)  # off by default
Esempio n. 14
0
def steer_thread():
    poller = zmq.Poller()

    logcan = messaging.sub_sock(service_list['can'].port)
    joystick_sock = messaging.sub_sock(service_list['testJoystick'].port,
                                       conflate=True,
                                       poller=poller)

    carstate = messaging.pub_sock(service_list['carState'].port)
    carcontrol = messaging.pub_sock(service_list['carControl'].port)
    sendcan = messaging.pub_sock(service_list['sendcan'].port)

    button_1_last = 0
    enabled = False

    CI, CP = get_car(logcan, sendcan)

    CC = car.CarControl.new_message()
    joystick = messaging.recv_one(joystick_sock)

    while True:

        # send
        for socket, event in poller.poll(0):
            if socket is joystick_sock:
                joystick = messaging.recv_one(socket)

        can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
        CS = CI.update(CC, can_strs)

        # Usually axis run in pairs, up/down for one, and left/right for
        # the other.
        actuators = car.CarControl.Actuators.new_message()

        axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1.,
                      1.)  # -1 to 1
        actuators.steer = axis_3
        actuators.steerAngle = axis_3 * 43.  # deg
        axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1.,
                      1.)  # -1 to 1
        actuators.gas = max(axis_1, 0.)
        actuators.brake = max(-axis_1, 0.)

        pcm_cancel_cmd = joystick.testJoystick.buttons[0]
        button_1 = joystick.testJoystick.buttons[1]
        if button_1 and not button_1_last:
            enabled = not enabled

        button_1_last = button_1

        #print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake

        hud_alert = 0
        audible_alert = 0
        if joystick.testJoystick.buttons[2]:
            audible_alert = "beepSingle"
        if joystick.testJoystick.buttons[3]:
            audible_alert = "chimeRepeated"
            hud_alert = "steerRequired"

        CC.actuators.gas = actuators.gas
        CC.actuators.brake = actuators.brake
        CC.actuators.steer = actuators.steer
        CC.actuators.steerAngle = actuators.steerAngle
        CC.hudControl.visualAlert = hud_alert
        CC.hudControl.setSpeed = 20
        CC.cruiseControl.cancel = pcm_cancel_cmd
        CC.enabled = enabled
        can_sends = CI.apply(CC)
        sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))

        # broadcast carState
        cs_send = messaging.new_message()
        cs_send.init('carState')
        cs_send.carState = copy(CS)
        carstate.send(cs_send.to_bytes())

        # broadcast carControl
        cc_send = messaging.new_message()
        cc_send.init('carControl')
        cc_send.carControl = copy(CC)
        carcontrol.send(cc_send.to_bytes())

        # Limit to 100 frames per second
        time.sleep(0.01)
Esempio n. 15
0
  def __init__(self, sm=None, pm=None, can_sock=None):
    config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)

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

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

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

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

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

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

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

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

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

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

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

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

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

    self.long_plan_source = 0
    self.controlsAllowed = False

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

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

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

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

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

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

    self.steerRatio_Max = float(int(params.get("SteerRatioMaxAdj", encoding="utf8")) * 0.1)
    self.angle_differ_range = [0, 15]
    self.steerRatio_range = [self.CP.steerRatio, self.steerRatio_Max]
    self.new_steerRatio = self.CP.steerRatio
    self.new_steerRatio_prev = self.CP.steerRatio
    self.steerRatio_to_send = 0
    
    self.model_long_alert_prev = True
    self.delayed_comm_issue_timer = 0
Esempio n. 16
0
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    context = zmq.Context()
    params = Params()

    # pub
    live100 = messaging.pub_sock(context, service_list['live100'].port)
    carstate = messaging.pub_sock(context, service_list['carState'].port)
    carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
    livempc = messaging.pub_sock(context, service_list['liveMpc'].port)

    is_metric = params.get("IsMetric") == "1"
    passive = params.get("Passive") != "0"
    if not passive:
        while 1:
            try:
                sendcan = messaging.pub_sock(context,
                                             service_list['sendcan'].port)
                break
            except zmq.error.ZMQError:
                kill_defaultd()
    else:
        sendcan = None

    # sub
    poller = zmq.Poller()
    thermal = messaging.sub_sock(context,
                                 service_list['thermal'].port,
                                 conflate=True,
                                 poller=poller)
    health = messaging.sub_sock(context,
                                service_list['health'].port,
                                conflate=True,
                                poller=poller)
    cal = messaging.sub_sock(context,
                             service_list['liveCalibration'].port,
                             conflate=True,
                             poller=poller)
    driver_monitor = messaging.sub_sock(context,
                                        service_list['driverMonitoring'].port,
                                        conflate=True,
                                        poller=poller)
    gps_location = messaging.sub_sock(context,
                                      service_list['gpsLocationExternal'].port,
                                      conflate=True,
                                      poller=poller)

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

    CC = car.CarControl.new_message()

    CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)

    if CI is None:
        raise Exception("unsupported car")

    # if stock camera is connected, then force passive behavior
    if not CP.enableCamera:
        passive = True
        sendcan = None

    if passive:
        CP.safetyModel = car.CarParams.SafetyModels.noOutput

    fcw_enabled = params.get("IsFcwEnabled") == "1"
    geofence = None

    PL = Planner(CP, fcw_enabled)
    LoC = LongControl(CP, CI.compute_gb)
    VM = VehicleModel(CP)
    LaC = LatControl(VM)
    AM = AlertManager()
    driver_status = DriverStatus()

    if not passive:
        AM.add("startup", False)

    # write CarParams
    params.put("CarParams", CP.to_bytes())

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    overtemp = False
    free_space = False
    cal_status = Calibration.INVALID
    cal_perc = 0
    mismatch_counter = 0
    low_battery = False

    rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)

    # learned angle offset
    angle_offset = default_bias
    calibration_params = params.get("CalibrationParams")
    if calibration_params:
        try:
            calibration_params = json.loads(calibration_params)
            angle_offset = calibration_params["angle_offset2"]
        except (ValueError, KeyError):
            pass

    prof = Profiler(False)  # off by default

    # Setup for real-time tuning
    rt_tuning_file = '/data/.openpilot_rtt_params.pkl'
    rtt_params = {}
    last_mod_time = 0

    while 1:

        prof.checkpoint("Ratekeeper", ignore=True)

        # sample data and compute car events
        CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(
            CI, CC, thermal, cal, health, driver_monitor, gps_location, poller,
            cal_status, cal_perc, overtemp, free_space, low_battery,
            driver_status, geofence, state, mismatch_counter, params)
        prof.checkpoint("Sample")

        # define plan
        plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph,
                                  driver_status, geofence)
        prof.checkpoint("Plan")

        if not passive:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # compute actuators
        actuators, v_cruise_kph, driver_status, angle_offset = state_control(
            plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM,
            rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive,
            is_metric, cal_perc)
        prof.checkpoint("State Control")

        # publish data
        CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM,
                       state, events, actuators, v_cruise_kph, rk, carstate,
                       carcontrol, live100, livempc, AM, driver_status, LaC,
                       LoC, angle_offset, passive)
        prof.checkpoint("Sent")

        ######################   Real-Time Tuning Add-on  ########################
        # TODO:  Move this into it's own function to clean things up
        # TODO:  Need to delay until fingerprint, or is this after already?
        # Run this once per second... on frame 29, of course.
        if rk.frame % 100 == 29:
            # Get the last update time of our real-time tuning file
            #print('Real-Time Tuning:  Checking tuning file modification time.')
            try:
                mod_time = os.path.getmtime(rt_tuning_file)
                #print('RTT mod_time:  {0}'.format(mod_time))
            except OSError:
                # File doesn't exist or is inaccessible
                mod_time = None
                print(
                    'Real-Time Tuning:  RT_TUNING_FILE did not exist or was inaccessible.'
                )

            # If rt_tuning_file doesn't exist, then create it from the current CarParams:
            if mod_time is None:
                rtt_params['steerKpBP'] = list(
                    CP.steerKpBP
                )  # Note that the Kp/Ki are lists!  But if you reference them directly they are <capnp list builder []>.. oops.
                rtt_params['steerKpV'] = list(CP.steerKpV)
                rtt_params['steerKiBP'] = list(CP.steerKiBP)
                rtt_params['steerKiV'] = list(CP.steerKiV)
                rtt_params['steerKf'] = CP.steerKf
                # TODO:  Give the option to link the front and rear tire stiffness changes together
                rtt_params['tireStiffnessFront'] = CP.tireStiffnessFront
                rtt_params['tireStiffnessRear'] = CP.tireStiffnessRear
                rtt_params['steerRatio'] = CP.steerRatio
                rtt_params['steerRateCost'] = CP.steerRateCost
                rtt_params['latPidDeadzone'] = 0.0
                rtt_params['steerActuatorDelay'] = CP.steerActuatorDelay
                # rtt_params['Camera Offset'] = PL.PP.cam_offset
                # Write the pickle file
                # TODO:  try/except the open
                with open(rt_tuning_file, "wb") as f_write:
                    pickle.dump(
                        rtt_params, f_write,
                        -1)  # Dump to file with highest protocol (fastest)
                # No need to update next time if we just wrote the file out...
                last_mod_time = os.path.getmtime(rt_tuning_file)
                #print('RTT Last_mod_time:  {0}'.format(last_mod_time))

            # If file exists and has been updated since the last time we read it in
            elif last_mod_time != mod_time:
                print(
                    'Real-Time Tuning:  Reading in the modified tuning file.')
                # Read in parameters from file
                # TODO:  try/except the open
                with open(rt_tuning_file, "rb") as f_read:
                    rtt_params = pickle.load(f_read)
                # Sanity check the data before setting it.. format is [min, max, failsafe]
                #   Failsafe is used if a value is not found or if the value sent is out of the range limits
                rt_data_limits = {
                    'steerKpBP': [0.0, 67.0, 0.0],
                    'steerKpV': [0.0, 1.0, 0.2],
                    'steerKiBP': [0.0, 67.0, 0.0],
                    'steerKiV': [0.0, 1.0, 0.05],
                    'steerKf': [0.0, 0.001, 0.00005],
                    'tireStiffnessFront': [20000, 1000000, 192150],
                    'tireStiffnessRear': [20000, 1000000, 202500],
                    'steerRatio': [8.0, 25.0, 14.0],
                    'steerRateCost': [0.05, 1.0, 0.5],
                    'latPidDeadzone': [0.0, 4.0, 0.0],
                    'steerActuatorDelay': [0.0, 0.5, 0.1]
                    # 'Camera Offset': [ -0.2, 0.2, 0.06 ]
                }
                # Do the checks and set the values
                for key in rt_data_limits:
                    rt_val = rtt_params.get(key)
                    if rt_val is None:
                        # If this key from data limits doesn't exist in our tuning data, then add it as the failsafe
                        # TODO:  Use CP value here instead of failsafe?
                        rtt_params[key] = rt_data_limits[key][2]
                        print(
                            'Real-Time Tuning:  Value did not exist in tuning file, replaced with failsafe.  Key: '
                            + key)
                        continue
                    # If it does exist, then check the values.  First see if it's a list
                    try:
                        # If it's an iterable list...
                        for i, val2 in enumerate(rt_val):
                            # Check each value in the list
                            if (val2 < rt_data_limits[key][0]) or (
                                    val2 > rt_data_limits[key][1]):
                                rt_val[i] = rt_data_limits[key][2]
                                print(
                                    'Real-Time Tuning:  Invalid value replaced!  Key: '
                                    + key)
                    except:
                        # Not interable, compare it and fix if necessary
                        if (rt_val < rt_data_limits[key][0]) or (
                                rt_val > rt_data_limits[key][1]):
                            rt_val = rt_data_limits[key][2]
                            print(
                                'Real-Time Tuning:  Invalid value replaced!  Key: '
                                + key)
                    # Set it back so if anything was fixed we have the updated value
                    rtt_params[key] = rt_val

                # Update CP with the new params
                CP.steerKpBP = rtt_params['steerKpBP']
                CP.steerKpV = rtt_params['steerKpV']
                CP.steerKiBP = rtt_params['steerKiBP']
                CP.steerKiV = rtt_params['steerKiV']
                CP.steerKf = rtt_params['steerKf']
                CP.tireStiffnessFront = rtt_params['tireStiffnessFront']
                CP.tireStiffnessRear = rtt_params['tireStiffnessRear']
                CP.steerRatio = rtt_params['steerRatio']
                CP.steerActuatorDelay = rtt_params['steerActuatorDelay']
                if CP.steerRateCost != rtt_params['steerRateCost']:
                    print(CP.steerRateCost)
                    print(rtt_params['steerRateCost'])
                    CP.steerRateCost = rtt_params['steerRateCost']
                    rt_mpc_flag = True
                    print(
                        'Real-Time Tuning:  CP.steerRateCost changed - Re-initializing lateral MPC.'
                    )
                else:
                    rt_mpc_flag = False
                # TODO:  try/except the open
                # Write the pickle file back so if we fixed any data errors the revised values will show up on the client-side
                with open(rt_tuning_file, "wb") as f_write:
                    pickle.dump(
                        rtt_params, f_write,
                        -1)  # Dump to file with highest protocol (fastest)
                    # Set the last modified time to this write.... we don't need to read back in what we just wrote out
                    # Only set this if we were able to successfully make the write (once the try/except is added)
                    last_mod_time = os.path.getmtime(rt_tuning_file)
                # Make updates in latcontrol, etc.  I'm not sure if this is actually necessary, depends on if the objects are referenced or not.  Anyway, one less thing to debug atm.
                VM.update_rt_params(CP)
                LaC.update_rt_params(VM,
                                     rt_mpc_flag,
                                     deadzone=rtt_params['latPidDeadzone'])
                #PL.PP.update_rt_params(rtt_params['Camera Offset'])
                #print('RTT Last_mod_time:  {0}'.format(last_mod_time))

        ####### END OF REAL-TIME TUNING ADD-ON #######

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

        prof.display()
Esempio n. 17
0
    passive = False
    # ***** connect to joystick *****
    # USB Gamepad Controller
    dev = InputDevice("/dev/input/event1")
    print "USB Controller"
    print dev

    button_values = [0] * 13
    axis_values = [0] * 7

    # ***** connect to car *****
    context = zmq.Context()
    logcan = messaging.sub_sock(context, service_list['can'].port)
    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)

    CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
    AM = AlertManager()

    rk = Ratekeeper(100)

    while 1:
        # **** handle joystick ****
        r, w, x = select([dev], [], [], 0.0)
        if dev in r:
            for event in dev.read():
                print "USB Controller event"
                print event
                ## Button events
                if event.type == 1:
                    # 287 is base id-1 for first button
                    btn = event.code - 287
Esempio n. 18
0
def steer_thread():
    poller = messaging.Poller()

    logcan = messaging.sub_sock('can')
    health = messaging.sub_sock('health')
    joystick_sock = messaging.sub_sock('testJoystick',
                                       conflate=True,
                                       poller=poller)

    carstate = messaging.pub_sock('carState')
    carcontrol = messaging.pub_sock('carControl')
    sendcan = messaging.pub_sock('sendcan')

    button_1_last = 0
    enabled = False

    # wait for health and CAN packets
    hw_type = messaging.recv_one(health).health.hwType
    has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
    print("Waiting for CAN messages...")
    messaging.get_one_can(logcan)

    CI, CP = get_car(logcan, sendcan, has_relay)
    Params().put("CarParams", CP.to_bytes())

    CC = car.CarControl.new_message()

    while True:

        # send
        joystick = messaging.recv_one(joystick_sock)
        can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
        CS = CI.update(CC, can_strs)

        # Usually axis run in pairs, up/down for one, and left/right for
        # the other.
        actuators = car.CarControl.Actuators.new_message()

        if joystick is not None:
            axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1.,
                          1.)  # -1 to 1
            actuators.steer = axis_3
            actuators.steerAngle = axis_3 * 43.  # deg
            axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1.,
                          1.)  # -1 to 1
            actuators.gas = max(axis_1, 0.)
            actuators.brake = max(-axis_1, 0.)

            pcm_cancel_cmd = joystick.testJoystick.buttons[0]
            button_1 = joystick.testJoystick.buttons[1]
            if button_1 and not button_1_last:
                enabled = not enabled

            button_1_last = button_1

            #print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake

            hud_alert = 0
            if joystick.testJoystick.buttons[3]:
                hud_alert = "steerRequired"

        CC.actuators.gas = actuators.gas
        CC.actuators.brake = actuators.brake
        CC.actuators.steer = actuators.steer
        CC.actuators.steerAngle = actuators.steerAngle
        CC.hudControl.visualAlert = hud_alert
        CC.hudControl.setSpeed = 20
        CC.cruiseControl.cancel = pcm_cancel_cmd
        CC.enabled = enabled
        can_sends = CI.apply(CC)
        sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))

        # broadcast carState
        cs_send = messaging.new_message('carState')
        cs_send.carState = copy(CS)
        carstate.send(cs_send.to_bytes())

        # broadcast carControl
        cc_send = messaging.new_message('carControl')
        cc_send.carControl = copy(CC)
        carcontrol.send(cc_send.to_bytes())
Esempio n. 19
0
def controlsd_thread(sm=None, pm=None, can_sock=None):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    params = Params()

    is_metric = params.get("IsMetric") == "1"
    passive = params.get("Passive") != "0"

    # Pub/Sub Sockets
    if pm is None:
        pm = messaging.PubMaster([
            'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
            'carParams'
        ])

    if sm is None:
        sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
                                  'gpsLocation'], ignore_alive=['gpsLocation'])

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

    # wait for health and CAN packets
    hw_type = messaging.recv_one(sm.sock['health']).health.hwType
    is_panda_black = hw_type == log.HealthData.HwType.blackPanda
    print("Waiting for CAN messages...")
    get_one_can(can_sock)

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

    car_recognized = CP.carName != 'mock'
    # If stock camera is disconnected, we loaded car controls and it's not chffrplus
    controller_available = CP.enableCamera and CI.CC is not None and not passive
    read_only = not car_recognized or not controller_available or CP.dashcamOnly
    if read_only:
        CP.safetyModel = CP.safetyModelPassive

    # Write CarParams for radard and boardd safety mode
    params.put("CarParams", CP.to_bytes())
    params.put("LongitudinalControl",
               "1" if CP.openpilotLongitudinalControl else "0")

    CC = car.CarControl.new_message()
    AM = AlertManager()

    startup_alert = get_startup_alert(car_recognized, controller_available)
    AM.add(sm.frame, startup_alert, False)

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

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

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

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    overtemp = False
    free_space = False
    cal_status = Calibration.INVALID
    cal_perc = 0
    mismatch_counter = 0
    low_battery = False
    events_prev = []

    sm['pathPlan'].sensorValid = True
    sm['pathPlan'].posenetValid = True

    # detect sound card presence
    sounds_available = not os.path.isfile('/EON') or (
        os.path.isdir('/proc/asound/card0')
        and open('/proc/asound/card0/state').read().strip() == 'ONLINE')

    # controlsd is driven by can recv, expected at 100Hz
    rk = Ratekeeper(100, print_delay_threshold=None)

    prof = Profiler(False)  # off by default

    while True:
        start_time = sec_since_boot()
        prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data and compute car events
        CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
          data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                      driver_status, state, mismatch_counter, params)
        prof.checkpoint("Sample")

        # Create alerts
        if not sm.all_alive_and_valid():
            events.append(
                create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not sm['pathPlan'].mpcSolutionValid:
            events.append(
                create_event('plannerError',
                             [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
        if not sm['pathPlan'].sensorValid:
            events.append(
                create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
        if not sm['pathPlan'].paramsValid:
            events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
        if not sm['pathPlan'].posenetValid:
            events.append(
                create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not sm['plan'].radarValid:
            events.append(
                create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if sm['plan'].radarCanError:
            events.append(
                create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not CS.canValid:
            events.append(
                create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
        if not sounds_available:
            events.append(
                create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and sm[
                'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
            events.append(
                create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        if not read_only:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
          state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
                        driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)

        prof.checkpoint("State Control")

        # Publish data
        CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
                                    actuators, v_cruise_kph, rk, AM,
                                    driver_status, LaC, LoC, read_only,
                                    start_time, v_acc, a_acc, lac_log,
                                    events_prev)
        prof.checkpoint("Sent")

        rk.monitor_time()
        prof.display()
Esempio n. 20
0
def controlsd_thread(gctx=None):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    context = zmq.Context()
    params = Params()

    # Pub Sockets
    controlsstate = messaging.pub_sock(context,
                                       service_list['controlsState'].port)
    carstate = messaging.pub_sock(context, service_list['carState'].port)
    carcontrol = messaging.pub_sock(context, service_list['carControl'].port)

    is_metric = params.get("IsMetric") == "1"
    passive = params.get("Passive") != "0"

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

    # Sub sockets
    poller = zmq.Poller()
    thermal = messaging.sub_sock(context,
                                 service_list['thermal'].port,
                                 conflate=True,
                                 poller=poller)
    health = messaging.sub_sock(context,
                                service_list['health'].port,
                                conflate=True,
                                poller=poller)
    cal = messaging.sub_sock(context,
                             service_list['liveCalibration'].port,
                             conflate=True,
                             poller=poller)
    driver_monitor = messaging.sub_sock(context,
                                        service_list['driverMonitoring'].port,
                                        conflate=True,
                                        poller=poller)
    plan_sock = messaging.sub_sock(context,
                                   service_list['plan'].port,
                                   conflate=True,
                                   poller=poller)
    path_plan_sock = messaging.sub_sock(context,
                                        service_list['pathPlan'].port,
                                        conflate=True,
                                        poller=poller)
    logcan = messaging.sub_sock(context, service_list['can'].port)

    CC = car.CarControl.new_message()
    CI, CP = get_car(logcan, sendcan)
    AM = AlertManager()

    car_recognized = CP.carName != 'mock'
    # If stock camera is disconnected, we loaded car controls and it's not chffrplus
    controller_available = CP.enableCamera and CI.CC is not None and not passive
    read_only = not car_recognized or not controller_available
    if read_only:
        CP.safetyModel = car.CarParams.SafetyModels.elm327  # diagnostic only

    startup_alert = get_startup_alert(car_recognized, controller_available)
    AM.add(startup_alert, False)

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

    if CP.lateralTuning.which() == 'pid':
        LaC = LatControlPID(CP)
    else:
        LaC = LatControlINDI(CP)

    driver_status = DriverStatus()

    # Write CarParams for radard and boardd safety mode
    params.put("CarParams", CP.to_bytes())
    params.put("LongitudinalControl",
               "1" if CP.openpilotLongitudinalControl else "0")

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    overtemp = False
    free_space = False
    cal_status = Calibration.INVALID
    cal_perc = 0
    mismatch_counter = 0
    low_battery = False

    rcv_times = defaultdict(int)

    plan = messaging.new_message()
    plan.init('plan')
    path_plan = messaging.new_message()
    path_plan.init('pathPlan')
    path_plan.pathPlan.sensorValid = True

    # controlsd is driven by can recv, expected at 100Hz
    rk = Ratekeeper(100, print_delay_threshold=None)
    controls_params = params.get("ControlsParams")

    # Read angle offset from previous drive
    angle_model_bias = 0.
    if controls_params is not None:
        try:
            controls_params = json.loads(controls_params)
            angle_model_bias = controls_params['angle_model_bias']
        except (ValueError, KeyError):
            pass

    prof = Profiler(False)  # off by default

    while True:
        start_time = sec_since_boot()
        prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data and compute car events
        CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan  =\
          data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
                      poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
                      state, mismatch_counter, params, plan, path_plan)
        prof.checkpoint("Sample")

        # Create alerts
        path_plan_age = start_time - rcv_times['pathPlan']
        plan_age = start_time - rcv_times['plan']

        if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
            events.append(
                create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not path_plan.pathPlan.sensorValid:
            events.append(
                create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
        if not path_plan.pathPlan.paramsValid:
            events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
        if not path_plan.pathPlan.modelValid:
            events.append(
                create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not plan.plan.radarValid:
            events.append(
                create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if plan.plan.radarCommIssue:
            events.append(
                create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
            events.append(
                create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        if not read_only:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc, lac_log = \
          state_control(rcv_times, plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
                        v_cruise_kph_last, AM, rk, driver_status,
                        LaC, LoC, VM, angle_model_bias, read_only, is_metric, cal_perc)

        prof.checkpoint("State Control")

        # Publish data
        CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events,
                       actuators, v_cruise_kph, rk, carstate, carcontrol,
                       controlsstate, sendcan, AM, driver_status, LaC, LoC,
                       angle_model_bias, read_only, start_time, v_acc, a_acc,
                       lac_log)
        prof.checkpoint("Sent")

        rk.monitor_time()
        prof.display()
Esempio n. 21
0
def controlsd_thread(gctx=None):
    gc.disable()

    # start the loop
    set_realtime_priority(3)

    params = Params()

    # Pub Sockets
    sendcan = messaging.pub_sock(service_list['sendcan'].port)
    controlsstate = messaging.pub_sock(service_list['controlsState'].port)
    carstate = messaging.pub_sock(service_list['carState'].port)
    carcontrol = messaging.pub_sock(service_list['carControl'].port)
    carevents = messaging.pub_sock(service_list['carEvents'].port)
    carparams = messaging.pub_sock(service_list['carParams'].port)

    is_metric = params.get("IsMetric") == "1"
    passive = params.get("Passive") != "0"

    sm = messaging.SubMaster([
        'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan',
        'pathPlan'
    ])

    logcan = messaging.sub_sock(service_list['can'].port)
    CI, CP = get_car(logcan, sendcan)
    logcan.close()

    # TODO: Use the logcan socket from above, but that will currenly break the tests
    can_sock = messaging.sub_sock(service_list['can'].port, timeout=100)

    CC = car.CarControl.new_message()
    AM = AlertManager()

    car_recognized = CP.carName != 'mock'
    # If stock camera is disconnected, we loaded car controls and it's not chffrplus
    controller_available = CP.enableCamera and CI.CC is not None and not passive
    read_only = not car_recognized or not controller_available
    if read_only:
        CP.safetyModel = car.CarParams.SafetyModel.elm327  # diagnostic only

    startup_alert = get_startup_alert(car_recognized, controller_available)
    AM.add(sm.frame, startup_alert, False)

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

    if CP.lateralTuning.which() == 'pid':
        LaC = LatControlPID(CP)
    else:
        LaC = LatControlINDI(CP)

    driver_status = DriverStatus()

    # Write CarParams for radard and boardd safety mode
    params.put("CarParams", CP.to_bytes())
    params.put("LongitudinalControl",
               "1" if CP.openpilotLongitudinalControl else "0")

    state = State.disabled
    soft_disable_timer = 0
    v_cruise_kph = 255
    v_cruise_kph_last = 0
    overtemp = False
    free_space = False
    cal_status = Calibration.INVALID
    cal_perc = 0
    mismatch_counter = 0
    low_battery = False
    events_prev = []

    sm['pathPlan'].sensorValid = True

    # controlsd is driven by can recv, expected at 100Hz
    rk = Ratekeeper(100, print_delay_threshold=None)

    prof = Profiler(False)  # off by default

    while True:
        start_time = sec_since_boot()
        prof.checkpoint("Ratekeeper", ignore=True)

        # Sample data and compute car events
        CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
          data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                      driver_status, state, mismatch_counter, params)
        prof.checkpoint("Sample")

        # Create alerts
        if not sm.all_alive_and_valid():
            events.append(
                create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not sm['pathPlan'].mpcSolutionValid:
            events.append(
                create_event('plannerError',
                             [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
        if not sm['pathPlan'].sensorValid:
            events.append(
                create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
        if not sm['pathPlan'].paramsValid:
            events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
        if not sm['plan'].radarValid:
            events.append(
                create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if sm['plan'].radarCanError:
            events.append(
                create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
        if not CS.canValid:
            events.append(
                create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        # Only allow engagement with brake pressed when stopped behind another stopped car
        if CS.brakePressed and sm[
                'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
            events.append(
                create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

        if not read_only:
            # update control state
            state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
              state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
            prof.checkpoint("State transition")

        # Compute actuators (runs PID loops and lateral MPC)
        actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
          state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
                        driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)

        prof.checkpoint("State Control")

        # Publish data
        CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events,
                                    actuators, v_cruise_kph, rk, carstate,
                                    carcontrol, carevents, carparams,
                                    controlsstate, sendcan, AM, driver_status,
                                    LaC, LoC, read_only, start_time, v_acc,
                                    a_acc, lac_log, events_prev)
        prof.checkpoint("Sent")

        rk.monitor_time()
        prof.display()
Esempio n. 22
0
  def __init__(self, sm=None, pm=None, can_sock=None):
    config_realtime_process(3, Priority.CTRL_HIGH)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    # controlsd is driven by can recv, expected at 100Hz
    self.rk = Ratekeeper(100, print_delay_threshold=None)
    self.prof = Profiler(False)  # off by default
Esempio n. 23
0
  def __init__(self, sm=None, pm=None, can_sock=None, arne_sm=None):
    gc.disable()
    set_realtime_priority(53)
    set_core_affinity(3)

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

    self.sm = sm
    if self.sm is None:
      self.sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration',
                                     'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'radarState'])
    self.arne_sm = arne_sm
    if self.arne_sm is None:
      self.arne_sm = messaging_arne.SubMaster(['arne182Status', 'dynamicFollowButton', 'trafficModelEvent', 'modelLongButton' ])

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

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

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

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

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

    # detect sound card presence and ensure successful init
    #sounds_available = not ANDROID or get_sound_card_online()

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

    # Write CarParams for radard and boardd safety mode
    cp_bytes = self.CP.to_bytes()
    params.put("CarParams", cp_bytes)
    put_nonblocking("CarParamsCache", cp_bytes)
    put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0")
    if self.CP.openpilotLongitudinalControl and self.CP.safetyModel in [car.CarParams.SafetyModel.hondaBoschGiraffe, car.CarParams.SafetyModel.hondaBoschHarness]:
      disable_radar(can_sock, pm.sock['sendcan'], 1 if has_relay else 0, timeout=1, retry=10)

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

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

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

    self.state = State.disabled
    self.enabled = False
    self.active = False
    self.can_rcv_error = False
    self.soft_disable_timer = 0
    self.v_cruise_kph = 255
    self.v_cruise_kph_last = 0
    self.mismatch_counter = 0
    self.can_error_counter = 0
    self.last_blinker_frame = 0
    self.last_ldw_frame = 0
    self.saturated_count = 0
    self.distance_traveled_now = 0
    if not travis:
      self.distance_traveled = float(params.get("DistanceTraveled", encoding='utf8'))
      self.distance_traveled_engaged = float(params.get("DistanceTraveledEngaged", encoding='utf8'))
      self.distance_traveled_override = float(params.get("DistanceTraveledOverride", encoding='utf8'))
    else:
      self.distance_traveled = 0
      self.distance_traveled_engaged = 0
      self.distance_traveled_override = 0

    self.distance_traveled_frame = 0
    self.events_prev = []
    self.current_alert_types = [ET.PERMANENT]

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

    self.startup_event = get_startup_event(car_recognized, controller_available)

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

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