def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
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 """
# *** apply brake hysteresis ***
final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
# *** no output if not enabled ***
if not enabled:
final_gas = 0.
final_brake = 0.
final_steer = 0.
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
if CS.pcm_acc_status:
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
self.final_brake_last = final_brake
# vehicle hud display, wait for one update from 10Hz 0x304 msg
#TODO: use enum!!
if hud_show_lanes:
hud_lanes = 0x04
else:
hud_lanes = 0x00
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 0xe0
else:
hud_car = 0xd0
else:
hud_car = 0xc0
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(hud_v_cruise), 0x41, hud_car,
0xc1, 0x41, hud_lanes + steer_required,
int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
tt = sec_since_boot()
GAS_MAX = 1004
BRAKE_MAX = 1024/4
STEER_MAX = 0xF00
GAS_OFFSET = 328
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = int(np.clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
apply_brake = int(np.clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
apply_steer = int(np.clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
# no gas if you are hitting the brake or the user is
if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
print "CANCELLING GAS", apply_brake
apply_gas = 0
# no computer brake if the gas is being pressed
if CS.car_gas > 0 and apply_brake != 0:
print "CANCELLING BRAKE"
apply_brake = 0
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
if CS.steer_not_allowed:
print "STEER ALERT, TORQUE INHIBITED"
apply_steer = 0
# *** entry into controls state ***
if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
CS.cruise_buttons == 0 and not self.controls_allowed:
print "CONTROLS ARE LIVE"
self.controls_allowed = True
# *** exit from controls state on cancel, gas, or brake ***
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
print "CONTROLS ARE DEAD"
self.controls_allowed = False
# *** controls fail on steer error, brake error, or invalid can ***
if CS.steer_error:
print "STEER ERROR"
self.controls_allowed = False
if CS.brake_error:
print "BRAKE ERROR"
self.controls_allowed = False
if not CS.can_valid and self.controls_allowed: # 200 ms
print "CAN INVALID"
self.controls_allowed = False
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(apply_steer, idx))
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
can_sends.append(
hondacan.create_brake_command(apply_brake, pcm_override,
pcm_cancel_cmd, hud.chime, idx))
if not CS.brake_only:
# 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
gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0)
can_sends.append(hondacan.create_gas_command(gas_amount, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame/10) % 4
can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.civic, idx))
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
if CS.civic:
radar_send_step = 5
else:
radar_send_step = 2
if (frame % radar_send_step) == 0:
idx = (frame/radar_send_step) % 4
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.civic, 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, \
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, 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.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.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 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) % 4 #Clarity: Why do we need this? -wirelessnet2
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.extend(
hondacan.create_brake_command(
self.packer,
apply_brake, #Clarity: We don't use comma's brake pump algo because it casues jerky braking on Clarity. -wirelessnet2
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))
#Clarity: This allows us to manually drive the radar since we don't have a factory ADAS camera to do so. -wirelessnet2
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
radar_send_step = 5
# if (frame % radar_send_step) == 0:
idx = (frame / radar_send_step) % 4
can_sends.extend(
hondacan.create_radar_commands(self.packer, CS.vEgoRawKph,
idx))
return can_sends