def create_wheel_buttons(frame):
# WHEEL_BUTTONS (762) Message sent to cancel ACC.
start = b"\x01" # acc cancel set
counter = (frame % 10) << 4
dat = start + counter.to_bytes(1, 'little') + b"\x00"
dat = dat[:-1] + calc_checksum(dat).to_bytes(1, 'little')
return make_can_msg(0x2fa, dat, 0)
def make_tester_present_msg(addr, bus, subaddr=None):
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
if subaddr is not None:
dat.insert(0, subaddr)
dat.extend([0x0] * (8 - len(dat)))
return make_can_msg(addr, bytes(dat), bus)
def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count,
lkas_car_model):
# LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed.
if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
msg = b'\x00\x00\x00\x03\x00\x00\x00\x00'
return make_can_msg(0x2a6, msg, 0)
color = 1 # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019
lines = 1
alerts = 0
if hud_count < (1 * 4): # first 3 seconds, 4Hz
alerts = 1
# CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID
# had color = 1 and lines = 1 but trying 2017 hybrid style for now.
if gear in (GearShifter.drive, GearShifter.reverse, GearShifter.low):
if lkas_active:
color = 2 # control active, display green.
lines = 6
else:
color = 1 # control off, display white.
lines = 1
values = {
"LKAS_ICON_COLOR": color, # byte 0, last 2 bits
"CAR_MODEL": lkas_car_model, # byte 1
"LKAS_LANE_LINES": lines, # byte 2, last 4 bits
"LKAS_ALERTS": alerts, # byte 3, last 4 bits
}
return packer.make_can_msg("LKAS_HUD", 0, values) # 0x2a6
def create_adas_time_status(bus, tt, idx):
dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff,
((tt & 0xf) << 4) + (idx << 2)]
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3]
chksum = chksum & 0xfff
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12]
return make_can_msg(0xa1, bytes(dat), bus)
def test_boardd_loopback():
# wait for boardd to init
spinner = Spinner()
time.sleep(2)
with Timeout(60, "boardd didn't start"):
sm = messaging.SubMaster(['pandaState'])
while sm.rcv_frame['pandaState'] < 1:
sm.update(1000)
# boardd blocks on CarVin and CarParams
cp = car.CarParams.new_message()
cp.safetyModel = car.CarParams.SafetyModel.allOutput
Params().put("CarVin", b"0" * 17)
Params().put_bool("ControlsReady", True)
Params().put("CarParams", cp.to_bytes())
sendcan = messaging.pub_sock('sendcan')
can = messaging.sub_sock('can', conflate=False, timeout=100)
time.sleep(1)
n = 1000
for i in range(n):
spinner.update(f"boardd loopback {i}/{n}")
sent_msgs = defaultdict(set)
for _ in range(random.randrange(10)):
to_send = []
for __ in range(random.randrange(100)):
bus = random.randrange(3)
addr = random.randrange(1, 1 << 29)
dat = bytes([
random.getrandbits(8)
for _ in range(random.randrange(1, 9))
])
sent_msgs[bus].add((addr, dat))
to_send.append(make_can_msg(addr, dat, bus))
sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
max_recv = 10
while max_recv > 0 and any(len(sent_msgs[bus]) for bus in range(3)):
recvd = messaging.drain_sock(can, wait_for_one=True)
for msg in recvd:
for m in msg.can:
if m.src >= 128:
k = (m.address, m.dat)
assert k in sent_msgs[m.src - 128]
sent_msgs[m.src - 128].discard(k)
max_recv -= 1
# if a set isn't empty, messages got dropped
for bus in range(3):
assert not len(
sent_msgs[bus]
), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages"
spinner.close()
def create_adas_accelerometer_speed_status(bus, speed_ms, idx): spd = int(speed_ms * 16) & 0xfff accel = 0 & 0xfff # 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L #stick = 0x08 near_range_cutoff = 0x27 near_range_mode = 1 if spd <= near_range_cutoff else 0 far_range_mode = 1 - near_range_mode dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] return make_can_msg(0x308, bytes(dat), bus)
def create_lka_icon_command(bus, active, critical, steer):
if active and steer == 1:
if critical:
dat = b"\x50\xc0\x14"
else:
dat = b"\x50\x40\x18"
elif active:
if critical:
dat = b"\x40\xc0\x14"
else:
dat = b"\x40\x40\x18"
else:
dat = b"\x00\x00\x00"
return make_can_msg(0x104c006c, dat, bus)
def create_lkas_command(packer, lkas_run, frame, apply_steer): #,
counter = frame % 0x10
values = {
"ALLFFFF": 0xffff,
"A1": 1,
"HIGH_TORQ": 0,
"ALL11": 3,
"COUNTER": counter,
"STEER_TORQ": apply_steer,
"LKAS_GREEN": 1
}
# cs cnt3 torq
# 0 1 2 3 4
dat = [0xeb, 0xff, 0xff, 0xe4, 0x00, 0x70, 0x00, 0x00]
torq, dat[0] = find_steer_torq(counter, apply_steer)
dat[3] = (((counter << 3) | ((torq & 0x700) >> 8)) | 0x80)
dat[4] = torq & 0xFF
candat = binascii.hexlify(bytearray(dat))
#return packer.make_can_msg("LKAS_RUN", 0, dat)
return make_can_msg(0x28F, codecs.decode(candat, 'hex'), 0)
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
can_sends = []
steer_alert = visual_alert in (VisualAlert.steerRequired,
VisualAlert.ldw)
apply_steer = actuators.steer
if pcm_cancel:
#print "CANCELING!!!!"
can_sends.append(spam_cancel_button(self.packer))
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(
math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
self.lkas_action += int(CS.out.genericToggle
and not self.generic_toggle_last)
self.lkas_action &= 0xf
else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(
create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.out.steeringAngleDeg,
curvature, self.lkas_action))
self.generic_toggle_last = CS.out.genericToggle
if (frame % 100) == 0:
can_sends.append(
make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
(self.steer_alert_last != steer_alert):
can_sends.append(
create_lkas_ui(self.packer, CS.out.cruiseState.available,
enabled, steer_alert))
if (frame % 200) == 0:
can_sends.append(
make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
if (frame % 10) == 0:
can_sends.append(
make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
can_sends.append(
make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
can_sends.append(
make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
can_sends.append(
make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
can_sends.append(
make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
can_sends.append(
make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
can_sends.append(
make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
can_sends.append(
make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
can_sends.append(
make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
static_msgs = range(1653, 1658)
for addr in static_msgs:
cnt = (frame % 10) + 1
can_sends.append(
make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') +
b'\x00\x00\x00\x00\x00\x00\x00', 1))
self.enabled_last = enabled
self.main_on_last = CS.out.cruiseState.available
self.steer_alert_last = steer_alert
return actuators, can_sends
def create_adas_headlights_status(bus):
return make_can_msg(0x310, b"\x42\x04", bus)
def create_adas_steering_status(bus, idx):
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
chksum = 0x60 + sum(dat)
dat += [chksum >> 8, chksum & 0xff]
return make_can_msg(0x306, bytes(dat), bus)
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel,
dragonconf):
can_sends = []
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
apply_steer = actuators.steer
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf, blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
if self.enable_camera:
if pcm_cancel:
#print "CANCELING!!!!"
can_sends.append(spam_cancel_button(self.packer))
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(
actuators.steerAngle * 3.1415 / 180., CS.out.vEgo)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
self.lkas_action += int(CS.out.genericToggle
and not self.generic_toggle_last)
self.lkas_action &= 0xf
else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(
create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.out.steeringAngle,
curvature, self.lkas_action))
self.generic_toggle_last = CS.out.genericToggle
if (frame % 100) == 0:
can_sends.append(
make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
(self.steer_alert_last != steer_alert):
can_sends.append(
create_lkas_ui(self.packer, CS.out.cruiseState.available,
enabled, steer_alert))
if (frame % 200) == 0:
can_sends.append(
make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
if (frame % 10) == 0:
can_sends.append(
make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
can_sends.append(
make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
can_sends.append(
make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
can_sends.append(
make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
can_sends.append(
make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
can_sends.append(
make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
can_sends.append(
make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
can_sends.append(
make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
can_sends.append(
make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
static_msgs = range(1653, 1658)
for addr in static_msgs:
cnt = (frame % 10) + 1
can_sends.append(
make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') +
b'\x00\x00\x00\x00\x00\x00\x00', 1))
self.enabled_last = enabled
self.main_on_last = CS.out.cruiseState.available
self.steer_alert_last = steer_alert
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
self.sm.update(0)
if self.sm.updated['radarState']:
self.lead_v = self.sm['radarState'].leadOne.vRel
self.lead_a = self.sm['radarState'].leadOne.aRel
self.lead_d = self.sm['radarState'].leadOne.dRel
if self.sm.updated['controlsState']:
self.LCS = self.sm['controlsState'].longControlState
if frame % 500 == 0: # *** 5 sec interval? ***
print(self.LCS)
# gas and brake
# apply_gas = 0.
# apply_accel = actuators.gas - actuators.brake
# if CS.CP.enableGasInterceptor and enabled and CS.out.vEgo < MIN_ACC_SPEED:
# apply_gas = clip(actuators.gas, 0., 1.)
# # converts desired acceleration to gas percentage for pedal
# # +0.06 offset to reduce ABS pump usage when applying very small gas
# if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo):
# apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.)
# apply_accel += 0.06
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 # Original
if lead:
apply_accel = 0.06 - actuators.brake
#apply_accel = 0.06
else:
apply_accel = 0.06
# End new
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 * CarControllerParams.ACCEL_SCALE,
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 abs(CS.out.steeringRateDeg) > 100:
#if not enabled or CS.steer_state in [9, 25] or CS.out.epsDisabled==1 or abs(CS.out.steeringRateDeg) > 100: #Original statement
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
#if not enabled and CS.pcm_acc_status: # Original
if not 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 = 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 and not self.standstill_hack:
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 = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
if (frame % 2 == 0):
can_sends.append(
create_lead_command(self.packer, self.lead_v, self.lead_a,
self.lead_d))
#*** 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.fwdCamera in self.fake_ecus:
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 and Ecu.fwdCamera in self.fake_ecus):
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, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
#can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) # Original value
can_sends.append(
create_accel_command(self.packer, 500, 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))
# 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 == VisualAlert.steerRequired
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) and Ecu.fwdCamera in self.fake_ecus:
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 Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** 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 CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, 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
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
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# 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.fwdCamera 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))
# 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 and Ecu.fwdCamera 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
# 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, 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))
# 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)
steer, fcw = 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
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart))
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:
can_sends.append(make_can_msg(addr, vl, 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):
# gas and brake
if CS.CP.enableGasInterceptor and active:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif CS.CP.carFingerprint in (CAR.COROLLA,):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, 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
# 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_standstill = CS.out.standstill
can_sends = []
if CS.CP.steerControlType == car.CarParams.SteerControlType.angle:
can_sends.append(create_steer_command(self.packer, 0, 0, frame))
# On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
# at 0 degrees, so we need to offset the commanded angle as well.
# Also need to pulse steer_req to prevent 5 second steering lockout in some cases
steer_req = active # TODO(thinh): Check and cut out steering while we are in a known fault state
percentage = interp(abs(CS.out.steeringTorque), [50, 100], [100, 0])
commanded_angle = interp(percentage, [-10, 100], [CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg])
can_sends.append(create_lta_steer_command(self.packer, commanded_angle, steer_req, frame))
else:
# Cut steering while we're in a known fault state (2s)
if not active or CS.steer_state in (9, 25):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# 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))
# 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 so ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
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, CS.distance_btn))
self.accel = pcm_accel_cmd
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance_btn))
if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
# 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_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
self.gas = interceptor_gas_cmd
# ui mesg is at 1Hz 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, enabled))
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))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.accel = self.accel
new_actuators.gas = self.gas
return new_actuators, can_sends
def update(self, enabled, 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.gas - actuators.brake
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 enabled:
# only send negative accel when using interceptor. gas handles acceleration
# +0.06 offset to reduce ABS pump usage when OP is engaged
interceptor_gas_cmd = clip(actuators.gas, 0., 1.)
pcm_accel_cmd = 0.06 - actuators.brake
pcm_accel_cmd, self.accel_steady = accel_hysteresis(
pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE,
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
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.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 create_adas_keepalive(bus):
dat = b"\x00\x00\x00\x00\x00\x00\x00"
return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)]
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart, dragonconf):
# *** 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
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
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] or abs(CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
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 not dragonconf.dpToyotaSng and 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
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(enabled,
dragonconf,
blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
self.last_steer = apply_steer
self.last_accel = apply_accel
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
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# 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.steerAngle, 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 and Ecu.fwdCamera in self.fake_ecus):
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 not dragonconf.dpAtl:
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, 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))
# 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 == VisualAlert.steerRequired
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
# dp
if dragonconf.dpToyotaLdw:
dragon_left_lane_depart = left_lane_depart
dragon_right_lane_depart = right_lane_depart
else:
dragon_left_lane_depart = False
dragon_right_lane_depart = False
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** 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 CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart, dragonconf):
# *** 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 enabled:
# 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
] or abs(CS.out.steeringRateDeg) > 100 or (
abs(CS.out.steeringAngleDeg) > 150
and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
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 not dragonconf.dpToyotaSng and 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
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf, blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
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))
if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled:
pcm_accel_cmd = 0.
if CS.CP.enableGasInterceptor:
interceptor_gas_cmd = 0.
# 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
if dragonconf.dpAtl and not dragonconf.dpAtlOpLong:
pass
# Lexus IS uses a different cancellation message
elif 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, CS.distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, CS.acc_type, CS.distance))
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
# dp
if not dragonconf.dpToyotaLdw:
left_lane_depart = False
right_lane_depart = False
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, enabled))
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 test_loopback(self):
# wait for boardd to init
time.sleep(2)
with Timeout(60, "boardd didn't start"):
sm = messaging.SubMaster(['pandaStates'])
while sm.rcv_frame['pandaStates'] < 1 and len(
sm['pandaStates']) == 0:
sm.update(1000)
num_pandas = len(sm['pandaStates'])
if TICI:
self.assertGreater(num_pandas, 1,
"connect another panda for multipanda tests")
# boardd blocks on CarVin and CarParams
cp = car.CarParams.new_message()
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.allOutput
cp.safetyConfigs = [safety_config] * num_pandas
params = Params()
params.put("CarVin", b"0" * 17)
params.put_bool("ControlsReady", True)
params.put("CarParams", cp.to_bytes())
sendcan = messaging.pub_sock('sendcan')
can = messaging.sub_sock('can', conflate=False, timeout=100)
time.sleep(0.2)
n = 200
for i in range(n):
self.spinner.update(f"boardd loopback {i}/{n}")
sent_msgs = defaultdict(set)
for _ in range(random.randrange(10)):
to_send = []
for __ in range(random.randrange(100)):
bus = random.choice(
[b for b in range(3 * num_pandas) if b % 4 != 3])
addr = random.randrange(1, 1 << 29)
dat = bytes([
random.getrandbits(8)
for _ in range(random.randrange(1, 9))
])
sent_msgs[bus].add((addr, dat))
to_send.append(make_can_msg(addr, dat, bus))
sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
for _ in range(100 * 2):
recvd = messaging.drain_sock(can, wait_for_one=True)
for msg in recvd:
for m in msg.can:
if m.src >= 128:
key = (m.address, m.dat)
assert key in sent_msgs[
m.src -
128], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}"
sent_msgs[m.src - 128].discard(key)
if all(len(v) == 0 for v in sent_msgs.values()):
break
# if a set isn't empty, messages got dropped
for bus in sent_msgs.keys():
assert not len(
sent_msgs[bus]
), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages"
def set_blindspot_debug_mode(lr, enable):
if enable:
m = lr + b'\x02\x10\x60\x00\x00\x00\x00'
else:
m = lr + b'\x02\x10\x01\x00\x00\x00\x00'
return make_can_msg(0x750, m, 0)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True
self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
# *** 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
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# 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
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 not self.dragon_toyota_sng_mod and CS.out.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_accel = apply_accel
self.last_standstill = CS.out.standstill
# dp
apply_steer_req = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer_req)
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.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# 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 and Ecu.fwdCamera in self.fake_ecus):
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 in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
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))
# 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 == VisualAlert.steerRequired
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
# dp
if self.dragon_lane_departure_warning:
dragon_left_lane_depart = left_lane_depart
dragon_right_lane_depart = right_lane_depart
else:
dragon_left_lane_depart = False
dragon_right_lane_depart = False
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** 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 CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def poll_blindspot_status(lr):
m = lr + b'\x02\x21\x69\x00\x00\x00\x00'
return make_can_msg(0x750, m, 0)
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
if CS.CP.enableGasInterceptor and enabled:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif CS.CP.carFingerprint in [CAR.COROLLA]:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, 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_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 in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
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_interceptor_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, enabled))
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(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
#if not enabled:
# self.sm.update(0)
#if self.sm.updated['pathPlan']:
# blinker = CS.out.leftBlinker or CS.out.rightBlinker
# ldw_allowed = CS.out.vEgo > 12.5 and not blinker
# CAMERA_OFFSET = op_params.get('camera_offset', 0.06)
# right_lane_visible = self.sm['pathPlan'].rProb > 0.5
# left_lane_visible = self.sm['pathPlan'].lProb > 0.5
# self.rightLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].rPoly[3] > -(0.93 + CAMERA_OFFSET) and right_lane_visible)
# self.leftLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].lPoly[3] < (0.93 - CAMERA_OFFSET) and left_lane_visible)
# print("blinker")
# print(blinker)
# print("ldw_allowed")
# print(ldw_allowed)
# print("CAMERA_OFFSET")
# print(CAMERA_OFFSET)
# print("right_lane_visible")
# print(right_lane_visible)
# print("left_lane_visible")
# print(left_lane_visible)
# print("self.rightLaneDepart")
# print(self.rightLaneDepart)
# print("self.leftLaneDepart")
# print(self.leftLaneDepart)
# *** 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)
if CS.CP.enableGasInterceptor:
if CS.out.gasPressed:
apply_accel = max(apply_accel, 0.06)
if CS.out.brakePressed:
apply_gas = 0.0
apply_accel = min(apply_accel, 0.00)
else:
if CS.out.gasPressed:
apply_accel = max(apply_accel, 0.0)
if CS.out.brakePressed and CS.out.vEgo > 1:
apply_accel = min(apply_accel, 0.0)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
# 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 1s after fault
if (frame - self.last_fault_frame < 100) or abs(
CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 100
and CS.CP.carFingerprint
in [CAR.RAV4H, CAR.PRIUS]):
new_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and right_lane_depart and CS.out.vEgo > 12.5 and not CS.out.rightBlinker:
new_steer = self.last_steer + 3
new_steer = min(new_steer, 800)
print("right")
print(new_steer)
apply_steer_req = 1
if not enabled and left_lane_depart and CS.out.vEgo > 12.5 and not CS.out.leftBlinker:
new_steer = self.last_steer - 3
new_steer = max(new_steer, -800)
print("left")
print(new_steer)
apply_steer_req = 1
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
if not enabled and abs(apply_steer) > 800 and not (right_lane_depart or
left_lane_depart):
apply_steer = 0
apply_steer_req = 0
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.out.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_accel = apply_accel
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
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
# 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 and Ecu.fwdCamera in self.fake_ecus):
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, 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))
# 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 == VisualAlert.steerRequired
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) and Ecu.fwdCamera in self.fake_ecus:
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 (Ecu.dsu in self.fake_ecus
or Ecu.unknown in self.fake_ecus):
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** 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 CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
# Enable blindspot debug mode once
if frame > 1000 and not (
CS.CP.carFingerprint in TSS2_CAR or CS.CP.carFingerprint
== CAR.CAMRY): # 10 seconds after start and not a tss2 car
if BLINDSPOTALWAYSON:
self.blindspot_blink_counter_left += 1
self.blindspot_blink_counter_right += 1
#print("debug blindspot alwayson!")
elif CS.out.leftBlinker:
self.blindspot_blink_counter_left += 1
#print("debug Left Blinker on")
elif CS.out.rightBlinker:
self.blindspot_blink_counter_right += 1
#print("debug Right Blinker on")
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x01\x00\x00\x00\x00', 0))
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))
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
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))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x60\x00\x00\x00\x00', 0))
#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.out.vEgo > 6: #polling at low speeds switches camera off
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x60\x00\x00\x00\x00', 0))
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print("debug Right blindspot debug enabled")
self.blindspot_debug_enabled_right = True
if CS.out.vEgo < 6 and self.blindspot_debug_enabled_right: # if enabled and speed falls below 6m/s
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
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_debug_enabled_left:
if frame % 20 == 0 and frame > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x21\x69\x00\x00\x00\x00', 0))
#print("debug Left blindspot poll")
if self.blindspot_debug_enabled_right:
if frame % 20 == 10 and frame > 1005: # Poll blindspots at 5 Hz
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x21\x69\x00\x00\x00\x00', 0))
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#print("debug Right blindspot poll")
return can_sends
