def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd,
hud_alert):
if not self.enable_camera:
return
### Steering Torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_driver,
SteerLimitParams)
if not enabled:
apply_steer = 0
steer_req = 1 if enabled else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
if self.camera_disconnected:
if (self.cnt % 10) == 0:
can_sends.append(create_lkas12())
if (self.cnt % 50) == 0:
can_sends.append(create_1191())
if (self.cnt % 7) == 0:
can_sends.append(create_1156())
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
apply_steer,
steer_req,
self.lkas11_cnt,
enabled,
CS.lkas11,
hud_alert,
keep_stock=(not self.camera_disconnected)))
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
self.last_resume_cnt = self.cnt
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
### Send messages to canbus
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
self.cnt += 1
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
### Steering Torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_driver,
SteerLimitParams)
if not enabled:
apply_steer = 0
steer_req = 1 if enabled else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
self.mdps12_cnt = self.cnt % 0x100
if self.camera_disconnected:
if (self.cnt % 10) == 0:
can_sends.append(create_lkas12())
if (self.cnt % 50) == 0:
can_sends.append(create_1191())
if (self.cnt % 7) == 0:
can_sends.append(create_1156())
else:
can_sends.append(
create_mdps12(self.packer, self.car_fingerprint,
self.mdps12_cnt, CS.mdps12, CS.lkas11))
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
apply_steer,
steer_req,
self.lkas11_cnt,
enabled,
CS.lkas11,
hud_alert,
keep_stock=(not self.camera_disconnected)))
#if pcm_cancel_cmd:
#can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
if CS.stopped and (self.cnt - self.last_resume_cnt) > 20:
if (self.cnt - self.last_resume_cnt) % 5 == 0:
self.last_resume_cnt = self.cnt
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL,
self.clu11_cnt))
self.cnt += 1
return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
### Error State Resets ###
disable_steer = False
can_sends = []
### Learn Checksum ###
if not self.checksum_found:
# Learn Checksum from the Camera
if self.checksum == "NONE":
self.checksum = learn_checksum(self.packer, CS.lkas11)
if self.checksum == "NONE" and self.checksum_learn_cnt < 50:
self.checksum_learn_cnt += 1
return
else:
cloudlog.info("Discovered Checksum %s" % self.checksum)
self.checksum_found = True
# If MDPS is faulted from bad checksum, then cycle through all Checksums until 1 works
if CS.steer_error == 1:
self.camera_disconnected = True
cloudlog.warning("Camera Not Detected: Brute Forcing Checksums")
if self.checksum_learn_cnt > 300:
self.checksum_learn_cnt = 50
if self.checksum == "NONE":
cloudlog.info("Testing 6B Checksum")
self.checksum = "6B"
elif self.checksum == "6B":
cloudlog.info("Testing 7B Checksum")
self.checksum = "7B"
elif self.checksum == "7B":
cloudlog.info("Testing CRC8 Checksum")
self.checksum = "crc8"
else:
self.checksum = "NONE"
return
else:
self.checksum_learn_cnt += 1
else:
cloudlog.info("Discovered Checksum %s" % self.checksum)
self.checksum_found = True
### Minimum Steer Speed ###
# Apply Usage of Minimum Steer Speed
if CS.low_speed_alert:
disable_steer = True
### Turning Indicators ###
if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1):
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
if self.turning_signal_timer > 0:
disable_steer = True
self.turning_signal_timer -= 1
### Steering Torque ###
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams)
if not enabled or disable_steer:
apply_steer = 0
steer_req = 0
else:
steer_req = 1
self.apply_steer_last = apply_steer
'''
### Auto Speed Limit ###
# Read Speed Limit and define if adjustment needed
if (self.cnt % 50) == 0 and self.speed_enable:
if not (enabled and CS.acc_active):
self.speed_adjusted = False
map_data = messaging.recv_one_or_none(self.map_data_sock)
if map_data is not None:
if bool(self.params.get("IsMetric")):
self.speed_conv = CV.MS_TO_KPH
else:
self.speed_conv = CV.MS_TO_MPH
if map_data.liveMapData.speedLimitValid:
last_speed = self.map_speed
v_speed = int(map_data.liveMapData.speedLimit * self.speed_offset)
self.map_speed = v_speed * self.speed_conv
if last_speed != self.map_speed:
self.speed_adjusted = False
else:
self.map_speed = 0
self.speed_adjusted = True
else:
self.map_speed = 0
self.speed_adjusted = True
# Spam buttons for Speed Adjustment
if CS.acc_active and not self.speed_adjusted and self.map_speed > (8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1):
if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005):
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0)))
elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005):
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0)))
else:
self.speed_adjusted = True
# Cancel Adjustment on Pedal
if CS.pedal_gas:
self.speed_adjusted = True
'''
### Generate CAN Messages ###
self.lkas11_cnt = self.cnt % 0x10
# self.clu11_cnt = self.cnt % 0x10
self.mdps12_cnt = self.cnt % 0x100
if self.camera_disconnected:
if (self.cnt % 10) == 0:
can_sends.append(create_lkas12())
if (self.cnt % 50) == 0:
can_sends.append(create_1191())
if (self.cnt % 7) == 0:
can_sends.append(create_1156())
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
enabled, CS.lkas11, hud_alert, (not self.camera_disconnected), self.checksum))
if not self.camera_disconnected:
can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \
self.checksum))
# if pcm_cancel_cmd:
# can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0))
if CS.stopped and (self.cnt - self.last_resume_cnt) > 20:
if (self.cnt - self.last_resume_cnt) > 20:
self.last_resume_cnt = self.cnt
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, self.clu11_cnt))
self.cnt += 1
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
left_line, right_line, left_lane_depart, right_lane_depart):
if CS.left_blinker_on or CS.right_blinker_on:
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
if self.turning_signal_timer:
enabled = 0
### Steering Torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams)
if not enabled:
apply_steer = 0
steer_req = 1 if enabled else 0
self.apply_steer_last = apply_steer
hud_alert, lane_visible, left_lane_warning, right_lane_warning =\
process_hud_alert(enabled, self.car_fingerprint, visual_alert,
left_line, right_line,left_lane_depart, right_lane_depart)
can_sends = []
self.lkas11_cnt = frame % 0x10
if self.camera_disconnected:
if (frame % 10) == 0:
can_sends.append(create_lkas12())
if (frame % 50) == 0:
can_sends.append(create_1191())
if (frame % 7) == 0:
can_sends.append(create_1156())
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
enabled, CS.lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart,
keep_stock=(not self.camera_disconnected)))
#if pcm_cancel_cmd:
#self.clu11_cnt = frame % 0x10
#can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL, self.clu11_cnt))
if CS.stopped:
# run only first time when the car stops
if self.last_lead_distance == 0:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.clu11_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance > self.last_lead_distance and (frame - self.last_resume_frame) > 5:
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, self.clu11_cnt))
self.clu11_cnt += 1
# interval after 6 msgs
if self.clu11_cnt > 5:
self.last_resume_frame = frame
self.clu11_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
return can_sends