def update(self, CC, CS):
can_sends = []
actuators = CC.actuators
hud_control = CC.hudControl
main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
if CC.cruiseControl.cancel:
# cancel stock ACC
can_sends.append(fordcan.spam_cancel_button(self.packer))
# apply rate limits
new_steer = actuators.steeringAngleDeg
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
self.steer_rate_limited = new_steer != apply_steer
# send steering commands at 20Hz
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
lca_rq = 1 if CC.latActive else 0
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
path_angle = apply_steer
# convert steer angle to curvature
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
# TODO: get other actuators
curvature_rate = 0
path_offset = 0
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
precision = 0 # 0=Comfortable, 1=Precise
self.apply_steer_last = apply_steer
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
path_offset, path_angle, curvature_rate, curvature))
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
# send lkas ui command at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
# send acc ui command at 20Hz or if ui state changes
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
self.main_on_last = main_on
self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert
new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_steer
self.frame += 1
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
can_sends = []
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
apply_steer = actuators.steer
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 * CV.DEG_TO_RAD, 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 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))
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, 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 update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
can_sends = []
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
apply_steer = actuators.steer
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.v_ego)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
self.lkas_action += int(CS.generic_toggle
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.angle_steers,
curvature, self.lkas_action))
self.generic_toggle_last = CS.generic_toggle
if (frame % 100) == 0:
can_sends.append(
make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0,
False))
#can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False))
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
(self.steer_alert_last != steer_alert):
can_sends.append(
create_lkas_ui(self.packer, CS.main_on, enabled,
steer_alert))
if (frame % 200) == 0:
can_sends.append(
make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1,
False))
if (frame % 10) == 0:
can_sends.append(
make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1,
False))
can_sends.append(
make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1,
False))
can_sends.append(
make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1,
False))
can_sends.append(
make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1,
False))
can_sends.append(
make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1,
False))
can_sends.append(
make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1,
False))
can_sends.append(
make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1,
False))
can_sends.append(
make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1,
False))
can_sends.append(
make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1,
False))
can_sends.append(
make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1,
False))
can_sends.append(
make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1,
False))
can_sends.append(
make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1,
False))
static_msgs = range(1653, 1658)
for addr in static_msgs:
cnt = (frame % 10) + 1
can_sends.append(
make_can_msg(
addr,
chr(cnt << 4) + '\x00\x00\x00\x00\x00\x00\x00', 1,
False))
self.enabled_last = enabled
self.main_on_last = CS.main_on
self.steer_alert_last = steer_alert
return can_sends
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
