You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
185 lines
7.0 KiB
185 lines
7.0 KiB
from collections import namedtuple
|
|
|
|
import common.numpy_fast as np
|
|
import selfdrive.controls.lib.hondacan as hondacan
|
|
from common.realtime import sec_since_boot
|
|
from selfdrive.config import CruiseButtons
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
|
from selfdrive.controls.lib.alert_database import process_hud_alert
|
|
from selfdrive.controls.lib.drive_helpers import actuator_hystereses, rate_limit
|
|
|
|
HUDData = namedtuple("HUDData",
|
|
["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
|
|
"lanes", "beep", "X8", "chime", "acc_alert"])
|
|
|
|
class CarController(object):
|
|
def __init__(self):
|
|
self.controls_allowed = False
|
|
self.mismatch_start, self.pcm_mismatch_start = 0, 0
|
|
self.braking = False
|
|
self.brake_steady = 0.
|
|
self.final_brake_last = 0.
|
|
|
|
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(pcm_accel, hud_v_cruise, 0x41, hud_car,
|
|
0xc1, 0x41, hud_lanes + steer_required,
|
|
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 ****
|
|
|
|
user_brake_ctrl = CS.user_brake/0.015625 # FIXME: factor needed to convert to old scale
|
|
|
|
# *** compute control surfaces ***
|
|
tt = sec_since_boot()
|
|
GAS_MAX = 1004
|
|
BRAKE_MAX = 1024/4
|
|
#STEER_MAX = 0xF00 if not CS.torque_mod else 0xF00/4 # ilx has 8x steering torque limit, used as a 2x
|
|
STEER_MAX = 0xF00 # ilx has 8x steering torque limit, used as a 2x
|
|
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 user_brake_ctrl > 10):
|
|
print "CANCELLING GAS", apply_brake, user_brake_ctrl
|
|
apply_gas = 0
|
|
|
|
# no computer brake if the user is hitting the gas
|
|
# if the computer is trying to brake, it can't be hitting the gas
|
|
# TODO: car_gas can override brakes without canceling... this is bad
|
|
if CS.car_gas > 0 and apply_brake != 0:
|
|
apply_brake = 0
|
|
|
|
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
|
|
|
|
# to avoid race conditions, check if control has been disabled for at least 0.2s
|
|
# keep resetting start timer if mismatch isn't true
|
|
if not (self.controls_allowed and not enabled):
|
|
self.mismatch_start = tt
|
|
|
|
# to avoid race conditions, check if control is disabled but pcm control is on for at least 0.2s
|
|
if not (not self.controls_allowed and CS.pcm_acc_status):
|
|
self.pcm_mismatch_start = tt
|
|
|
|
# something is very wrong, since pcm control is active but controls should not be allowed; TODO: send pcm fault cmd?
|
|
if (tt - self.pcm_mismatch_start) > 0.2:
|
|
pcm_cancel_cmd = True
|
|
|
|
# TODO: clean up gear condition, ideally only D (and P for debug) shall be valid gears
|
|
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
|
|
CS.user_gas_pressed or (tt - self.mismatch_start) > 0.2 or
|
|
not CS.main_on or not CS.gear_shifter_valid or
|
|
(CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
|
|
self.controls_allowed = False
|
|
|
|
# 5 is a permanent fault, no torque request will be fullfilled
|
|
if CS.steer_error:
|
|
print "STEER ERROR"
|
|
self.controls_allowed = False
|
|
|
|
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
|
|
elif CS.steer_not_allowed:
|
|
print "STEER ALERT, TORQUE INHIBITED"
|
|
apply_steer = 0
|
|
|
|
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
|
|
|
|
if not self.controls_allowed:
|
|
apply_steer = 0
|
|
apply_gas = 0
|
|
apply_brake = 0
|
|
pcm_speed = 0 # make sure you send 0 target speed to pcm
|
|
#pcm_cancel_cmd = 1 # prevent pcm control from turning on. FIXME: we can't just do this
|
|
|
|
# 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).to_bytes())
|
|
|