mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-12 16:32:09 +08:00
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
This commit is contained in:
Shane Smiskol
@@ -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
|
||||
accel = actuators.accel
|
||||
jerk = 0
|
||||
|
||||
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])
|
||||
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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user