def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlack,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx,
CS.CP.isPandaBlack))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
pass # TODO: implement
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0,
P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.append(
hondacan.create_brake_command(
self.packer, apply_brake, pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint,
CS.CP.isPandaBlack, CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert):
P = self.params
if c.longActive:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo,
CS.CP.carFingerprint)
else:
accel = 0.0
gas, brake = 0.0, 0.0
# *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(
brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2.,
DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if c.enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
# Send CAN commands.
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if (frame % 10) == 0:
can_sends.append(
(0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, c.latActive, CS.CP.carFingerprint,
idx, CS.CP.openpilotLongitudinalControl))
stopping = actuators.longControlState == LongCtrlState.stopping
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0],
[0.001, 0.002, 0.15])
# all of this is only relevant for HONDA NIDEC
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP,
P.NIDEC_MAX_ACCEL_V)
# TODO this 1.44 is just to maintain previous behavior
pcm_speed_BP = [-wind_brake, -wind_brake * (3 / 4), 0.0, 0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if CS.CP.enableGasInterceptor:
pcm_speed = 0.0
pcm_accel = int(0.0)
elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
pcm_speed_V = [
0.0,
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)
]
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int((1.0) * 0xc6)
else:
pcm_speed_V = [
0.0,
clip(CS.out.vEgo - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)
]
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
self.accel = clip(accel, P.BOSCH_ACCEL_MIN,
P.BOSCH_ACCEL_MAX)
self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP,
P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(
hondacan.create_acc_commands(self.packer, c.enabled,
c.longActive, accel,
self.gas, idx, stopping,
CS.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(
clip(apply_brake * P.NIDEC_BRAKE_MAX, 0,
P.NIDEC_BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
pcm_override = True
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd,
fcw_display, idx,
CS.CP.carFingerprint,
CS.stock_brake))
self.apply_brake_last = apply_brake
self.brake = apply_brake / P.NIDEC_BRAKE_MAX
if CS.CP.enableGasInterceptor:
# way too aggressive at low speed without this
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable.
if c.longActive:
self.gas = clip(
gas_mult * (gas - brake + wind_brake * 3 / 4),
0., 1.)
else:
self.gas = 0.0
can_sends.append(
create_gas_interceptor_command(
self.packer, self.gas, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
can_sends.extend(
hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud,
CS.is_metric, idx, CS.stock_hud))
if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint
not in HONDA_BOSCH):
self.speed = pcm_speed
if not CS.CP.enableGasInterceptor:
self.gas = pcm_accel / 0xc6
new_actuators = actuators.copy()
new_actuators.speed = self.speed
new_actuators.accel = self.accel
new_actuators.gas = self.gas
new_actuators.brake = self.brake
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# Never send cancel command if we never enter cruise state (no cruise if pedal)
# Cancel cmd causes brakes to release at a standstill causing grinding
pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if (frame % 10) == 0:
can_sends.append(
(0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.openpilotLongitudinalControl))
accel = actuators.gas - actuators.brake
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -1.0 if stopping else accel
if CS.CP.carFingerprint in HONDA_BOSCH:
apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP,
P.BOSCH_ACCEL_LOOKUP_V)
else:
apply_accel = interp(accel, P.NIDEC_ACCEL_LOOKUP_BP,
P.NIDEC_ACCEL_LOOKUP_V)
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0],
[0.001, 0.002, 0.15])
if CS.CP.carFingerprint in OLD_NIDEC_LONG_CONTROL:
#pcm_speed = pcm_speed
pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6)
else:
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP,
P.NIDEC_MAX_ACCEL_V)
pcm_accel = int(clip(apply_accel / max_accel, 0.0, 1.0) * 0xc6)
pcm_speed_BP = [-wind_brake, -wind_brake * (3 / 4), 0.0]
pcm_speed_V = [
0.0,
clip(CS.out.vEgo + apply_accel / 2.0 - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + apply_accel / 2.0 + 2.0, 0.0, 100.0)
]
pcm_speed = interp(accel, pcm_speed_BP, pcm_speed_V)
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP,
P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(
hondacan.create_acc_commands(self.packer, enabled,
apply_accel, apply_gas,
idx, stopping, starting,
CS.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(
clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd,
fcw_display, idx,
CS.CP.carFingerprint,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# way too aggressive at low speed without this
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
apply_gas = clip(gas_mult * actuators.gas, 0., 1.)
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
return can_sends
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep != 0 else snd_chime
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), hud_car, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.read_distance_lines, CS.lkMode)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx, CS.CP.isPandaBlack))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlack,
CS.stock_hud))
if CS.CP.radarOffCan:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx,
CS.CP.isPandaBlack))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.out.cruiseState.standstill:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15,
CAR.ACCORDH):
rough_lead_speed = self.rough_speed(CS.lead_distance)
if CS.lead_distance > (self.stopped_lead_distance +
15.0) or rough_lead_speed > 0.1:
self.stopped_lead_distance = 0.0
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint, CS.CP.isPandaBlack))
# print("spamming")
# print(self.stopped_lead_distance, CS.lead_distance, rough_lead_speed)
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
self.stopped_lead_distance = CS.lead_distance
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
# Do NOT send the cancel command if we are using the pedal. Sending cancel causes the car firmware to
# turn the brake pump off, and we don't want that. Stock ACC does not send the cancel cmd when it is braking.
if CS.CP.enableGasInterceptor:
pcm_cancel_cmd = False
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
dragonconf, hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# Never send cancel command if we never enter cruise state (no cruise if pedal)
# Cancel cmd causes brakes to release at a standstill causing grinding
pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required,
CS.lkMode)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if (frame % 10) == 0:
can_sends.append(
(0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# 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
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if not dragonconf.dpAtl and (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx))
if dragonconf.dpAtl:
pass
# If using stock ACC, spam cancel command to kill gas when OP disengages.
elif not dragonconf.dpAllowGas and pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH,
CAR.INSIGHT):
rough_lead_speed = self.rough_speed(CS.lead_distance)
if CS.lead_distance > (self.stopped_lead_distance +
15.0) or rough_lead_speed > 0.1:
self.stopped_lead_distance = 0.0
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx,
CS.CP.carFingerprint))
else:
self.stopped_lead_distance = CS.lead_distance
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if dragonconf.dpAtl:
pass
elif CS.CP.carFingerprint in HONDA_BOSCH:
accel = actuators.gas - actuators.brake
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -1.0 if stopping else accel
apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP,
P.BOSCH_ACCEL_LOOKUP_V)
apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP,
P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(
hondacan.create_acc_commands(self.packer, enabled,
apply_accel, apply_gas,
idx, stopping, starting,
CS.CP.carFingerprint))
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0,
P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw,
idx,
CS.CP.carFingerprint,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
