def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.automated_cruise_action_time = 0
self.radarState = messaging.sub_sock('radarState', conflate=True)
self.last_update_time = 0
self.enable_adaptive_cruise = False
self.prev_enable_adaptive_cruise = False
# Whether to re-engage automatically after being paused due to low speed or
# user-initated deceleration.
self.autoresume = False
self.adaptive = False
self.last_brake_press_time = 0
self.last_cruise_stalk_pull_time = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.prev_pcm_acc_status = 0
self.acc_speed_kph = 0.
self.speed_limit_kph = 0.
self.prev_speed_limit_kph = 0.
self.user_has_braked = False
self.has_gone_below_min_speed = False
self.fast_decel_time = 0
self.lead_last_seen_time_ms = 0
# BB speed for testing
self.new_speed = 0
average_speed_over_x_suggestions = 20 # 1 second (20x a second)
self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions)
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.pcc_available = self.prev_pcc_available = False
self.pedal_timeout_frame = 0
self.accelerator_pedal_pressed = self.prev_accelerator_pedal_pressed = False
self.automated_cruise_action_time = 0
self.last_angle = 0.0
self.radarState = messaging.sub_sock("radarState", conflate=True)
self.live_map_data = messaging.sub_sock("liveMapData", conflate=True)
self.lead_1 = None
self.last_update_time = 0
self.enable_pedal_cruise = False
self.stalk_pull_time_ms = 0
self.prev_stalk_pull_time_ms = -1000
self.prev_pcm_acc_status = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.pedal_speed_kph = 0.0
self.speed_limit_kph = 0.0
self.prev_speed_limit_kph = 0.0
self.pedal_idx = 0
self.pedal_steady = 0.0
self.prev_tesla_accel = 0.0
self.prev_tesla_pedal = 0.0
self.torqueLevel_last = 0.0
self.prev_v_ego = 0.0
self.PedalForZeroTorque = (
18.0 # starting number for a S85, adjusts down automatically
)
self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL
self.v_pid = 0.0
self.a_pid = 0.0
self.last_output_gb = 0.0
self.last_speed_kph = None
# for smoothing the changes in speed
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_sol = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.a_acc_sol = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
# Long Control
self.LoC = None
# when was radar data last updated?
self.lead_last_seen_time_ms = 0
self.continuous_lead_sightings = 0
self.params = Params()
average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second)
self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions)
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
class PCCController():
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.pcc_available = self.prev_pcc_available = False
self.pedal_timeout_frame = 0
self.accelerator_pedal_pressed = self.prev_accelerator_pedal_pressed = False
self.automated_cruise_action_time = 0
self.last_angle = 0.
self.radarState = messaging.sub_sock('radarState', conflate=True)
self.live_map_data = messaging.sub_sock('liveMapData', conflate=True)
self.lead_1 = None
self.last_update_time = 0
self.enable_pedal_cruise = False
self.stalk_pull_time_ms = 0
self.prev_stalk_pull_time_ms = -1000
self.prev_pcm_acc_status = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.pedal_speed_kph = 0.
self.speed_limit_kph = 0.
self.prev_speed_limit_kph = 0.
self.pedal_idx = 0
self.pedal_steady = 0.
self.prev_tesla_accel = 0.
self.prev_tesla_pedal = 0.
self.torqueLevel_last = 0.
self.prev_v_ego = 0.
self.PedalForZeroTorque = 18. #starting number, works on my S85
self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL
self.v_pid = 0.
self.a_pid = 0.
self.last_output_gb = 0.
self.last_speed_kph = None
#for smoothing the changes in speed
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_sol = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.a_acc_sol = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
#Long Control
self.LoC = None
#when was radar data last updated?
self.lead_last_seen_time_ms = 0
self.continuous_lead_sightings = 0
self.params = Params()
average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second)
self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions)
def load_pid(self):
try:
v_pid_json = open(V_PID_FILE)
data = json.load(v_pid_json)
if (self.LoC):
if self.LoC.pid:
self.LoC.pid.p = data['p']
self.LoC.pid.i = data['i']
self.LoC.pid.f = data['f']
else:
print("self.LoC not initialized!")
except:
print("file not present, creating at next reset")
#Helper function for saving the PCC pid constants across drives
def save_pid(self, pid):
data = {}
data['p'] = pid.p
data['i'] = pid.i
data['f'] = pid.f
try:
with open(V_PID_FILE, 'w') as outfile:
json.dump(data, outfile)
except IOError:
print("PDD pid parameters could not be saved to file")
def reset(self, v_pid):
if self.LoC and RESET_PID_ON_DISENGAGE:
self.LoC.reset(v_pid)
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_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
# function to calculate the cruise speed based on a safe follow distance
def calc_follow_speed_ms(self, CS, alca_enabled):
# Make sure we were able to populate lead_1.
if self.lead_1 is None:
return None, None, None
# dRel is in meters.
lead_dist_m = self.lead_1.dRel
if not CS.useTeslaRadar:
lead_dist_m = _visual_radar_adjusted_dist_m(lead_dist_m, CS)
# Grab the relative speed.
v_rel = self.lead_1.vRel if abs(self.lead_1.vRel) > .5 else 0
a_rel = self.lead_1.aRel if abs(self.lead_1.aRel) > .5 else 0
rel_speed_kph = (v_rel + 0 * CS.apFollowTimeInS * a_rel) * CV.MS_TO_KPH
# v_ego is in m/s, so safe_distance is in meters.
safe_dist_m = _safe_distance_m(CS.v_ego, CS)
# Current speed in kph
actual_speed_kph = CS.v_ego * CV.MS_TO_KPH
# speed and brake to issue
if self.last_speed_kph is None:
self.last_speed_kph = actual_speed_kph
new_speed_kph = self.last_speed_kph
### Logic to determine best cruise speed ###
if self.enable_pedal_cruise:
# If no lead is present, accel up to max speed
if lead_dist_m == 0 or lead_dist_m > MAX_RADAR_DISTANCE:
new_speed_kph = self.pedal_speed_kph
elif lead_dist_m > 0:
#BB Use the Kalman lead speed and acceleration
lead_absolute_speed_kph = actual_speed_kph + rel_speed_kph #(self.lead_1.vLeadK + _DT * self.lead_1.aLeadK) * CV.MS_TO_KPH
rel_speed_kph = lead_absolute_speed_kph - actual_speed_kph
if lead_dist_m < MIN_SAFE_DIST_M and rel_speed_kph >= 3:
# If lead is going faster, but we're not at a safe distance, hold
# speed and let the lead car move father away from us
new_speed_kph = actual_speed_kph
# If lead is not going faster than 3kpm from us, lets slow down a
# bit to get him moving faster relative to us
elif lead_dist_m < MIN_SAFE_DIST_M:
new_speed_kph = MIN_PCC_V_KPH
# In a 10 meter cruise zone, lets match the car in front
elif safe_dist_m + 2 > lead_dist_m > MIN_SAFE_DIST_M: # BB we might want to try this and rel_speed_kph > 0:
min_vrel_kph_map = OrderedDict([
# (distance in m, min allowed relative kph)
(0.5 * safe_dist_m, 3.0),
(0.8 * safe_dist_m, 2.0),
(1.0 * safe_dist_m, 0.0)
])
min_vrel_kph = _interp_map(lead_dist_m, min_vrel_kph_map)
new_speed_kph = lead_absolute_speed_kph - min_vrel_kph
else:
# Force speed into a band that is generally slower than lead if too
# close, and faster than lead if too far. Allow a range of speeds at
# any given distance, to prevent continuous jerky adjustments.
# BB band should be % of v_ego
min_vrel_kph_map = OrderedDict([
# (distance in m, min allowed relative kph)
(0.5 * safe_dist_m, 3),
(1.0 * safe_dist_m,
-1 - 0.025 * CS.v_ego * CV.MS_TO_KPH),
(1.5 * safe_dist_m,
-5 - 0.05 * CS.v_ego * CV.MS_TO_KPH),
(3.0 * safe_dist_m,
-10 - 0.1 * CS.v_ego * CV.MS_TO_KPH)
])
min_vrel_kph = _interp_map(lead_dist_m, min_vrel_kph_map)
max_vrel_kph_map = OrderedDict([
# (distance in m, max allowed relative kph)
(0.5 * safe_dist_m, 6),
(1.0 * safe_dist_m, 2),
(1.5 * safe_dist_m, -3),
# With visual radar the relative velocity is 0 until the confidence
# gets high. So even a small negative number here gives constant
# accel until lead lead car gets close enough to read.
(3 * safe_dist_m, -7)
])
max_vrel_kph = _interp_map(lead_dist_m, max_vrel_kph_map)
#if CS.useTeslaRadar:
# min_vrel_kph = -1
# max_vrel_kph = -2
min_kph = lead_absolute_speed_kph - max_vrel_kph
max_kph = lead_absolute_speed_kph - min_vrel_kph
# In the special case were we are going faster than intended but it's
# still an acceptable speed, accept it. This could happen if the
# driver manually accelerates, or if we roll down a hill. In either
# case, don't fight the extra velocity unless necessary.
if lead_dist_m >= 0.8 * safe_dist_m and lead_absolute_speed_kph > actual_speed_kph and self.lead_1.aLeadK >= 0.:
new_speed_kph = lead_absolute_speed_kph
new_speed_kph = clip(new_speed_kph, min_kph, max_kph)
if (actual_speed_kph > new_speed_kph) and (
min_kph < actual_speed_kph <
max_kph) and (lead_absolute_speed_kph > 30):
new_speed_kph = actual_speed_kph
#BB disabled this condition as it might not allow fast enough braking
#if (actual_speed_kph > 80) and abs(actual_speed_kph - new_speed_kph) < 3.:
# new_speed_kph = (actual_speed_kph + new_speed_kph)/2.0
# Enforce limits on speed in the presence of a lead car.
new_speed_kph = min(
new_speed_kph, _max_safe_speed_kph(self.lead_1, CS),
max(
lead_absolute_speed_kph - _min_safe_vrel_kph(
self.lead_1, CS, actual_speed_kph), 2))
# Enforce limits on speed
new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH)
new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH,
self.pedal_speed_kph)
if CS.turn_signal_blinking or (abs(CS.angle_steers) >
ANGLE_STOP_ACCEL) or alca_enabled:
# Don't accelerate during manual turns, curves or ALCA.
new_speed_kph = min(new_speed_kph, self.last_speed_kph)
#BB Last safety check. Zero if below MIN_SAFE_DIST_M
if (MIN_SAFE_DIST_M > lead_dist_m > 0) and (rel_speed_kph < 3.):
new_speed_kph = MIN_PCC_V_KPH
self.last_speed_kph = new_speed_kph
return new_speed_kph * CV.KPH_TO_MS
def pedal_hysteresis(self, pedal, enabled):
# for small accel oscillations within PEDAL_HYST_GAP, don't change the command
if not enabled:
# send 0 when disabled, otherwise acc faults
self.pedal_steady = 0.
elif pedal > self.pedal_steady + PEDAL_HYST_GAP:
self.pedal_steady = pedal - PEDAL_HYST_GAP
elif pedal < self.pedal_steady - PEDAL_HYST_GAP:
self.pedal_steady = pedal + PEDAL_HYST_GAP
return self.pedal_steady
def _update_pedal_state(self, CS, frame):
if CS.pedal_idx != CS.prev_pedal_idx:
# time out pedal after 500ms without receiving a new CAN message from it
self.pedal_timeout_frame = frame + 50
self.prev_pcc_available = self.pcc_available
pedal_ready = frame < self.pedal_timeout_frame and CS.pedal_interceptor_state == 0
acc_disabled = CS.forcePedalOverCC or CruiseState.is_off(
CS.pcm_acc_status)
# Mark pedal unavailable while traditional cruise is on.
self.pcc_available = pedal_ready and acc_disabled
if self.pcc_available != self.prev_pcc_available:
CS.config_ui_buttons(self.pcc_available, pedal_ready
and not acc_disabled)
class ACCController():
# Tesla cruise only functions above 17 MPH
MIN_CRUISE_SPEED_MS = 17.1 * CV.MPH_TO_MS
def __init__(self, carcontroller):
self.CC = carcontroller
self.human_cruise_action_time = 0
self.automated_cruise_action_time = 0
self.radarState = messaging.sub_sock('radarState', conflate=True)
self.last_update_time = 0
self.enable_adaptive_cruise = False
self.prev_enable_adaptive_cruise = False
# Whether to re-engage automatically after being paused due to low speed or
# user-initated deceleration.
self.autoresume = False
self.adaptive = False
self.last_brake_press_time = 0
self.last_cruise_stalk_pull_time = 0
self.prev_cruise_buttons = CruiseButtons.IDLE
self.prev_pcm_acc_status = 0
self.acc_speed_kph = 0.
self.speed_limit_kph = 0.
self.prev_speed_limit_kph = 0.
self.user_has_braked = False
self.has_gone_below_min_speed = False
self.fast_decel_time = 0
self.lead_last_seen_time_ms = 0
# BB speed for testing
self.new_speed = 0
average_speed_over_x_suggestions = 20 # 1 second (20x a second)
self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions)
# Updates the internal state of this controller based on user input,
# specifically the steering wheel mounted cruise control stalk, and OpenPilot
# UI buttons.
def update_stat(self, CS, enabled):
# Check if the cruise stalk was double pulled, indicating that adaptive
# cruise control should be enabled. Twice in .75 seconds counts as a double
# pull.
self.prev_enable_adaptive_cruise = self.enable_adaptive_cruise
acc_string = CS.cstm_btns.get_button_label2(ACCMode.BUTTON_NAME)
acc_mode = ACCMode.from_label(acc_string)
CS.cstm_btns.get_button(
ACCMode.BUTTON_NAME).btn_label2 = acc_mode.label
self.autoresume = acc_mode.autoresume
self.adaptive = acc_mode.adaptive
curr_time_ms = _current_time_millis()
# Handle pressing the enable button.
if (CS.cruise_buttons == CruiseButtons.MAIN
or CS.cruise_buttons == CruiseButtons.DECEL_SET
) and self.prev_cruise_buttons != CS.cruise_buttons:
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(
ACCMode.BUTTON_NAME) > ACCState.OFF and
(enabled or (CS.cruise_buttons == CruiseButtons.DECEL_SET
and not self.adaptive))
and CruiseState.is_enabled_or_standby(CS.pcm_acc_status)
and CS.v_ego > self.MIN_CRUISE_SPEED_MS)
if ready and double_pull and (
(self.adaptive and CS.cruise_buttons == CruiseButtons.MAIN) or
((not self.adaptive)
and CS.cruise_buttons == CruiseButtons.DECEL_SET)):
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_adaptive_cruise = True
# Increase ACC speed to match current, if applicable.
if self.adaptive:
self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH,
self.speed_limit_kph)
else:
self.acc_speed_kph = CS.v_ego_raw * CV.MS_TO_KPH
self.user_has_braked = False
self.has_gone_below_min_speed = False
else:
# A single pull disables ACC (falling back to just steering).
if CS.cruise_buttons == CruiseButtons.MAIN:
self.enable_adaptive_cruise = False
# Handle pressing the cancel button.
if CS.cruise_buttons == CruiseButtons.CANCEL:
self.enable_adaptive_cruise = False
self.acc_speed_kph = 0.
self.last_cruise_stalk_pull_time = 0
# Handle pressing up and down buttons.
elif (CS.cruise_buttons != CruiseButtons.MAIN
and self.enable_adaptive_cruise
and CS.cruise_buttons != self.prev_cruise_buttons):
self._update_max_acc_speed(CS)
if CS.brake_pressed:
self.user_has_braked = True
self.last_brake_press_time = _current_time_millis()
if not self.autoresume:
self.enable_adaptive_cruise = False
if CS.v_ego < self.MIN_CRUISE_SPEED_MS:
self.has_gone_below_min_speed = True
# If autoresume is not enabled and not in standard CC, disable if we hit the brakes or gone below 18mph
if not self.autoresume:
if (self.adaptive and not enabled
) or self.user_has_braked or self.has_gone_below_min_speed:
self.enable_adaptive_cruise = False
# Notify if ACC was toggled
if self.prev_enable_adaptive_cruise and not self.enable_adaptive_cruise:
CS.UE.custom_alert_message(
3, "%s Disabled" % ("ACC" if self.adaptive else "CC"), 150, 4)
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.STANDBY)
self.fleet_speed.reset_averager()
elif self.enable_adaptive_cruise:
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.ENABLED)
if not self.prev_enable_adaptive_cruise:
CS.UE.custom_alert_message(
2, "%s Enabled" % ("ACC" if self.adaptive else "CC"), 150)
# Update the UI to show whether the current car state allows ACC.
if CS.cstm_btns.get_button_status(
ACCMode.BUTTON_NAME) in [ACCState.STANDBY, ACCState.NOT_READY]:
if ((enabled or not self.adaptive)
and CruiseState.is_enabled_or_standby(CS.pcm_acc_status)
and CS.v_ego > self.MIN_CRUISE_SPEED_MS):
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.STANDBY)
else:
CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME,
ACCState.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_max_acc_speed(self, CS):
# Adjust the max ACC speed based on user cruise stalk actions.
half_press_kph, full_press_kph = self._get_cc_units_kph(
CS.imperial_speed_units)
speed_change_map = {
CruiseButtons.RES_ACCEL: half_press_kph,
CruiseButtons.RES_ACCEL_2ND: full_press_kph,
CruiseButtons.DECEL_SET: -1 * half_press_kph,
CruiseButtons.DECEL_2ND: -1 * full_press_kph
}
self.acc_speed_kph += speed_change_map.get(CS.cruise_buttons, 0)
# Clip ACC speed between 0 and 170 KPH.
self.acc_speed_kph = min(self.acc_speed_kph, 170)
self.acc_speed_kph = max(self.acc_speed_kph, 0)
# Decide which cruise control buttons to simluate to get the car to the desired speed.
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
# function to calculate the cruise button based on a safe follow distance
def _calc_follow_button(self, CS, lead_car, speed_limit_kph,
set_speed_limit_active, speed_limit_offset, frame):
if lead_car is None:
return None
# Desired gap (in seconds) between cars.
follow_time_s = CS.apFollowTimeInS
# v_ego is in m/s, so safe_dist_m is in meters.
safe_dist_m = CS.v_ego * follow_time_s
current_time_ms = _current_time_millis()
# Make sure we were able to populate lead_1.
# dRel is in meters.
lead_dist_m = lead_car.dRel
lead_speed_kph = (lead_car.vRel + CS.v_ego) * CV.MS_TO_KPH
# Relative velocity between the lead car and our set cruise speed.
future_vrel_kph = lead_speed_kph - CS.v_cruise_actual
# How much we can accelerate without exceeding the max allowed speed.
max_acc_speed_kph = self.fleet_speed.adjust(
CS, self.acc_speed_kph * CV.KPH_TO_MS, frame) * CV.MS_TO_KPH
available_speed_kph = max_acc_speed_kph - CS.v_cruise_actual
half_press_kph, full_press_kph = self._get_cc_units_kph(
CS.imperial_speed_units)
# button to issue
button = None
# debug msg
msg = None
# Automatically engage traditional cruise if ACC is active.
if self._should_autoengage_cc(
CS,
lead_car=lead_car) and self._no_action_for(milliseconds=100):
button = CruiseButtons.RES_ACCEL
# If traditional cruise is engaged, then control it.
elif CS.pcm_acc_status == CruiseState.ENABLED:
# Disengage cruise control if a slow object is seen ahead. This triggers
# full regen braking, which is stronger than the braking that happens if
# you just reduce cruise speed.
if self._fast_decel_required(
CS,
lead_car) and self._no_human_action_for(milliseconds=500):
msg = "Off (Slow traffic)"
button = CruiseButtons.CANCEL
self.new_speed = 1
# if cruise is set to faster than the max speed, slow down
elif CS.v_cruise_actual > max_acc_speed_kph and self._no_action_for(
milliseconds=300):
msg = "Slow to max"
button = CruiseButtons.DECEL_SET
self.new_speed = max_acc_speed_kph
elif ( # if we have a populated lead_distance
lead_dist_m > 0 and self._no_action_for(milliseconds=300)
# and we're moving
and CS.v_cruise_actual > full_press_kph):
### Slowing down ###
# Reduce speed significantly if lead_dist < safe dist
# and if the lead car isn't already pulling away.
if lead_dist_m < safe_dist_m * .5 and future_vrel_kph < 2:
msg = "-5 (Significantly too close)"
button = CruiseButtons.DECEL_2ND
self.new_speed = CS.v_ego * CV.MS_TO_KPH - full_press_kph
# Don't rush up to lead car
elif future_vrel_kph < -15:
msg = "-5 (approaching too fast)"
button = CruiseButtons.DECEL_2ND
self.new_speed = CS.v_ego * CV.MS_TO_KPH - full_press_kph
elif future_vrel_kph < -8:
msg = "-1 (approaching too fast)"
button = CruiseButtons.DECEL_SET
self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph
elif lead_dist_m < safe_dist_m and future_vrel_kph <= 0:
msg = "-1 (Too close)"
button = CruiseButtons.DECEL_SET
self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph
# Make slow adjustments if close to the safe distance.
# only adjust every 1 secs
elif (lead_dist_m < safe_dist_m * 1.3
and future_vrel_kph < -1 * half_press_kph
and self._no_action_for(milliseconds=1000)):
msg = "-1 (Near safe distance)"
button = CruiseButtons.DECEL_SET
self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph
### Speed up ###
elif (available_speed_kph > half_press_kph
and lead_dist_m > safe_dist_m
and self._no_human_action_for(milliseconds=1000)):
lead_is_far = lead_dist_m > safe_dist_m * 1.75
closing = future_vrel_kph < -2
lead_is_pulling_away = future_vrel_kph > 4
if lead_is_far and not closing or lead_is_pulling_away:
msg = "+1 (Beyond safe distance and speed)"
button = CruiseButtons.RES_ACCEL
self.new_speed = CS.v_ego * CV.MS_TO_KPH + half_press_kph
# If lead_dist is reported as 0, no one is detected in front of you so you
# can speed up. Only accel on straight-aways; vision radar often
# loses lead car in a turn.
elif (lead_dist_m == 0 and CS.angle_steers < 2.0
and half_press_kph < available_speed_kph
and self._no_action_for(milliseconds=500)
and self._no_human_action_for(milliseconds=1000)
and current_time_ms > self.lead_last_seen_time_ms + 4000):
msg = "+1 (road clear)"
button = CruiseButtons.RES_ACCEL
self.new_speed = CS.v_ego * CV.MS_TO_KPH + half_press_kph
if (current_time_ms > self.last_update_time + 1000):
ratio = 0
if safe_dist_m > 0:
ratio = (lead_dist_m / safe_dist_m) * 100
print(
"Ratio: {0:.1f}% lead: {1:.1f}m avail: {2:.1f}kph vRel: {3:.1f}kph Angle: {4:.1f}deg"
.format(ratio, lead_dist_m, available_speed_kph,
lead_car.vRel * CV.MS_TO_KPH, CS.angle_steers))
self.last_update_time = current_time_ms
if msg is not None:
print("ACC: " + msg)
return button
def _should_autoengage_cc(self, CS, lead_car=None):
# Automatically (re)engage cruise control so long as
# 1) The carstate allows cruise control
# 2) There is no imminent threat of collision
# 3) The user did not cancel ACC by pressing the brake
cruise_ready = (self.enable_adaptive_cruise
and CS.pcm_acc_status == CruiseState.STANDBY
and CS.v_ego >= self.MIN_CRUISE_SPEED_MS and
_current_time_millis() > self.fast_decel_time + 2000)
slow_lead = lead_car and lead_car.dRel > 0 and lead_car.vRel < 0 or self._fast_decel_required(
CS, lead_car) # pylint: disable=chained-comparison
# "Autoresume" mode allows cruise to engage even after brake events, but
# shouldn't trigger DURING braking.
autoresume_ready = (
self.autoresume and CS.a_ego >= 0.1
and not self.CC.HSO.human_control
and _current_time_millis() > self.last_brake_press_time + 1000)
braked = self.user_has_braked or self.has_gone_below_min_speed
return cruise_ready and not slow_lead and (autoresume_ready
or not braked)
def _fast_decel_required(self, CS, lead_car):
""" Identifies situations which call for rapid deceleration. """
if not lead_car or not lead_car.dRel:
return False
collision_imminent = self._seconds_to_collision(CS, lead_car) < 4
lead_absolute_speed_ms = lead_car.vRel + CS.v_ego
lead_too_slow = lead_absolute_speed_ms < self.MIN_CRUISE_SPEED_MS
return collision_imminent or lead_too_slow
def _seconds_to_collision(self, CS, lead_car):
if not lead_car or not lead_car.dRel:
return sys.maxsize
elif lead_car.vRel >= 0:
return sys.maxsize
return abs(float(lead_car.dRel) / lead_car.vRel)
def _get_cc_units_kph(self, is_imperial_units):
# Cruise control buttons behave differently depending on whether the car
# is configured for metric or imperial units.
if is_imperial_units:
# Imperial unit cars adjust cruise in units of 1 and 5 mph.
half_press_kph = 1 * CV.MPH_TO_KPH
full_press_kph = 5 * CV.MPH_TO_KPH
else:
# Metric cars adjust cruise in units of 1 and 5 kph.
half_press_kph = 1
full_press_kph = 5
return half_press_kph, full_press_kph
# Adjust speed based off OP's longitudinal model. As of OpenPilot 0.5.3, this
# is inoperable because the planner crashes when given only visual radar
# inputs. (Perhaps this can be used in the future with a radar install, or if
# OpenPilot planner changes.)
def _calc_button(self, CS, desired_speed_ms):
button_to_press = None
# Automatically engange traditional cruise if appropriate.
if self._should_autoengage_cc(CS) and desired_speed_ms >= CS.v_ego:
button_to_press = CruiseButtons.RES_ACCEL
# If traditional cruise is engaged, then control it.
elif (CS.pcm_acc_status == CruiseState.ENABLED
# But don't make adjustments if a human has manually done so in
# the last 3 seconds. Human intention should not be overridden.
and self._no_human_action_for(milliseconds=3000) and
self._no_automated_action_for(milliseconds=500)):
# The difference between OP's target speed and the current cruise
# control speed, in KPH.
speed_offset_kph = (desired_speed_ms * CV.MS_TO_KPH -
CS.v_cruise_actual)
half_press_kph, full_press_kph = self._get_cc_units_kph(
CS.imperial_speed_units)
# Reduce cruise speed significantly if necessary. Multiply by a % to
# make the car slightly more eager to slow down vs speed up.
if desired_speed_ms < self.MIN_CRUISE_SPEED_MS:
button_to_press = CruiseButtons.CANCEL
if speed_offset_kph < -2 * full_press_kph and CS.v_cruise_actual > 0:
button_to_press = CruiseButtons.CANCEL
elif speed_offset_kph < -0.6 * full_press_kph and CS.v_cruise_actual > 0:
# Send cruise stalk dn_2nd.
button_to_press = CruiseButtons.DECEL_2ND
# Reduce speed slightly if necessary.
elif speed_offset_kph < -0.9 * half_press_kph and CS.v_cruise_actual > 0:
# Send cruise stalk dn_1st.
button_to_press = CruiseButtons.DECEL_SET
# Increase cruise speed if possible.
elif CS.v_ego > self.MIN_CRUISE_SPEED_MS:
# How much we can accelerate without exceeding max allowed speed.
available_speed_kph = self.acc_speed_kph - CS.v_cruise_actual
if speed_offset_kph >= full_press_kph and full_press_kph < available_speed_kph:
# Send cruise stalk up_2nd.
button_to_press = CruiseButtons.RES_ACCEL_2ND
elif speed_offset_kph >= half_press_kph and half_press_kph < available_speed_kph:
# Send cruise stalk up_1st.
button_to_press = CruiseButtons.RES_ACCEL
return button_to_press
def _no_human_action_for(self, milliseconds):
return _current_time_millis(
) > self.human_cruise_action_time + milliseconds
def _no_automated_action_for(self, milliseconds):
return _current_time_millis(
) > self.automated_cruise_action_time + milliseconds
def _no_action_for(self, milliseconds):
return self._no_human_action_for(
milliseconds) and self._no_automated_action_for(milliseconds)