def data_sample(self): """Receive data from sockets and update carState""" # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) CS = self.CI.update(self.CC, can_strs) self.sm.update(0) all_valid = CS.canValid and self.sm.all_alive_and_valid() if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 2.0): self.initialized = True Params().put_bool("ControlsReady", True) # Check for CAN timeout if not can_strs: self.can_error_counter += 1 self.can_rcv_error = True else: self.can_rcv_error = False # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. self.controlsAllowed = self.sm['pandaState'].controlsAllowed if not self.enabled: self.mismatch_counter = 0 elif not self.controlsAllowed and self.enabled: self.mismatch_counter += 1 self.distance_traveled += CS.vEgo * DT_CTRL return CS
def radard_thread(sm=None, pm=None, can_sock=None): config_realtime_process(5, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface # *** setup messaging if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster( ['modelV2', 'carState'], ignore_avg_freq=[ 'modelV2', 'carState' ]) # Can't check average frequency, since radar determines timing if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) # TODO: always log leads once we can hide them conditionally enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(sm, rr, enable_lead) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def data_sample(self): """Receive data from sockets and update carState""" # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) CS = self.CI.update(self.CC, can_strs) self.sm.update(0) # Check for CAN timeout if not can_strs: self.can_error_counter += 1 self.can_rcv_error = True else: self.can_rcv_error = False # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not self.enabled: self.mismatch_counter = 0 if not self.sm['health'].controlsAllowed and self.enabled: self.mismatch_counter += 1 self.distance_traveled += CS.vEgo * DT_CTRL return CS
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) has_radar = not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(rk.frame, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def flyingcand_thread(sm=None, pm=None, flying_can_sock=None): set_realtime_priority(52) # wait for stats about the car to come in from controls cloudlog.info("flyingcand is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("flyingcand got CarParams") # import the wifi interfaces cloudlog.info("flyingcand is importing flying can interfaces") flyingCanInterface = importlib.import_module('selfdrive.controls.lib.flyingcan_interface').FlyingCANInterface if flying_can_sock is None: flying_can_sock = messaging.sub_sock('flying_can') if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'gpsLocation', 'gpsNMEA', 'sensorEvents']) # *** publish wifiState and liveTracks if pm is None: pm = messaging.PubMaster(['wifiState', 'liveTracks']) FI = flyingCanInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) FCD = FlyingCanD(CP.radarTimeStep, FI.delay) has_radar = not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(flying_can_sock, wait_for_one=True) rr = FI.update(can_strings) if rr is None: continue sm.update(0) dat = FCD.update(rk.frame, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = FCD.tracks dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]], responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]: ecu_responses: Set[Tuple[int, Optional[int], int]] = set() # set((addr, subaddr, bus),) try: msgs = [ make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries ] messaging.drain_sock_raw(logcan) sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) start_time = time.monotonic() while time.monotonic() - start_time < timeout: can_packets = messaging.drain_sock(logcan, wait_for_one=True) for packet in can_packets: for msg in packet.can: subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0] if (msg.address, subaddr, msg.src ) in responses and is_tester_present_response( msg, subaddr): if debug: print( f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}" ) if (msg.address, subaddr, msg.src) in ecu_responses: print( f"Duplicate ECU address: {hex(msg.address)}" ) ecu_responses.add((msg.address, subaddr, msg.src)) except Exception: cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}") return ecu_responses
def data_sample(self): """Receive data from sockets and update carState""" # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) CS = self.CI.update(self.CC, can_strs) self.sm.update(0) if not self.initialized: all_valid = CS.canValid and self.sm.all_alive_and_valid() if all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION: if not self.read_only: self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) self.initialized = True if REPLAY and self.sm['pandaStates'][0].controlsAllowed: self.state = State.enabled Params().put_bool("ControlsReady", True) # Check for CAN timeout if not can_strs: self.can_rcv_error_counter += 1 self.can_rcv_error = True else: self.can_rcv_error = False # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not self.enabled: self.mismatch_counter = 0 # All pandas not in silent mode must have controlsAllowed when openpilot is enabled if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 self.distance_traveled += CS.vEgo * DT_CTRL return CS
def sendcan_function(sendcan): sc = messaging.drain_sock_raw(sendcan) cp.update_strings(sc, sendcan=True) if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248 else: brake = 0.0 if cp.vl[0x200]['GAS_COMMAND'] > 0: gas = cp.vl[0x200]['GAS_COMMAND'] / 256.0 else: gas = 0.0 if cp.vl[0xe4]['STEER_TORQUE_REQUEST']: steer_torque = cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0x1000 else: steer_torque = 0.0 return (gas, brake, steer_torque)
def sendcan_function(sendcan): sc = messaging.drain_sock_raw(sendcan) cp.update_strings(sc, sendcan=True) if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] / 1024. else: brake = 0.0 if cp.vl[0x200]['GAS_COMMAND'] > 0: gas = (cp.vl[0x200]['GAS_COMMAND'] + 83.3) / (0.253984064 * 2**16) else: gas = 0.0 if cp.vl[0xe4]['STEER_TORQUE_REQUEST']: steer_torque = cp.vl[0xe4]['STEER_TORQUE'] / 3840 else: steer_torque = 0.0 return gas, brake, steer_torque
def test_conflate(self): sock = random_sock() pub_sock = messaging.pub_sock(sock) for conflate in [True, False]: for _ in range(10): num_msgs = random.randint(3, 10) sub_sock = messaging.sub_sock(sock, conflate=conflate, timeout=None) zmq_sleep() sent_msgs = [] for __ in range(num_msgs): msg = random_bytes() pub_sock.send(msg) sent_msgs.append(msg) time.sleep(0.1) recvd_msgs = messaging.drain_sock_raw(sub_sock) if conflate: self.assertEqual(len(recvd_msgs), 1) else: # TODO: compare actual data self.assertEqual(len(recvd_msgs), len(sent_msgs))
def data_sample(self): """Receive data from sockets and update carState""" # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) CS, CS_arne182 = self.CI.update(self.CC, can_strs) self.sm.update(0) # Check for CAN timeout if not can_strs: self.can_error_counter += 1 self.can_rcv_error = True else: self.can_rcv_error = False # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not self.enabled: self.mismatch_counter = 0 if not self.sm['health'].controlsAllowed and self.enabled: self.mismatch_counter += 1 self.distance_traveled_now += CS.vEgo * DT_CTRL self.distance_traveled += CS.vEgo * DT_CTRL if self.enabled: self.distance_traveled_engaged += CS.vEgo * DT_CTRL if CS.steeringPressed: self.distance_traveled_override += CS.vEgo * DT_CTRL if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis: y = threading.Thread(target=send_params, args=(str(self.distance_traveled),str(self.distance_traveled_engaged),str(self.distance_traveled_override),)) y.start() self.distance_traveled_frame = self.sm.frame return CS, CS_arne182
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) sm.update(0) events = list(CS.events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) lane_change_bsm = sm['pathPlan'].laneChangeBSM # Check for CAN timeout if not can_strs: events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red free_space = sm[ 'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed low_battery = sm['thermal'].batteryPercent < 1 and sm[ 'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed mem_low = sm['thermal'].memUsedPercent > 90 #bsm alerts if lane_change_bsm == LaneChangeBSM.left: events.append(create_event('preventLCA', [ET.WARNING])) if lane_change_bsm == LaneChangeBSM.right: events.append(create_event('preventLCA', [ET.WARNING])) # Create events for battery, temperature and disk space if low_battery: events.append( create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) if mem_low: events.append( create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) if CS.stockAeb: events.append(create_event('stockAeb', [])) # GPS coords RHD parsing, once every restart if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked: is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude) driver_status.is_rhd_region = is_rhd driver_status.is_rhd_region_checked = True put_nonblocking("IsRHD", "1" if is_rhd else "0") # Handle calibration cal_status = sm['liveCalibration'].calStatus cal_perc = sm['liveCalibration'].calPerc cal_rpy = [0, 0, 0] if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append( create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) else: events.append( create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) else: rpy = sm['liveCalibration'].rpyCalib if len(rpy) == 3: cal_rpy = rpy # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not enabled: mismatch_counter = 0 controls_allowed = sm['health'].controlsAllowed if not controls_allowed and enabled: mismatch_counter += 1 if mismatch_counter >= 200: events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) # Driver monitoring if sm.updated['model']: driver_status.set_policy(sm['model']) if sm.updated['driverMonitoring']: driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled) 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])) return CS, events, cal_perc, mismatch_counter
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())
def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) sm.update(0) events = list(CS.events) events += list(sm['dMonitoringState'].events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) # Check for CAN timeout if not can_strs: can_error_counter += 1 events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red free_space = sm[ 'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed low_battery = sm['thermal'].batteryPercent < 1 and sm[ 'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed mem_low = sm['thermal'].memUsedPercent > 90 # Create events for battery, temperature and disk space if low_battery: events.append( create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) if mem_low: events.append( create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) if CS.stockAeb: events.append(create_event('stockAeb', [])) # Handle calibration cal_status = sm['liveCalibration'].calStatus cal_perc = sm['liveCalibration'].calPerc if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append( create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) else: events.append( create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if CS.vEgo > 92 * CV.MPH_TO_MS: events.append( create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not enabled: mismatch_counter = 0 controls_allowed = sm['health'].controlsAllowed if not controls_allowed and enabled: mismatch_counter += 1 if mismatch_counter >= 200: events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) return CS, events, cal_perc, mismatch_counter, can_error_counter
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model=True): gen_signals, gen_checks = get_can_signals(CP) sgs = [s[0] for s in gen_signals] msgs = [s[1] for s in gen_signals] cks_msgs = set(check[0] for check in gen_checks) cks_msgs.add(0x18F) cks_msgs.add(0x30C) # ******** get messages sent to the car ******** can_strings = messaging.drain_sock_raw(Plant.sendcan, wait_for_one=self.response_seen) # After the first response the car is done fingerprinting, so we can run in lockstep with controlsd if can_strings: self.response_seen = True self.cp.update_strings(can_strings, sendcan=True) # ******** get controlsState messages for plotting *** controls_state_msgs = [] for a in messaging.drain_sock(Plant.controls_state, wait_for_one=self.response_seen): controls_state_msgs.append(a.controlsState) fcw = None for a in messaging.drain_sock(Plant.plan): if a.longitudinalPlan.fcw: fcw = True if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248 else: brake = 0.0 if self.cp.vl[0x200]['GAS_COMMAND'] > 0: gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0 else: gas = 0.0 if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']: steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00 else: steer_torque = 0.0 distance_lead = self.distance_lead_prev + v_lead * self.ts # ******** run the car ******** speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake) distance = self.distance_prev + speed * self.ts speed = self.speed_prev + self.ts * acceleration if speed <= 0: speed = 0 acceleration = 0 # ******** lateral ******** self.angle_steer -= (steer_torque / 10.0) * self.ts # *** radar model *** if self.lead_relevancy: d_rel = np.maximum(0., distance_lead - distance) v_rel = v_lead - speed else: d_rel = 200. v_rel = 0. lateral_pos_rel = 0. # print at 5hz if (self.frame % (self.rate // 5)) == 0: print( "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)) # ******** publish the car ******** vls_tuple = namedtuple('vls', [ 'XMISSION_SPEED', 'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR', 'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', 'STEER_TORQUE_MOTOR', 'LEFT_BLINKER', 'RIGHT_BLINKER', 'GEAR', 'WHEELS_MOVING', 'BRAKE_ERROR_1', 'BRAKE_ERROR_2', 'SEATBELT_DRIVER_LAMP', 'SEATBELT_DRIVER_LATCHED', 'BRAKE_PRESSED', 'BRAKE_SWITCH', 'CRUISE_BUTTONS', 'ESP_DISABLED', 'HUD_LEAD', 'USER_BRAKE', 'STEER_STATUS', 'GEAR_SHIFTER', 'PEDAL_GAS', 'CRUISE_SETTING', 'ACC_STATUS', 'CRUISE_SPEED_PCM', 'CRUISE_SPEED_OFFSET', 'DOOR_OPEN_FL', 'DOOR_OPEN_FR', 'DOOR_OPEN_RL', 'DOOR_OPEN_RR', 'CAR_GAS', 'MAIN_ON', 'EPB_STATE', 'BRAKE_HOLD_ACTIVE', 'INTERCEPTOR_GAS', 'INTERCEPTOR_GAS2', 'IMPERIAL_UNIT', 'MOTOR_TORQUE', ]) vls = vls_tuple( self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.angle_steer, self.angle_steer_rate, 0, 0, # Steer torque sensor 0, 0, # Blinkers self.gear_choice, speed != 0, self.brake_error, self.brake_error, not self.seatbelt, self.seatbelt, # Seatbelt self.brake_pressed, 0., # Brake pressed, Brake switch cruise_buttons, self.esp_disabled, 0, # HUD lead self.user_brake, self.steer_error, self.gear_shifter, self.pedal_gas, self.cruise_setting, self.acc_status, self.v_cruise, 0, # Cruise speed offset 0, 0, 0, 0, # Doors self.user_gas, self.main_on, 0, # EPB State 0, # Brake hold 0, # Interceptor feedback 0, # Interceptor 2 feedback False, 0, ) # TODO: publish each message at proper frequency can_msgs = [] for msg in set(msgs): msg_struct = {} indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]] for i in indxs: msg_struct[sgs[i]] = getattr(vls, sgs[i]) if "COUNTER" in honda.get_signals(msg): msg_struct["COUNTER"] = self.frame % 4 if "COUNTER_PEDAL" in honda.get_signals(msg): msg_struct["COUNTER_PEDAL"] = self.frame % 0xf msg = honda.lookup_msg_id(msg) msg_data = honda.encode(msg, msg_struct) if "CHECKSUM" in honda.get_signals(msg): msg_data = fix(msg_data, msg) if "CHECKSUM_PEDAL" in honda.get_signals(msg): msg_struct["CHECKSUM_PEDAL"] = crc8_pedal(msg_data[:-1]) msg_data = honda.encode(msg, msg_struct) can_msgs.append([msg, 0, msg_data, 0]) # add the radar message # TODO: use the DBC if self.frame % 5 == 0: radar_state_msg = b'\x79\x00\x00\x00\x00\x00\x00\x00' radar_msg = to_3_byte(d_rel * 16.0) + \ to_3_byte(int(lateral_pos_rel * 16.0) & 0x3ff) + \ to_3s_byte(int(v_rel * 32.0)) + \ b"0f00000" radar_msg = binascii.unhexlify(radar_msg) can_msgs.append([0x400, 0, radar_state_msg, 1]) can_msgs.append([0x445, 0, radar_msg, 1]) # add camera msg so controlsd thinks it's alive msg_struct["COUNTER"] = self.frame % 4 msg = honda.lookup_msg_id(0xe4) msg_data = honda.encode(msg, msg_struct) msg_data = fix(msg_data, 0xe4) can_msgs.append([0xe4, 0, msg_data, 2]) # Fake sockets that controlsd subscribes to live_parameters = messaging.new_message('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.sensorValid = True live_parameters.liveParameters.posenetValid = True live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) dmon_state = messaging.new_message('driverMonitoringState') dmon_state.driverMonitoringState.isDistracted = False Plant.driverMonitoringState.send(dmon_state.to_bytes()) pandaState = messaging.new_message('pandaState') pandaState.pandaState.safetyModel = car.CarParams.SafetyModel.hondaNidec pandaState.pandaState.controlsAllowed = True Plant.pandaState.send(pandaState.to_bytes()) deviceState = messaging.new_message('deviceState') deviceState.deviceState.freeSpacePercent = 1. deviceState.deviceState.batteryPercent = 100 Plant.deviceState.send(deviceState.to_bytes()) live_location_kalman = messaging.new_message('liveLocationKalman') live_location_kalman.liveLocationKalman.inputsOK = True live_location_kalman.liveLocationKalman.gpsOK = True Plant.live_location_kalman.send(live_location_kalman.to_bytes()) # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step if publish_model and self.frame % 5 == 0: md = messaging.new_message('modelV2') cal = messaging.new_message('liveCalibration') md.modelV2.frameId = 0 if self.lead_relevancy: d_rel = np.maximum(0., distance_lead - distance) v_rel = v_lead - speed prob = 1.0 else: d_rel = 200. v_rel = 0. prob = 0.0 lead = log.ModelDataV2.LeadDataV2.new_message() lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0] lead.xyvaStd = [1.0, 1.0, 1.0, 1.0] lead.prob = prob md.modelV2.leads = [lead, lead] cal.liveCalibration.calStatus = 1 cal.liveCalibration.calPerc = 100 cal.liveCalibration.rpyCalib = [0.] * 3 # fake values? Plant.model.send(md.to_bytes()) Plant.cal.send(cal.to_bytes()) for s in Plant.pm.sock.keys(): try: Plant.pm.send(s, messaging.new_message(s)) except Exception: Plant.pm.send(s, messaging.new_message(s, 1)) Plant.logcan.send(can_list_to_can_capnp(can_msgs)) # ******** update prevs ******** self.frame += 1 if self.response_seen: self.rk.monitor_time() self.speed = speed self.distance = distance self.distance_lead = distance_lead self.speed_prev = speed self.distance_prev = distance self.distance_lead_prev = distance_lead else: # Don't advance time when controlsd is not yet ready self.rk.keep_time() self.rk._frame = 0 return { "distance": distance, "speed": speed, "acceleration": acceleration, "distance_lead": distance_lead, "brake": brake, "gas": gas, "steer_torque": steer_torque, "fcw": fcw, "controls_state_msgs": controls_state_msgs, }
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) # TODO: always log leads once we can hide them conditionally enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) # This looks like a useless tesla hack. See if it can be removed? if CP.carName == "tesla": rr, rrext, ahbCarDetected = RI.update(can_strings, v_ego=0) else: rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(rk.frame, sm, rr, enable_lead) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) use_tesla_radar = CarSettings().get_value("useTeslaRadar") mocked = (CP.carName == "mock") or ((CP.carName == "tesla") and not use_tesla_radar) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster( ['model', 'controlsState', 'liveParameters', 'pathPlan']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) icLeads = messaging.pub_sock('uiIcLeads') ahbInfo = messaging.pub_sock('ahbInfo') RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, mocked, RI, use_tesla_radar, RI.delay) has_radar = not CP.radarOffCan or mocked last_md_ts = 0. v_ego = 0. while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) sm.update(0) if sm.updated['controlsState']: v_ego = sm['controlsState'].vEgo rr, rrext, ahbCarDetected = RI.update(can_strings, v_ego) if rr is None: continue dat, datext = RD.update(rk.frame, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) icLeads.send(datext.to_bytes()) ahbInfoMsg = tesla.AHBinfo.new_message() ahbInfoMsg.source = 0 ahbInfoMsg.radarCarDetected = ahbCarDetected ahbInfoMsg.cameraCarDetected = False ahbInfo.send(ahbInfoMsg.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
MINY = -1.0 MAXY = 1.0 def get_rrext_by_trackId(rrext, trackId): if rrext is not None: for p in rrext: if p.trackId == trackId: return p return None if __name__ == "__main__": CP = None RI = RadarInterface(CP) can_sock = messaging.sub_sock('can') while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr, rrext, ahb = RI.update(can_strings, 0.) if (rr is None) or (rrext is None): continue print(chr(27) + "[2J") for pt in rr.points: if (pt.dRel >= MINX) and (pt.dRel <= MAXX) and ( pt.yRel >= MINY) and (pt.yRel <= MAXY): extpt = get_rrext_by_trackId(rrext, pt.trackId) print(pt, extpt)
def can_sync_thread(): can_sock = messaging.sub_sock('can', conflate=True, timeout=100) can_pub = None os.environ["ZMQ"] = "1" pub_context = messaging_pyx.Context() can_pub = messaging_pyx.PubSocket() can_pub.connect(pub_context, 'testJoystick') del os.environ["ZMQ"] rk = Ratekeeper(100.0, print_delay_threshold=None) cc_main = 0 cc_status = 0 speed = 0.0 dbc_file = "toyota_yaris" signals = [ # sig_name, sig_address, default ("GEAR", "GEAR_PACKET", 0), ("BRAKE_PRESSED", "BRAKE_MODULE2", 0), ("GAS_PEDAL", "GAS_PEDAL", 0), ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 0), ("DOOR_OPEN_FL", "SEATS_DOORS", 0), ("DOOR_OPEN_FR", "SEATS_DOORS", 0), ("DOOR_OPEN_RL", "SEATS_DOORS", 0), ("DOOR_OPEN_RR", "SEATS_DOORS", 0), ("MAIN_ON", "PCM_CRUISE_SM", 0), ("CRUISE_CONTROL_STATE", "PCM_CRUISE_SM", 0), ("TURN_SIGNALS", "STEERING_LEVERS", 0), # 3 is no blinkers ("ENGINE_RPM", "POWERTRAIN", 0), ("SPEED", "SPEED", 0), ("MAY_CONTAIN_LIGHTS", "BOOLS", 0), ("CHANGES_EACH_RIDE", "SLOW_VARIABLE_INFOS", 0), ("INCREASING_VALUE_FUEL", "SLOW_VARIABLE_INFOS", 0) ] checks = [] parser = CANParser(dbc_file, signals, checks, 0) play_time = 0 while True: try: can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) parser.update_strings(can_strs) # print (parser.vl) cc_main = parser.vl['PCM_CRUISE_SM']['MAIN_ON'] cc_status = parser.vl['PCM_CRUISE_SM']['CRUISE_CONTROL_STATE'] speed = parser.vl['SPEED']['SPEED'] doorOpen = any([ parser.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], parser.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], parser.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], parser.vl["SEATS_DOORS"]['DOOR_OPEN_RR'] ]) seatbeltUnlatched = parser.vl["SEATS_DOORS"][ 'SEATBELT_DRIVER_UNLATCHED'] != 0 light = parser.vl["BOOLS"]["MAY_CONTAIN_LIGHTS"] a = parser.vl["SLOW_VARIABLE_INFOS"]["CHANGES_EACH_RIDE"] b = parser.vl["SLOW_VARIABLE_INFOS"]["INCREASING_VALUE_FUEL"] if doorOpen: play_time = play_sound(SOUND_PATH + 'door_open.wav', play_time, 3) if seatbeltUnlatched: play_time = play_sound(SOUND_PATH + 'seatbelt.wav', play_time, 3) if light != 0: play_time = play_sound(SOUND_PATH + 'turn_signal.wav', play_time, 3) cc_main_v = 0 if cc_main: cc_main_v = 1 can_dict = { 'cc_main': cc_main_v, 'cc_status': cc_status, 'speed': speed, 'light': light, 'a': a, 'b': b } json_str = json.dumps(can_dict) json_str = json_str.replace('\'', '"') can_pub.send(json_str) except messaging_pyx.MessagingError: print('MessagingError error happens') rk.keep_time()
def fingerprint(logcan, sendcan): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) if not fixed_fingerprint and not skip_fw_query: # Vin query only reliably works thorugh OBDII bus = 1 cached_params = Params().get("CarParamsCache") if cached_params is not None: cached_params = car.CarParams.from_bytes(cached_params) if cached_params.carName == "mock": cached_params = None if cached_params is not None and len( cached_params.carFw ) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") vin = cached_params.carVin car_fw = list(cached_params.carFw) else: cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan) exact_fw_match, fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN exact_fw_match, fw_candidates, car_fw = True, set(), [] if len(vin) != 17: cloudlog.event("Malformed VIN", vin=vin, error=True) vin = VIN_UNKNOWN cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1] } # attempt fingerprint on both bus 0 and 1 frame = 0 frame_fingerprint = 25 # 0.25s car_fingerprint = None done = False # drain CAN socket so we always get the latest messages messaging.drain_sock_raw(logcan) while not done: a = get_one_can(logcan) for can in a.can: # The fingerprint dict is generated for all buses, this way the car interface # can use it to detect a (valid) multipanda setup and initialize accordingly if can.src < 128: if can.src not in finger: finger[can.src] = {} finger[can.src][can.address] = len(can.dat) for b in candidate_cars: # Ignore extended messages and VIN query response. if can.src == b and can.address < 0x800 and can.address not in ( 0x7df, 0x7e0, 0x7e8): candidate_cars[b] = eliminate_incompatible_cars( can, candidate_cars[b]) # if we only have one car choice and the time since we got our first # message has elapsed, exit for b in candidate_cars: if len(candidate_cars[b]) == 1 and frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] # bail if no cars left or we've been waiting for more than 2s failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded frame += 1 exact_match = True source = car.CarParams.FingerprintSource.can # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw exact_match = exact_fw_match if fixed_fingerprint: car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, fw_count=len(car_fw)) return car_fingerprint, finger, vin, car_fw, source, exact_match