def test_correctness(self):
# Test all commands, randomize the params.
for _ in range(1000):
has_relay = False
car_fingerprint = HONDA_BOSCH[0]
apply_brake = (random.randint(0, 2) % 2 == 0)
pump_on = (random.randint(0, 2) % 2 == 0)
pcm_override = (random.randint(0, 2) % 2 == 0)
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
fcw = random.randint(0, 65536)
idx = random.randint(0, 65536)
m_old = hondacan.create_brake_command(self.honda_cp_old,
apply_brake, pump_on,
pcm_override, pcm_cancel_cmd,
fcw, idx, car_fingerprint,
has_relay)
m = hondacan.create_brake_command(self.honda_cp, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, fcw, idx,
car_fingerprint, has_relay)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
lkas_active = (random.randint(0, 2) % 2 == 0)
idx = random.randint(0, 65536)
m_old = hondacan.create_steering_control(self.honda_cp_old,
apply_steer, lkas_active,
car_fingerprint, idx,
has_relay)
m = hondacan.create_steering_control(self.honda_cp, apply_steer,
lkas_active, car_fingerprint,
idx, has_relay)
self.assertEqual(m_old, m)
pcm_speed = random.randint(0, 65536)
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536),
1, random.randint(0, 65536), 0xc1,
random.randint(0, 65536), random.randint(0, 65536),
random.randint(0, 65536), random.randint(0, 65536),
random.randint(0, 65536), random.randint(0, 65536))
idx = random.randint(0, 65536)
is_metric = (random.randint(0, 2) % 2 == 0)
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed,
hud, car_fingerprint,
is_metric, idx, has_relay)
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud,
car_fingerprint, is_metric, idx,
has_relay)
self.assertEqual(m_old, m)
button_val = random.randint(0, 65536)
idx = random.randint(0, 65536)
m_old = hondacan.spam_buttons_command(self.honda_cp_old,
button_val, idx,
car_fingerprint, has_relay)
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx,
car_fingerprint, has_relay)
self.assertEqual(m_old, m)
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, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# 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
# 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 is not 0 else snd_chime
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
STEER_MAX = 0x1000
# 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 * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
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))
# 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, idx))
if CS.CP.radarOffCan:
# 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))
elif CS.stopped:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
can_sends.extend(
hondacan.create_brake_command(self.packer, apply_brake,
pcm_override, pcm_cancel_cmd,
hud.chime, hud.fcw,
CS.CP.carFingerprint, idx))
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(
hondacan.create_gas_command(self.packer, apply_gas,
idx))
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
if CS.CP.carFingerprint == CAR.ACURA_ILX:
radar_send_step = 2
else:
radar_send_step = 5
if (frame % radar_send_step) == 0:
idx = (frame / radar_send_step) % 4
if not self.new_radar_config: # only change state once
self.new_radar_config = car.RadarState.Error.wrongConfig in radar_error
can_sends.extend(
hondacan.create_radar_commands(CS.v_ego,
CS.CP.carFingerprint,
self.new_radar_config, idx))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, 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):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on:
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 is not 0 else snd_chime
# Do not send audible alert when steering is disabled or blinkers on
#if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on:
# snd_chime = 0
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.read_distance_lines, CS.lkMode,
self.speed_units)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
else:
STEER_MAX = 0x1000
# 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 * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering
# 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))
# 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, idx))
if CS.CP.radarOffCan:
# 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))
elif CS.stopped:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15,
CAR.ACCORDH, CAR.INSIGHT):
if CS.lead_distance > (self.prev_lead_distance +
float(kegman.conf['leadDistance'])):
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame / 2
pump_on, self.last_pump_ts = brake_pump_hysteresys(
apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx))
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))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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):
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = 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 ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 // 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
STEER_MAX = 0x1400
else:
STEER_MAX = 0x1000
# 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 * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
if apply_steer > 0xA00:
apply_steer = (apply_steer - 0xA00) / 2 + 0xA00
elif apply_steer < -0xA00:
apply_steer = (apply_steer + 0xA00) / 2 - 0xA00
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))
# 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 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.stopped:
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
# pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
pump_on, self.last_pump_on_state = brake_pump_hysteresis(
apply_brake, self.apply_brake_last,
self.last_pump_on_state, ts)
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, 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, sendcan, 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):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# 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
# 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
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
STEER_MAX = 0x1000
#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)
# 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 * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX))
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))
# 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, idx))
if CS.CP.radarOffCan:
# 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))
elif CS.stopped:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
pump_on, self.last_pump_ts = brake_pump_hysteresys(
apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx))
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(
hondacan.create_gas_command(self.packer, apply_gas,
idx))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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 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.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.)
# return minimum of brake_last*MAX, or MAX-1, but not less than zero
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 #and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering
# 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 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.stopped:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT):
if CS.lead_distance > (self.prev_lead_distance + float(kegman.conf['leadDistance'])):
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH):
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.prev_lead_distance = CS.lead_distance
'''
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
pump_on, self.last_pump_on_state = brake_pump_hysteresis(
apply_brake, self.apply_brake_last,
self.last_pump_on_state, 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,
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):
# *** apply brake hysteresis ***
#brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
#self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes or 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
# Do not send audible alert when steering is disabled or blinkers on
#if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on:
# snd_chime = 0
#print("{0} {1} {2}".format(chime, alert_id, hud_alert))
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.lkMode)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 // 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
else:
STEER_MAX = 0x1000
if self.BP2 is None:
self.BP2 = int(kegman.conf['BP2']) if int(
kegman.conf['BP2']) != 0 else STEER_MAX
self.BP1 = int(
kegman.conf['BP1']) if int(kegman.conf['BP1']) != 0 else 2560
print("torque breakpoints set!", self.BP1, self.BP2)
# 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 * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -self.BP2, self.BP2))
apply_steer = int(
interp(apply_steer, [-self.BP2, -self.BP1, self.BP1, self.BP2],
[-STEER_MAX, -self.BP1, self.BP1, STEER_MAX]))
lkas_active = 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))
can_sends.append(
hondacan.create_steering_control_x2(self.packer,
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))
if CS.CP.radarOffCan:
# 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.standstill and enabled and CS.stopped and CS.lead_distance < 200:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15,
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, CS.CP.isPandaBlack))
print("spamming")
print(self.stopped_lead_distance, CS.lead_distance,
rough_lead_speed)
elif False and CS.CP.carFingerprint in (CAR.CIVIC_BOSCH,
CAR.CRV_HYBRID):
print(' this is a civic or crv')
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:
print(' doing the regular SPAM RESUME')
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.v_ego == 0:
#print(' not quite "stopped" yet, but lead distance is %f' % CS.lead_distance)
self.stopped_lead_distance = CS.lead_distance
self.prev_lead_distance = CS.lead_distance
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)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
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