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):
P = self.params
if enabled:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
else:
accel = 0.0
gas, brake = 0.0, 0.0
# *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# 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
# Never send cancel command if we never enter cruise state (no cruise if pedal)
# Cancel cmd causes brakes to release at a standstill causing grinding
pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if (frame % 10) == 0:
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -4.0 if stopping else accel
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
if CS.CP.carFingerprint in OLD_NIDEC_LONG_CONTROL:
#pcm_speed = pcm_speed
pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6)
else:
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V)
# TODO this 1.44 is just to maintain previous behavior
pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6)
pcm_speed_BP = [-wind_brake,
-wind_brake*(3/4),
0.0]
pcm_speed_V = [0.0,
clip(CS.out.vEgo + accel/2.0 - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0)]
pcm_speed = interp(-brake, pcm_speed_BP, pcm_speed_V)
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# way too aggressive at low speed without this
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
apply_gas = clip(gas_mult * gas, 0., 1.)
can_sends.append(create_gas_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
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)
else:
apply_angle = CS.angle_steers
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 = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, frame / 2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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 = False
if self.lead_1:
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,following)]
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
def thermald_thread():
health_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected health frequency
# now loop
thermal_sock = messaging.pub_sock('thermal')
health_sock = messaging.sub_sock('health', timeout=health_timeout)
location_sock = messaging.sub_sock('gpsLocation')
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
charging_disabled = False
should_start_prev = False
handle_fan = None
is_uno = False
has_relay = False
params = Params()
pm = PowerMonitoring()
no_panda_cnt = 0
thermal_config = get_thermal_config()
ts_last_ip = 0
ip_addr = '255.255.255.255'
# sound trigger
sound_trigger = 1
opkrAutoShutdown = 0
shutdown_trigger = 1
is_openpilot_view_enabled = 0
env = dict(os.environ)
env['LD_LIBRARY_PATH'] = mediaplayer
getoff_alert = Params().get('OpkrEnableGetoffAlert') == b'1'
if int(params.get('OpkrAutoShutdown')) == 0:
opkrAutoShutdown = 0
elif int(params.get('OpkrAutoShutdown')) == 1:
opkrAutoShutdown = 5
elif int(params.get('OpkrAutoShutdown')) == 2:
opkrAutoShutdown = 30
elif int(params.get('OpkrAutoShutdown')) == 3:
opkrAutoShutdown = 60
elif int(params.get('OpkrAutoShutdown')) == 4:
opkrAutoShutdown = 180
elif int(params.get('OpkrAutoShutdown')) == 5:
opkrAutoShutdown = 300
elif int(params.get('OpkrAutoShutdown')) == 6:
opkrAutoShutdown = 600
elif int(params.get('OpkrAutoShutdown')) == 7:
opkrAutoShutdown = 1800
elif int(params.get('OpkrAutoShutdown')) == 8:
opkrAutoShutdown = 3600
elif int(params.get('OpkrAutoShutdown')) == 9:
opkrAutoShutdown = 10800
else:
opkrAutoShutdown = 18000
while 1:
ts = sec_since_boot()
health = messaging.recv_sock(health_sock, wait=True)
location = messaging.recv_sock(location_sock)
location = location.gpsLocation if location else None
msg = read_thermal(thermal_config)
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if health.health.hwType == log.HealthData.HwType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
shutdown_trigger = 1
else:
no_panda_cnt = 0
startup_conditions[
"ignition"] = health.health.ignitionLine or health.health.ignitionCan
sound_trigger == 1
# Setup fan handler on first connect to panda
if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown:
is_uno = health.health.hwType == log.HealthData.HwType.uno
has_relay = health.health.hwType in [
log.HealthData.HwType.blackPanda,
log.HealthData.HwType.uno, log.HealthData.HwType.dos
]
if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if health_prev is not None:
if health.health.hwType == log.HealthData.HwType.unknown and \
health_prev.health.hwType != log.HealthData.HwType.unknown:
params.panda_disconnect()
health_prev = health
elif int(params.get("IsOpenpilotViewEnabled")) == 1 and int(
params.get("IsDriverViewEnabled")
) == 0 and is_openpilot_view_enabled == 0:
is_openpilot_view_enabled = 1
startup_conditions["ignition"] = True
elif int(params.get("IsOpenpilotViewEnabled")) == 0 and int(
params.get("IsDriverViewEnabled")
) == 0 and is_openpilot_view_enabled == 1:
shutdown_trigger = 0
sound_trigger == 0
is_openpilot_view_enabled = 0
startup_conditions["ignition"] = False
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(
psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
msg.thermal.networkType = network_type
msg.thermal.networkStrength = network_strength
msg.thermal.batteryPercent = get_battery_capacity()
msg.thermal.batteryStatus = get_battery_status()
msg.thermal.batteryCurrent = get_battery_current()
msg.thermal.batteryVoltage = get_battery_voltage()
msg.thermal.usbOnline = get_usb_present()
# Fake battery levels on uno for frame
if (not EON) or is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
msg.thermal.bat = 0
# update ip every 10 seconds
ts = sec_since_boot()
if ts - ts_last_ip >= 10.:
try:
result = subprocess.check_output(["ifconfig", "wlan0"],
encoding='utf8') # pylint: disable=unexpected-keyword-arg
ip_addr = re.findall(r"inet addr:((\d+\.){3}\d+)",
result)[0][0]
except:
ip_addr = 'N/A'
ts_last_ip = ts
msg.thermal.ipAddr = ip_addr
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu))
max_comp_temp = max(max_cpu_temp, msg.thermal.mem,
max(msg.thermal.gpu))
bat_temp = msg.thermal.bat
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed,
startup_conditions["ignition"])
msg.thermal.fanSpeed = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
# We only do this if there is a relay that prevents the car from faulting
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if max_cpu_temp > 107. or bat_temp >= 63. or (has_relay
and is_offroad_for_5_min
and max_cpu_temp > 70.0):
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.0 or bat_temp > 60.:
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 94.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
# all good
thermal_status = ThermalStatus.green
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
startup_conditions["time_valid"] = now.year >= 2019
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
# try:
# last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
# except (TypeError, ValueError):
# last_update = now
# dt = now - last_update
#
# update_failed_count = params.get("UpdateFailedCount")
# update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
# last_update_exception = params.get("LastUpdateException", encoding='utf8')
#
# if update_failed_count > 15 and last_update_exception is not None:
# if current_branch in ["release2", "dashcam"]:
# extra_text = "Ensure the software is correctly installed"
# else:
# extra_text = last_update_exception
#
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
# set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
# elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
# set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
# elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
# remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
# set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
# else:
# set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
startup_conditions["not_uninstalling"] = not params.get(
"DoUninstall") == b"1"
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
completed_training = params.get(
"CompletedTrainingVersion") == training_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed(
"Offroad_PandaFirmwareMismatch",
(not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02
startup_conditions["completed_training"] = completed_training or (
current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get(
"IsDriverViewEnabled") == b"1"
startup_conditions["not_taking_snapshot"] = not params.get(
"IsTakingSnapshot") == b"1"
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not startup_conditions["device_temp_good"]))
should_start = all(startup_conditions.values())
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
os.system(
'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor'
)
else:
if startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system(
'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor'
)
if shutdown_trigger == 1 and sound_trigger == 1 and msg.thermal.batteryStatus == "Discharging" and started_seen and (
sec_since_boot() - off_ts) > 1 and getoff_alert:
subprocess.Popen([
mediaplayer + 'mediaplayer',
'/data/openpilot/selfdrive/assets/sounds/eondetach.wav'
],
shell=False,
stdin=None,
stdout=None,
stderr=None,
env=env,
close_fds=True)
sound_trigger = 0
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
if shutdown_trigger == 1 and msg.thermal.batteryStatus == "Discharging" and \
started_seen and opkrAutoShutdown and (sec_since_boot() - off_ts) > opkrAutoShutdown and not os.path.isfile(pandaflash_ongoing):
os.system('LD_LIBRARY_PATH="" svc power shutdown')
charging_disabled = check_car_battery_voltage(should_start, health,
charging_disabled, msg)
if msg.thermal.batteryCurrent > 0:
msg.thermal.batteryStatus = "Discharging"
else:
msg.thermal.batteryStatus = "Charging"
msg.thermal.chargingDisabled = charging_disabled
prebuiltlet = Params().get('PutPrebuiltOn') == b'1'
if not os.path.isfile(prebuiltfile) and prebuiltlet:
os.system("cd /data/openpilot; touch prebuilt")
elif os.path.isfile(prebuiltfile) and not prebuiltlet:
os.system("cd /data/openpilot; rm -f prebuilt")
# Offroad power monitoring
pm.calculate(health)
msg.thermal.offroadPowerUsage = pm.get_power_used()
msg.thermal.carBatteryCapacity = max(0, pm.get_car_battery_capacity())
# # Check if we need to disable charging (handled by boardd)
# msg.thermal.chargingDisabled = pm.should_disable_charging(health, off_ts)
#
# # Check if we need to shut down
# if pm.should_shutdown(health, off_ts, started_seen, LEON):
# cloudlog.info(f"shutting device down, offroad since {off_ts}")
# # TODO: add function for blocking cloudlog instead of sleep
# time.sleep(10)
# os.system('LD_LIBRARY_PATH="" svc power shutdown')
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9 * (started_ts or 0))
msg.thermal.thermalStatus = thermal_status
thermal_sock.send(msg.to_bytes())
set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
location=(location.to_dict() if location else None),
thermal=msg.to_dict())
count += 1
def steer_thread():
poller = messaging.Poller()
logcan = messaging.sub_sock('can')
joystick_sock = messaging.sub_sock('testJoystick',
conflate=True,
poller=poller)
carstate = messaging.pub_sock('carState')
carcontrol = messaging.pub_sock('carControl')
sendcan = messaging.pub_sock('sendcan')
button_1_last = 0
enabled = False
# wait for CAN packets
print("Waiting for CAN messages...")
get_one_can(logcan)
CI, CP = get_car(logcan, sendcan)
Params().put("CarParams", CP.to_bytes())
CC = car.CarControl.new_message()
while True:
# send
joystick = messaging.recv_one(joystick_sock)
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
CS = CI.update(CC, can_strs)
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
actuators = car.CarControl.Actuators.new_message()
if joystick is not None:
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1.,
1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1.,
1.) # -1 to 1
actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.)
pcm_cancel_cmd = joystick.testJoystick.buttons[0]
button_1 = joystick.testJoystick.buttons[1]
if button_1 and not button_1_last:
enabled = not enabled
button_1_last = button_1
#print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake
hud_alert = 0
if joystick.testJoystick.buttons[3]:
hud_alert = "steerRequired"
CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd
CC.enabled = enabled
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
# broadcast carState
cs_send = messaging.new_message('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled)
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo, self.sm['longitudinalPlan'].vTarget)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric])
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only:
# send car controls over can
can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
steer_angle_rad = (CS.steeringAngleDeg - self.sm['lateralPlan'].angleOffsetDeg) * CV.DEG_TO_RAD
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.steeringAngleDesiredDeg = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
# Update VehicleModel
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
sr = max(params.steerRatio, 0.1)
self.VM.update_params(x, sr)
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
actuators.longControlState = self.LoC.long_control_state
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(
self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
actuators.accel = self.LoC.update(self.active, CS, self.CP,
long_plan, pid_accel_limits)
# Steering PID loop and lateral MPC
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(
self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
self.active, CS, self.CP, self.VM, params, desired_curvature,
desired_curvature_rate)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
actuators.accel = 4.0 * clip(self.sm['testJoystick'].axes[0],
-1, 1)
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# max angle is 45 for angle-based cars
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
lac_log.active = True
lac_log.steeringAngleDeg = CS.steeringAngleDeg
lac_log.output = steer
lac_log.saturated = abs(steer) >= 0.9
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
0] < -0.1
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
0] > 0.1
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
if not isinstance(attr, Number):
continue
if not math.isfinite(attr):
cloudlog.error(
f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
return actuators, lac_log
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd,
visual_alert, left_lane, right_lane, left_lane_depart,
right_lane_depart, set_speed, lead_visible, lead_dist,
lead_vrel, lead_yrel, sm):
# *** compute control surfaces ***
# gas and brake
self.accel_lim_prev = self.accel_lim
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
self.accel_lim = apply_accel
apply_accel = accel_rate_limit(self.accel_lim, self.accel_lim_prev)
param = self.p
#self.model_speed = 255 - self.SC.calc_va(sm, CS.out.vEgo)
#atom model_speed
#self.model_speed = self.SC.cal_model_speed(sm, CS.out.vEgo)
if frame % 10 == 0:
self.curve_speed = self.SC.cal_curve_speed(sm, CS.out.vEgo)
plan = sm['longitudinalPlan']
self.dRel = int(plan.dRel1) #EON Lead
self.yRel = int(plan.yRel1) #EON Lead
self.vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead
self.dRel2 = int(plan.dRel2) #EON Lead
self.yRel2 = int(plan.yRel2) #EON Lead
self.vRel2 = int(plan.vRel2 * 3.6 + 0.5) #EON Lead
self.lead2_status = plan.status2
self.target_map_speed_camera = plan.targetSpeedCamera
lateral_plan = sm['lateralPlan']
self.outScale = lateral_plan.outputScale
self.vCruiseSet = lateral_plan.vCruiseSet
#self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0002, 0.01], [255, 30])
#Hoya
self.model_speed = interp(abs(lateral_plan.vCurvature),
[0.0, 0.0002, 0.00074, 0.0025, 0.008, 0.02],
[255, 255, 130, 90, 60, 20])
if CS.out.vEgo > 8:
if self.variable_steer_max:
self.steerMax = interp(int(abs(self.model_speed)),
self.model_speed_range,
self.steerMax_range)
else:
self.steerMax = int(
self.params.get("SteerMaxBaseAdj", encoding="utf8"))
if self.variable_steer_delta:
self.steerDeltaUp = interp(int(abs(self.model_speed)),
self.model_speed_range,
self.steerDeltaUp_range)
self.steerDeltaDown = interp(int(abs(self.model_speed)),
self.model_speed_range,
self.steerDeltaDown_range)
else:
self.steerDeltaUp = int(
self.params.get("SteerDeltaUpBaseAdj", encoding="utf8"))
self.steerDeltaDown = int(
self.params.get("SteerDeltaDownBaseAdj", encoding="utf8"))
else:
self.steerMax = int(
self.params.get("SteerMaxBaseAdj", encoding="utf8"))
self.steerDeltaUp = int(
self.params.get("SteerDeltaUpBaseAdj", encoding="utf8"))
self.steerDeltaDown = int(
self.params.get("SteerDeltaDownBaseAdj", encoding="utf8"))
param.STEER_MAX = min(CarControllerParams.STEER_MAX,
self.steerMax) # variable steermax
param.STEER_DELTA_UP = min(CarControllerParams.STEER_DELTA_UP,
self.steerDeltaUp) # variable deltaUp
param.STEER_DELTA_DOWN = min(CarControllerParams.STEER_DELTA_DOWN,
self.steerDeltaDown) # variable deltaDown
# Steering Torque
if 0 <= self.driver_steering_torque_above_timer < 100:
new_steer = int(
round(actuators.steer * self.steerMax *
(self.driver_steering_torque_above_timer / 100)))
else:
new_steer = int(round(actuators.steer * self.steerMax))
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
param)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
if self.opkr_maxanglelimit >= 90 and not self.steer_wind_down_enabled:
lkas_active = enabled and abs(
CS.out.steeringAngleDeg
) < self.opkr_maxanglelimit and CS.out.gearShifter == GearShifter.drive
else:
lkas_active = enabled and not CS.out.steerWarning and CS.out.gearShifter == GearShifter.drive
if (
(CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker and not CS.out.leftBlinker)
) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN and self.opkr_turnsteeringdisable:
self.lanechange_manual_timer = 50
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 50
if self.lanechange_manual_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if abs(CS.out.steeringTorque
) > 180 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
self.driver_steering_torque_above = True
else:
self.driver_steering_torque_above = False
if self.driver_steering_torque_above == True:
self.driver_steering_torque_above_timer -= 1
if self.driver_steering_torque_above_timer <= 0:
self.driver_steering_torque_above_timer = 0
elif self.driver_steering_torque_above == False:
self.driver_steering_torque_above_timer += 5
if self.driver_steering_torque_above_timer >= 100:
self.driver_steering_torque_above_timer = 100
if not lkas_active:
apply_steer = 0
if self.apply_steer_last != 0:
self.steer_wind_down = 1
if lkas_active or CS.out.steeringPressed:
self.steer_wind_down = 0
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
if CS.acc_active and CS.lead_distance > 149 and self.dRel < ((CS.out.vEgo * CV.MS_TO_KPH)+5) < 100 and \
self.vRel < -(CS.out.vEgo * CV.MS_TO_KPH * 0.16) and CS.out.vEgo > 7 and abs(lateral_plan.steerAngleDesireDeg) < 10 and not self.longcontrol:
self.need_brake_timer += 1
if self.need_brake_timer > 50:
self.need_brake = True
else:
self.need_brake = False
self.need_brake_timer = 0
sys_warning, sys_state, left_lane_warning, right_lane_warning =\
process_hud_alert(lkas_active, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active or CS.out.gearShifter != GearShifter.drive:
enabled_speed = clu11_speed
can_sends = []
can_sends.append(
create_lkas11(self.packer, frame, self.car_fingerprint,
apply_steer, lkas_active, self.steer_wind_down,
CS.lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane, left_lane_warning,
right_lane_warning, 0))
if CS.CP.mdpsBus: # send lkas11 bus 1 if mdps is bus 1
can_sends.append(
create_lkas11(self.packer, frame, self.car_fingerprint,
apply_steer, lkas_active, self.steer_wind_down,
CS.lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane, left_lane_warning,
right_lane_warning, 1))
if frame % 2 and CS.CP.mdpsBus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.NONE,
enabled_speed, CS.CP.mdpsBus))
str_log1 = 'M/C={:03.0f}/{:03.0f} TQ={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} AQ={:+04.2f}'.format(
abs(self.model_speed), self.curve_speed, abs(new_steer),
max(self.steerMax, abs(new_steer)), self.steerDeltaUp,
self.steerDeltaDown, CS.scc12["aReqValue"])
try:
if self.params.get_bool("OpkrLiveTune"):
if int(self.params.get("LateralControlMethod",
encoding="utf8")) == 0:
self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(
float(
int(self.params.get("PidKp", encoding="utf8")) *
0.01),
float(
int(self.params.get("PidKi", encoding="utf8")) *
0.001),
float(
int(self.params.get("PidKd", encoding="utf8")) *
0.01),
float(
int(self.params.get("PidKf", encoding="utf8")) *
0.00001))
elif int(
self.params.get("LateralControlMethod",
encoding="utf8")) == 1:
self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(
float(
int(
self.params.get("InnerLoopGain",
encoding="utf8")) * 0.1),
float(
int(
self.params.get("OuterLoopGain",
encoding="utf8")) * 0.1),
float(
int(
self.params.get("TimeConstant",
encoding="utf8")) * 0.1),
float(
int(
self.params.get("ActuatorEffectiveness",
encoding="utf8")) * 0.1))
elif int(
self.params.get("LateralControlMethod",
encoding="utf8")) == 2:
self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(
float(
int(self.params.get("Scale", encoding="utf8")) *
1.0),
float(
int(self.params.get("LqrKi", encoding="utf8")) *
0.001),
float(
int(self.params.get("DcGain", encoding="utf8")) *
0.0001))
except:
pass
trace1.printf1('{} {}'.format(str_log1, self.str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and (
CS.out.cruiseState.modeSel > 0)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "편도1차선"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "맵감속ONLY"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if self.cruise_gap != CS.cruiseGapSet:
self.cruise_gap = CS.cruiseGapSet
if CS.lead_distance < 149:
self.leadcar_status = "O"
else:
self.leadcar_status = "-"
str_log2 = 'MODE={:s} MDPS={:s} LKAS={:s} CSG={:1.0f} LEAD={:s} FR={:03.0f}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch,
self.cruise_gap, self.leadcar_status, self.timer1.sampleTime())
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL,
clu11_speed, CS.CP.sccBus))
if CS.out.cruiseState.standstill:
self.standstill_status = 1
if self.opkr_autoresume:
# run only first time when the car stopped
if self.last_lead_distance == 0:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
self.res_switch_timer = 0
self.standstill_fault_reduce_timer += 1
elif self.res_switch_timer > 0:
self.res_switch_timer -= 1
self.standstill_fault_reduce_timer += 1
# at least 1 sec delay after entering the standstill
elif 100 < self.standstill_fault_reduce_timer and CS.lead_distance != self.last_lead_distance:
self.acc_standstill_timer = 0
self.acc_standstill = False
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL)
) if not self.longcontrol else can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL, clu11_speed,
CS.CP.sccBus))
self.resume_cnt += 1
if self.resume_cnt > 5:
self.resume_cnt = 0
self.res_switch_timer = randint(10, 15)
self.standstill_fault_reduce_timer += 1
# gap save
elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume and self.opkr_cruisegap_auto_adj:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
# gap adjust to 1 for fast start
elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume and self.opkr_cruisegap_auto_adj:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.GAP_DIST)
) if not self.longcontrol else can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.GAP_DIST, clu11_speed,
CS.CP.sccBus))
self.cruise_gap_switch_timer = 0
elif self.opkr_autoresume:
self.standstill_fault_reduce_timer += 1
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.clu11,
self.SC.btn_type, self.SC.sc_clu_speed)
) if not self.longcontrol else can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.clu11,
self.SC.btn_type, self.SC.sc_clu_speed,
CS.CP.sccBus))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if self.opkr_cruisegap_auto_adj:
# gap restore
if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 50:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.GAP_DIST)
) if not self.longcontrol else can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.GAP_DIST, clu11_speed,
CS.CP.sccBus))
self.cruise_gap_switch_timer = 0
elif self.cruise_gap_prev == CS.cruiseGapSet and self.opkr_autoresume:
self.cruise_gap_set_init = 0
self.cruise_gap_prev = 0
if CS.cruise_buttons == 4:
self.cancel_counter += 1
elif CS.acc_active:
self.cancel_counter = 0
if self.res_speed_timer > 0:
self.res_speed_timer -= 1
else:
self.v_cruise_kph_auto_res = 0
self.res_speed = 0
if self.model_speed > 95 and self.cancel_counter == 0 and not CS.acc_active and not CS.out.brakeLights and int(CS.VSetDis) > 30 and \
(CS.lead_distance < 149 or int(CS.clu_Vanz) > 30) and int(CS.clu_Vanz) >= 3 and self.auto_res_timer <= 0 and self.opkr_cruise_auto_res:
if self.opkr_cruise_auto_res_option == 0:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL)
) if not self.longcontrol else can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL, clu11_speed,
CS.CP.sccBus)) # auto res
self.res_speed = int(CS.clu_Vanz * 1.1)
self.res_speed_timer = 350
elif self.opkr_cruise_auto_res_option == 1:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.SET_DECEL)
) if not self.longcontrol else can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.SET_DECEL, clu11_speed, CS.CP.sccBus)
) # auto res but set_decel to set current speed
self.v_cruise_kph_auto_res = int(CS.clu_Vanz)
self.res_speed_timer = 50
if self.auto_res_timer <= 0:
self.auto_res_timer = randint(10, 15)
elif self.auto_res_timer > 0 and self.opkr_cruise_auto_res:
self.auto_res_timer -= 1
if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active:
self.standstill_status_timer += 1
if self.standstill_status_timer > 200:
self.standstill_status = 1
self.standstill_status_timer = 0
if self.standstill_status == 1 and CS.out.vEgo > 1:
self.standstill_status = 0
self.standstill_fault_reduce_timer = 0
self.last_resume_frame = frame
self.res_switch_timer = 0
self.resume_cnt = 0
if CS.out.vEgo <= 1:
self.sm.update(0)
long_control_state = self.sm['controlsState'].longControlState
if long_control_state == LongCtrlState.stopping and CS.out.vEgo < 0.1 and not CS.out.gasPressed:
self.acc_standstill_timer += 1
if self.acc_standstill_timer >= 200:
self.acc_standstill_timer = 200
self.acc_standstill = True
else:
self.acc_standstill_timer = 0
self.acc_standstill = False
elif CS.out.gasPressed or CS.out.vEgo > 1:
self.acc_standstill = False
self.acc_standstill_timer = 0
else:
self.acc_standstill = False
self.acc_standstill_timer = 0
if CS.CP.mdpsBus: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
if CS.CP.sccBus == 2 and self.counter_init and self.longcontrol:
if frame % 2 == 0:
self.scc12cnt += 1
self.scc12cnt %= 0xF
self.scc11cnt += 1
self.scc11cnt %= 0x10
self.fca11supcnt += 1
self.fca11supcnt %= 0xF
if self.fca11alivecnt == 1:
self.fca11inc = 0
if self.fca11cnt13 == 3:
self.fca11maxcnt = 0x9
self.fca11cnt13 = 0
else:
self.fca11maxcnt = 0xD
self.fca11cnt13 += 1
else:
self.fca11inc += 4
self.fca11alivecnt = self.fca11maxcnt - self.fca11inc
lead_objspd = CS.lead_objspd # vRel (km/h)
aReqValue = CS.scc12["aReqValue"]
if 0 < CS.out.radarDistance <= 149:
if aReqValue > 0.:
stock_weight = interp(CS.out.radarDistance, [3., 25.],
[0.7, 0.])
elif aReqValue < 0.:
stock_weight = interp(CS.out.radarDistance, [3., 25.],
[1., 0.])
if lead_objspd < 0:
vRel_weight = interp(abs(lead_objspd), [0, 20],
[1, 2])
stock_weight = interp(
CS.out.radarDistance,
[3.**vRel_weight, 25. * vRel_weight], [1., 0.])
else:
stock_weight = 0.
apply_accel = apply_accel * (
1. - stock_weight) + aReqValue * stock_weight
else:
stock_weight = 0.
can_sends.append(
create_scc11(self.packer, frame, set_speed, lead_visible,
self.scc_live, lead_dist, lead_vrel,
lead_yrel, self.car_fingerprint, CS.clu_Vanz,
CS.scc11))
if (CS.brake_check
or CS.cancel_check) and self.car_fingerprint not in [
CAR.NIRO_EV
]:
can_sends.append(
create_scc12(self.packer, apply_accel, enabled,
self.scc_live, CS.out.gasPressed, 1,
CS.out.stockAeb, self.car_fingerprint,
CS.clu_Vanz, CS.scc12))
else:
can_sends.append(
create_scc12(self.packer, apply_accel, enabled,
self.scc_live, CS.out.gasPressed,
CS.out.brakePressed, CS.out.stockAeb,
self.car_fingerprint, CS.clu_Vanz,
CS.scc12))
can_sends.append(
create_scc14(self.packer, enabled, CS.scc14,
CS.out.stockAeb, lead_visible, lead_dist,
CS.out.vEgo, self.acc_standstill,
self.car_fingerprint))
if CS.CP.fcaBus == -1:
can_sends.append(
create_fca11(self.packer, CS.fca11, self.fca11alivecnt,
self.fca11supcnt))
if frame % 20 == 0:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.CP.fcaBus == -1:
can_sends.append(create_fca12(self.packer))
if frame % 50 == 0:
can_sends.append(create_scc42a(self.packer))
elif CS.CP.sccBus == 2 and self.longcontrol:
self.counter_init = True
self.scc12cnt = CS.scc12init["CR_VSM_Alive"]
self.scc11cnt = CS.scc11init["AliveCounterACC"]
self.fca11alivecnt = CS.fca11init["CR_FCA_Alive"]
self.fca11supcnt = CS.fca11init["Supplemental_Counter"]
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES[
"send_lfahda_mfa"]:
can_sends.append(create_lfahda_mfc(self.packer, frame,
lkas_active))
return can_sends
def update(self, enabled, can_sends, packer, CC, CS, frame, apply_accel,
controls):
clu11_speed = CS.clu11["CF_Clu_Vanz"]
road_limit_speed, left_dist, max_speed_log = self.cal_max_speed(
frame, CC, CS, controls.sm, clu11_speed)
CC.sccSmoother.roadLimitSpeed = road_limit_speed
CC.sccSmoother.roadLimitSpeedLeftDist = left_dist
if not self.scc_smoother_enabled:
self.reset()
return
if self.dispatch_cancel_buttons(CC, CS):
self.reset()
return
if self.state == CruiseState.STOCK or not CS.acc_mode or \
not enabled or not CS.cruiseState_enabled or CS.cruiseState_speed < 1. or \
CS.cruiseState_speed > 254 or CS.standstill or \
CS.cruise_buttons != Buttons.NONE or \
CS.brake_pressed:
#CC.sccSmoother.logMessage = '{:.2f},{:d},{:d},{:d},{:d},{:.1f},{:d},{:d},{:d}' \
# .format(float(apply_accel*CV.MS_TO_KPH), int(CS.acc_mode), int(enabled), int(CS.cruiseState_enabled), int(CS.standstill), float(CS.cruiseState_speed),
# int(CS.cruise_buttons), int(CS.brake_pressed), int(CS.gas_pressed))
CC.sccSmoother.logMessage = max_speed_log
self.reset()
self.wait_timer = ALIVE_COUNT + max(WAIT_COUNT)
return
current_set_speed = CS.cruiseState_speed * CV.MS_TO_KPH
accel, override_acc = self.cal_acc(apply_accel, CS, clu11_speed,
controls.sm)
if CS.gas_pressed:
self.target_speed = clu11_speed
if clu11_speed > controls.cruiseOpMaxSpeed and self.sync_set_speed_while_gas_pressed:
set_speed = clip(clu11_speed, MIN_SET_SPEED, MAX_SET_SPEED)
CC.cruiseOpMaxSpeed = controls.cruiseOpMaxSpeed = controls.v_cruise_kph = set_speed
else:
self.target_speed = clu11_speed + accel
self.target_speed = clip(self.target_speed, MIN_SET_SPEED,
self.max_set_speed)
CC.sccSmoother.logMessage = '{:.1f}/{:.1f}, {:d}/{:d}/{:d}, {:d}' \
.format(float(override_acc), float(accel), int(self.target_speed), int(self.curve_speed), int(road_limit_speed), int(self.btn))
#CC.sccSmoother.logMessage = max_speed_log
if self.wait_timer > 0:
self.wait_timer -= 1
else:
if self.alive_timer == 0:
self.btn = self.get_button(clu11_speed, current_set_speed)
self.alive_count = ALIVE_COUNT
if self.btn != Buttons.NONE:
can_sends.append(
SccSmoother.create_clu11(packer, self.alive_timer,
CS.scc_bus, CS.clu11, self.btn))
if self.alive_timer == 0:
self.started_frame = frame
self.alive_timer += 1
if self.alive_timer >= self.alive_count:
self.alive_timer = 0
self.wait_timer = SccSmoother.get_wait_count()
self.btn = Buttons.NONE
def update(self, active, v_ego, angle_steers, angle_steers_rate,
steer_override, CP, VM, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(angle_steers)],
[math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if v_ego < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = path_plan.angleSteers
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (
1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des -
self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enfore_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output +
delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(
new_output_steer_cmd, prev_output_steer_cmd,
prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
self.sat_flag = False
return float(self.output_steer), float(self.angle_steers_des), indi_log
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):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# 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
# Never send cancel command if we never enter cruise state (no cruise if pedal)
# Cancel cmd causes brakes to release at a standstill causing grinding
pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.enableCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.isPandaBlackDEPRECATED,
CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlackDEPRECATED,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx,
CS.CP.isPandaBlackDEPRECATED))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint, CS.CP.isPandaBlackDEPRECATED))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint, CS.CP.isPandaBlackDEPRECATED))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
pass # TODO: implement
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0,
P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.append(
hondacan.create_brake_command(
self.packer, apply_brake, pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint,
CS.CP.isPandaBlackDEPRECATED, CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
def thermald_thread():
pm = messaging.PubMaster(['deviceState'])
pandaState_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaState',
timeout=pandaState_timeout)
location_sock = messaging.sub_sock('gpsLocationExternal')
managerState_sock = messaging.sub_sock('managerState', conflate=True)
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
pandaState_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
ui_running_prev = False
params = Params()
power_monitor = PowerMonitoring()
no_panda_cnt = 0
thermal_config = HARDWARE.get_thermal_config()
# CPR3 logging
if EON:
base_path = "/sys/kernel/debug/cpr3-regulator/"
cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()]
cpr_files = ["/sys/kernel/debug/regulator/pm8994_s11/voltage"
] + cpr_files
cpr_data = {}
for cf in cpr_files:
with open(cf, "r") as f:
try:
cpr_data[str(cf)] = f.read().strip()
except Exception:
pass
cloudlog.event("CPR", data=cpr_data)
while 1:
pandaState = messaging.recv_sock(pandaState_sock, wait=True)
msg = read_thermal(thermal_config)
if pandaState is not None:
usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions[
"ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
startup_conditions[
"hardware_supported"] = pandaState.pandaState.pandaType not in [
log.PandaState.PandaType.whitePanda,
log.PandaState.PandaType.greyPanda
]
set_offroad_alert_if_changed(
"Offroad_HardwareUnsupported",
not startup_conditions["hardware_supported"])
# Setup fan handler on first connect to panda
if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if pandaState_prev is not None:
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \
pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown:
params.panda_disconnect()
pandaState_prev = pandaState
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(
round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryStatus = HARDWARE.get_battery_status()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
# Fake battery levels on uno for frame
if (not EON) or is_uno:
msg.deviceState.batteryPercent = 100
msg.deviceState.batteryStatus = "Charging"
msg.deviceState.batteryTempC = 0
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC))
max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC,
max(msg.deviceState.gpuTempC))
bat_temp = msg.deviceState.batteryTempC
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed,
startup_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
# We only do this if there is a relay that prevents the car from faulting
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min
and max_cpu_temp > 70.0):
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.0 or bat_temp > 60.:
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 94.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
thermal_status = ThermalStatus.green # default to good condition
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(
update_failed_count)
last_update_exception = params.get("LastUpdateException",
encoding='utf8')
if update_failed_count > 15 and last_update_exception is not None:
if current_branch in ["release2", "dashcam"]:
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_UpdateFailed",
True,
extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
True,
extra_text=f"{remaining_time} days.")
else:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get_bool(
"DisableUpdates")
startup_conditions["not_uninstalling"] = not params.get_bool(
"DoUninstall")
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed(
"Offroad_PandaFirmwareMismatch",
(not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get_bool(
"IsDriverViewEnabled")
startup_conditions["not_taking_snapshot"] = not params.get_bool(
"IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not startup_conditions["device_temp_good"]))
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOffroad", not should_start)
HARDWARE.set_power_save(not should_start)
if TICI and not params.get_bool("EnableLteOnroad"):
fxn = "stop" if should_start else "start"
os.system(f"sudo systemctl {fxn} --no-block lte")
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(pandaState)
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
pandaState, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(pandaState, off_ts, started_seen):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
HARDWARE.shutdown()
# If UI has crashed, set the brightness to reasonable non-zero value
manager_state = messaging.recv_one_or_none(managerState_sock)
if manager_state is not None:
ui_running = "ui" in (p.name
for p in manager_state.managerState.processes
if p.running)
if ui_running_prev and not ui_running:
HARDWARE.set_screen_brightness(20)
ui_running_prev = ui_running
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0))
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
if EON and not is_uno:
set_offroad_alert_if_changed("Offroad_ChargeDisabled",
(not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once every 10 minutes
if (count % int(600. / DT_TRML)) == 0:
if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40:
cloudlog.event("High offroad memory usage",
mem=msg.deviceState.memoryUsagePercent)
location = messaging.recv_sock(location_sock)
cloudlog.event("STATUS_PACKET",
count=count,
pandaState=(strip_deprecated_keys(
pandaState.to_dict()) if pandaState else None),
location=(strip_deprecated_keys(
location.gpsLocationExternal.to_dict())
if location else None),
deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
def update(self, active, CS, CP, path_plan):
self.v_ego = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngle)],
[math.radians(CS.steeringRate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = path_plan.angleSteers
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (
1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des -
self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output +
delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(
new_output_steer_cmd, prev_output_steer_cmd,
prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
check_saturation = (
CS.vEgo >
10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(
self.output_steer, check_saturation, steers_max)
return float(self.output_steer), float(self.angle_steers_des), indi_log
def update(self, CS, extra_params):
v_ego = CS.vEgo
self.handle_passable(CS, extra_params)
if not self.supported_car: # disable dynamic gas if car not supported
return float(interp(v_ego, self.CP.gasMaxBP, self.CP.gasMaxV))
gas = interp(v_ego, self.gasMaxBP, self.gasMaxV)
if self.lead_data['status']: # if lead
if v_ego <= 8.9408: # if under 20 mph
x = [
0.0, 0.24588812499999999, 0.432818589, 0.593044697,
0.730381365, 1.050833588, 1.3965, 1.714627481
] # relative velocity mod
y = [0.9901, 0.905, 0.8045, 0.625, 0.431, 0.2083, .0667, 0]
gas_mod = -(gas * interp(self.lead_data['v_rel'], x, y))
x = [0.44704, 1.1176, 1.34112] # lead accel mod
y = [
1.0, 0.75, 0.625
] # maximum we can reduce gas_mod is 40 percent (never increases mod)
gas_mod *= interp(self.lead_data['a_lead'], x, y)
x = [
6.1, 9.15, 15.24
] # as lead gets further from car, lessen gas mod/reduction
y = [1.0, 0.75, 0.0]
gas_mod *= interp(self.lead_data['x_lead'], x, y)
if not self.CP.enableGasInterceptor: # this will hopefuly let TSS2 use dynamic gas, need to tune
gas_mod *= 0.33
new_gas = gas + gas_mod
x = [1.78816, 6.0,
8.9408] # slowly ramp mods down as we approach 20 mph
y = [new_gas, (new_gas * 0.6 + gas * 0.4), gas]
gas = interp(v_ego, x, y)
else:
x = [
-3.12928, -1.78816, -0.89408, 0, 0.33528, 1.78816, 2.68224
] # relative velocity mod
y = [
-gas * 0.2625, -gas * 0.18, -gas * 0.12, 0.0, gas * 0.075,
gas * 0.135, gas * 0.19
]
gas_mod = interp(self.lead_data['v_rel'], x, y)
current_TR = self.lead_data['x_lead'] / v_ego
x = [
self.mpc_TR - 0.22, self.mpc_TR, self.mpc_TR + 0.2,
self.mpc_TR + 0.4
]
y = [-gas_mod * 0.15, 0.0, gas_mod * 0.25, gas_mod * 0.4]
gas_mod -= interp(current_TR, x, y)
gas += gas_mod
if self.blinker_status:
x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
y = [1.0, 1.2875, 1.4625]
gas *= interp(v_ego, x, y)
return float(clip(gas, 0.0, 1.0))
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# *** compute control surfaces ***
#Leave this here, will someday use accel...
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque - leave minimum for steer torque incase it's needed
apply_steer = int(round(actuators.steer * STEER_MAX))
# steer angle - Currently using steer angle
if enabled:
apply_angle = actuators.steerAngle
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)
else:
apply_angle = CS.angle_steers
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_steer = int(round(alca_steer * STEER_MAX))
apply_angle = alca_angle
#Disable if not enabled
if not enabled:
apply_angle = 0
apply_steer_req = 0
else:
apply_steer_req = 1
#Multply by 100 to allow 2 decmals sent over CAN. Arduino will divde by 100.
angle_send = apply_angle * 100
#update desired angle for safety loop
CS.desired_angle = apply_angle
#Reset enabled time if blinker pressed to not diable during lane change
if CS.left_blinker_on or CS.right_blinker_on or CS.brake_pressed:
CS.enabled_time = (
sec_since_boot() * 1e3
) #reset time to not trigger safety after releaaed
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#Always send CAN steer message
can_sends.append(
create_steer_command(self.packer, angle_send, apply_steer_req,
frame))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd,
False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def thermald_thread():
# prevent LEECO from undervoltage
BATT_PERC_OFF = 10 if LEON else 3
health_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected health frequency
# now loop
thermal_sock = messaging.pub_sock('thermal')
health_sock = messaging.sub_sock('health', timeout=health_timeout)
location_sock = messaging.sub_sock('gpsLocation')
fan_speed = 0
count = 0
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
thermal_status_prev = ThermalStatus.green
usb_power = True
usb_power_prev = True
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
health_prev = None
fw_version_match_prev = True
current_connectivity_alert = None
time_valid_prev = True
should_start_prev = False
is_uno = (read_tz(29, clip=False) < -1000)
if is_uno or not ANDROID:
handle_fan = handle_fan_uno
else:
setup_eon_fan()
handle_fan = handle_fan_eon
params = Params()
while 1:
health = messaging.recv_sock(health_sock, wait=True)
location = messaging.recv_sock(location_sock)
location = location.gpsLocation if location else None
msg = read_thermal()
# clear car params when panda gets disconnected
if health is None and health_prev is not None:
params.panda_disconnect()
health_prev = health
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(
psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
try:
with open("/sys/class/power_supply/battery/capacity") as f:
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/battery/current_now") as f:
msg.thermal.batteryCurrent = int(f.read())
with open("/sys/class/power_supply/battery/voltage_now") as f:
msg.thermal.batteryVoltage = int(f.read())
with open("/sys/class/power_supply/usb/present") as f:
msg.thermal.usbOnline = bool(int(f.read()))
except FileNotFoundError:
pass
# Fake battery levels on uno for frame
if is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10.,
msg.thermal.gpu / 10.)
bat_temp = msg.thermal.bat / 1000.
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed)
msg.thermal.fanSpeed = fan_speed
# thermal logic with hysterisis
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 87.5:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
# all good
thermal_status = ThermalStatus.green
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.now()
# show invalid date/time alert
time_valid = now.year >= 2019
if time_valid and not time_valid_prev:
params.delete("Offroad_InvalidTime")
if not time_valid and time_valid_prev:
params.put("Offroad_InvalidTime",
json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"]))
time_valid_prev = time_valid
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
if dt.days > DAYS_NO_CONNECTIVITY_MAX:
if current_connectivity_alert != "expired":
current_connectivity_alert = "expired"
params.delete("Offroad_ConnectivityNeededPrompt")
params.put(
"Offroad_ConnectivityNeeded",
json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"]))
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(DAYS_NO_CONNECTIVITY_MAX - dt.days)
if current_connectivity_alert != "prompt" + remaining_time:
current_connectivity_alert = "prompt" + remaining_time
alert_connectivity_prompt = copy.copy(
OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"])
alert_connectivity_prompt["text"] += remaining_time + " days."
params.delete("Offroad_ConnectivityNeeded")
params.put("Offroad_ConnectivityNeededPrompt",
json.dumps(alert_connectivity_prompt))
elif current_connectivity_alert is not None:
current_connectivity_alert = None
params.delete("Offroad_ConnectivityNeeded")
params.delete("Offroad_ConnectivityNeededPrompt")
# start constellation of processes when the car starts
ignition = health is not None and (health.health.ignitionLine
or health.health.ignitionCan)
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get(
"CompletedTrainingVersion") == training_version
panda_signature = params.get("PandaFirmware")
fw_version_match = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
should_start = ignition
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and msg.thermal.freeSpace > 0.02
# confirm we have completed training and aren't uninstalling
should_start = should_start and accepted_terms and completed_training and (
not do_uninstall)
# check for firmware mismatch
should_start = should_start and fw_version_match
# check if system time is valid
should_start = should_start and time_valid
# don't start while taking snapshot
if not should_start_prev:
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
should_start = should_start and (not is_taking_snapshot)
if fw_version_match and not fw_version_match_prev:
params.delete("Offroad_PandaFirmwareMismatch")
if not fw_version_match and fw_version_match_prev:
params.put(
"Offroad_PandaFirmwareMismatch",
json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"]))
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
if thermal_status >= ThermalStatus.danger:
should_start = False
if thermal_status_prev < ThermalStatus.danger:
params.put(
"Offroad_TemperatureTooHigh",
json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"]))
else:
if thermal_status_prev >= ThermalStatus.danger:
params.delete("Offroad_TemperatureTooHigh")
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
os.system(
'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor'
)
else:
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system(
'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor'
)
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \
started_seen and (sec_since_boot() - off_ts) > 60:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9 * (started_ts or 0))
msg.thermal.thermalStatus = thermal_status
thermal_sock.send(msg.to_bytes())
if usb_power_prev and not usb_power:
params.put("Offroad_ChargeDisabled",
json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"]))
elif usb_power and not usb_power_prev:
params.delete("Offroad_ChargeDisabled")
thermal_status_prev = thermal_status
usb_power_prev = usb_power
fw_version_match_prev = fw_version_match
should_start_prev = should_start
#print(msg)
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
location=(location.to_dict() if location else None),
thermal=msg.to_dict())
count += 1
def update_v_cruise(v_cruise_kph, v_ego, gas_pressed, buttonEvents, enabled,
metric):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
global ButtonCnt, LongPressed, ButtonPrev, PrevDisable, CurrentVspeed, PrevGaspressed
if enabled:
if ButtonCnt:
ButtonCnt += 1
for b in buttonEvents:
if b.pressed and not ButtonCnt and (
b.type == ButtonType.accelCruise
or b.type == ButtonType.decelCruise):
ButtonCnt = FIRST_PRESS_TIME
ButtonPrev = b.type
elif not b.pressed:
LongPressed = False
ButtonCnt = 0
CurrentVspeed = clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN,
V_CRUISE_MAX)
CurrentVspeed = CurrentVspeed if metric else (CurrentVspeed *
CV.KPH_TO_MPH)
CurrentVspeed = int(round(CurrentVspeed))
v_cruise = v_cruise_kph if metric else int(
round(v_cruise_kph * CV.KPH_TO_MPH))
if ButtonCnt > LONG_PRESS_TIME:
LongPressed = True
V_CRUISE_DELTA = V_CRUISE_LONG_PRESS_DELTA_KPH if metric else V_CRUISE_LONG_PRESS_DELTA_MPH
if ButtonPrev == ButtonType.accelCruise:
v_cruise += V_CRUISE_DELTA - v_cruise % V_CRUISE_DELTA
elif ButtonPrev == ButtonType.decelCruise:
v_cruise -= V_CRUISE_DELTA - -v_cruise % V_CRUISE_DELTA
ButtonCnt = FIRST_PRESS_TIME
elif ButtonCnt == FIRST_PRESS_TIME and not LongPressed and not PrevDisable:
if ButtonPrev == ButtonType.accelCruise:
v_cruise = CurrentVspeed if (
gas_pressed and not PrevGaspressed and
(v_cruise < CurrentVspeed)) else (v_cruise + 1)
PrevGaspressed = gas_pressed
elif ButtonPrev == ButtonType.decelCruise:
v_cruise = CurrentVspeed if (
gas_pressed and not PrevGaspressed) else (v_cruise - 1)
PrevGaspressed = gas_pressed
elif not gas_pressed:
PrevGaspressed = False
v_cruise_min = V_CRUISE_MIN if metric else V_CRUISE_MIN * CV.KPH_TO_MPH
v_cruise_max = V_CRUISE_MAX if metric else V_CRUISE_MAX * CV.KPH_TO_MPH
v_cruise = clip(v_cruise, v_cruise_min, v_cruise_max)
v_cruise_kph = v_cruise if metric else v_cruise * CV.MPH_TO_KPH
v_cruise_kph = int(round(v_cruise_kph))
PrevDisable = False
else:
PrevDisable = True
return v_cruise_kph
def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk,
carstate, carcontrol, carevents, carparams, controlsstate,
sendcan, AM, driver_status, LaC, LoC, read_only, start_time,
v_acc, a_acc, lac_log, events_prev):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
try:
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (
not isEnabled(state) and CS.cruiseState.enabled)
# Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)
) # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(
max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) *
brake_discount) if CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = CI.calc_accel_override(
CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = sm['plan'].hasLead
right_lane_visible = sm['pathPlan'].rProb > 0.5
left_lane_visible = sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
blinker = CS.leftBlinker or CS.rightBlinker
ldw_allowed = CS.vEgo > 12.5 and not blinker
if len(list(sm['pathPlan'].rPoly)) == 4:
CC.hudControl.rightLaneDepart = bool(
ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET)
and right_lane_visible)
if len(list(sm['pathPlan'].lPoly)) == 4:
CC.hudControl.leftLaneDepart = bool(ldw_allowed
and sm['pathPlan'].lPoly[3] <
(1 - CAMERA_OFFSET)
and left_lane_visible)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
if not read_only:
# send car controls over can
can_sends = CI.apply(CC)
sendcan.send(
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = driver_status.awareness < 0.
# controlsState
dat = messaging.new_message()
dat.init('controlsState')
dat.valid = CS.canValid
dat.controlsState = {
"alertText1":
AM.alert_text_1,
"alertText2":
AM.alert_text_2,
"alertSize":
AM.alert_size,
"alertStatus":
AM.alert_status,
"alertBlinkingRate":
AM.alert_rate,
"alertType":
AM.alert_type,
"alertSound":
"", # no EON sounds yet
"awarenessStatus":
max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn":
bool(driver_status.monitor_on and driver_status.face_detected),
"canMonoTimes":
list(CS.canMonoTimes),
"planMonoTime":
sm.logMonoTime['plan'],
"pathPlanMonoTime":
sm.logMonoTime['pathPlan'],
"enabled":
isEnabled(state),
"active":
isActive(state),
"vEgo":
CS.vEgo,
"vEgoRaw":
CS.vEgoRaw,
"angleSteers":
CS.steeringAngle,
"curvature":
VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) *
CV.DEG_TO_RAD, CS.vEgo),
"steerOverride":
CS.steeringPressed,
"state":
state,
"engageable":
not bool(get_events(events, [ET.NO_ENTRY])),
"longControlState":
LoC.long_control_state,
"vPid":
float(LoC.v_pid),
"vCruise":
float(v_cruise_kph),
"upAccelCmd":
float(LoC.pid.p),
"uiAccelCmd":
float(LoC.pid.i),
"ufAccelCmd":
float(LoC.pid.f),
"angleSteersDes":
float(LaC.angle_steers_des),
"vTargetLead":
float(v_acc),
"aTarget":
float(a_acc),
"jerkFactor":
float(sm['plan'].jerkFactor),
"angleModelBias":
0.,
"gpsPlannerActive":
sm['plan'].gpsPlannerActive,
"vCurvature":
sm['plan'].vCurvature,
"decelForModel":
sm['plan'].longitudinalPlanSource ==
log.Plan.LongitudinalPlanSource.model,
"cumLagMs":
-rk.remaining * 1000.,
"startMonoTime":
int(start_time * 1e9),
"mapValid":
sm['plan'].mapValid,
"forceDecel":
bool(force_decel),
}
if CP.lateralTuning.which() == 'pid':
dat.controlsState.lateralControlState.pidState = lac_log
else:
dat.controlsState.lateralControlState.indiState = lac_log
controlsstate.send(dat.to_bytes())
# carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# carEvents - logged every second or on change
events_bytes = events_to_bytes(events)
if (sm.frame % int(1. / DT_CTRL)
== 0) or (events_bytes != events_prev):
ce_send = messaging.new_message()
ce_send.init('carEvents', len(events))
ce_send.carEvents = events
carevents.send(ce_send.to_bytes())
# carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message()
cp_send.init('carParams')
cp_send.carParams = CP
carparams.send(cp_send.to_bytes())
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
return CC, events_bytes
except Exception as e:
print e
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only send brake values if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_steer = int(round(alca_steer * STEER_MAX))
# steer torque
#apply_steer = int(round(actuators.steer * STEER_MAX))
max_lim = min(
max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX),
STEER_MAX)
min_lim = max(
min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX),
-STEER_MAX)
apply_steer = clip(apply_steer, min_lim, max_lim)
# slow rate if steer torque increases in magnitude
if self.last_steer > 0:
apply_steer = clip(
apply_steer,
max(self.last_steer - STEER_DELTA_DOWN, -STEER_DELTA_UP),
self.last_steer + STEER_DELTA_UP)
else:
apply_steer = clip(
apply_steer, self.last_steer - STEER_DELTA_UP,
min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
#apply_angle = actuators.steerAngle
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)
else:
apply_angle = CS.angle_steers
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 = 1
# on entering standstill, send standstill request
#if CS.standstill and not self.last_standstill:
# self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# Enable blindspot debug mode once
if BLINDSPOTDEBUG:
self.blindspot_poll_counter += 1
if self.blindspot_poll_counter > 1000: # 10 seconds after start
if CS.left_blinker_on:
self.blindspot_blink_counter_left += 1
#print "debug Left Blinker on"
elif CS.right_blinker_on:
self.blindspot_blink_counter_right += 1
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
#print "debug Left Blinker off"
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self.blindspot_debug_enabled_left = False
#print "debug Left blindspot debug disabled"
if self.blindspot_debug_enabled_right:
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print "debug Right blindspot debug disabled"
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(
LEFT_BLINDSPOT, True))
#print "debug Left blindspot debug enabled"
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.v_ego > 6: #polling at low speeds switches camera off
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print "debug Right blindspot debug enabled"
self.blindspot_debug_enabled_right = True
if self.blindspot_debug_enabled_left:
if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if self.blindspot_debug_enabled_right:
if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
if CS.lane_departure_toggle_on:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
else:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
# rav4h with dsu disconnected
if self.angle_control:
if CS.lane_departure_toggle_on:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
else:
can_sends.append(
create_ipas_steer_command(self.packer, 0, 0, True))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
if CS.cstm_btns.get_button_status("tr") > 0:
distance = 0b01110011 #x73 comma with toggle - toggle is 5th bit from right
else:
distance = 0b01100011 #x63 comma with toggle - toggle is 5th bit from right
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
distance))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sm, CP):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
measured_curvature = sm['controlsState'].curvature
md = sm['modelV2']
self.LP.parse_model(sm['modelV2'])
if len(md.position.x) == TRAJECTORY_SIZE and len(
md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack(
[md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = list(md.orientation.z)
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (
not one_blinker) or (not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) or \
self.auto_lane_change_enabled and \
(AUTO_LCA_START_TIME+0.25) > self.auto_lane_change_timer > AUTO_LCA_START_TIME
blindspot_detected = (
(sm['carState'].leftBlindspot
and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot
and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and (not blindspot_detected
or self.prev_torque_applied):
self.lane_change_state = LaneChangeState.laneChangeStarting
elif torque_applied and blindspot_detected and self.auto_lane_change_timer != 10.0:
self.auto_lane_change_timer = 10.0
elif not torque_applied and self.auto_lane_change_timer == 10.0 and not self.prev_torque_applied:
self.prev_torque_applied = True
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
if self.lane_change_state == LaneChangeState.off:
self.auto_lane_change_timer = 0.0
self.prev_torque_applied = False
elif self.auto_lane_change_timer < (
AUTO_LCA_START_TIME +
0.25): # stop afer 3 sec resume from 10 when torque applied
self.auto_lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
self.desire = DESIRES[self.lane_change_direction][
self.lane_change_state]
# Turn off lanes during lane change
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob
if self.use_lanelines:
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
else:
d_path_xyz = self.path_xyz
y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1],
np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1],
np.linalg.norm(self.path_xyz, axis=1),
self.plan_yaw)
self.y_pts = y_pts
assert len(y_pts) == MPC_N + 1
assert len(heading_pts) == MPC_N + 1
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
float(v_ego), CAR_ROTATION_RADIUS, list(y_pts),
list(heading_pts))
# init state for next
self.cur_state.x = 0.0
self.cur_state.y = 0.0
self.cur_state.psi = 0.0
self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1],
self.mpc_solution.curvature)
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = ntune_get('steerActuatorDelay') + .1
current_curvature = self.mpc_solution.curvature[0]
psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi)
next_curvature_rate = self.mpc_solution.curvature_rate[0]
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) *
delay) - current_curvature
next_curvature = current_curvature + 2 * curvature_diff_from_psi
self.desired_curvature = next_curvature
self.desired_curvature_rate = next_curvature_rate
max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS,
MAX_CURVATURE_RATES)
self.safe_desired_curvature_rate = clip(self.desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
self.safe_desired_curvature = clip(
self.desired_curvature,
self.safe_desired_curvature - max_curvature_rate / DT_MDL,
self.safe_desired_curvature + max_curvature_rate / DT_MDL)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
ntune_get('steerRateCost'))
self.cur_state.curvature = measured_curvature
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log, CS_arne182):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled)
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['plan'].hasLead
right_lane_visible = self.sm['pathPlan'].rProb > 0.5
left_lane_visible = self.sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED # and not self.active
meta = self.sm['model'].meta
if len(meta.desirePrediction) and ldw_allowed:
#l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
#r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < 0.8)
r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -0.8)
CC.hudControl.leftLaneDepart = bool(l_lane_close) # l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and
CC.hudControl.rightLaneDepart = bool(r_lane_close) # r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and
if (CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart) and (self.sm.frame - self.last_ldw_frame) * DT_CTRL > 5.0:
self.events.add(EventName.ldw)
self.last_ldw_frame = self.sm.frame
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric])
alertsArne182 = self.eventsArne182.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric])
self.last_model_long = self.arne_sm['modelLongButton'].enabled
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.AM.add_many(self.sm.frame, alertsArne182, self.enabled)
df_out = self.df_manager.update()
frame = self.sm.frame
if self.arne_sm['modelLongButton'].enabled != self.last_model_long:
extra_text_1 = 'disabled!' if self.last_model_long else 'enabled!'
self.AM.add_custom(frame, 'modelLongAlert', ET.WARNING, self.enabled, extra_text_1=extra_text_1)
return
if df_out.changed:
df_alert = 'dfButtonAlert'
if df_out.is_auto and df_out.last_is_auto:
# only show auto alert if engaged, not hiding auto, and time since lane speed alert not showing
if CS.cruiseState.enabled and not self.hide_auto_df_alerts:
df_alert += 'Silent'
self.AM.add_custom(frame, df_alert, ET.WARNING, self.enabled, extra_text_1=df_out.model_profile_text + ' (auto)')
return
else:
self.AM.add_custom(frame, df_alert, ET.WARNING, self.enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text))
return
if self.traffic_light_alerts:
traffic_status = self.arne_sm['trafficModelEvent'].status
traffic_confidence = round(self.arne_sm['trafficModelEvent'].confidence * 100, 2)
if traffic_confidence >= 75:
if traffic_status == 'SLOW':
self.AM.add_custom(self.sm.frame, 'trafficSlow', ET.WARNING, self.enabled, extra_text_2=' ({}%)'.format(traffic_confidence))
elif traffic_status == 'GREEN':
self.AM.add_custom(self.sm.frame, 'trafficGreen', ET.WARNING, self.enabled, extra_text_2=' ({}%)'.format(traffic_confidence))
elif traffic_status == 'DEAD': # confidence will be 100
self.AM.add_custom(self.sm.frame, 'trafficDead', ET.WARNING, self.enabled)
self.AM.process_alerts(self.sm.frame)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only:
# send car controls over can
can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.planMonoTime = self.sm.logMonoTime['plan']
controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.vEgo = CS.vEgo
controlsState.vEgoRaw = CS.vEgoRaw
controlsState.angleSteers = CS.steeringAngle
controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
controlsState.decelForTurn = self.sm['plan'].decelForTurn
controlsState.steerOverride = CS.steeringPressed
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.id)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
controlsState.vCurvature = self.sm['plan'].vCurvature
controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.mapValid = self.sm['plan'].mapValid
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def update(self, CS, lead):
v_ego = CS.vEgo
# Setup current mpc state
self.cur_state[0].x_ego = 0.0
if lead is not None and lead.status:
x_lead = lead.dRel
v_lead = max(0.0, lead.vLead)
a_lead = lead.aLeadK
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0
a_lead = 0.0
self.a_lead_tau = lead.aLeadTau
self.new_lead = False
if not self.prev_lead_status or abs(x_lead -
self.prev_lead_x) > 2.5:
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead,
a_lead, self.a_lead_tau)
self.new_lead = True
self.prev_lead_status = True
self.prev_lead_x = x_lead
self.cur_state[0].x_l = x_lead
self.cur_state[0].v_l = v_lead
else:
self.prev_lead_status = False
# Fake a fast lead car, so mpc keeps running
self.cur_state[0].x_l = 50.0
self.cur_state[0].v_l = v_ego + 10.0
a_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
# Calculate mpc
t = sec_since_boot()
# scc smoother
cruise_gap = int(clip(CS.cruiseGap, 1., 4.))
TR = interp(float(cruise_gap), [1., 2., 3., 4.], [1.3, 1.6, 1.9, 2.3])
self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
self.a_lead_tau, a_lead, TR)
self.duration = int((sec_since_boot() - t) * 1e9)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
self.a_mpc = self.mpc_solution[0].a_ego[1]
self.v_mpc_future = self.mpc_solution[0].v_ego[10]
# Reset if NaN or goes through lead car
crashing = any(lead - ego < -50 for (
lead,
ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
backwards = min(self.mpc_solution[0].v_ego) < -0.01
if ((backwards or crashing) and self.prev_lead_status) or nans:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning(
"Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s"
% (self.mpc_id, backwards, crashing, nans))
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0
self.v_mpc = v_ego
self.a_mpc = CS.aEgo
self.prev_lead_status = False
def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
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):
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** 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
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
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, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 // 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
else:
STEER_MAX = 0x1000
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx, CS.CP.isPandaBlack))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlack,
CS.stock_hud))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.stopped:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd,
visual_alert, left_lane, right_lane, left_lane_depart,
right_lane_depart, set_speed, lead_visible):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# SPAS limit angle extremes for safety
if CS.spas_enabled:
apply_steer_ang_req = clip(actuators.steerAngle,
-1 * (STEER_ANG_MAX), STEER_ANG_MAX)
# SPAS limit angle rate for safety
if abs(self.apply_steer_ang -
apply_steer_ang_req) > STEER_ANG_MAX_RATE:
if apply_steer_ang_req > self.apply_steer_ang:
self.apply_steer_ang += STEER_ANG_MAX_RATE
else:
self.apply_steer_ang -= STEER_ANG_MAX_RATE
else:
self.apply_steer_ang = apply_steer_ang_req
spas_active = CS.spas_enabled and enabled and (
self.spas_always or CS.out.vEgo < 7.0) # 25km/h
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(
CS.out.steeringAngle) < 90. and not spas_active
# fix for Genesis hard fault at low speed
if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
lkas_active = False
# Disable steering while turning blinker on and speed below 60 kph
if CS.out.leftBlinker or CS.out.rightBlinker:
self.turning_signal_timer = 0.5 / DT_CTRL # Disable for 0.5 Seconds after blinker turned off
if self.turning_indicator_alert: # set and clear by interface
lkas_active = 0
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
if not lkas_active:
apply_steer = 0
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
sys_warning, sys_state, left_lane_warning, right_lane_warning = \
process_hud_alert(lkas_active, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
if not (min_set_speed < set_speed < 255 * CV.KPH_TO_MS):
set_speed = min_set_speed
set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
self.scc12_cnt = CS.scc12[
"CR_VSM_Alive"] + 1 if not CS.no_radar else 0
# TODO: fix this
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
# check if SCC is alive
# if frame % 7 == 0:
# if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt:
# if frame - self.scc_update_frame > 20 and self.scc_live:
# self.scc_live = False
# else:
# self.scc_live = True
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
self.scc12_cnt %= 0xF
can_sends = []
can_sends.append(
create_lkas11(self.packer, frame, self.car_fingerprint,
apply_steer, lkas_active, CS.lkas11, sys_warning,
sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1
can_sends.append(
create_lkas11(self.packer, frame, self.car_fingerprint,
apply_steer, lkas_active, CS.lkas11, sys_warning,
sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 1))
if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame // 2 % 0x10, CS.mdps_bus,
CS.clu11, Buttons.NONE, enabled_speed))
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(
create_clu11(self.packer, frame % 0x10, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
# fix auto resume - by neokii
if CS.out.cruiseState.standstill:
if self.last_lead_distance == 0:
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
self.resume_wait_timer = 0
elif self.resume_wait_timer > 0:
self.resume_wait_timer -= 1
elif CS.lead_distance != self.last_lead_distance:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt >= 6:
self.resume_cnt = 0
self.resume_wait_timer = randint(8, 15)
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
# send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
if self.longcontrol and (CS.scc_bus
or not self.scc_live) and frame % 2 == 0:
can_sends.append(
create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt,
self.scc_live, CS.scc12))
can_sends.append(
create_scc11(self.packer, frame, enabled, set_speed,
lead_visible, self.scc_live, CS.scc11))
if CS.has_scc13 and frame % 20 == 0:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.has_scc14:
can_sends.append(create_scc14(self.packer, enabled, CS.scc14))
self.scc12_cnt += 1
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, lkas_active))
if CS.spas_enabled:
if CS.mdps_bus:
can_sends.append(
create_ems11(self.packer, CS.ems11, spas_active))
# SPAS11 50hz
if (frame % 2) == 0:
if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
self.en_spas = 7
self.en_cnt = 0
if self.en_spas == 7 and self.en_cnt >= 8:
self.en_spas = 3
self.en_cnt = 0
if self.en_cnt < 8 and spas_active:
self.en_spas = 4
elif self.en_cnt >= 8 and spas_active:
self.en_spas = 5
if not spas_active:
self.apply_steer_ang = CS.mdps11_strang
self.en_spas = 3
self.en_cnt = 0
self.mdps11_stat_last = CS.mdps11_stat
self.en_cnt += 1
can_sends.append(
create_spas11(self.packer, self.car_fingerprint,
(frame // 2), self.en_spas,
self.apply_steer_ang, CS.mdps_bus))
# SPAS12 20Hz
if (frame % 5) == 0:
can_sends.append(create_spas12(CS.mdps_bus))
return can_sends
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
# gas and brake
interceptor_gas_cmd = 0.
pcm_accel_cmd = actuators.accel
if CS.CP.enableGasInterceptor:
# handle hysteresis when around the minimum acc speed
if CS.out.vEgo < MIN_ACC_SPEED:
self.use_interceptor = True
elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
self.use_interceptor = False
if self.use_interceptor and active:
# only send negative accel when using interceptor. gas handles acceleration
# +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED],
[0.2, 0.5])
interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0.,
MAX_INTERCEPTOR_GAS)
pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
pcm_accel_cmd, self.accel_steady = accel_hysteresis(
pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [9, 25]:
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(
create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0
and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(
create_accel_command(self.packer, pcm_accel_cmd,
pcm_cancel_cmd, self.standstill_req,
lead, CS.acc_type))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, CS.acc_type))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, interceptor_gas_cmd,
frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui):
can_sends.append(
create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return 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
pedalcan = 2
if CS.useWithoutHarness:
pedalcan = 0
can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx, pedalcan))
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 data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, AM, driver_status,
LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
CC = car.CarControl.new_message()
if not passive:
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead
CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5)
CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
force_decel = driver_status.awareness < 0.
# live100
dat = messaging.new_message()
dat.init('live100')
dat.live100 = {
"alertText1": AM.alert_text_1,
"alertText2": AM.alert_text_2,
"alertSize": AM.alert_size,
"alertStatus": AM.alert_status,
"alertBlinkingRate": AM.alert_rate,
"alertType": AM.alert_type,
"alertSound": "", # no EON sounds yet
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn": bool(driver_status.monitor_on),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": plan_ts,
"pathPlanMonoTime": path_plan.logMonoTime,
"enabled": isEnabled(state),
"active": isActive(state),
"vEgo": CS.vEgo,
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
"curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
"longControlState": LoC.long_control_state,
"vPid": float(LoC.v_pid),
"vCruise": float(v_cruise_kph),
"upAccelCmd": float(LoC.pid.p),
"uiAccelCmd": float(LoC.pid.i),
"ufAccelCmd": float(LoC.pid.f),
"angleSteersDes": float(LaC.angle_steers_des),
"upSteer": float(LaC.pid.p),
"uiSteer": float(LaC.pid.i),
"ufSteer": float(LaC.pid.f),
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(plan.jerkFactor),
"angleOffset": float(angle_offset),
"gpsPlannerActive": plan.gpsPlannerActive,
"vCurvature": plan.vCurvature,
"decelForTurn": plan.decelForTurn,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": start_time,
"mapValid": plan.mapValid,
"forceDecel": bool(force_decel),
}
live100.send(dat.to_bytes())
# carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
params.put("ControlsParams", json.dumps({'angle_offset': angle_offset}))
return CC
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 thermald_thread():
# if limitting battery to Min-Max%. edit /data/bb_openpilot.cfg
car_set = CarSettings()
limitBatteryMinMax = car_set.get_value("limitBatteryMinMax")
batt_min = car_set.get_value("limitBattery_Min")
batt_max = car_set.get_value("limitBattery_Max")
setup_eon_fan()
# prevent LEECO from undervoltage
BATT_PERC_OFF = 10 if LEON else 3
# now loop
thermal_sock = messaging.pub_sock(service_list['thermal'].port)
health_sock = messaging.sub_sock(service_list['health'].port)
location_sock = messaging.sub_sock(service_list['gpsLocation'].port)
fan_speed = 0
count = 0
off_ts = None
started_ts = None
ignition_seen = False
started_seen = False
thermal_status = ThermalStatus.green
health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML) # 2x the expected health frequency
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
health_prev = None
# Make sure charging is enabled
charging_disabled = False
os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
params = Params()
while 1:
health = messaging.recv_sock(health_sock, wait=True)
location = messaging.recv_sock(location_sock)
location = location.gpsLocation if location else None
msg = read_thermal()
# clear car params when panda gets disconnected
if health is None and health_prev is not None:
params.panda_disconnect()
health_prev = health
# loggerd is gated based on free space
avail = get_available_percent() / 100.0
# thermal message now also includes free space
msg.thermal.freeSpace = avail
charger_off = False
with open("/sys/class/power_supply/battery/capacity") as f:
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/battery/current_now") as f:
msg.thermal.batteryCurrent = int(f.read())
with open("/sys/class/power_supply/battery/voltage_now") as f:
msg.thermal.batteryVoltage = int(f.read())
with open("/sys/class/power_supply/usb/present") as f:
msg.thermal.usbOnline = bool(int(f.read()))
with open("/sys/class/power_supply/battery/charge_type") as f:
charger_status = f.read().strip()
charger_off = (charger_status == "N/A")
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.)
bat_temp = msg.thermal.bat/1000.
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed)
msg.thermal.fanSpeed = fan_speed
# thermal logic with hysterisis
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
elif max_cpu_temp > 87.5:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow)
else:
# all good
thermal_status = ThermalStatus.green
# **** starting logic ****
# start constellation of processes when the car starts
ignition = health is not None and health.health.started
# print "Ignition from panda: ", ignition
ignition_seen = ignition_seen or ignition
# add voltage check for ignition
#if not ignition_seen and health is not None and health.health.voltage > 13500:
# ignition = True
do_uninstall = params.get("DoUninstall") == "1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get("CompletedTrainingVersion") == training_version
should_start = ignition
# have we seen a panda?
passive = (params.get("Passive") == "1")
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and msg.thermal.freeSpace > 0.02
# confirm we have completed training and aren't uninstalling
should_start = should_start and accepted_terms and (passive or completed_training) and (not do_uninstall)
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
if thermal_status >= ThermalStatus.danger:
# TODO: Add a better warning when this is happening
should_start = False
if should_start:
off_ts = None
if started_ts is None:
params.car_start()
started_ts = sec_since_boot()
started_seen = True
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
else:
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor')
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \
started_seen and (sec_since_boot() - off_ts) > 60:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled, msg, limitBatteryMinMax, batt_min, batt_max)
msg.thermal.chargingDisabled = charging_disabled
#BB added "and not charging_disabled" below so we don't show red LED when not charging
msg.thermal.chargingError = (current_filter.x > 0.) and (msg.thermal.batteryPercent < 90) and not charging_disabled # if current is > 1A out, then charger might be off
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9*(started_ts or 0))
msg.thermal.thermalStatus = thermal_status
thermal_sock.send(msg.to_bytes())
#print msg
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
location=(location.to_dict() if location else None),
thermal=msg.to_dict())
count += 1
