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

selfdrive/car/gm/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

selfdrive/car/gm/interface.py

@@ -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

selfdrive/car/hyundai/carcontroller.py

@@ -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))

selfdrive/car/hyundai/hyundaican.py

@@ -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
   }

selfdrive/car/tesla/carcontroller.py

@@ -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

selfdrive/car/tesla/interface.py

@@ -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

selfdrive/car/tesla/teslacan.py

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

selfdrive/car/toyota/carcontroller.py

@@ -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