spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

cars: cleanup CS usage (#23957)

pull/24004/head
Adeeb Shihadeh 2022-03-18 15:39:22 -07:00 committed by GitHub
parent 1af3c52049
commit 2219cb4478
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 49 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
selfdrive/car/chrysler/carcontroller.py
View File

@ -7,6 +7,7 @@ from opendbc.can.packer import CANPacker
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.ccframe = 0
self.prev_frame = -1
@ -29,11 +30,11 @@ class CarController():
CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
moving_fast = CS.out.vEgo > self.CP.minSteerSpeed # for status message
if CS.out.vEgo > (self.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled

35
selfdrive/car/honda/carcontroller.py
View File

@ -98,6 +98,7 @@ HUDData = namedtuple("HUDData",
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
@ -119,13 +120,13 @@ class CarController():
if c.longActive:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.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)
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, self.CP.carFingerprint)
# *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
@ -156,14 +157,14 @@ class CarController():
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if self.CP.carFingerprint in HONDA_BOSCH and self.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))
c.latActive, self.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
stopping = actuators.longControlState == LongCtrlState.stopping
@ -178,10 +179,10 @@ class CarController():
0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if CS.CP.enableGasInterceptor:
if self.CP.enableGasInterceptor:
pcm_speed = 0.0
pcm_accel = int(0.0)
elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
elif self.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),
@ -196,15 +197,15 @@ class CarController():
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 not self.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx))
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.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))
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, self.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint))
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, self.CP.carFingerprint))
else:
# Send gas and brake commands.
@ -212,10 +213,10 @@ class CarController():
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
if self.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))
can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, self.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))
@ -223,11 +224,11 @@ class CarController():
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))
pcm_override, pcm_cancel_cmd, fcw_display, idx, self.CP.carFingerprint, CS.stock_brake))
self.apply_brake_last = apply_brake
self.brake = apply_brake / P.NIDEC_BRAKE_MAX
if CS.CP.enableGasInterceptor:
if self.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.
@ -245,12 +246,12 @@ class CarController():
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))
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH):
if (self.CP.openpilotLongitudinalControl) and (self.CP.carFingerprint not in HONDA_BOSCH):
self.speed = pcm_speed
if not CS.CP.enableGasInterceptor:
if not self.CP.enableGasInterceptor:
self.gas = pcm_accel / 0xc6
new_actuators = actuators.copy()

11
selfdrive/car/hyundai/carcontroller.py
View File

@ -37,6 +37,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.p = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
@ -65,7 +66,7 @@ class CarController():
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.openpilotLongitudinalControl:
if self.CP.openpilotLongitudinalControl:
if (frame % 100) == 0:
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
@ -74,7 +75,7 @@ class CarController():
left_lane, right_lane,
left_lane_warning, right_lane_warning))
if not CS.CP.openpilotLongitudinalControl:
if not self.CP.openpilotLongitudinalControl:
if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
@ -84,7 +85,7 @@ class CarController():
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
self.last_resume_frame = frame
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
if frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
lead_visible = False
accel = actuators.accel if c.longActive else 0
@ -109,11 +110,11 @@ class CarController():
can_sends.append(create_lfahda_mfc(self.packer, c.enabled))
# 5 Hz ACC options
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
if frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(create_acc_opt(self.packer))
# 2 Hz front radar options
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
if frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(create_frt_radar_opt(self.packer))
new_actuators = actuators.copy()

7
selfdrive/car/mazda/carcontroller.py
View File

@ -8,6 +8,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
@ -30,7 +31,7 @@ class CarController():
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
# TODO: improve the resume trigger logic by looking at actual radar data
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
if c.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
@ -41,7 +42,7 @@ class CarController():
if frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
@ -56,7 +57,7 @@ class CarController():
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
new_actuators = c.actuators.copy()

5
selfdrive/car/subaru/carcontroller.py
View File

@ -6,6 +6,7 @@ from opendbc.can.packer import CANPacker
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.es_distance_cnt = -1
self.es_lkas_cnt = -1
@ -33,7 +34,7 @@ class CarController():
if not c.latActive:
apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
else:
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
@ -43,7 +44,7 @@ class CarController():
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged

23
selfdrive/car/toyota/carcontroller.py
View File

@ -12,6 +12,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.frame = 0
self.last_steer = 0
self.alert_active = False
@ -29,12 +30,12 @@ class CarController:
pcm_cancel_cmd = CC.cruiseControl.cancel
# gas and brake
if CS.CP.enableGasInterceptor and CC.longActive:
if self.CP.enableGasInterceptor and CC.longActive:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif CS.CP.carFingerprint in (CAR.COROLLA,):
elif self.CP.carFingerprint in (CAR.COROLLA,):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
@ -64,7 +65,7 @@ class CarController:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
if CS.out.standstill and not self.last_standstill and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
@ -82,7 +83,7 @@ class CarController:
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, self.frame))
if self.frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
@ -91,19 +92,19 @@ class CarController:
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, self.frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (self.frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
elif self.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
self.accel = pcm_accel_cmd
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
if self.frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
@ -129,12 +130,12 @@ class CarController:
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.enabled))
if self.frame % 100 == 0 and CS.CP.enableDsu:
if self.frame % 100 == 0 and self.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
# *** static msgs ***
for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS:
if self.frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
new_actuators = actuators.copy()

3
selfdrive/car/volkswagen/carcontroller.py
View File

@ -9,6 +9,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController():
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.CP = CP
self.packer_pt = CANPacker(DBC_FILES.mqb)
@ -86,7 +87,7 @@ class CarController():
# FIXME: this entire section is in desperate need of refactoring
if CS.CP.pcmCruise:
if self.CP.pcmCruise:
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
if c.cruiseControl.cancel:
# Cancel ACC if it's engaged with OP disengaged.