def update( self, rate=40.43 ): # in the future, pass in the current rate of long_mpc to accurate calculate disconnect time phantomData = messaging.recv_one_or_none(self.phantom_Data_sock) if phantomData is not None: self.data = { "status": phantomData.phantomData.status, "speed": phantomData.phantomData.speed, "angle": phantomData.phantomData.angle, "time": phantomData.phantomData.time } self.last_phantom_data = dict(self.data) self.last_receive_counter = 0 self.to_disable = not self.data["status"] else: if self.to_disable: # if last message is status: False, disable phantom mode, also disable by default self.data = {"status": False, "speed": 0.0} elif self.last_receive_counter > int( rate * 1.0 ) and self.timeout: # lost connection, don't disable. keep phantom on but set speed to 0 self.data = { "status": True, "speed": 0.0, "angle": 0.0, "time": 0.0 } else: # if waiting between messages from app, message becomes none, this uses the data from last message self.data = dict(self.last_phantom_data) self.last_receive_counter = min(self.last_receive_counter + 1, 900) # don't infinitely increment
def calibrationd_thread(gctx=None, addr="127.0.0.1"): context = zmq.Context() live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True) orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True) livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port) calibrator = Calibrator(param_put=True) # buffer with all the messages that still need to be input into the kalman while 1: of = messaging.recv_one(orbfeatures) l100 = messaging.recv_one_or_none(live100) new_vp = calibrator.handle_orb_features(of) if DEBUG and new_vp is not None: print 'got new vp', new_vp if l100 is not None: calibrator.handle_live100(l100) if DEBUG: calibrator.handle_debug() calibrator.send_data(livecalibration)
def fingerprint(msgs, pub_socks, sub_socks): print "start fingerprinting" manager.prepare_managed_process("logmessaged") manager.start_managed_process("logmessaged") can = pub_socks["can"] logMessage = messaging.sub_sock(service_list["logMessage"].port) time.sleep(1) messaging.drain_sock(logMessage) # controlsd waits for a health packet before fingerprinting msg = messaging.new_message() msg.init("health") pub_socks["health"].send(msg.to_bytes()) canmsgs = filter(lambda msg: msg.which() == "can", msgs) for msg in canmsgs[:200]: can.send(msg.as_builder().to_bytes()) time.sleep(0.005) log = messaging.recv_one_or_none(logMessage) if log is not None and "fingerprinted" in log.logMessage: break manager.kill_managed_process("logmessaged") print "finished fingerprinting"
def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if not self.enable_camera: return if CS.camcan > 0: if self.checksum == "NONE": self.checksum = learn_checksum(self.packer, CS.lkas11) print("Discovered Checksum", self.checksum) if self.checksum == "NONE": return elif CS.steer_error == 1: if self.checksum_learn_cnt > 200: self.checksum_learn_cnt = 0 if self.checksum == "NONE": print("Testing 6B Checksum") self.checksum == "6B" elif self.checksum == "6B": print("Testing 7B Checksum") self.checksum == "7B" elif self.checksum == "7B": print("Testing CRC8 Checksum") self.checksum == "crc8" else: self.checksum == "NONE" else: self.checksum_learn_cnt += 1 force_enable = False # I don't care about your opinion, deal with it! if (CS.cstm_btns.get_button_status("alwon") > 0) and CS.acc_active: enabled = True force_enable = True if (self.car_fingerprint in FEATURES["soft_disable"] and CS.v_wheel < 16.8): enabled = False force_enable = False if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1): self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off #update custom UI buttons and alerts CS.UE.update_custom_ui() if (self.cnt % 100 == 0): CS.cstm_btns.send_button_info() CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name) # Get the angle from ALCA. alca_enabled = False alca_steer = 0. alca_angle = 0. turn_signal_needed = 0 # Update ALCA status and custom button every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update( enabled, CS, self.cnt, actuators) if force_enable and not CS.acc_active: apply_steer = int( round(actuators.steer * SteerLimitParams.STEER_MAX)) else: apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX)) # SPAS limit angle extremes for safety apply_steer_ang_req = np.clip(actuators.steerAngle, -1 * (SteerLimitParams.STEER_ANG_MAX), SteerLimitParams.STEER_ANG_MAX) # SPAS limit angle rate for safety if abs(self.apply_steer_ang - apply_steer_ang_req) > 0.6: if apply_steer_ang_req > self.apply_steer_ang: self.apply_steer_ang += 0.5 else: self.apply_steer_ang -= 0.5 else: self.apply_steer_ang = apply_steer_ang_req # Limit steer rate for safety apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last, SteerLimitParams, CS.steer_torque_driver) if alca_enabled: self.turning_signal_timer = 0 if self.turning_signal_timer > 0: self.turning_signal_timer = self.turning_signal_timer - 1 turning_signal = 1 else: turning_signal = 0 # Use LKAS or SPAS if CS.mdps11_stat == 7 or CS.v_wheel > 2.7: self.lkas = True elif CS.v_wheel < 0.1: self.lkas = False if self.spas_present: self.lkas = True # If ALCA is disabled, and turning indicators are turned on, we do not want OP to steer, if not enabled or (turning_signal and not alca_enabled): if self.lkas: apply_steer = 0 else: self.apply_steer_ang = 0.0 self.en_cnt = 0 steer_req = 1 if enabled and self.lkas else 0 self.apply_steer_last = apply_steer can_sends = [] self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 self.mdps12_cnt = self.cnt % 0x100 self.spas_cnt = self.cnt % 0x200 can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, \ enabled if self.lkas else False, False if CS.camcan == 0 else CS.lkas11, hud_alert, (CS.cstm_btns.get_button_status("cam") > 0), \ (False if CS.camcan == 0 else True), self.checksum)) if CS.camcan > 0: can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \ CS.camcan, self.checksum)) # SPAS11 50hz if (self.cnt % 2) == 0 and not self.spas_present: if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7: self.en_spas == 7 self.en_cnt = 0 if self.en_spas == 7 and self.en_cnt >= 8: self.en_spas = 3 self.en_cnt = 0 if self.en_cnt < 8 and enabled and not self.lkas: self.en_spas = 4 elif self.en_cnt >= 8 and enabled and not self.lkas: self.en_spas = 5 if self.lkas or not enabled: self.apply_steer_ang = CS.mdps11_strang self.en_spas = 3 self.en_cnt = 0 self.mdps11_stat_last = CS.mdps11_stat self.en_cnt += 1 can_sends.append( create_spas11(self.packer, (self.spas_cnt / 2), self.en_spas, self.apply_steer_ang, self.checksum)) # SPAS12 20Hz if (self.cnt % 5) == 0 and not self.spas_present: can_sends.append(create_spas12(self.packer)) # Force Disable if pcm_cancel_cmd and (not force_enable): can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0)) elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0)) # Speed Limit Related Stuff Lot's of comments for others to understand! # Run this twice a second if (self.cnt % 50) == 0: if self.params.get("LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None: # If Not Enabled, or cruise not set, allow auto speed adjustment again if not (enabled and CS.acc_active_real): self.speed_adjusted = False # Attempt to read the speed limit from zmq map_data = messaging.recv_one_or_none(self.map_data_sock) # If we got a message if map_data != None: # See if we use Metric or dead kings extremeties for measurements, and set a variable to the conversion value if bool(self.params.get("IsMetric")): self.speed_conv = CV.MS_TO_KPH else: self.speed_conv = CV.MS_TO_MPH # If the speed limit is valid if map_data.liveMapData.speedLimitValid: last_speed = self.map_speed # Get the speed limit, and add the offset to it, v_speed = (map_data.liveMapData.speedLimit + float(self.params.get("SpeedLimitOffset"))) ## Stolen curvature code from planner.py, and updated it for us v_curvature = 45.0 if map_data.liveMapData.curvatureValid: v_curvature = math.sqrt( 1.85 / max(1e-4, abs(map_data.liveMapData.curvature))) # Use the minimum between Speed Limit and Curve Limit, and convert it as needed #self.map_speed = min(v_speed, v_curvature) * self.speed_conv self.map_speed = v_speed * self.speed_conv # Compare it to the last time the speed was read. If it is different, set the flag to allow it to auto set our speed if last_speed != self.map_speed: self.speed_adjusted = False else: # If it is not valid, set the flag so the cruise speed won't be changed. self.map_speed = 0 self.speed_adjusted = True else: self.speed_adjusted = True # Ensure we have cruise IN CONTROL, so we don't do anything dangerous, like turn cruise on # Ensure the speed limit is within range of the stock cruise control capabilities # Do the spamming 10 times a second, we might get from 0 to 10 successful # Only do this if we have not yet set the cruise speed if CS.acc_active_real and not self.speed_adjusted and self.map_speed > ( 8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1): # Use some tolerance because of Floats being what they are... if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005): can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0))) elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005): can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0))) # If nothing needed adjusting, then the speed has been set, which will lock out this control else: self.speed_adjusted = True ### If Driver Overrides using accelerator (or gas for the antiquated), cancel auto speed adjustment if CS.pedal_gas: self.speed_adjusted = True ### Send messages to canbus sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) self.cnt += 1
def ui_thread(addr, frame_address): context = zmq.Context() # TODO: Detect car from replay and use that to select carparams CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {}) VM = VehicleModel(CP) CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]]) vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]]) pygame.init() pygame.font.init() assert pygame_modules_have_loaded() if HOR: size = (640+384+640, 960) write_x = 5 write_y = 680 else: size = (640+384, 960+300) write_x = 645 write_y = 970 pygame.display.set_caption("openpilot debug UI") screen = pygame.display.set_mode(size, pygame.DOUBLEBUF) alert1_font = pygame.font.SysFont("arial", 30) alert2_font = pygame.font.SysFont("arial", 20) info_font = pygame.font.SysFont("arial", 15) camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert() cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert() cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24) top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y),0,8) frame = context.socket(zmq.SUB) frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port)) frame.setsockopt(zmq.SUBSCRIBE, "") carState = sub_sock(context, service_list['carState'].port, addr=addr, conflate=True) plan = sub_sock(context, service_list['plan'].port, addr=addr, conflate=True) carControl = sub_sock(context, service_list['carControl'].port, addr=addr, conflate=True) radar_state_sock = sub_sock(context, service_list['radarState'].port, addr=addr, conflate=True) liveCalibration = sub_sock(context, service_list['liveCalibration'].port, addr=addr, conflate=True) controls_state_sock = sub_sock(context, service_list['controlsState'].port, addr=addr, conflate=True) liveTracks = sub_sock(context, service_list['liveTracks'].port, addr=addr, conflate=True) model = sub_sock(context, service_list['model'].port, addr=addr, conflate=True) test_model = sub_sock(context, 8040, addr=addr, conflate=True) liveMpc = sub_sock(context, service_list['liveMpc'].port, addr=addr, conflate=True) liveParameters = sub_sock(context, service_list['liveParameters'].port, addr=addr, conflate=True) v_ego, angle_steers, angle_steers_des, model_bias = 0., 0., 0., 0. params_ao, params_ao_average, params_stiffness, params_sr = None, None, None, None enabled = False gas = 0. accel_override = 0. computer_gas = 0. brake = 0. steer_torque = 0. curvature = 0. computer_brake = 0. plan_source = 'none' long_control_state = 'none' model_data = None test_model_data = None a_ego = 0.0 a_target = 0.0 d_rel, y_rel, lead_status = 0., 0., False d_rel2, y_rel2, lead_status2 = 0., 0., False v_ego, v_pid, v_cruise, v_override = 0., 0., 0., 0. brake_lights = False alert_text1, alert_text2 = "", "" intrinsic_matrix = None calibration = None #img = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype='uint8') img = np.zeros((480, 640, 3), dtype='uint8') imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8) imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image good_lt = None lid_overlay_blank = get_blank_lid_overlay(UP) img_offset = (0, 0) if vision_test: visiontest = VisionTest(FULL_FRAME_SIZE, MODEL_INPUT_SIZE, None) # plots name_to_arr_idx = { "gas": 0, "computer_gas": 1, "user_brake": 2, "computer_brake": 3, "v_ego": 4, "v_pid": 5, "angle_steers_des": 6, "angle_steers": 7, "steer_torque": 8, "v_override": 9, "v_cruise": 10, "a_ego": 11, "a_target": 12, "accel_override": 13} plot_arr = np.zeros((100, len(name_to_arr_idx.values()))) plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])] plot_ylims = [(-0.1, 1.1), (-5., 5.), (0., 75.), (-3.0, 2.0)] plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"], ["angle_steers", "angle_steers_des", "steer_torque"], ["v_ego", "v_override", "v_pid", "v_cruise"], ["a_ego", "a_target"]] plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "r"], ["b", "g", "r", "y"], ["b", "r"]] plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]] draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True) while 1: list(pygame.event.get()) screen.fill((64,64,64)) lid_overlay = lid_overlay_blank.copy() top_down = top_down_surface, lid_overlay # ***** frame ***** fpkt = recv_one(frame) yuv_img = fpkt.frame.image if fpkt.frame.transform: yuv_transform = np.array(fpkt.frame.transform).reshape(3,3) else: # assume frame is flipped yuv_transform = np.array([ [-1.0, 0.0, FULL_FRAME_SIZE[0]-1], [ 0.0, -1.0, FULL_FRAME_SIZE[1]-1], [ 0.0, 0.0, 1.0] ]) if yuv_img and len(yuv_img) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3 // 2: yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(FULL_FRAME_SIZE[1] * 3 // 2, -1) cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff) cv2.warpAffine(imgff, np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) intrinsic_matrix = eon_intrinsics else: img.fill(0) intrinsic_matrix = np.eye(3) if calibration is not None and yuv_img and vision_test: model_input_yuv = visiontest.transform_contiguous(yuv_img, np.dot(yuv_transform, calibration.model_to_full_frame).reshape(-1).tolist()) cv2.cvtColor( np.frombuffer(model_input_yuv, dtype=np.uint8).reshape(MODEL_INPUT_SIZE[1] * 3 // 2, -1), cv2.COLOR_YUV2RGB_I420, dst=imgw) else: imgw.fill(0) imgw_test_model = imgw.copy() # ***** controlsState ***** controls_state = recv_one_or_none(controls_state_sock) if controls_state is not None: v_ego = controls_state.controlsState.vEgo angle_steers = controls_state.controlsState.angleSteers model_bias = controls_state.controlsState.angleModelBias curvature = controls_state.controlsState.curvature v_pid = controls_state.controlsState.vPid enabled = controls_state.controlsState.enabled alert_text1 = controls_state.controlsState.alertText1 alert_text2 = controls_state.controlsState.alertText2 long_control_state = controls_state.controlsState.longControlState cs = recv_one_or_none(carState) if cs is not None: gas = cs.carState.gas brake_lights = cs.carState.brakeLights a_ego = cs.carState.aEgo brake = cs.carState.brake v_cruise = cs.carState.cruiseState.speed cc = recv_one_or_none(carControl) if cc is not None: v_override = cc.carControl.cruiseControl.speedOverride computer_brake = cc.carControl.actuators.brake computer_gas = cc.carControl.actuators.gas steer_torque = cc.carControl.actuators.steer * 5. angle_steers_des = cc.carControl.actuators.steerAngle accel_override = cc.carControl.cruiseControl.accelOverride p = recv_one_or_none(plan) if p is not None: a_target = p.plan.aTarget plan_source = p.plan.longitudinalPlanSource plot_arr[:-1] = plot_arr[1:] plot_arr[-1, name_to_arr_idx['angle_steers']] = angle_steers plot_arr[-1, name_to_arr_idx['angle_steers_des']] = angle_steers_des plot_arr[-1, name_to_arr_idx['gas']] = gas plot_arr[-1, name_to_arr_idx['computer_gas']] = computer_gas plot_arr[-1, name_to_arr_idx['user_brake']] = brake plot_arr[-1, name_to_arr_idx['steer_torque']] = steer_torque plot_arr[-1, name_to_arr_idx['computer_brake']] = computer_brake plot_arr[-1, name_to_arr_idx['v_ego']] = v_ego plot_arr[-1, name_to_arr_idx['v_pid']] = v_pid plot_arr[-1, name_to_arr_idx['v_override']] = v_override plot_arr[-1, name_to_arr_idx['v_cruise']] = v_cruise plot_arr[-1, name_to_arr_idx['a_ego']] = a_ego plot_arr[-1, name_to_arr_idx['a_target']] = a_target plot_arr[-1, name_to_arr_idx['accel_override']] = accel_override # ***** model **** # live model md = recv_one_or_none(model) if md: model_data = extract_model_data(md) if model_data: plot_model(model_data, VM, v_ego, curvature, imgw, calibration, top_down) if test_model is not None: test_md = recv_one_or_none(test_model) if test_md: test_model_data = extract_model_data(test_md) if test_model_data: plot_model(test_model_data, VM, v_ego, curvature, imgw_test_model, calibration, top_down, 215) # MPC mpc = recv_one_or_none(liveMpc) if mpc: draw_mpc(mpc, top_down) # LiveParams params = recv_one_or_none(liveParameters) if params: params_ao = params.liveParameters.angleOffset params_ao_average = params.liveParameters.angleOffsetAverage params_stiffness = params.liveParameters.stiffnessFactor params_sr = params.liveParameters.steerRatio # **** tracks ***** # draw all radar points lt = recv_one_or_none(liveTracks) if lt is not None: good_lt = lt if good_lt is not None: maybe_update_radar_points(good_lt, top_down[1]) # ***** radarState ***** radar_state = recv_one_or_none(radar_state_sock) if radar_state is not None: d_rel = radar_state.radarState.leadOne.dRel + RDR_TO_LDR y_rel = radar_state.radarState.leadOne.yRel lead_status = radar_state.radarState.leadOne.status d_rel2 = radar_state.radarState.leadTwo.dRel + RDR_TO_LDR y_rel2 = radar_state.radarState.leadTwo.yRel lead_status2 = radar_state.radarState.leadTwo.status lcal = recv_one_or_none(liveCalibration) if lcal is not None: calibration_message = lcal.liveCalibration extrinsic_matrix = np.asarray(calibration_message.extrinsicMatrix).reshape(3, 4) warp_matrix = np.asarray(calibration_message.warpMatrix2).reshape(3, 3) calibration = CalibrationTransformsForWarpMatrix(warp_matrix, intrinsic_matrix, extrinsic_matrix) # draw red pt for lead car in the main img if lead_status: if calibration is not None: dx, dy = draw_lead_on(img, d_rel, y_rel, img_offset, calibration, color=(192,0,0)) # draw red line for lead car draw_lead_car(d_rel, top_down) # draw red pt for lead car2 in the main img if lead_status2: if calibration is not None: dx2, dy2 = draw_lead_on(img, d_rel2, y_rel2, img_offset, calibration, color=(192,0,0)) # draw red line for lead car draw_lead_car(d_rel2, top_down) # *** blits *** pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1)) screen.blit(camera_surface, (0, 0)) # display alerts alert_line1 = alert1_font.render(alert_text1, True, (255,0,0)) alert_line2 = alert2_font.render(alert_text2, True, (255,0,0)) screen.blit(alert_line1, (180, 150)) screen.blit(alert_line2, (180, 190)) if calibration is not None and img is not None: cpw = warp_points(CalP, calibration.model_to_bb) vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb) pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1) pygame.draw.circle(screen, BLUE, map(int, map(round, vanishing_pointw[0])), 2) if HOR: screen.blit(draw_plots(plot_arr), (640+384, 0)) else: screen.blit(draw_plots(plot_arr), (0, 600)) pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1)) screen.blit(cameraw_surface, (320, 480)) pygame.surfarray.blit_array(cameraw_test_surface, imgw_test_model.swapaxes(0, 1)) screen.blit(cameraw_test_surface, (0, 480)) pygame.surfarray.blit_array(*top_down) screen.blit(top_down[0], (640,0)) i = 0 SPACING = 25 # enabled enabled_line = info_font.render("ENABLED", True, GREEN if enabled else BLACK) screen.blit(enabled_line, (write_x, write_y + i * SPACING)) i += 1 # brake lights brake_lights_line = info_font.render("BRAKE LIGHTS", True, RED if brake_lights else BLACK) screen.blit(brake_lights_line, (write_x, write_y + i * SPACING)) i += 1 # speed v_ego_line = info_font.render("SPEED: " + str(round(v_ego, 1)) + " m/s", True, YELLOW) screen.blit(v_ego_line, (write_x, write_y + i * SPACING)) i += 1 # angle offset model_bias_line = info_font.render("MODEL BIAS: " + str(round(model_bias, 2)) + " deg", True, YELLOW) screen.blit(model_bias_line, (write_x, write_y + i * SPACING)) i += 1 # long control state long_control_state_line = info_font.render("LONG CONTROL STATE: " + str(long_control_state), True, YELLOW) screen.blit(long_control_state_line, (write_x, write_y + i * SPACING)) i += 1 # long mpc source plan_source_line = info_font.render("LONG MPC SOURCE: " + str(plan_source), True, YELLOW) screen.blit(plan_source_line, (write_x, write_y + i * SPACING)) i += 1 if params_ao is not None: i += 1 angle_offset_avg_line = info_font.render("ANGLE OFFSET (AVG): " + str(round(params_ao_average, 2)) + " deg", True, YELLOW) screen.blit(angle_offset_avg_line, (write_x, write_y + i * SPACING)) i += 1 angle_offset_line = info_font.render("ANGLE OFFSET (INSTANT): " + str(round(params_ao, 2)) + " deg", True, YELLOW) screen.blit(angle_offset_line, (write_x, write_y + i * SPACING)) i += 1 angle_offset_line = info_font.render("STIFFNESS: " + str(round(params_stiffness * 100., 2)) + " %", True, YELLOW) screen.blit(angle_offset_line, (write_x, write_y + i * SPACING)) i += 1 steer_ratio_line = info_font.render("STEER RATIO: " + str(round(params_sr, 2)), True, YELLOW) screen.blit(steer_ratio_line, (write_x, write_y + i * SPACING)) i += 1 # this takes time...vsync or something pygame.display.flip()
def mapsd_thread(): global last_gps context = zmq.Context() gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True) map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port) cur_way = None curvature_valid = False curvature = None upcoming_curvature = 0. dist_to_turn = 0. road_points = None while True: gps = messaging.recv_one(gps_sock) gps_ext = messaging.recv_one_or_none(gps_external_sock) if gps_ext is not None: gps = gps_ext.gpsLocationExternal else: gps = gps.gpsLocation last_gps = gps fix_ok = gps.flags & 1 if not fix_ok or last_query_result is None or not cache_valid: cur_way = None curvature = None curvature_valid = False upcoming_curvature = 0. dist_to_turn = 0. road_points = None map_valid = False else: map_valid = True lat = gps.latitude lon = gps.longitude heading = gps.bearing speed = gps.speed query_lock.acquire() cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) if cur_way is not None: pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) xs = pnts[:, 0] ys = pnts[:, 1] road_points = [float(x) for x in xs], [float(y) for y in ys] if speed < 10: curvature_valid = False if curvature_valid and pnts.shape[0] <= 3: curvature_valid = False # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found if curvature_valid: # Compute the curvature for each point with np.errstate(divide='ignore'): circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])] circles = np.asarray(circles) radii = np.nan_to_num(circles[:, 2]) radii[radii < 10] = np.inf curvature = 1. / radii # Index of closest point closest = np.argmin(np.linalg.norm(pnts, axis=1)) dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close # Compute distance along path dists = list() dists.append(0) for p, p_prev in zip(pnts, pnts[1:, :]): dists.append(dists[-1] + np.linalg.norm(p - p_prev)) dists = np.asarray(dists) dists = dists - dists[closest] + dist_to_closest dists = dists[1:-1] close_idx = np.logical_and(dists > 0, dists < 500) dists = dists[close_idx] curvature = curvature[close_idx] if len(curvature): # TODO: Determine left or right turn curvature = np.nan_to_num(curvature) # Outlier rejection new_curvature = np.percentile(curvature, 90, interpolation='lower') k = 0.6 upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature in_turn_indices = curvature > 0.8 * new_curvature if np.any(in_turn_indices): dist_to_turn = np.min(dists[in_turn_indices]) else: dist_to_turn = 999 else: upcoming_curvature = 0. dist_to_turn = 999 query_lock.release() dat = messaging.new_message() dat.init('liveMapData') if last_gps is not None: dat.liveMapData.lastGps = last_gps if cur_way is not None: dat.liveMapData.wayId = cur_way.id # Seed limit max_speed = cur_way.max_speed() if max_speed is not None: dat.liveMapData.speedLimitValid = True dat.liveMapData.speedLimit = max_speed # TODO: use the function below to anticipate upcoming speed limits #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) #if max_speed_ahead is not None and max_speed_ahead_dist is not None: # dat.liveMapData.speedLimitAheadValid = True # dat.liveMapData.speedLimitAhead = float(max_speed_ahead) # dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) advisory_max_speed = cur_way.advisory_max_speed() if advisory_max_speed is not None: dat.liveMapData.speedAdvisoryValid = True dat.liveMapData.speedAdvisory = advisory_max_speed # Curvature dat.liveMapData.curvatureValid = curvature_valid dat.liveMapData.curvature = float(upcoming_curvature) dat.liveMapData.distToTurn = float(dist_to_turn) if road_points is not None: dat.liveMapData.roadX, dat.liveMapData.roadY = road_points if curvature is not None: dat.liveMapData.roadCurvatureX = [float(x) for x in dists] dat.liveMapData.roadCurvature = [float(x) for x in curvature] dat.liveMapData.mapValid = map_valid map_data_sock.send(dat.to_bytes())
def update(self, cp, cp_cam): # copy can_valid self.can_valid = cp.can_valid self.cam_can_valid = cp_cam.can_valid msg = messaging.recv_one_or_none(self.gps_location) if msg is not None: gps_pkt = msg.gpsLocationExternal self.inaccuracy = gps_pkt.accuracy self.prev_distance = self.distance self.distance, self.includeradius, self.approachradius, self.speedlimit = gps_distance( gps_pkt.latitude, gps_pkt.longitude, gps_pkt.altitude, gps_pkt.accuracy) #self.distance = 6371010*acos(sin(radians(gps_pkt.latitude))*sin(radians(48.12893908))+cos(radians(gps_pkt.latitude))*cos(radians(48.12893908))*cos(radians(gps_pkt.longitude-9.797879048))) # update prevs, update must run once per loop self.prev_left_blinker_on = self.left_blinker_on self.prev_right_blinker_on = self.right_blinker_on self.door_all_closed = not any([ cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR'] ]) self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] if self.CP.enableGasInterceptor: self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] else: self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] self.car_gas = self.pedal_gas self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] # calc best v_ego estimate, by averaging two opposite corners self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS self.v_wheel = float( np.mean([ self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr ])) # Kalman filter if abs( self.v_wheel - self.v_ego ) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_x = np.matrix([[self.v_wheel], [0.0]]) self.v_ego_raw = self.v_wheel v_ego_x = self.v_ego_kf.update(self.v_wheel) self.v_ego = float(v_ego_x[0]) self.a_ego = float(v_ego_x[1]) #self.standstill = not self.v_wheel > 0.001 self.standstill = False self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl[ "STEER_ANGLE_SENSOR"]['STEER_FRACTION'] self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values) self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 self.blind_spot_side = cp.vl["DEBUG"]['BLINDSPOTSIDE'] if (cp.vl["DEBUG"]['BLINDSPOTD1'] > 10) or (cp.vl["DEBUG"]['BLINDSPOTD1'] > 10): self.blind_spot_on = bool(1) else: self.blind_spot_on = bool(0) # we could use the override bit from dbc, but it's triggered at too high torque values self.steer_override = abs( cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100 # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [ 1, 5, 9, 17, 25 ] self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3 self.brake_error = 0 self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"][ 'STEER_TORQUE_DRIVER'] self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"][ 'STEER_TORQUE_EPS'] if bool(cp.vl["JOEL_ID"] ['LANE_WARNING']) <> self.lane_departure_toggle_on_prev: self.lane_departure_toggle_on = bool( cp.vl["JOEL_ID"]['LANE_WARNING']) if self.lane_departure_toggle_on: self.cstm_btns.set_button_status("lka", 1) else: self.cstm_btns.set_button_status("lka", 0) self.lane_departure_toggle_on_prev = self.lane_departure_toggle_on else: if self.cstm_btns.get_button_status("lka") == 0: self.lane_departure_toggle_on = False else: self.lane_departure_toggle_on = True self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE'] self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES'] if self.distance_toggle <> self.distance_toggle_prev: if self.read_distance_lines == self.distance_toggle: self.distance_toggle_prev = self.distance_toggle else: self.cstm_btns.set_button_status("tr", 1) if self.read_distance_lines <> self.read_distance_lines_prev: self.cstm_btns.set_button_status("tr", 0) if self.read_distance_lines == 1: self.UE.custom_alert_message(2, "Following distance set to 0.9s", 200, 3) if self.read_distance_lines == 2: self.UE.custom_alert_message(2, "Following distance set to 1.8s", 200, 3) if self.read_distance_lines == 3: self.UE.custom_alert_message(2, "Following distance set to 2.7s", 200, 3) self.read_distance_lines_prev = self.read_distance_lines if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev: self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW']) if self.acc_slow_on: self.cstm_btns.set_button_status("slow", 1) else: self.cstm_btns.set_button_status("slow", 0) self.acc_slow_on_prev = self.acc_slow_on else: if self.cstm_btns.get_button_status("slow") == 0: self.acc_slow_on = False else: self.acc_slow_on = True # we could use the override bit from dbc, but it's triggered at too high torque values self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD self.user_brake = 0 if self.acc_slow_on: self.v_cruise_pcm = max(7, cp.vl["PCM_CRUISE_2"]['SET_SPEED'] - 34.0) else: self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] self.v_cruise_pcm = int( min(self.v_cruise_pcm, interp(abs(self.angle_steers), self.Angle, self.Angle_Speed))) #print "distane" #print self.distance if self.distance < self.approachradius + self.includeradius: print "speed" print self.prev_distance - self.distance #if speed is 5% higher than the speedlimit if self.prev_distance - self.distance > self.speedlimit * 0.00263889: if self.v_cruise_pcm > self.speedlimit: self.v_cruise_pcm = self.speedlimit if self.distance < self.includeradius: print "inside" if self.v_cruise_pcm > self.speedlimit: self.v_cruise_pcm = self.speedlimit self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED'] self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][ 'LOW_SPEED_LOCKOUT'] == 2 self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) if self.CP.carFingerprint == CAR.PRIUS: self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 else: self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
def manager_thread(): # now loop global manager_sock context = zmq.Context() thermal_sock = messaging.sub_sock(context, service_list['thermal'].port) gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) manager_sock = messaging.pub_sock(context, service_list['managerData'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) for p in persistent_processes: start_managed_process(p) # start frame pm_apply_packages('enable') system("am start -n ai.comma.plus.frame/.MainActivity") if os.getenv("NOBOARD") is None: start_managed_process("pandad") params = Params() logger_dead = False while 1: # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) gps = messaging.recv_one_or_none(gps_sock) if gps: if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839: logger_dead = True else: logger_dead = False # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") else: start_managed_process("uploader") if msg.thermal.freeSpace < 0.18: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False for p in car_started_processes: kill_managed_process(p) # check the status of all processes, did any of them die? #for p in running: # cloudlog.debug(" running %s %s" % (p, running[p])) send_running_processes() # is this still needed? if params.get("DoUninstall") == "1": break
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, set_speed_limit_active, speed_limit_offset, alca_enabled): idx = self.pedal_idx self.prev_speed_limit_kph = self.speed_limit_kph ###################################################################################### # Determine pedal "zero" # #save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH ###################################################################################### if (CS.torqueLevel < TORQUE_LEVEL_ACC and CS.torqueLevel > TORQUE_LEVEL_DECEL and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) < abs(self.lastTorqueForPedalForZeroTorque) and self.prev_tesla_accel > 0.): self.PedalForZeroTorque = self.prev_tesla_accel self.lastTorqueForPedalForZeroTorque = CS.torqueLevel #print ("Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)) #print ("Torque level at detection %s" % (CS.torqueLevel)) #print ("Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)) if set_speed_limit_active and speed_limit_ms > 0: self.speed_limit_kph = (speed_limit_ms + speed_limit_offset) * CV.MS_TO_KPH if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph): self.pedal_speed_kph = self.speed_limit_kph # reset MovingAverage for fleet speed when speed limit changes self.fleet_speed.reset_averager() else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway) self.speed_limit_kph = 0. self.pedal_idx = (self.pedal_idx + 1) % 16 if not self.pcc_available or not enabled: return 0., 0, idx # Alternative speed decision logic that uses the lead car's distance # and speed more directly. # Bring in the lead car distance from the radarState feed radSt = messaging.recv_one_or_none(self.radarState) mapd = messaging.recv_one_or_none(self.live_map_data) if radSt is not None: self.lead_1 = radSt.radarState.leadOne if _is_present(self.lead_1): self.lead_last_seen_time_ms = _current_time_millis() self.continuous_lead_sightings += 1 else: self.continuous_lead_sightings = 0 v_ego = CS.v_ego following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1) accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph) jerk_limits = [ min(-0.1, accel_limits[0] / 2.), max(0.1, accel_limits[1] / 2.) ] # TODO: make a separate lookup for jerk tuning #accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP) output_gb = 0 #################################################################### # this mode (Follow) uses the Follow logic created by JJ for ACC # # once the speed is detected we have to use our own PID to determine # how much accel and break we have to do #################################################################### if PCCModes.is_selected(FollowMode(), CS.cstm_btns): enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [ LongCtrlState.pid, LongCtrlState.stopping ] # determine if pedal is pressed by human self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10 #reset PID if we just lifted foot of accelerator if (not self.accelerator_pedal_pressed ) and self.prev_accelerator_pedal_pressed: self.reset(CS.v_ego) if self.enable_pedal_cruise and not self.accelerator_pedal_pressed: self.v_pid = self.calc_follow_speed_ms(CS, alca_enabled) if mapd is not None: v_curve = max_v_in_mapped_curve_ms(mapd.liveMapData, self.pedal_speed_kph) if v_curve: self.v_pid = min(self.v_pid, v_curve) # take fleet speed into consideration self.v_pid = min( self.v_pid, self.fleet_speed.adjust( CS, self.pedal_speed_kph * CV.KPH_TO_MS, frame)) # cruise speed can't be negative even if user is distracted self.v_pid = max(self.v_pid, 0.) jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, self.v_pid, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], #jerk_max, jerk_min, _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) self.v_acc = self.v_cruise self.a_acc = self.a_cruise self.v_acc_future = self.v_pid # Interpolation of trajectory self.a_acc_sol = self.a_acc_start + (_DT / _DT_MPC) * ( self.a_acc - self.a_acc_start) self.v_acc_sol = self.v_acc_start + _DT * ( self.a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol # we will try to feed forward the pedal position.... we might want to feed the last_output_gb.... # op feeds forward self.a_acc_sol # it's all about testing now. vTarget = clip(self.v_acc_sol, 0, self.v_cruise) self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid) feedforward = self.a_acc_sol #feedforward = self.last_output_gb t_go, t_brake = self.LoC.update( self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0, CS.standstill, False, self.v_cruise, vTarget, self.vTargetFuture, feedforward, CS.CP) output_gb = t_go - t_brake #print ("Output GB Follow:", output_gb) else: self.reset(CS.v_ego) #print ("PID reset") output_gb = 0. starting = self.LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.a_ego, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.v_ego reset_accel = CS.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.v_acc_sol = reset_speed self.a_acc_sol = reset_accel self.v_pid = reset_speed self.last_speed_kph = None ############################################################## # This mode uses the longitudinal MPC built in OP # # we use the values from actuator.accel and actuator.brake ############################################################## elif PCCModes.is_selected(OpMode(), CS.cstm_btns): output_gb = actuators.gas - actuators.brake self.v_pid = -10. self.last_output_gb = output_gb # accel and brake apply_accel = clip( output_gb, 0., _accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS)) MPC_BRAKE_MULTIPLIER = 6. apply_brake = -clip( output_gb * MPC_BRAKE_MULTIPLIER, _brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph), 0.) # if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero pedal_zero = 0. if CS.v_ego >= 5. * CV.MPH_TO_MS: pedal_zero = self.PedalForZeroTorque tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero) tesla_accel = clip(apply_accel * (MAX_PEDAL_VALUE - pedal_zero), 0, MAX_PEDAL_VALUE - pedal_zero) tesla_pedal = tesla_brake + tesla_accel tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled) tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN, self.prev_tesla_pedal + PEDAL_MAX_UP) tesla_pedal = clip(tesla_pedal, 0., MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0. enable_pedal = 1. if self.enable_pedal_cruise else 0. self.torqueLevel_last = CS.torqueLevel self.prev_tesla_pedal = tesla_pedal * enable_pedal self.prev_tesla_accel = apply_accel * enable_pedal self.prev_v_ego = CS.v_ego return self.prev_tesla_pedal, enable_pedal, idx
y.append(np.ones(LEN)*np.nan) lines.append(ax.plot(x[i], y[i])[0]) for l in lines: l.set_marker("*") cur_t = 0. ax.legend(subs_name) ax.set_xlabel('time [s]') while 1: print 1./(time.time() - cur_t) cur_t = time.time() for i, s in enumerate(subs): #msg = messaging.recv_sock(s) msg = messaging.recv_one_or_none(s) if msg is not None: x[i] = np.append(x[i], getattr(msg, 'logMonoTime') / float(1e9)) x[i] = np.delete(x[i], 0) y[i] = np.append(y[i], recursive_getattr(msg, subs_name[i])) y[i] = np.delete(y[i], 0) lines[i].set_xdata(x[i]) lines[i].set_ydata(y[i]) ax.relim() ax.autoscale_view(True, scaley=True, scalex=True) fig.canvas.blit(ax.bbox) fig.canvas.flush_events()
def mapsd_thread(): global last_gps context = zmq.Context() gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) gps_external_sock = messaging.sub_sock( context, service_list['gpsLocationExternal'].port, conflate=True) map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port) cur_way = None curvature_valid = False curvature = None upcoming_curvature = 0. dist_to_turn = 0. road_points = None xx = np.arange(0, MAPS_LOOKAHEAD_DISTANCE, 10) while True: gps = messaging.recv_one(gps_sock) gps_ext = messaging.recv_one_or_none(gps_external_sock) if gps_ext is not None: gps = gps_ext.gpsLocationExternal else: gps = gps.gpsLocation last_gps = gps fix_ok = gps.flags & 1 if not fix_ok or last_query_result is None: cur_way = None curvature = None curvature_valid = False upcoming_curvature = 0. dist_to_turn = 0. road_points = None else: lat = gps.latitude lon = gps.longitude heading = gps.bearing speed = gps.speed query_lock.acquire() cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) if cur_way is not None: pnts, curvature_valid = cur_way.get_lookahead( last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) xs = pnts[:, 0] ys = pnts[:, 1] road_points = map(float, xs), map(float, ys) if speed < 10: curvature_valid = False if curvature_valid and pnts.shape[0] <= 3: curvature_valid = False # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found if curvature_valid: # Compute the curvature for each point with np.errstate(divide='ignore'): circles = [ circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:]) ] circles = np.asarray(circles) radii = np.nan_to_num(circles[:, 2]) radii[radii < 10] = np.inf curvature = 1. / radii # Index of closest point closest = np.argmin(np.linalg.norm(pnts, axis=1)) dist_to_closest = pnts[ closest, 0] # We can use x distance here since it should be close # Compute distance along path dists = list() dists.append(0) for p, p_prev in zip(pnts, pnts[1:, :]): dists.append(dists[-1] + np.linalg.norm(p - p_prev)) dists = np.asarray(dists) dists = dists - dists[closest] + dist_to_closest # TODO: Determine left or right turn curvature = np.nan_to_num(curvature) curvature_interp = np.interp(xx, dists[1:-1], curvature) curvature_lookahead = curvature_interp[:int( speed * LOOKAHEAD_TIME / 10)] # Outlier rejection new_curvature = np.percentile(curvature_lookahead, 90) k = 0.9 upcoming_curvature = k * upcoming_curvature + ( 1 - k) * new_curvature in_turn_indices = curvature_interp > 0.8 * new_curvature if np.any(in_turn_indices): dist_to_turn = np.min(xx[in_turn_indices]) else: dist_to_turn = 999 query_lock.release() dat = messaging.new_message() dat.init('liveMapData') if last_gps is not None: dat.liveMapData.lastGps = last_gps if cur_way is not None: dat.liveMapData.wayId = cur_way.id # Seed limit max_speed = cur_way.max_speed if max_speed is not None: dat.liveMapData.speedLimitValid = True dat.liveMapData.speedLimit = max_speed # Curvature dat.liveMapData.curvatureValid = curvature_valid dat.liveMapData.curvature = float(upcoming_curvature) dat.liveMapData.distToTurn = float(dist_to_turn) if road_points is not None: dat.liveMapData.roadX, dat.liveMapData.roadY = road_points if curvature is not None: dat.liveMapData.roadCurvatureX = map(float, xx) dat.liveMapData.roadCurvature = map(float, curvature_interp) map_data_sock.send(dat.to_bytes())
def update(self, CS, frame, ahbLead1): tms_now = _current_time_millis() ahbInfoMsg = self.ahbInfo.receive(non_blocking=True) frameInfoMsg = messaging.recv_one_or_none(self.frameInfo) if ahbInfoMsg is not None: self.ahbInfoData = tesla.AHBinfo.from_bytes(ahbInfoMsg) if frameInfoMsg is not None: self.frameInfoData = frameInfoMsg.frame frameInfoGain = self.frameInfoData.globalGain exposureTime = self.frameInfoData.androidCaptureResult.exposureTime frameDuration = self.frameInfoData.androidCaptureResult.frameDuration if frameInfoGain != self.frameInfoGain: self.frameInfoGain = frameInfoGain _debug("AHB Camera has new data [ frame - " + str(self.frameInfoData.frameId) + "] = " + str(frameInfoGain) + ", exposure = " + str(exposureTime) + ", frame duration = " + str(frameDuration)) #if AHB not enabled, then return OFF if CS.ahbEnabled != 1: _debug("AHB not enabled in CID") highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) #if stalk is not in the high beam position, return UNDECIDED if (CS.ahbHighBeamStalkPosition != 1): _debug("High Beam not on") highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA self.ahbIsEnabled = False return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) else: self.ahbIsEnabled = True #if moving below 10mph take no decision, then return undecided if CS.v_ego < 4.47: _debug("moving too slow for decision") highLowBeamStatus = self.prev_highLowBeamStatus highLowBeamReason = self.prev_highLowBeamReason return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) if self.ahbInfoData is None: _debug("No radar info") highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) # if gain less than max, we might see incoming car if self.frameInfoGain < 510: _debug("OP camera gain < 510") self.time_last_car_detected = tms_now highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) #if lead car detected by radarD, i.e. OP has Lead, then reset timer and return OFF if False and ahbLead1 is not None: _debug("OP detected car") self.time_last_car_detected = tms_now highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) #if lead car detected by radar, then reset timer and return OFF if self.ahbInfoData.radarCarDetected: _debug("Radar detected car") self.time_last_car_detected = tms_now highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) #if lead car detected by vision, then reset timer and return OFF if self.ahbInfoData.cameraCarDetected: _debug("Vision detected car") self.time_last_car_detected = tms_now highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_VISION_TARGET return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) #if still waiting for the delay after car detected, send off if (tms_now - self.time_last_car_detected < 1000 * CS.ahbOffDuration): _debug("Waiting for time delay since last car") highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason) #we got here, high beam should be on highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED highLowBeamReason = AHBReason.HIGH_BEAM_ON _debug("All conditions met, turn lights ON") return self.set_and_return(CS, frame, highLowBeamStatus, highLowBeamReason)
def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph, set_speed_limit_active, speed_limit_offset): # Adaptive cruise control self.prev_speed_limit_kph = self.speed_limit_kph if set_speed_limit_active and speed_limit_kph > 0: self.speed_limit_kph = speed_limit_kph + speed_limit_offset if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph): self.acc_speed_kph = self.speed_limit_kph self.fleet_speed.reset_averager() else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway) self.speed_limit_kph = 0. current_time_ms = _current_time_millis() if CruiseButtons.should_be_throttled(CS.cruise_buttons): self.human_cruise_action_time = current_time_ms button_to_press = None # If ACC is disabled, disengage traditional cruise control. if ((self.prev_enable_adaptive_cruise) and (not self.enable_adaptive_cruise) and (CS.pcm_acc_status == CruiseState.ENABLED)): button_to_press = CruiseButtons.CANCEL #if non addaptive and we just engaged ACC but pcm is not engaged, then engage if (not self.adaptive) and self.enable_adaptive_cruise and ( CS.pcm_acc_status != CruiseState.ENABLED): button_to_press = CruiseButtons.MAIN #if plain cc, not adaptive, then just return None or Cancel if (not self.adaptive) and self.enable_adaptive_cruise: self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH return button_to_press #disengage if cruise is canceled if (not self.enable_adaptive_cruise) and (CS.pcm_acc_status >= 2) and ( CS.pcm_acc_status <= 4): button_to_press = CruiseButtons.CANCEL lead_1 = None #if enabled: lead = messaging.recv_one_or_none(self.radarState) if lead is not None: lead_1 = lead.radarState.leadOne if lead_1.dRel: self.lead_last_seen_time_ms = current_time_ms if self.enable_adaptive_cruise and enabled: # and (button_to_press == None): if CS.cstm_btns.get_button_label2( ACCMode.BUTTON_NAME) in ["OP", "AutoOP"]: button_to_press = self._calc_button(CS, pcm_speed) self.new_speed = pcm_speed * CV.MS_TO_KPH else: # Alternative speed decision logic that uses the lead car's distance # and speed more directly. # Bring in the lead car distance from the radarState feed button_to_press = self._calc_follow_button( CS, lead_1, speed_limit_kph, set_speed_limit_active, speed_limit_offset, frame) if button_to_press: self.automated_cruise_action_time = current_time_ms # If trying to slow below the min cruise speed, just cancel cruise. # This prevents a SCCM crash which is triggered by repeatedly pressing # stalk-down when already at min cruise speed. if (CruiseButtons.is_decel(button_to_press) and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): button_to_press = CruiseButtons.CANCEL if button_to_press == CruiseButtons.CANCEL: self.fast_decel_time = current_time_ms # Debug logging (disable in production to reduce latency of commands) #print "***ACC command: %s***" % button_to_press return button_to_press
speed_txt = ax.text(-500, 900, '') curv_txt = ax.text(-500, 775, '') ax = fig.add_subplot(2, 1, 2) ax.set_title('Curvature') curvature_plt, = ax.plot([0.0], [0.0], "--xk") ax.set_xlim([0, 500]) ax.set_ylim([0, 1e-2]) ax.set_xlabel('Distance along path [m]') ax.set_ylabel('Curvature [1/m]') ax.grid(True) plt.show() while True: m = messaging.recv_one_or_none(live_map_sock) p = messaging.recv_one_or_none(plan_sock) if p is not None: v = p.plan.vCurvature * CV.MS_TO_MPH speed_txt.set_text('Desired curvature speed: %.2f mph' % v) if m is not None: print("Current way id: %d" % m.liveMapData.wayId) curv_txt.set_text('Curvature valid: %s Dist: %03.0f m\nSpeedlimit valid: %s Speed: %.0f mph' % (str(m.liveMapData.curvatureValid), m.liveMapData.distToTurn, str(m.liveMapData.speedLimitValid), m.liveMapData.speedLimit * CV.MS_TO_MPH)) points_plt.set_xdata(m.liveMapData.roadX) points_plt.set_ydata(m.liveMapData.roadY)
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime,leftLaneVisible,rightLaneVisible): if (not enabled) and (self.ALCA.laneChange_cancelled): self.ALCA.laneChange_cancelled = False self.ALCA.laneChange_cancelled_counter = 0 self.warningNeeded = 1 if self.warningCounter > 0: self.warningCounter = self.warningCounter - 1 if self.warningCounter == 0: self.warningNeeded = 1 if self.warningCounter == 0 or not enabled: # when zero reset all warnings self.DAS_222_accCameraBlind = 0 #we will see what we can use this for self.DAS_219_lcTempUnavailableSpeed = 0 self.DAS_220_lcTempUnavailableRoad = 0 self.DAS_221_lcAborting = 0 self.DAS_211_accNoSeatBelt = 0 self.DAS_207_lkasUnavailable = 0 #use for manual not in drive? self.DAS_208_rackDetected = 0 #use for low battery? self.DAS_202_noisyEnvironment = 0 #use for planner error? self.DAS_025_steeringOverride = 0 #use for manual steer? self.DAS_206_apUnavailable = 0 #Ap disabled from CID if CS.keepEonOff: if CS.cstm_btns.get_button_status("dsp") != 9: CS.cstm_btns.set_button_status("dsp",9) else: if CS.cstm_btns.get_button_status("dsp") != 1: CS.cstm_btns.set_button_status("dsp",1) # """ Controls thread """ if not CS.useTeslaMapData: if self.speedlimit is None: self.speedlimit = messaging.sub_sock('liveMapData', conflate=True) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print ("INVALID HUD", hud) hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 100 == 0): CS.cstm_btns.send_button_info() #read speed limit params if CS.hasTeslaIcIntegration: self.set_speed_limit_active = True self.speed_limit_offset = CS.userSpeedLimitOffsetKph else: self.set_speed_limit_active = (self.params.get("SpeedLimitOffset") is not None) and (self.params.get("LimitSetSpeed") == "1") if self.set_speed_limit_active: self.speed_limit_offset = float(self.params.get("SpeedLimitOffset")) if not self.isMetric: self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS else: self.speed_limit_offset = 0. if CS.useTeslaGPS and (frame % 10 == 0): if self.gpsLocationExternal is None: self.gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') sol = gen_solution(CS) sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) self.gpsLocationExternal.send(sol.to_bytes()) #get pitch/roll/yaw every 0.1 sec if (frame %10 == 0): (self.accPitch, self.accRoll, self.accYaw),(self.magPitch, self.magRoll, self.magYaw),(self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update(CS.v_ego,CS.a_ego,CS.angle_steers) CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,self.magPitch, self.magRoll, self.magYaw,self.gyroPitch, self.gyroRoll, self.gyroYaw) # Update statuses for custom buttons every 0.1 sec. if (frame % 10 == 0): self.ALCA.update_status((CS.cstm_btns.get_button_status("alca") > 0) and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or (CS.hasTeslaIcIntegration and CS.alcaEnabled))) self.blinker.update_state(CS, frame) # update PCC module info pedal_can_sends = self.PCC.update_stat(CS, frame) if self.PCC.pcc_available: self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # update CS.v_cruise_pcm based on module selected. speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.KPH_TO_MPH if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph else: CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame, self.blinker) self.should_ldw = self._should_ldw(CS, frame) apply_angle = -actuators.steerAngle # Tesla is reversed vs OP. # Update HSO module info. human_control = self.HSO.update_stat(self,CS, enabled, actuators, frame) human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control and vehicle_moving) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # Windup slower. if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) #BB disable limits to test 0.5.8 # apply_angle = clip(apply_angle , self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] #if using radar, we need to send the VIN if CS.useTeslaRadar and (frame % 100 == 0): useRadar=0 if CS.useTeslaRadar: useRadar=1 can_sends.append(teslacan.create_radar_VIN_msg(self.radarVin_idx,CS.radarVIN,1,0x108,useRadar,CS.radarPosition,CS.radarEpasType)) self.radarVin_idx += 1 self.radarVin_idx = self.radarVin_idx % 3 #First we emulate DAS. # DAS_longC_enabled (1),DAS_speed_override (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4) # DAS_speed_kph(8), # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3), # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5), # DAS_acc_speed_limit (8), # DAS_speed_limit_units(8) #send fake_das data as 0x553 # TODO: forward collision warning if frame % 10 == 0: speedlimitMsg = None if self.speedlimit is not None: speedlimitMsg = messaging.recv_one_or_none(self.speedlimit) icLeadsMsg = self.icLeads.receive(non_blocking=True) radarStateMsg = messaging.recv_one_or_none(self.radarState) alcaStateMsg = self.alcaState.receive(non_blocking=True) pathPlanMsg = messaging.recv_one_or_none(self.pathPlan) icCarLRMsg = self.icCarLR.receive(non_blocking=True) trafficeventsMsgs = None if self.trafficevents is not None: trafficeventsMsgs = messaging.recv_sock(self.trafficevents) if CS.hasTeslaIcIntegration: self.speed_limit_ms = CS.speed_limit_ms if (speedlimitMsg is not None) and not CS.useTeslaMapData: lmd = speedlimitMsg.liveMapData self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0 if icLeadsMsg is not None: self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg) if radarStateMsg is not None: #to show lead car on IC if self.icLeadsData is not None: can_messages = self.showLeadCarOnICCanMessage(radarStateMsg = radarStateMsg) can_sends.extend(can_messages) if alcaStateMsg is not None: self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg) if pathPlanMsg is not None: #to show curvature and lanes on IC if self.alcaStateData is not None: self.handlePathPlanSocketForCurvatureOnIC(pathPlanMsg = pathPlanMsg, alcaStateData = self.alcaStateData,CS = CS) if icCarLRMsg is not None: can_messages = self.showLeftAndRightCarsOnICCanMessages(icCarLRMsg = tesla.ICCarsLR.from_bytes(icCarLRMsg)) can_sends.extend(can_messages) if trafficeventsMsgs is not None: can_messages = self.handleTrafficEvents(trafficEventsMsgs = trafficeventsMsgs) can_sends.extend(can_messages) op_status = 0x02 hands_on_state = 0x00 forward_collision_warning = 0 #1 if needed if hud_alert == AH.FCW: forward_collision_warning = hud_alert[1] if forward_collision_warning > 1: forward_collision_warning = 1 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold cc_state = 1 alca_state = 0x00 speed_override = 0 collision_warning = 0x00 speed_control_enabled = 0 accel_min = -15 accel_max = 5 acc_speed_kph = 0 send_fake_warning = False send_fake_msg = False if enabled: #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning alca_state = 0x01 if self.opState == 0: op_status = 0x02 if self.opState == 1: op_status = 0x03 if self.opState == 2: op_status = 0x08 if self.opState == 3: op_status = 0x01 if self.opState == 5: op_status = 0x03 if self.blinker.override_direction > 0: alca_state = 0x08 + self.blinker.override_direction elif (self.lLine > 1) and (self.rLine > 1): alca_state = 0x08 elif (self.lLine > 1): alca_state = 0x06 elif (self.rLine > 1): alca_state = 0x07 else: alca_state = 0x01 #canceled by user if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0): alca_state = 0x14 #min speed for ALCA if (CS.CL_MIN_V > CS.v_ego): alca_state = 0x05 #max angle for ALCA if (abs(actuators.steerAngle) >= CS.CL_MAX_A): alca_state = 0x15 if not enable_steer_control: #op_status = 0x08 hands_on_state = 0x02 if hud_alert == AH.STEER: if snd_chime == CM.MUTE: hands_on_state = 0x03 else: hands_on_state = 0x05 if self.PCC.pcc_available: acc_speed_kph = self.PCC.pedal_speed_kph if hud_alert == AH.FCW: collision_warning = hud_alert[1] if collision_warning > 1: collision_warning = 1 #use disabling for alerts/errors to make them aware someting is goin going on if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW): op_status = 0x08 if self.ACC.enable_adaptive_cruise: acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH if (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise): speed_control_enabled = 1 cc_state = 2 if not self.ACC.adaptive: cc_state = 3 CS.speed_control_enabled = 1 else: CS.speed_control_enabled = 0 if (CS.pcm_acc_status == 4): #car CC enabled but not OP, display the HOLD message cc_state = 3 else: if (CS.pcm_acc_status == 4): cc_state = 3 if enabled: if frame % 2 == 0: send_fake_msg = True if frame % 25 == 0: send_fake_warning = True else: if frame % 23 == 0: send_fake_msg = True if frame % 60 == 0: send_fake_warning = True if frame % 10 == 0: can_sends.append(teslacan.create_fake_DAS_obj_lane_msg(self.leadDx,self.leadDy,self.leadClass,self.rLine,self.lLine,self.curv0,self.curv1,self.curv2,self.curv3,self.laneRange,self.laneWidth)) speed_override = 0 if (CS.pedal_interceptor_value > 10) and (cc_state > 1): speed_override = 0 #force zero for now if (not enable_steer_control) and op_status == 3: #hands_on_state = 0x03 self.DAS_219_lcTempUnavailableSpeed = 1 self.warningCounter = 100 self.warningNeeded = 1 if enabled and self.ALCA.laneChange_cancelled and (not CS.steer_override) and (CS.turn_signal_stalk_state == 0) and (self.ALCA.laneChange_cancelled_counter > 0): self.DAS_221_lcAborting = 1 self.warningCounter = 300 self.warningNeeded = 1 if CS.hasTeslaIcIntegration: highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1) if frame % 5 == 0: self.cc_counter = (self.cc_counter + 1) % 40 #use this to change status once a second self.fleet_speed_state = 0x00 #fleet speed unavailable if FleetSpeed.is_available(CS): if self.ACC.fleet_speed.is_active(frame) or self.PCC.fleet_speed.is_active(frame): self.fleet_speed_state = 0x02 #fleet speed enabled else: self.fleet_speed_state = 0x01 #fleet speed available can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,self.fleet_speed_state)) if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02): CS.v_cruise_pcm = CS.v_cruise_pcm + 1 send_fake_msg = True if (self.cc_counter == 3): send_fake_msg = True if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 park_brake_request = int(CS.ahbEnabled) if park_brake_request == 1: print("Park Brake Request received") adaptive_cruise = 1 if (not self.PCC.pcc_available and self.ACC.adaptive) or self.PCC.pcc_available else 0 can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \ cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \ CS.v_cruise_pcm, CS.DAS_fusedSpeedLimit, apply_angle, 1 if enable_steer_control else 0, park_brake_request)) if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (self.stopSignWarning != self.stopSignWarning_last) or (self.stopLightWarning != self.stopLightWarning_last) or (self.warningNeeded == 1) or (frame % 100 == 0): #if it's time to send OR we have a warning or emergency disable can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \ self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \ self.stopSignWarning, self.stopLightWarning, \ self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \ self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,0,CS.useWithoutHarness)) self.stopLightWarning_last = self.stopLightWarning self.stopSignWarning_last = self.stopSignWarning self.warningNeeded = 0 # end of DAS emulation """ if frame % 100 == 0: # and CS.hasTeslaIcIntegration: #IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake can_sends.append(teslacan.create_fake_IC_msg()) # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) if (not self.PCC.pcc_available) and frame % 5 == 0: # acc processed at 20Hz cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms * CV.MS_TO_KPH, self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat= 0, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if self.PCC.pcc_available and frame % 5 == 0: # pedal processed at 20Hz apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms, self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled) can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) self.last_angle = apply_angle self.last_accel = apply_accel return pedal_can_sends + can_sends