Clean up CarInterface.apply and CarControllers (#24060)

* do tesla

* parentheses

* Do Chrysler

* Make sure Hyundai long never actuates when long inactive

.

* clean up Hyundai

* more clear

* formatting

* Stock sets these to zero

* fix params

* move jerk logic into CC

clean up
pull/24149/head
Shane Smiskol 3 years ago committed by GitHub
parent 596417da37
commit c8f5b21744
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 61
      selfdrive/car/gm/carcontroller.py
  2. 11
      selfdrive/car/gm/interface.py
  3. 15
      selfdrive/car/hyundai/carcontroller.py
  4. 10
      selfdrive/car/hyundai/hyundaican.py
  5. 32
      selfdrive/car/tesla/carcontroller.py
  6. 3
      selfdrive/car/tesla/interface.py
  7. 1
      selfdrive/car/tesla/teslacan.py
  8. 1
      selfdrive/car/toyota/carcontroller.py

@ -10,12 +10,13 @@ from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController():
class CarController:
def __init__(self, dbc_name, CP, VM):
self.start_time = 0.
self.apply_steer_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.frame = 0
self.lka_steering_cmd_counter_last = -1
self.lka_icon_status_last = (False, False)
@ -27,9 +28,13 @@ class CarController():
self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
hud_alert = hud_control.visualAlert
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
# Send CAN commands.
can_sends = []
@ -39,11 +44,11 @@ class CarController():
# next Panda loopback confirmation in the current CS frame.
if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
elif (frame % P.STEER_STEP) == 0:
lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED
elif (self.frame % self.params.STEER_STEP) == 0:
lkas_enabled = CC.latActive and CS.out.vEgo > self.params.MIN_STEER_SPEED
if lkas_enabled:
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
@ -56,44 +61,45 @@ class CarController():
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
if not c.longActive:
if (self.frame % 4) == 0:
if not CC.longActive:
# Stock ECU sends max regen when not enabled.
self.apply_gas = P.MAX_ACC_REGEN
self.apply_gas = self.params.MAX_ACC_REGEN
self.apply_brake = 0
else:
self.apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
idx = (frame // 4) % 4
idx = (self.frame // 4) % 4
at_full_stop = c.longActive and CS.out.standstill
near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.longActive, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
if (self.frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.longActive,
hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
tt = self.frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
if self.frame % time_and_headlights_step == 0:
idx = (self.frame // time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
if self.frame % speed_and_accelerometer_step == 0:
idx = (self.frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
if self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
@ -103,14 +109,15 @@ class CarController():
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
if self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
self.frame += 1
return new_actuators, can_sends

@ -216,14 +216,5 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
hud_control = c.hudControl
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
hud_v_cruise, hud_control.lanesVisible,
hud_control.leadVisible, hud_control.visualAlert)
self.frame += 1
ret = self.CC.update(c, self.CS)
return ret

@ -88,17 +88,18 @@ class CarController:
self.last_resume_frame = self.frame
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
lead_visible = False
accel = actuators.accel if CC.longActive else 0
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
accel = actuators.accel
jerk = 0
if accel < 0:
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
if CC.longActive:
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
if accel < 0:
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = (actuators.longControlState == LongCtrlState.stopping)
lead_visible = False
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
can_sends.extend(create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible,
set_speed_in_units, stopping, CS.out.gasPressed))

@ -96,9 +96,9 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
scc12_values = {
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0,
"StopReq": 1 if enabled and stopping else 0,
"aReqRaw": accel if enabled else 0,
"aReqValue": accel if enabled else 0, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"StopReq": 1 if stopping else 0,
"aReqRaw": accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"CR_VSM_Alive": idx % 0xF,
}
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
@ -109,8 +109,8 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
scc14_values = {
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
"JerkUpperLimit": max(jerk, 1.0) if (enabled and not stopping) else 0, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": max(-jerk, 1.0) if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
"JerkUpperLimit": max(jerk, 1.0) if not stopping else 0, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": max(-jerk, 1.0), # stock usually is 0.5 but sometimes uses higher values
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
}

@ -3,41 +3,46 @@ from opendbc.can.packer import CANPacker
from selfdrive.car.tesla.teslacan import TeslaCAN
from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
class CarController():
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.frame = 0
self.last_angle = 0
self.long_control_counter = 0
self.packer = CANPacker(dbc_name)
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
def update(self, c, CS, frame, actuators, cruise_cancel):
def update(self, CC, CS):
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
can_sends = []
# Temp disable steering on a hands_on_fault, and allow for user override
hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3)
lkas_enabled = c.latActive and (not hands_on_fault)
hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3
lkas_enabled = CC.latActive and not hands_on_fault
if lkas_enabled:
apply_angle = actuators.steeringAngleDeg
# Angular rate limit based on speed
steer_up = (self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle))
steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle)
rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
apply_angle = clip(apply_angle, (self.last_angle - max_angle_diff), (self.last_angle + max_angle_diff))
apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff)
# To not fault the EPS
apply_angle = clip(apply_angle, (CS.out.steeringAngleDeg - 20), (CS.out.steeringAngleDeg + 20))
apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20)
else:
apply_angle = CS.out.steeringAngleDeg
self.last_angle = apply_angle
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, frame))
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame))
# Longitudinal control (40Hz)
if self.CP.openpilotLongitudinalControl and ((frame % 5) in (0, 2)):
if self.CP.openpilotLongitudinalControl and self.frame % 5 in (0, 2):
target_accel = actuators.accel
target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
max_accel = 0 if target_accel < 0 else target_accel
@ -48,17 +53,18 @@ class CarController():
# Cancel on user steering override, since there is no steering torque blending
if hands_on_fault:
cruise_cancel = True
pcm_cancel_cmd = True
if ((frame % 10) == 0 and cruise_cancel):
if self.frame % 10 == 0 and pcm_cancel_cmd:
# Spam every possible counter value, otherwise it might not be accepted
for counter in range(16):
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.chassis, counter))
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot_chassis, counter))
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter))
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter))
# TODO: HUD control
new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_angle
self.frame += 1
return new_actuators, can_sends

@ -71,6 +71,5 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
self.frame += 1
ret = self.CC.update(c, self.CS)
return ret

@ -1,5 +1,6 @@
import copy
import crcmod
from common.conversions import Conversions as CV
from selfdrive.car.tesla.values import CANBUS, CarControllerParams

@ -7,6 +7,7 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert

Loading…
Cancel
Save