def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status): # inputs are all 10Hz DS = DriverStatus() events_from_DM = [] for idx in range(len(driver_state_msgs)): DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx]) # cal_rpy and car_speed don't matter here event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx]) events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong' return events_from_DM, DS
def dmonitoringd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster( ['driverState', 'liveCalibration', 'carState', 'model']) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].standstill = True cal_rpy = [0, 0, 0] v_cruise_last = 0 driver_engaged = False # 10Hz <- dmonitoringmodeld while True: sm.update() # Handle calibration if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: if len(sm['liveCalibration'].rpyCalib) == 3: cal_rpy = sm['liveCalibration'].rpyCalib # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ v_cruise != v_cruise_last or \ sm['carState'].steeringPressed if driver_engaged: _ = driver_status.update([], True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) v_cruise_last = v_cruise # Get model meta if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld if sm.updated['driverState']: events = [] driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) # Block any engage after certain distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) # Update events from driver state events = driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events, "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "rhdChecked": driver_status.is_rhd_region_checked, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, "stepChange": driver_status.step_change, "awarenessActive": driver_status.awareness_active, "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isPreview": False, } pm.send('dMonitoringState', dat)
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()
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()
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()
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()
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()
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()
def dmonitoringd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster( ['driverState', 'liveCalibration', 'carState', 'model']) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].standstill = True cal_rpy = [0, 0, 0] v_cruise_last = 0 driver_engaged = False # dragonpilot last_ts = 0 dp_last_modified = None dp_enable_driver_monitoring = True # 10Hz <- dmonitoringmodeld while True: cur_time = sec_since_boot() if cur_time - last_ts >= 5.: modified = get_last_modified() if dp_last_modified != modified: dp_enable_driver_monitoring = False if params.get( "DragonEnableDriverMonitoring", encoding='utf8') == "0" else True try: dp_awareness_time = int( params.get("DragonSteeringMonitorTimer", encoding='utf8')) except (TypeError, ValueError): dp_awareness_time = 70. driver_status.awareness_time = 86400 if dp_awareness_time <= 0. else dp_awareness_time * 60. dp_last_modified = modified last_ts = cur_time # reset all awareness val and set to rhd region, this will enforce steering monitor. if not dp_enable_driver_monitoring: driver_status.active_monitoring_mode = False driver_status.awareness = 1. driver_status.awareness_active = 1. driver_status.awareness_passive = 1. driver_status.terminal_alert_cnt = 0 driver_status.terminal_time = 0 driver_status.face_detected = False driver_status.hi_stds = 0 sm.update() # Handle calibration if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: if len(sm['liveCalibration'].rpyCalib) == 3: cal_rpy = sm['liveCalibration'].rpyCalib # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ v_cruise != v_cruise_last or \ sm['carState'].steeringPressed if driver_engaged: _ = driver_status.update([], True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) v_cruise_last = v_cruise # Get model meta if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld if sm.updated['driverState']: events = [] driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) # Block any engage after certain distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) # Update events from driver state events = driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events, "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "rhdChecked": driver_status.is_rhd_region_checked, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, "stepChange": driver_status.step_change, "awarenessActive": driver_status.awareness_active, "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isPreview": False, } pm.send('dMonitoringState', dat)