def update_stat(self, CS, enabled, sendcan): if not self.LoC: self.LoC = LongControl(CS.CP, tesla_compute_gb) can_sends = [] if CS.pedal_interceptor_available and not CS.cstm_btns.get_button_status( "pedal"): # pedal hardware, enable button CS.cstm_btns.set_button_status("pedal", 1) print "enabling pedal" elif not CS.pedal_interceptor_available: if CS.cstm_btns.get_button_status("pedal"): # no pedal hardware, disable button CS.cstm_btns.set_button_status("pedal", 0) print "disabling pedal" print "Pedal unavailable." return # check if we had error before if self.pedal_interceptor_state != CS.pedal_interceptor_state: self.pedal_interceptor_state = CS.pedal_interceptor_state CS.cstm_btns.set_button_status( "pedal", 1 if self.pedal_interceptor_state > 0 else 0) if self.pedal_interceptor_state > 0: # send reset command idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) CS.UE.custom_alert_message( 3, "Pedal Interceptor Off (state %s)" % self.pedal_interceptor_state, 150, 3) else: CS.UE.custom_alert_message(3, "Pedal Interceptor On", 150, 3) # disable on brake if CS.brake_pressed and self.enable_pedal_cruise: self.enable_pedal_cruise = False self.reset(0.) CS.UE.custom_alert_message(3, "PDL Disabled", 150, 4) CS.cstm_btns.set_button_status("pedal", 1) print "brake pressed" prev_enable_pedal_cruise = self.enable_pedal_cruise # process any stalk movement curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.MPH_TO_KPH if (CS.cruise_buttons == CruiseButtons.MAIN and self.prev_cruise_buttons != CruiseButtons.MAIN): double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 self.last_cruise_stalk_pull_time = curr_time_ms ready = (CS.cstm_btns.get_button_status("pedal") > PCCState.OFF and enabled and CruiseState.is_off(CS.pcm_acc_status)) if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_pedal_cruise = True self.LoC.reset(CS.v_ego) # Increase PCC speed to match current, if applicable. self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH, self.pedal_speed_kph) else: # A single pull disables PCC (falling back to just steering). self.enable_pedal_cruise = False # Handle pressing the cancel button. elif CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_pedal_cruise = False self.pedal_speed_kph = 0. self.last_cruise_stalk_pull_time = 0 # Handle pressing up and down buttons. elif (self.enable_pedal_cruise and CS.cruise_buttons != self.prev_cruise_buttons): # Real stalk command while PCC is already enabled. Adjust the max PCC # speed if necessary. actual_speed_kph = CS.v_ego * CV.MS_TO_KPH if CS.cruise_buttons == CruiseButtons.RES_ACCEL: self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + speed_uom_kph elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: self.pedal_speed_kph = max( self.pedal_speed_kph, actual_speed_kph) + 5 * speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_SET: self.pedal_speed_kph = min(self.pedal_speed_kph, actual_speed_kph) - speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: self.pedal_speed_kph = min( self.pedal_speed_kph, actual_speed_kph) - 5 * speed_uom_kph # Clip PCC speed between 0 and 170 KPH. self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) # If something disabled cruise control, disable PCC too elif self.enable_pedal_cruise and CS.pcm_acc_status: self.enable_pedal_cruise = False # Notify if PCC was toggled if prev_enable_pedal_cruise and not self.enable_pedal_cruise: CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4) CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY) elif self.enable_pedal_cruise and not prev_enable_pedal_cruise: CS.UE.custom_alert_message(2, "PCC Enabled", 150) CS.cstm_btns.set_button_status("pedal", PCCState.ENABLED) # Update the UI to show whether the current car state allows PCC. if CS.cstm_btns.get_button_status("pedal") in [ PCCState.STANDBY, PCCState.NOT_READY ]: if enabled and CruiseState.is_off(CS.pcm_acc_status): CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY) else: CS.cstm_btns.set_button_status("pedal", PCCState.NOT_READY) # Update prev state after all other actions. self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime): """ Controls thread """ ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars) if not self.enable_camera: return # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** STEER_MAX = 420 # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) # Basic highway lane change logic changing_lanes = CS.right_blinker_on or CS.left_blinker_on #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 1000 == 0): CS.cstm_btns.send_button_info() # Update statuses for custom buttons every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) #print CS.cstm_btns.get_button_status("alca") if CS.pedal_interceptor_available: #update PCC module info self.PCC.update_stat(CS, True, sendcan) self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # Update HSO module info. human_control = False # update CS.v_cruise_pcm based on module selected. if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph else: CS.v_cruise_pcm = CS.v_cruise_actual # Get the angle from ALCA. alca_enabled = False turn_signal_needed = 0 alca_steer = 0. apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update( enabled, CS, frame, actuators) apply_angle = -apply_angle # Tesla is reversed vs OP. human_control = self.HSO.update_stat(CS, enabled, actuators, frame) human_lane_changing = changing_lanes and not alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # Windup slower. if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # If blinker is on send the actual angle. #if (changing_lanes and (CS.laneChange_enabled < 2)): # apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] send_step = 5 if (True): #First we emulate DAS. #send DAS_bootID if not self.sent_DAS_bootID: can_sends.append(teslacan.create_DAS_bootID_msg()) self.sent_DAS_bootID = True else: #get speed limit for socket, _ in self.poller.poll(0): if socket is self.speedlimit: self.speedlimit_mph = ui.SpeedLimitData.from_bytes( socket.recv()).speed * CV.MS_TO_MPH #send DAS_info if frame % 100 == 0: can_sends.append( teslacan.create_DAS_info_msg(CS.DAS_info_msg)) CS.DAS_info_msg += 1 CS.DAS_info_msg = CS.DAS_info_msg % 10 #send DAS_status if frame % 50 == 0: op_status = 0x02 hands_on_state = 0x00 speed_limit_kph = int(self.speedlimit_mph) alca_state = 0x08 if enabled: op_status = 0x03 alca_state = 0x08 + turn_signal_needed #if not enable_steer_control: #op_status = 0x04 #hands_on_state = 0x03 if hud_alert == AH.STEER: if snd_chime == CM.MUTE: hands_on_state = 0x03 else: hands_on_state = 0x05 can_sends.append( teslacan.create_DAS_status_msg(CS.DAS_status_idx, op_status, speed_limit_kph, alca_state, hands_on_state)) CS.DAS_status_idx += 1 CS.DAS_status_idx = CS.DAS_status_idx % 16 #send DAS_status2 if frame % 50 == 0: collision_warning = 0x00 acc_speed_limit_mph = CS.v_cruise_pcm * CV.KPH_TO_MPH if hud_alert == AH.FCW: collision_warning = 0x01 can_sends.append( teslacan.create_DAS_status2_msg( CS.DAS_status2_idx, acc_speed_limit_mph, collision_warning)) CS.DAS_status2_idx += 1 CS.DAS_status2_idx = CS.DAS_status2_idx % 16 #send DAS_bodyControl if frame % 50 == 0: can_sends.append( teslacan.create_DAS_bodyControls_msg( CS.DAS_bodyControls_idx, turn_signal_needed)) CS.DAS_bodyControls_idx += 1 CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16 #send DAS_control if frame % 4 == 0: acc_speed_limit_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH accel_min = -15 accel_max = 5 speed_control_enabled = enabled and (acc_speed_limit_kph > 0) can_sends.append( teslacan.create_DAS_control(CS.DAS_control_idx, speed_control_enabled, acc_speed_limit_kph, accel_min, accel_max)) CS.DAS_control_idx += 1 CS.DAS_control_idx = CS.DAS_control_idx % 8 #send DAS_lanes if frame % 10 == 0: can_sends.append( teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx)) CS.DAS_lanes_idx += 1 CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16 #send DAS_pscControl if frame % 4 == 0: can_sends.append( teslacan.create_DAS_pscControl_msg( CS.DAS_pscControl_idx)) CS.DAS_pscControl_idx += 1 CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16 #send DAS_telemetryPeriodic if frame % 4 == 0: can_sends.append( teslacan.create_DAS_telemetryPeriodic( CS.DAS_telemetryPeriodic1_idx, CS.DAS_telemetryPeriodic2_idx)) CS.DAS_telemetryPeriodic2_idx += 1 CS.DAS_telemetryPeriodic2_idx = CS.DAS_telemetryPeriodic2_idx % 10 if CS.DAS_telemetryPeriodic2_idx == 0: CS.DAS_telemetryPeriodic1_idx += 2 CS.DAS_telemetryPeriodic1_idx = CS.DAS_telemetryPeriodic1_idx % 16 #send DAS_telemetryEvent if frame % 10 == 0: #can_sends.append(teslacan.create_DAS_telemetryEvent(CS.DAS_telemetryEvent1_idx,CS.DAS_telemetryEvent2_idx)) CS.DAS_telemetryEvent2_idx += 1 CS.DAS_telemetryEvent2_idx = CS.DAS_telemetryEvent2_idx % 10 if CS.DAS_telemetryEvent2_idx == 0: CS.DAS_telemetryEvent1_idx += 2 CS.DAS_telemetryEvent1_idx = CS.DAS_telemetryEvent1_idx % 16 #send DAS_visualDebug if (frame + 1) % 10 == 0: can_sends.append(teslacan.create_DAS_visualDebug_msg()) #send DAS_chNm if (frame + 2) % 10 == 0: can_sends.append(teslacan.create_DAS_chNm()) #send DAS_objects if frame % 3 == 0: can_sends.append( teslacan.create_DAS_objects_msg(CS.DAS_objects_idx)) CS.DAS_objects_idx += 1 CS.DAS_objects_idx = CS.DAS_objects_idx % 16 #send DAS_warningMatrix0 if frame % 6 == 0: can_sends.append( teslacan.create_DAS_warningMatrix0( CS.DAS_warningMatrix0_idx)) CS.DAS_warningMatrix0_idx += 1 CS.DAS_warningMatrix0_idx = CS.DAS_warningMatrix0_idx % 16 #send DAS_warningMatrix3 if (frame + 3) % 6 == 0: driverResumeRequired = 0 if enabled and not enable_steer_control: driverResumeRequired = 1 can_sends.append( teslacan.create_DAS_warningMatrix3( CS.DAS_warningMatrix3_idx, driverResumeRequired)) CS.DAS_warningMatrix3_idx += 1 CS.DAS_warningMatrix3_idx = CS.DAS_warningMatrix3_idx % 16 #send DAS_warningMatrix1 if frame % 100 == 0: can_sends.append( teslacan.create_DAS_warningMatrix1( CS.DAS_warningMatrix1_idx)) CS.DAS_warningMatrix1_idx += 1 CS.DAS_warningMatrix1_idx = CS.DAS_warningMatrix1_idx % 16 # end of DAS emulation """ idx = frame % 16 can_sends.append( teslacan.create_steering_control(enable_steer_control, apply_angle, idx)) can_sends.append(teslacan.create_epb_enable_signal(idx)) cruise_btn = None if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available: cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed) #add fake carConfig to trigger IC to display AP if frame % 2 == 0: carConfig_msg = teslacan.create_GTW_carConfig_msg( real_carConfig_data=CS.real_carConfig, dasHw=1, autoPilot=1, fRadarHw=1) #can_sends.append(carConfig_msg) if cruise_btn or (turn_signal_needed > 0 and frame % 2 == 0): cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat=0, #turn_signal_needed, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz apply_accel, accel_needed, accel_idx = self.PCC.update_pdl( enabled, CS, frame, actuators, pcm_speed) can_sends.append( teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) self.last_angle = apply_angle self.last_accel = apply_accel sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime,leftLaneVisible,rightLaneVisible): if (not enabled) and (self.ALCA.laneChange_cancelled): self.ALCA.laneChange_cancelled = False self.ALCA.laneChange_cancelled_counter = 0 self.warningNeeded = 1 if self.warningCounter > 0: self.warningCounter = self.warningCounter - 1 if self.warningCounter == 0: self.warningNeeded = 1 if self.warningCounter == 0 or not enabled: # when zero reset all warnings self.DAS_222_accCameraBlind = 0 #we will see what we can use this for self.DAS_219_lcTempUnavailableSpeed = 0 self.DAS_220_lcTempUnavailableRoad = 0 self.DAS_221_lcAborting = 0 self.DAS_211_accNoSeatBelt = 0 self.DAS_207_lkasUnavailable = 0 #use for manual not in drive? self.DAS_208_rackDetected = 0 #use for low battery? self.DAS_202_noisyEnvironment = 0 #use for planner error? self.DAS_025_steeringOverride = 0 #use for manual steer? self.DAS_206_apUnavailable = 0 #Ap disabled from CID if CS.keepEonOff: if CS.cstm_btns.get_button_status("dsp") != 9: CS.cstm_btns.set_button_status("dsp", 9) else: if CS.cstm_btns.get_button_status("dsp") != 1: CS.cstm_btns.set_button_status("dsp", 1) # """ Controls thread """ if not CS.useTeslaMapData: if self.speedlimit is None: self.speedlimit = messaging.sub_sock('liveMapData', conflate=True) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print("INVALID HUD", hud) hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 100 == 0): CS.cstm_btns.send_button_info() #read speed limit params if CS.hasTeslaIcIntegration: self.set_speed_limit_active = True self.speed_limit_offset = CS.userSpeedLimitOffsetKph else: self.set_speed_limit_active = ( self.params.get("SpeedLimitOffset") is not None) and (self.params.get("LimitSetSpeed") == "1") if self.set_speed_limit_active: self.speed_limit_offset = float( self.params.get("SpeedLimitOffset")) if not self.isMetric: self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS else: self.speed_limit_offset = 0. if CS.useTeslaGPS and (frame % 10 == 0): if self.gpsLocationExternal is None: self.gpsLocationExternal = messaging.pub_sock( 'gpsLocationExternal') sol = gen_solution(CS) sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) self.gpsLocationExternal.send(sol.to_bytes()) #get pitch/roll/yaw every 0.1 sec if (frame % 10 == 0): (self.accPitch, self.accRoll, self.accYaw), (self.magPitch, self.magRoll, self.magYaw), (self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update( CS.v_ego, CS.a_ego, CS.angle_steers) CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw, self.magPitch, self.magRoll, self.magYaw, self.gyroPitch, self.gyroRoll, self.gyroYaw) # Update statuses for custom buttons every 0.1 sec. if (frame % 10 == 0): self.ALCA.update_status( (CS.cstm_btns.get_button_status("alca") > 0) and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or (CS.hasTeslaIcIntegration and CS.alcaEnabled))) self.blinker.update_state(CS, frame) # update PCC module info pedal_can_sends = self.PCC.update_stat(CS, frame) if self.PCC.pcc_available: self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # update CS.v_cruise_pcm based on module selected. speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.KPH_TO_MPH if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph else: CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame, self.blinker) self.should_ldw = self._should_ldw(CS, frame) apply_angle = -actuators.steerAngle # Tesla is reversed vs OP. # Update HSO module info. human_control = self.HSO.update_stat(self, CS, enabled, actuators, frame) human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control and vehicle_moving) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # Windup slower. if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) #BB disable limits to test 0.5.8 # apply_angle = clip(apply_angle , self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] #if using radar, we need to send the VIN if CS.useTeslaRadar and (frame % 100 == 0): useRadar = 0 if CS.useTeslaRadar: useRadar = 1 can_sends.append( teslacan.create_radar_VIN_msg(self.radarVin_idx, CS.radarVIN, 1, 0x108, useRadar, CS.radarPosition, CS.radarEpasType)) self.radarVin_idx += 1 self.radarVin_idx = self.radarVin_idx % 3 #First we emulate DAS. # DAS_longC_enabled (1),DAS_speed_override (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4) # DAS_speed_kph(8), # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3), # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5), # DAS_acc_speed_limit (8), # DAS_speed_limit_units(8) #send fake_das data as 0x553 # TODO: forward collision warning if frame % 10 == 0: speedlimitMsg = None if self.speedlimit is not None: speedlimitMsg = messaging.recv_one_or_none(self.speedlimit) icLeadsMsg = self.icLeads.receive(non_blocking=True) radarStateMsg = messaging.recv_one_or_none(self.radarState) alcaStateMsg = self.alcaState.receive(non_blocking=True) pathPlanMsg = messaging.recv_one_or_none(self.pathPlan) icCarLRMsg = self.icCarLR.receive(non_blocking=True) trafficeventsMsgs = None if self.trafficevents is not None: trafficeventsMsgs = messaging.recv_sock(self.trafficevents) if CS.hasTeslaIcIntegration: self.speed_limit_ms = CS.speed_limit_ms if (speedlimitMsg is not None) and not CS.useTeslaMapData: lmd = speedlimitMsg.liveMapData self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0 if icLeadsMsg is not None: self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg) if radarStateMsg is not None: #to show lead car on IC if self.icLeadsData is not None: can_messages = self.showLeadCarOnICCanMessage( radarStateMsg=radarStateMsg) can_sends.extend(can_messages) if alcaStateMsg is not None: self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg) if pathPlanMsg is not None: #to show curvature and lanes on IC if self.alcaStateData is not None: self.handlePathPlanSocketForCurvatureOnIC( pathPlanMsg=pathPlanMsg, alcaStateData=self.alcaStateData, CS=CS) if icCarLRMsg is not None: can_messages = self.showLeftAndRightCarsOnICCanMessages( icCarLRMsg=tesla.ICCarsLR.from_bytes(icCarLRMsg)) can_sends.extend(can_messages) if trafficeventsMsgs is not None: can_messages = self.handleTrafficEvents( trafficEventsMsgs=trafficeventsMsgs) can_sends.extend(can_messages) op_status = 0x02 hands_on_state = 0x00 forward_collision_warning = 0 #1 if needed if hud_alert == AH.FCW: forward_collision_warning = hud_alert[1] if forward_collision_warning > 1: forward_collision_warning = 1 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold cc_state = 1 alca_state = 0x00 speed_override = 0 collision_warning = 0x00 speed_control_enabled = 0 accel_min = -15 accel_max = 5 acc_speed_kph = 0 send_fake_warning = False send_fake_msg = False if enabled: #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning alca_state = 0x01 if self.opState == 0: op_status = 0x02 if self.opState == 1: op_status = 0x03 if self.opState == 2: op_status = 0x08 if self.opState == 3: op_status = 0x01 if self.opState == 5: op_status = 0x03 if self.blinker.override_direction > 0: alca_state = 0x08 + self.blinker.override_direction elif (self.lLine > 1) and (self.rLine > 1): alca_state = 0x08 elif (self.lLine > 1): alca_state = 0x06 elif (self.rLine > 1): alca_state = 0x07 else: alca_state = 0x01 #canceled by user if self.ALCA.laneChange_cancelled and ( self.ALCA.laneChange_cancelled_counter > 0): alca_state = 0x14 #min speed for ALCA if (CS.CL_MIN_V > CS.v_ego): alca_state = 0x05 #max angle for ALCA if (abs(actuators.steerAngle) >= CS.CL_MAX_A): alca_state = 0x15 if not enable_steer_control: #op_status = 0x08 hands_on_state = 0x02 if hud_alert == AH.STEER: if snd_chime == CM.MUTE: hands_on_state = 0x03 else: hands_on_state = 0x05 if self.PCC.pcc_available: acc_speed_kph = self.PCC.pedal_speed_kph if hud_alert == AH.FCW: collision_warning = hud_alert[1] if collision_warning > 1: collision_warning = 1 #use disabling for alerts/errors to make them aware someting is goin going on if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW): op_status = 0x08 if self.ACC.enable_adaptive_cruise: acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH if (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or ( self.ACC.enable_adaptive_cruise): speed_control_enabled = 1 cc_state = 2 if not self.ACC.adaptive: cc_state = 3 CS.speed_control_enabled = 1 else: CS.speed_control_enabled = 0 if (CS.pcm_acc_status == 4): #car CC enabled but not OP, display the HOLD message cc_state = 3 else: if (CS.pcm_acc_status == 4): cc_state = 3 if enabled: if frame % 2 == 0: send_fake_msg = True if frame % 25 == 0: send_fake_warning = True else: if frame % 23 == 0: send_fake_msg = True if frame % 60 == 0: send_fake_warning = True if frame % 10 == 0: can_sends.append( teslacan.create_fake_DAS_obj_lane_msg( self.leadDx, self.leadDy, self.leadClass, self.rLine, self.lLine, self.curv0, self.curv1, self.curv2, self.curv3, self.laneRange, self.laneWidth)) speed_override = 0 if (CS.pedal_interceptor_value > 10) and (cc_state > 1): speed_override = 0 #force zero for now if (not enable_steer_control) and op_status == 3: #hands_on_state = 0x03 self.DAS_219_lcTempUnavailableSpeed = 1 self.warningCounter = 100 self.warningNeeded = 1 if enabled and self.ALCA.laneChange_cancelled and ( not CS.steer_override) and ( CS.turn_signal_stalk_state == 0) and (self.ALCA.laneChange_cancelled_counter > 0): self.DAS_221_lcAborting = 1 self.warningCounter = 300 self.warningNeeded = 1 if CS.hasTeslaIcIntegration: highLowBeamStatus, highLowBeamReason, ahbIsEnabled = self.AHB.update( CS, frame, self.ahbLead1) if frame % 5 == 0: self.cc_counter = ( self.cc_counter + 1) % 40 #use this to change status once a second self.fleet_speed_state = 0x00 #fleet speed unavailable if FleetSpeed.is_available(CS): if self.ACC.fleet_speed.is_active( frame) or self.PCC.fleet_speed.is_active(frame): self.fleet_speed_state = 0x02 #fleet speed enabled else: self.fleet_speed_state = 0x01 #fleet speed available can_sends.append( teslacan.create_fake_DAS_msg2(highLowBeamStatus, highLowBeamReason, ahbIsEnabled, self.fleet_speed_state)) if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02): CS.v_cruise_pcm = CS.v_cruise_pcm + 1 send_fake_msg = True if (self.cc_counter == 3): send_fake_msg = True if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 park_brake_request = 0 #experimental; disabled for now if park_brake_request == 1: print("Park Brake Request received") adaptive_cruise = 1 if ( not self.PCC.pcc_available and self.ACC.adaptive) or self.PCC.pcc_available else 0 can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \ cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \ CS.v_cruise_pcm, CS.DAS_fusedSpeedLimit, apply_angle, 1 if enable_steer_control else 0, park_brake_request)) if send_fake_warning or (self.opState == 2) or (self.opState == 5) or ( self.stopSignWarning != self.stopSignWarning_last) or ( self.stopLightWarning != self.stopLightWarning_last) or ( self.warningNeeded == 1) or (frame % 100 == 0): #if it's time to send OR we have a warning or emergency disable can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \ self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \ self.stopSignWarning, self.stopLightWarning, \ self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \ self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,CS.useWithoutHarness,CS.usesApillarHarness)) self.stopLightWarning_last = self.stopLightWarning self.stopSignWarning_last = self.stopSignWarning self.warningNeeded = 0 # end of DAS emulation """ if frame % 100 == 0: # and CS.hasTeslaIcIntegration: #IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake can_sends.append(teslacan.create_fake_IC_msg()) # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) if (not self.PCC.pcc_available ) and frame % 5 == 0: # acc processed at 20Hz cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms * CV.MS_TO_KPH, self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat=0, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if self.PCC.pcc_available and frame % 5 == 0: # pedal processed at 20Hz pedalcan = 2 if CS.useWithoutHarness: pedalcan = 0 apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms, self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled) can_sends.append( teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx, pedalcan)) self.last_angle = apply_angle self.last_accel = apply_accel return pedal_can_sends + can_sends
def update_stat(self, CS, frame): if not self.LoC: self.LoC = LongControl(CS.CP, tesla_compute_gb) # Get v_id from the stored file when initiating the LoC and reset_on_disengage==false if (not RESET_PID_ON_DISENGAGE): self.load_pid() self._update_pedal_state(CS, frame) can_sends = [] if not self.pcc_available: timed_out = frame >= self.pedal_timeout_frame if timed_out or CS.pedal_interceptor_state > 0: if self.prev_pcc_available: CS.UE.custom_alert_message( 4, "Pedal Interceptor %s" % ("timed out" if timed_out else "fault (state %s)" % CS.pedal_interceptor_state), 200, 4) if frame % 50 == 0: # send reset command idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 can_sends.append( teslacan.create_pedal_command_msg(0, 0, idx)) return can_sends prev_enable_pedal_cruise = self.enable_pedal_cruise # disable on brake if CS.brake_pressed and self.enable_pedal_cruise: self.enable_pedal_cruise = False self.reset(0.) # process any stalk movement curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.MPH_TO_KPH if (CS.cruise_buttons == CruiseButtons.MAIN and self.prev_cruise_buttons != CruiseButtons.MAIN): self.prev_stalk_pull_time_ms = self.stalk_pull_time_ms self.stalk_pull_time_ms = curr_time_ms double_pull = self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms < STALK_DOUBLE_PULL_MS ready = (CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) > PCCState.OFF and (CruiseState.is_off(CS.pcm_acc_status)) or CS.forcePedalOverCC) if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_pedal_cruise = True self.reset(CS.v_ego) # Increase PCC speed to match current, if applicable. # We round the target speed in the user's units of measurement to avoid jumpy speed readings current_speed_kph_uom_rounded = int(CS.v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph self.pedal_speed_kph = max(current_speed_kph_uom_rounded, self.speed_limit_kph) # Handle pressing the cancel button. elif CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_pedal_cruise = False self.pedal_speed_kph = 0. self.stalk_pull_time_ms = 0 self.prev_stalk_pull_time_ms = -1000 # Handle pressing up and down buttons. elif (self.enable_pedal_cruise and CS.cruise_buttons != self.prev_cruise_buttons): # Real stalk command while PCC is already enabled. Adjust the max PCC speed if necessary. # We round the target speed in the user's units of measurement to avoid jumpy speed readings actual_speed_kph_uom_rounded = int( CS.v_ego * CV.MS_TO_KPH / speed_uom_kph + 0.5) * speed_uom_kph if CS.cruise_buttons == CruiseButtons.RES_ACCEL: self.pedal_speed_kph = max( self.pedal_speed_kph, actual_speed_kph_uom_rounded) + speed_uom_kph elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: self.pedal_speed_kph = max( self.pedal_speed_kph, actual_speed_kph_uom_rounded) + 5 * speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_SET: self.pedal_speed_kph = self.pedal_speed_kph - speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: self.pedal_speed_kph = self.pedal_speed_kph - 5 * speed_uom_kph # Clip PCC speed between 0 and 170 KPH. self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) # If something disabled cruise control, disable PCC too elif self.enable_pedal_cruise and CS.pcm_acc_status and not CS.forcePedalOverCC: self.enable_pedal_cruise = False # A single pull disables PCC (falling back to just steering). Wait some time # in case a double pull comes along. elif (self.enable_pedal_cruise and curr_time_ms - self.stalk_pull_time_ms > STALK_DOUBLE_PULL_MS and self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms > STALK_DOUBLE_PULL_MS): self.enable_pedal_cruise = False # Notify if PCC was toggled if prev_enable_pedal_cruise and not self.enable_pedal_cruise: CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4) CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.STANDBY) self.fleet_speed.reset_averager() #save the pid parameters to params file self.save_pid(self.LoC.pid) elif self.enable_pedal_cruise and not prev_enable_pedal_cruise: CS.UE.custom_alert_message(2, "PCC Enabled", 150) CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.ENABLED) # Update the UI to show whether the current car state allows PCC. if CS.cstm_btns.get_button_status(PCCModes.BUTTON_NAME) in [ PCCState.STANDBY, PCCState.NOT_READY ]: if CruiseState.is_off(CS.pcm_acc_status) or CS.forcePedalOverCC: CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.STANDBY) else: CS.cstm_btns.set_button_status(PCCModes.BUTTON_NAME, PCCState.NOT_READY) # Update prev state after all other actions. self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime): """ Controls thread """ ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars) if not self.enable_camera: return # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** STEER_MAX = 420 # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) # Basic highway lane change logic changing_lanes = CS.right_blinker_on or CS.left_blinker_on #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 1000 == 0): CS.cstm_btns.send_button_info() # Update statuses for custom buttons every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) #print CS.cstm_btns.get_button_status("alca") if CS.pedal_hardware_present: #update PCC module info self.PCC.update_stat(CS, True, sendcan) self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # Update HSO module info. human_control = False # update CS.v_cruise_pcm based on module selected. if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph else: CS.v_cruise_pcm = CS.v_cruise_actual # Get the angle from ALCA. alca_enabled = False turn_signal_needed = 0 alca_steer = 0. apply_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update( enabled, CS, frame, actuators) apply_angle = -apply_angle # Tesla is reversed vs OP. human_control = self.HSO.update_stat(CS, enabled, actuators, frame) human_lane_changing = changing_lanes and not alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # Windup slower. if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs( self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # If blinker is on send the actual angle. #if (changing_lanes and (CS.laneChange_enabled < 2)): # apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] send_step = 5 if (True): """#First we emulate DAS. if (CS.DAS_info_frm == -1): #initialize all frames CS.DAS_info_frm = frame # 1.00 s interval CS.DAS_status_frm = (frame + 10) % 100 # 0.50 s interval CS.DAS_status2_frm = (frame + 35) % 100 # 0.50 s interval in between DAS_status CS.DAS_bodyControls_frm = (frame + 40) % 100 # 0.50 s interval CS.DAS_lanes_frm = (frame + 5) % 100 # 0.10 s interval CS.DAS_objects_frm = (frame + 2) % 100 # 0.03 s interval CS.DAS_pscControl_frm = (frame + 3) % 100 # 0.04 s interval if (CS.DAS_info_frm == frame): can_sends.append(teslacan.create_DAS_info_msg(CS.DAS_info_msg)) CS.DAS_info_msg += 1 CS.DAS_info_msg = CS.DAS_info_msg % 10 if (CS.DAS_status_frm == frame): can_sends.append(teslacan.create_DAS_status_msg(CS.DAS_status_idx)) CS.DAS_status_idx += 1 CS.DAS_status_idx = CS.DAS_status_idx % 16 CS.DAS_status_frm = (CS.DAS_status_frm + 50) % 100 if (CS.DAS_status2_frm == frame): can_sends.append(teslacan.create_DAS_status2_msg(CS.DAS_status2_idx)) CS.DAS_status2_idx += 1 CS.DAS_status2_idx = CS.DAS_status2_idx % 16 CS.DAS_status2_frm = (CS.DAS_status2_frm + 50) % 100 if (CS.DAS_bodyControls_frm == frame): can_sends.append(teslacan.create_DAS_bodyControls_msg(CS.DAS_bodyControls_idx)) CS.DAS_bodyControls_idx += 1 CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16 CS.DAS_bodyControls_frm = (CS.DAS_bodyControls_frm + 50) % 100 if (CS.DAS_lanes_frm == frame): can_sends.append(teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx)) CS.DAS_lanes_idx += 1 CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16 CS.DAS_lanes_frm = (CS.DAS_lanes_frm + 10) % 100 if (CS.DAS_pscControl_frm == frame): can_sends.append(teslacan.create_DAS_pscControl_msg(CS.DAS_pscControl_idx)) CS.DAS_pscControl_idx += 1 CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16 CS.DAS_pscControl_frm = (CS.DAS_pscControl_frm + 4) % 100 if (CS.DAS_objects_frm == frame): can_sends.append(teslacan.create_DAS_objects_msg(CS.DAS_objects_idx)) CS.DAS_objects_idx += 1 CS.DAS_objects_idx = CS.DAS_objects_idx % 16 CS.DAS_objects_frm = (CS.DAS_objects_frm + 3) % 100 # end of DAS emulation """ idx = frame % 16 can_sends.append( teslacan.create_steering_control(enable_steer_control, apply_angle, idx)) can_sends.append(teslacan.create_epb_enable_signal(idx)) cruise_btn = None if self.ACC.enable_adaptive_cruise and not self.PCC.pedal_hardware_present: cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed) if (cruise_btn != None) or ((turn_signal_needed > 0) and (frame % 2 == 0)): cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat=turn_signal_needed, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if self.PCC.pedal_hardware_present: # and (frame % 10) == 0: apply_accel, accel_needed, accel_idx = self.PCC.update_pdl( enabled, CS, frame, actuators, pcm_speed) can_sends.append( teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) self.last_angle = apply_angle self.last_accel = apply_accel sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update_stat(self, CS, enabled, sendcan): if self.pid == None: CP = CS.CP self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), (CP.longitudinalKiBP, CP.longitudinalKiV), rate=100.0, sat_limit=0.8) #,convert=compute_gb) self.reset(0.) can_sends = [] #BBTODO: a better way to engage the pedal early and reset its CAN # on first brake press check if hardware present; being on CAN2 values are not included in fingerprinting self.pedal_hardware_present = CS.pedal_hardware_present if not self.pedal_hardware_present: if (CS.cstm_btns.get_button_status("pedal") > 0): #no pedal hardware, disable button CS.cstm_btns.set_button_status("pedal", 0) print "disabling pedal" print "no pedal hardware" return if self.pedal_hardware_present: if (CS.cstm_btns.get_button_status("pedal") == 0): #pedal hardware, enable button CS.cstm_btns.set_button_status("pedal", 1) print "enabling pedal" # check if we had error before if self.user_pedal_state != CS.user_pedal_state: self.user_pedal_state = CS.user_pedal_state CS.cstm_btns.set_button_status( "pedal", 1 if self.user_pedal_state > 0 else 0) if self.user_pedal_state > 0: CS.UE.custom_alert_message( 3, "Pedal Interceptor Error (" + ` self.user_pedal_state ` + ")", 150, 4) # send reset command idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx)) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) # disable on brake if CS.brake_pressed and self.enable_pedal_cruise: self.enable_pedal_cruise = False self.reset(0.) CS.UE.custom_alert_message(3, "PDL Disabled", 150, 4) CS.cstm_btns.set_button_status("pedal", 1) print "brake pressed" prev_enable_pedal_cruise = self.enable_pedal_cruise # process any stalk movement curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.MPH_TO_KPH if (CS.cruise_buttons == CruiseButtons.MAIN and self.prev_cruise_buttons != CruiseButtons.MAIN): double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 self.last_cruise_stalk_pull_time = curr_time_ms ready = (CS.cstm_btns.get_button_status("pedal") > PCCState.OFF and enabled and CruiseState.is_off(CS.pcm_acc_status)) if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_pedal_cruise = True # Increase ACC speed to match current, if applicable. self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH, self.pedal_speed_kph) else: # A single pull disables ACC (falling back to just steering). self.enable_pedal_cruise = False # Handle pressing the cancel button. elif CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_pedal_cruise = False self.pedal_speed_kph = 0. self.last_cruise_stalk_pull_time = 0 # Handle pressing up and down buttons. elif (self.enable_pedal_cruise and CS.cruise_buttons != self.prev_cruise_buttons): # Real stalk command while ACC is already enabled. Adjust the max ACC # speed if necessary. For example if max speed is 50 but you're currently # only going 30, the cruise speed can be increased without any change to # max ACC speed. If actual speed is already 50, the code also increases # the max cruise speed. if CS.cruise_buttons == CruiseButtons.RES_ACCEL: requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + speed_uom_kph self.pedal_speed_kph = max(self.pedal_speed_kph, requested_speed_kph) elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + 5 * speed_uom_kph self.pedal_speed_kph = max(self.pedal_speed_kph, requested_speed_kph) elif CS.cruise_buttons == CruiseButtons.DECEL_SET: self.pedal_speed_kph -= speed_uom_kph elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: self.pedal_speed_kph -= 5 * speed_uom_kph # Clip ACC speed between 0 and 170 KPH. self.pedal_speed_kph = min(self.pedal_speed_kph, 170) self.pedal_speed_kph = max(self.pedal_speed_kph, 1) # If something disabled cruise control, disable PCC too elif (self.enable_pedal_cruise == True and CS.pcm_acc_status != 0): self.enable_pedal_cruise = False # Notify if PCC was toggled if prev_enable_pedal_cruise and not self.enable_pedal_cruise: CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4) CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY) elif self.enable_pedal_cruise and not prev_enable_pedal_cruise: CS.UE.custom_alert_message(2, "PCC Enabled", 150) CS.cstm_btns.set_button_status("pedal", PCCState.ENABLED) # Update the UI to show whether the current car state allows PCC. if CS.cstm_btns.get_button_status("pedal") in [ PCCState.STANDBY, PCCState.NOT_READY ]: if (enabled and CruiseState.is_off(CS.pcm_acc_status)): CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY) else: CS.cstm_btns.set_button_status("pedal", PCCState.NOT_READY) # Update prev state after all other actions. self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status
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(service_list['liveMapData'].port, conflate=True, poller=self.poller) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** STEER_MAX = 420 # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) # Basic highway lane change logic changing_lanes = CS.right_blinker_on or CS.left_blinker_on #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 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 self.speed_limit_for_cc = CS.userSpeedLimitKph #print self.speed_limit_for_cc 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")) else: self.speed_limit_offset = 0. if not self.isMetric: self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS if CS.useTeslaGPS: if self.gpsLocationExternal is None: self.gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port) sol = gen_solution(CS) sol.logMonoTime = int(frame * DT_CTRL * 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(False) 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))) pedal_can_sends = [] if CS.pedal_interceptor_available: #update PCC module info pedal_can_sends = self.PCC.update_stat(CS, True) self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # Update HSO module info. human_control = False # update CS.v_cruise_pcm based on module selected. if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph else: CS.v_cruise_pcm = max(0.,CS.v_ego * CV.MS_TO_KPH +0.5) #BB try v_ego to reduce the false FCW warnings; was: vCS.v_cruise_actual # Get the turn signal from ALCA. turn_signal_needed, self.alca_enabled = self.ALCA.update(enabled, CS, actuators) apply_angle = -actuators.steerAngle # Tesla is reversed vs OP. human_control = self.HSO.update_stat(self,CS, enabled, actuators, frame) human_lane_changing = changing_lanes 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) des_angle_factor = interp(CS.v_ego, DES_ANGLE_ADJUST_FACTOR_BP, DES_ANGLE_ADJUST_FACTOR ) if self.alca_enabled or not CS.enableSpeedVariableDesAngle: des_angle_factor = 1. #BB disable limits to test 0.5.8 # apply_angle = clip(apply_angle * des_angle_factor, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] #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 (4), # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5), # DAS_acc_speed_limit_mph (8), # DAS_speed_limit_units(8) #send fake_das data as 0x553 # TODO: forward collission warning if CS.hasTeslaIcIntegration: self.set_speed_limit_active = True self.speed_limit_offset = CS.userSpeedLimitOffsetKph # only change the speed limit when we have a valid vaue if CS.userSpeedLimitKph >= 10: self.speed_limit_for_cc = CS.userSpeedLimitKph if CS.useTeslaMapData: self.speedlimit_ms = CS.speedLimitKph * CV.KPH_TO_MS self.speedlimit_valid = True if self.speedlimit_ms == 0: self.speedlimit_valid = False self.speedlimit_units = self.speedUnits(fromMetersPerSecond = self.speedlimit_ms) if frame % 10 == 0: for socket, _ in self.poller.poll(1): if socket is self.speedlimit and not CS.useTeslaMapData: #get speed limit lmd = messaging.recv_one(socket).liveMapData self.speedlimit_ms = lmd.speedLimit self.speedlimit_valid = lmd.speedLimitValid self.speedlimit_units = self.speedUnits(fromMetersPerSecond = self.speedlimit_ms) self.speed_limit_for_cc = self.speedlimit_ms * CV.MS_TO_KPH elif socket is self.icLeads: self.icLeadsData = tesla.ICLeads.from_bytes(socket.recv()) elif socket is self.radarState: #to show lead car on IC if self.icLeadsData is not None: can_messages = self.showLeadCarOnICCanMessage(radarSocket = socket) can_sends.extend(can_messages) elif socket is self.alcaState: self.alcaStateData = tesla.ALCAState.from_bytes(socket.recv()) elif socket is self.pathPlan: #to show curvature and lanes on IC if self.alcaStateData is not None: self.handlePathPlanSocketForCurvatureOnIC(pathPlanSocket = socket, alcaStateData = self.alcaStateData,CS = CS) elif socket is self.icCarLR: can_messages = self.showLeftAndRightCarsOnICCanMessages(icCarLRSocket = socket) can_sends.extend(can_messages) elif socket is self.trafficevents: can_messages = self.handleTrafficEvents(trafficEventsSocket = socket) can_sends.extend(can_messages) if (CS.roadCurvRange > 20) and self.useMap: if self.useZeroC0: self.curv0 = 0. elif self.clipC0: self.curv0 = -clip(CS.roadCurvC0,-0.5,0.5) #else: # self.curv0 = -CS.roadCurvC0 #if CS.v_ego > 9: # self.curv1 = -CS.roadCurvC1 #else: # self.curv1 = 0. self.curv2 = -CS.roadCurvC2 self.curv3 = -CS.roadCurvC3 self.laneRange = CS.roadCurvRange #else: # self.curv0 = 0. # self.curv1 = 0. # self.curv2 = 0. # self.curv3 = 0. # self.laneRange = 0 if (CS.csaRoadCurvRange > 2.) and self.useMap and not self.useMapOnly: self.curv2 = -CS.csaRoadCurvC2 self.curv3 = -CS.csaRoadCurvC3 #if self.laneRange > 0: # self.laneRange = min(self.laneRange,CS.csaRoadCurvRange) #else: self.laneRange = CS.csaRoadCurvRange elif (CS.csaOfframpCurvRange > 2.) and self.useMap and not self.useMapOnly: #self.curv2 = -CS.csaOfframpCurvC2 #self.curv3 = -CS.csaOfframpCurvC3 #self.curv0 = 0. #self.curv1 = 0. #if self.laneRange > 0: # self.laneRange = min(self.laneRange,CS.csaOfframpCurvRange) #else: self.laneRange = CS.csaOfframpCurvRange else: self.laneRange = 50 self.laneRange = int(clip(self.laneRange,0,159)) op_status = 0x02 hands_on_state = 0x00 forward_collision_warning = 0 #1 if needed if hud_alert == AH.FCW: forward_collision_warning = hud_alert[1] if forward_collision_warning > 1: forward_collision_warning = 1 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold cc_state = 1 speed_limit_to_car = int(self.speedlimit_units) alca_state = 0x00 speed_override = 0 collision_warning = 0x00 acc_speed_limit_mph = 0 speed_control_enabled = 0 accel_min = -15 accel_max = 5 acc_speed_kph = 0 if enabled: #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning if self.opState == 0: op_status = 0x02 if self.opState == 1: op_status = 0x03 if self.opState == 2: op_status = 0x08 if self.opState == 3: op_status = 0x01 if self.opState == 5: op_status = 0x03 alca_state = 0x08 + turn_signal_needed #canceled by user if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0): alca_state = 0x14 #min speed for ALCA if CS.CL_MIN_V > CS.v_ego: alca_state = 0x05 if not enable_steer_control: #op_status = 0x08 hands_on_state = 0x02 if hud_alert == AH.STEER: if snd_chime == CM.MUTE: hands_on_state = 0x03 else: hands_on_state = 0x05 acc_speed_limit_mph = max(self.ACC.acc_speed_kph * CV.KPH_TO_MPH,1) if CS.pedal_interceptor_available: acc_speed_limit_mph = max(self.PCC.pedal_speed_kph * CV.KPH_TO_MPH,1) 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 (CS.pedal_interceptor_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise): speed_control_enabled = 1 cc_state = 2 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 send_fake_msg = False send_fake_warning = False if enabled: if frame % 2 == 0: send_fake_msg = True if frame % 25 == 0: send_fake_warning = True else: if frame % 23 == 0: send_fake_msg = True if frame % 60 == 0: send_fake_warning = True if 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 (not CS.blinker_on) and (self.ALCA.laneChange_cancelled_counter > 0): self.DAS_221_lcAborting = 1 self.warningCounter = 300 self.warningNeeded = 1 if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ turn_signal_needed,forward_collision_warning,hands_on_state, \ cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \ #acc_speed_limit_mph, CS.v_cruise_pcm * CV.KPH_TO_MPH, speed_limit_to_car, apply_angle, 1 if enable_steer_control else 0)) if send_fake_warning or (self.opState == 2) or (self.opState == 5) 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()) idx = frame % 16 cruise_btn = None # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available: cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_for_cc, self.speedlimit_valid, \ self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat= 0, #turn_signal_needed, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_for_cc * CV.KPH_TO_MS, self.speedlimit_valid, \ 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
def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime): """ Controls thread """ ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars) if not self.enable_camera: return # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print "INVALID HUD", hud hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** STEER_MAX = 420 # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) # Basic highway lane change logic changing_lanes = CS.right_blinker_on or CS.left_blinker_on #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 1000 == 0): CS.cstm_btns.send_button_info() #read speed limit params self.set_speed_limit_active = self.params.get("SpeedLimitOffset") is not None #self.params.get("LimitSetSpeed") == "1" and if self.set_speed_limit_active: self.speed_limit_offset = float(self.params.get("SpeedLimitOffset")) else: self.speed_limit_offset = 0. #get pitch/roll/yaw every 0.1 sec if (frame %10 == 0): (self.accPitch, self.accRoll, self.accYaw),(self.magPitch, self.magRoll, self.magYaw),(self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update(CS.v_ego,CS.a_ego,CS.angle_steers) CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,self.magPitch, self.magRoll, self.magYaw,self.gyroPitch, self.gyroRoll, self.gyroYaw) # Update statuses for custom buttons every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) if (frame % 10 == 0): self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0 and CS.enableALCA) #print CS.cstm_btns.get_button_status("alca") if CS.pedal_interceptor_available: #update PCC module info self.PCC.update_stat(CS, True, sendcan) self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # Update HSO module info. human_control = False # update CS.v_cruise_pcm based on module selected. if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph else: CS.v_cruise_pcm = CS.v_cruise_actual # Get the angle from ALCA. alca_enabled = False turn_signal_needed = 0 alca_steer = 0. apply_angle, alca_steer,alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators) apply_angle = -apply_angle # Tesla is reversed vs OP. human_control = self.HSO.update_stat(CS, enabled, actuators, frame) human_lane_changing = changing_lanes and not alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control and vehicle_moving) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # Windup slower. if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) des_angle_factor = interp(CS.v_ego, DES_ANGLE_ADJUST_FACTOR_BP, DES_ANGLE_ADJUST_FACTOR ) if alca_enabled or not CS.enableSpeedVariableDesAngle: des_angle_factor = 1. #BB disable limits to test 0.5.8 # apply_angle = clip(apply_angle * des_angle_factor, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] #First we emulate DAS. # DAS_longC_enabled (1),DAS_gas_to_resume (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4) # DAS_speed_kph(8), # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (4), # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5), # DAS_acc_speed_limit_mph (8), # DAS_speed_limit_units(8) #send fake_das data as 0x553 # TODO: forward collission warning if frame % 10 == 0: #get speed limit for socket, _ in self.poller.poll(0): if socket is self.speedlimit: lmd = messaging.recv_one(socket).liveMapData self.speedlimit_ms = lmd.speedLimit self.speedlimit_valid = lmd.speedLimitValid if (self.params.get("IsMetric") == "1"): self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_KPH + 0.5 else: self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_MPH + 0.5 op_status = 0x02 hands_on_state = 0x00 forward_collision_warning = 0 #1 if needed if hud_alert == AH.FCW: forward_collision_warning = hud_alert[1] if forward_collision_warning > 1: forward_collision_warning = 1 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold cc_state = 1 speed_limit_to_car = int(self.speedlimit_units) alca_state = 0x00 apUnavailable = 0 gas_to_resume = 0 collision_warning = 0x00 acc_speed_limit_mph = 0 speed_control_enabled = 0 accel_min = -15 accel_max = 5 acc_speed_kph = 0 if enabled: #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning if self.opState == 0: op_status = 0x02 if self.opState == 1: op_status = 0x03 if self.opState == 2: op_status = 0x08 if self.opState == 3: op_status = 0x01 if self.opState == 5: op_status = 0x03 alca_state = 0x08 + turn_signal_needed #canceled by user if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0): alca_state = 0x14 #min speed for ALCA if CS.CL_MIN_V > CS.v_ego: alca_state = 0x05 if not enable_steer_control: #op_status = 0x08 hands_on_state = 0x02 apUnavailable = 1 if hud_alert == AH.STEER: if snd_chime == CM.MUTE: hands_on_state = 0x03 else: hands_on_state = 0x05 acc_speed_limit_mph = max(self.ACC.acc_speed_kph * CV.KPH_TO_MPH,1) if CS.pedal_interceptor_available: acc_speed_limit_mph = max(self.PCC.pedal_speed_kph * CV.KPH_TO_MPH,1) if hud_alert == AH.FCW: collision_warning = hud_alert[1] if collision_warning > 1: collision_warning = 1 #use disabling for alerts/errors to make them aware someting is goin going on if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW): op_status = 0x08 if self.ACC.enable_adaptive_cruise: acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH if (CS.pedal_interceptor_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise): speed_control_enabled = 1 cc_state = 2 else: if (CS.pcm_acc_status == 4): #car CC enabled but not OP, display the HOLD message cc_state = 3 send_fake_msg = False send_fake_warning = False if enabled: if frame % 2 == 0: send_fake_msg = True if frame % 25 == 0: send_fake_warning = True else: if frame % 23 == 0: send_fake_msg = True if frame % 60 == 0: send_fake_warning = True if send_fake_msg: can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,gas_to_resume,apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ turn_signal_needed,forward_collision_warning,hands_on_state, \ cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \ acc_speed_limit_mph, speed_limit_to_car, apply_angle, 1 if enable_steer_control else 0)) if send_fake_warning or (self.opState == 2) or (self.opState == 5): #if it's time to send OR we have a warning or emergency disable can_sends.append(teslacan.create_fake_DAS_warning(CS.DAS_noSeatbelt, CS.DAS_canErrors, \ CS.DAS_plannerErrors, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation)) # end of DAS emulation """ idx = frame % 16 cruise_btn = None if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available: cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speedlimit_ms * CV.MS_TO_KPH,self.speedlimit_valid, \ self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat= 0, #turn_signal_needed, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ self.speedlimit_ms * CV.MS_TO_KPH, self.speedlimit_valid, \ self.set_speed_limit_active, self.speed_limit_offset) can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) self.last_angle = apply_angle self.last_accel = apply_accel sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())