def test_hud(self):
packer = CANPacker('chrysler_pacifica_2017_hybrid')
self.assertEqual(
[0x2a6, 0, '0100010100000000'.decode('hex'), 0],
chryslercan.create_lkas_hud(
packer,
'park', False, False, 1, 0))
self.assertEqual(
[0x2a6, 0, '0100010000000000'.decode('hex'), 0],
chryslercan.create_lkas_hud(
packer,
'park', False, False, 5*4, 0))
self.assertEqual(
[0x2a6, 0, '0100010000000000'.decode('hex'), 0],
chryslercan.create_lkas_hud(
packer,
'park', False, False, 99999, 0))
self.assertEqual(
[0x2a6, 0, '0200060000000000'.decode('hex'), 0],
chryslercan.create_lkas_hud(
packer,
'drive', True, False, 99999, 0))
self.assertEqual(
[0x2a6, 0, '0264060000000000'.decode('hex'), 0],
chryslercan.create_lkas_hud(
packer,
'drive', True, False, 99999, 0x64))
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3]
lkas_active = (random.randint(0, 2) % 2 == 0)
hud_alert = random.randint(0, 6)
hud_count = random.randint(0, 65536)
lkas_car_model = random.randint(0, 65536)
m_old = chryslercan.create_lkas_hud(self.chrysler_cp_old, gear,
lkas_active, hud_alert,
hud_count, lkas_car_model)
m = chryslercan.create_lkas_hud(self.chrysler_cp, gear,
lkas_active, hud_alert, hud_count,
lkas_car_model)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
moving_fast = (random.randint(0, 2) % 2 == 0)
frame = random.randint(0, 65536)
m_old = chryslercan.create_lkas_command(self.chrysler_cp_old,
apply_steer, moving_fast,
frame)
m = chryslercan.create_lkas_command(self.chrysler_cp, apply_steer,
moving_fast, frame)
self.assertEqual(m_old, m)
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_motor,
SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# frame is 100Hz (0.01s period)
if (self.ccframe % 10 == 0): # 0.1s period
new_msg = create_lkas_heartbit(self.car_fingerprint)
can_sends.append(new_msg)
if (self.ccframe % 25 == 0): # 0.25s period
new_msg = create_lkas_hud(self.packer, CS.gear_shifter,
lkas_active, hud_alert,
self.car_fingerprint, self.hud_count)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer), frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return car.CarControl.Actuators.new_message(), []
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.PACIFICA_2020,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends = []
#*** control msgs ***
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.packer,
self.ccframe,
cancel=True)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.apply_steer_last,
CS.steer_torque_motor, SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(
self.packer, CS.gear_shifter, lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, CC, CS, low_speed_alert):
can_sends = []
# EPS faults if LKAS re-enables too quickly
lkas_active = CC.latActive and not low_speed_alert and (
self.frame - self.last_lkas_falling_edge > 200)
# *** control msgs ***
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
if CC.cruiseControl.cancel:
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
cancel=True))
elif CC.enabled and CS.out.cruiseState.standstill:
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
resume=True))
# HUD alerts
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(
create_lkas_hud(self.packer, self.CP, lkas_active,
CC.hudControl.visualAlert, self.hud_count,
CS.lkas_car_model, CS.auto_high_beam))
self.hud_count += 1
# steering
if self.frame % 2 == 0:
# steer torque
new_steer = int(
round(CC.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
if not lkas_active:
apply_steer = 0
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
idx = self.frame // 2
can_sends.append(
create_lkas_command(self.packer, self.CP, int(apply_steer),
lkas_active, idx))
self.frame += 1
if not lkas_active and self.lkas_active_prev:
self.last_lkas_falling_edge = self.frame
self.lkas_active_prev = lkas_active
new_actuators = CC.actuators.copy()
new_actuators.steer = self.apply_steer_last / CarControllerParams.STEER_MAX
return new_actuators, can_sends
def test_hud(self):
packer = CANPacker('chrysler_pacifica_2017_hybrid')
self.assertEqual([0x2a6, 0, b'\x01\x00\x01\x01\x00\x00\x00\x00', 0],
chryslercan.create_lkas_hud(packer, GearShifter.park,
False, False, 1, 0))
self.assertEqual([0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0],
chryslercan.create_lkas_hud(packer, GearShifter.park,
False, False, 5 * 4, 0))
self.assertEqual([0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0],
chryslercan.create_lkas_hud(packer, GearShifter.park,
False, False, 99999, 0))
self.assertEqual([0x2a6, 0, b'\x02\x00\x06\x00\x00\x00\x00\x00', 0],
chryslercan.create_lkas_hud(packer, GearShifter.drive,
True, False, 99999, 0))
self.assertEqual([0x2a6, 0, b'\x02\x64\x06\x00\x00\x00\x00\x00', 0],
chryslercan.create_lkas_hud(packer, GearShifter.drive,
True, False, 99999, 0x64))
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
leftLaneVisible, rightLaneVisible): # TODO hud_alert
P = CarControllerParams
steer_ready = CS.out.vEgo > CS.CP.minSteerSpeed
if steer_ready:
self.steer_command_bit = True
if not steer_ready:
self.steer_command_bit = False
bad_to_bone = enabled and steer_ready
if bad_to_bone:
new_steer = int(round(actuators.steer * P.STEER_MAX))
else:
new_steer = 0
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
# *** control msgs ***
can_sends = []
# *** control msgs ***
# if pcm_cancel_cmd: # TODO: ENABLE ONCE STEERING WORKS
# new_msg = create_wheel_buttons(self.packer, self.frame, cancel=True)
# can_sends.append(new_msg)
counter = (frame / 2) % 16
if frame % 2 == 0:
can_sends.append(
create_lkas_command(self.packer, int(apply_steer), counter,
self.steer_command_bit))
if frame % 5 == 0:
can_sends.append(
create_lkas_hud(self.packer, enabled, leftLaneVisible,
rightLaneVisible))
return can_sends
def update(self, CC, CS):
can_sends = []
lkas_active = CC.latActive and self.lkas_control_bit_prev
# cruise buttons
if (self.frame - self.last_button_frame) * DT_CTRL > 0.05:
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
# ACC cancellation
if CC.cruiseControl.cancel:
self.last_button_frame = self.frame
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
cancel=True))
# ACC resume from standstill
elif CC.cruiseControl.resume:
self.last_button_frame = self.frame
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
resume=True))
# HUD alerts
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(
create_lkas_hud(self.packer, self.CP, lkas_active,
CC.hudControl.visualAlert, self.hud_count,
CS.lkas_car_model, CS.auto_high_beam))
self.hud_count += 1
# steering
if self.frame % 2 == 0:
# TODO: can we make this more sane? why is it different for all the cars?
lkas_control_bit = self.lkas_control_bit_prev
if CS.out.vEgo > self.CP.minSteerSpeed:
lkas_control_bit = True
elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.PACIFICA_2020,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
lkas_control_bit = False
elif self.CP.carFingerprint in RAM_CARS:
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
lkas_control_bit = False
# EPS faults if LKAS re-enables too quickly
lkas_control_bit = lkas_control_bit and (
self.frame - self.last_lkas_falling_edge > 200)
if not lkas_control_bit and self.lkas_control_bit_prev:
self.last_lkas_falling_edge = self.frame
self.lkas_control_bit_prev = lkas_control_bit
# steer torque
new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
self.params)
if not lkas_active or not lkas_control_bit:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends.append(
create_lkas_command(self.packer, self.CP, int(apply_steer),
lkas_control_bit))
self.frame += 1
new_actuators = CC.actuators.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
return new_actuators, can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert,
dragonconf):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
# *** compute control surfaces ***
# steer torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
# 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.dpLatCtrl, dragonconf.dpSteeringOnSignal,
blinker_on or frame < self.blinker_end_frame, apply_steer)
self.last_blinker_on = blinker_on
self.apply_steer_last = apply_steer
can_sends = []
#*** control msgs ***
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.packer,
self.ccframe,
cancel=True)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
return can_sends
def lkas_control(self, CS, actuators, can_sends, enabled, hud_alert,
jvepilot_state):
if self.prev_frame == CS.frame:
return
self.prev_frame = CS.frame
self.lkas_frame += 1
lkas_counter = CS.lkas_counter
if self.prev_lkas_counter == lkas_counter:
lkas_counter = (self.prev_lkas_counter +
1) % 16 # Predict the next frame
self.prev_lkas_counter = lkas_counter
# *** compute control surfaces ***
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
low_steer_models = self.car_fingerprint in (CAR.JEEP_CHEROKEE,
CAR.PACIFICA_2017_HYBRID,
CAR.PACIFICA_2018,
CAR.PACIFICA_2018_HYBRID)
if not self.min_steer_check:
self.moving_fast = True
self.torq_enabled = enabled or low_steer_models
elif low_steer_models:
self.moving_fast = not CS.out.steerError and CS.lkas_active
self.torq_enabled = self.torq_enabled or CS.torq_status > 1
else:
self.moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed -
0.5): # for command high bit
self.torq_enabled = True
elif CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.torq_enabled = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = self.moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if self.lkas_frame % 10 == 0: # 0.1s period
new_msg = create_lkas_heartbit(
self.packer,
0 if jvepilot_state.carControl.useLaneLines else 1,
CS.lkasHeartbit)
can_sends.append(new_msg)
if self.lkas_frame % 25 == 0: # 0.25s period
if CS.lkas_car_model != -1:
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.torq_enabled, lkas_counter)
can_sends.append(new_msg)
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)
# 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)
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = alca_steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_motor,
SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
if not CS.lane_departure_toggle_on:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# frame is 100Hz (0.01s period)
if (self.ccframe % 10 == 0): # 0.1s period
new_msg = create_lkas_heartbit(self.packer, CS.lkas_status_ok)
can_sends.append(new_msg)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.gear_shifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
# *** compute control surfaces ***
if self.on_timer < 200 and CS.veh_on:
self.on_timer += 1
wp_type = int(0)
if Params().get('LkasFullRangeAvailable') == b'1':
wp_type = int(1)
if Params().get('ChryslerMangoMode') == b'1':
wp_type = int(2)
if enabled:
if CS.out.steeringAngleDeg > 50. and wp_type == 1 and self.timer > 97:
self.timer = 97
if self.timer < 99 and wp_type == 1 and CS.out.vEgo < 65:
self.timer += 1
else:
self.timer = 99
else:
self.timer = 0
lkas_active = self.timer == 99
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
#self.steer_rate_limited = new_steer != apply_steer
#moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
#if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
# self.gone_fast_yet = True
#elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
# if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
# self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
#lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
if CS.out.standstill:
self.steer_type = wp_type
if wp_type != 2:
self.steerErrorMod = CS.steerError
if self.steerErrorMod:
self.steer_type = int(0)
elif CS.apaFault or CS.out.gearShifter not in (GearShifter.drive, GearShifter.low) or \
abs(CS.out.steeringAngleDeg) > 330. or self.on_timer < 200 or CS.apa_steer_status:
self.steer_type = int(0)
self.apaActive = CS.apasteerOn and self.steer_type == 2
can_sends = []
#*** control msgs ***
# if pcm_cancel_cmd:
# # TODO: would be better to start from frame_2b3
# new_msg = create_wheel_buttons(self.packer, self.ccframe, cancel=True)
# can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 2 == 0) and wp_type == 2: # 0.02s period
if (CS.lkas_car_model != -1):
new_msg = create_apa_hud(self.packer, CS.out.gearShifter,
self.apaActive, CS.apaFault,
hud_alert, lkas_active,
self.hud_count, CS.lkas_car_model,
self.steer_type)
can_sends.append(new_msg)
self.hud_count += 1
if (self.ccframe % 2 == 0) and wp_type != 2: # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model,
self.steer_type)
can_sends.append(new_msg)
self.hud_count += 1
#new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
new_msg = create_lkas_command(self.packer, int(apply_steer),
lkas_active, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
return can_sends
