Log actuators after applying rate limits in CarController (commaai#23230

)

* return actuators from carcontroller

* log it

* pass to latcontrol

* chrysler

* gm

* honda

* more brands

* rest of the brands

* gm cleanup

* hyundai cleanup

* update ref

* rename field

* fix subaru

* add types

* more subaru fixes commaai#23240

* consistent whitespace

* bump cereal

selfdrive/car/chrysler/carcontroller.py

@@ -1,3 +1,4 @@
+from cereal import car
 from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
                                                create_wheel_buttons
@@ -20,9 +21,8 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     # this seems needed to avoid steering faults and to force the sync with the EPS counter
     frame = CS.lkas_counter
     if self.prev_frame == frame:
-      return []
+      return car.CarControl.Actuators.new_message(), []
 
-    # *** compute control surfaces ***
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
     apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
@@ -67,4 +67,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     self.ccframe += 1
     self.prev_frame = frame
 
-    return can_sends
+    new_actuators = actuators.copy()
+    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
+
+    return new_actuators, can_sends

selfdrive/car/chrysler/interface.py

@@ -78,8 +78,6 @@ def update(self, c, can_strings):
   def apply(self, c):
 
     if (self.CS.frame == -1):
-      return []  # if we haven't seen a frame 220, then do not update.
+      return car.CarControl.Actuators.new_message(), []  # if we haven't seen a frame 220, then do not update.
 
-    can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
-
-    return can_sends
+    return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)

selfdrive/car/ford/carcontroller.py

@@ -83,4 +83,4 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
     self.main_on_last = CS.out.cruiseState.available
     self.steer_alert_last = steer_alert
 
-    return can_sends
+    return actuators, can_sends

selfdrive/car/ford/interface.py

@@ -63,8 +63,8 @@ def update(self, c, can_strings):
   # to be called @ 100hz
   def apply(self, c):
 
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
                                c.hudControl.visualAlert, c.cruiseControl.cancel)
 
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/gm/carcontroller.py

@@ -14,6 +14,9 @@ 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.lka_steering_cmd_counter_last = -1
     self.lka_icon_status_last = (False, False)
     self.steer_rate_limited = False
@@ -53,22 +56,22 @@ def update(self, enabled, CS, frame, actuators,
 
       can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
 
-    if not enabled:
-      # Stock ECU sends max regen when not enabled.
-      apply_gas = P.MAX_ACC_REGEN
-      apply_brake = 0
-    else:
-      apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
-      apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
-
     # Gas/regen and brakes - all at 25Hz
     if (frame % 4) == 0:
+      if not enabled:
+        # Stock ECU sends max regen when not enabled.
+        self.apply_gas = P.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)))
+
       idx = (frame // 4) % 4
 
       at_full_stop = enabled and CS.out.standstill
       near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
-      can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop))
-      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop))
+      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, enabled, at_full_stop))
 
     # Send dashboard UI commands (ACC status), 25hz
     if (frame % 4) == 0:
@@ -106,4 +109,9 @@ def update(self, enabled, CS, frame, actuators,
       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
 
-    return can_sends
+    new_actuators = actuators.copy()
+    new_actuators.steer = self.apply_steer_last / P.STEER_MAX
+    new_actuators.gas = self.apply_gas
+    new_actuators.brake = self.apply_brake
+
+    return new_actuators, can_sends

selfdrive/car/gm/interface.py

@@ -220,10 +220,10 @@ def apply(self, c):
     # In GM, PCM faults out if ACC command overlaps user gas.
     enabled = c.enabled and not self.CS.out.gasPressed
 
-    can_sends = self.CC.update(enabled, self.CS, self.frame,
-                               c.actuators,
-                               hud_v_cruise, c.hudControl.lanesVisible,
-                               c.hudControl.leadVisible, c.hudControl.visualAlert)
+    ret = self.CC.update(enabled, self.CS, self.frame,
+                         c.actuators,
+                         hud_v_cruise, c.hudControl.lanesVisible,
+                         c.hudControl.leadVisible, c.hudControl.visualAlert)
 
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/honda/carcontroller.py

@@ -105,6 +105,11 @@ def __init__(self, dbc_name, CP, VM):
     self.last_pump_ts = 0.
     self.packer = CANPacker(dbc_name)
 
+    self.accel = 0
+    self.speed = 0
+    self.gas = 0
+    self.brake = 0
+
     self.params = CarControllerParams(CP)
 
   def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
@@ -211,10 +216,9 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
         ts = frame * DT_CTRL
 
         if CS.CP.carFingerprint in HONDA_BOSCH:
-          accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
-          bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
-          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
-
+          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, enabled, active, accel, self.gas, idx, stopping, starting, CS.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))
@@ -224,6 +228,7 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
           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))
           self.apply_brake_last = apply_brake
+          self.brake = apply_brake / P.NIDEC_BRAKE_MAX
 
           if CS.CP.enableGasInterceptor:
             # way too aggressive at low speed without this
@@ -233,17 +238,28 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
             # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
             # when you do enable.
             if active:
-              apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
+              self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
             else:
-              apply_gas = 0.0
-            can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx))
-
-    hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
-                  hud_lanes, fcw_display, acc_alert, steer_required)
+              self.gas = 0.0
+            can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
 
     # Send dashboard UI commands.
     if (frame % 10) == 0:
       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))
 
-    return can_sends
+      if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH):
+        self.speed = pcm_speed
+
+        if not CS.CP.enableGasInterceptor:
+          self.gas = pcm_accel / 0xc6
+
+    new_actuators = actuators.copy()
+    new_actuators.speed = self.speed
+    new_actuators.accel = self.accel
+    new_actuators.gas = self.gas
+    new_actuators.brake = self.brake
+
+    return new_actuators, can_sends

selfdrive/car/honda/interface.py

@@ -438,13 +438,13 @@ def apply(self, c):
     else:
       hud_v_cruise = 255
 
-    can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
-                               c.actuators,
-                               c.cruiseControl.cancel,
-                               hud_v_cruise,
-                               c.hudControl.lanesVisible,
-                               hud_show_car=c.hudControl.leadVisible,
-                               hud_alert=c.hudControl.visualAlert)
+    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
+                         c.actuators,
+                         c.cruiseControl.cancel,
+                         hud_v_cruise,
+                         c.hudControl.lanesVisible,
+                         hud_show_car=c.hudControl.leadVisible,
+                         hud_alert=c.hudControl.visualAlert)
 
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/hyundai/carcontroller.py

@@ -44,6 +44,7 @@ def __init__(self, dbc_name, CP, VM):
     self.car_fingerprint = CP.carFingerprint
     self.steer_rate_limited = False
     self.last_resume_frame = 0
+    self.accel = 0
 
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
              left_lane, right_lane, left_lane_depart, right_lane_depart):
@@ -100,6 +101,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu
       stopping = (actuators.longControlState == LongCtrlState.stopping)
       set_speed_in_units = hud_speed * (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, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
+      self.accel = accel
 
     # 20 Hz LFA MFA message
     if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
@@ -116,4 +118,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hu
     if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
       can_sends.append(create_frt_radar_opt(self.packer))
 
-    return can_sends
+    new_actuators = actuators.copy()
+    new_actuators.steer = apply_steer / self.p.STEER_MAX
+    new_actuators.accel = self.accel
+
+    return new_actuators, can_sends

selfdrive/car/hyundai/interface.py

@@ -347,8 +347,8 @@ def update(self, c, can_strings):
     return self.CS.out
 
   def apply(self, c):
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
-                               c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
+                         c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
+                         c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/interfaces.py

@@ -1,6 +1,7 @@
 import os
 import time
-from typing import Dict
+from abc import abstractmethod
+from typing import Dict, Tuple, List
 
 from cereal import car
 from common.kalman.simple_kalman import KF1D
@@ -48,8 +49,9 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed):
     return ACCEL_MIN, ACCEL_MAX
 
   @staticmethod
+  @abstractmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    raise NotImplementedError
+    pass
 
   @staticmethod
   def init(CP, logcan, sendcan):
@@ -99,13 +101,13 @@ def get_std_params(candidate, fingerprint):
     ret.longitudinalActuatorDelayUpperBound = 0.15
     return ret
 
-  # returns a car.CarState, pass in car.CarControl
-  def update(self, c, can_strings):
-    raise NotImplementedError
+  @abstractmethod
+  def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
+    pass
 
-  # return sendcan, pass in a car.CarControl
-  def apply(self, c):
-    raise NotImplementedError
+  @abstractmethod
+  def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
+    pass
 
   def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True):
     events = Events()

selfdrive/car/mazda/carcontroller.py

@@ -58,4 +58,8 @@ def update(self, c, CS, frame):
     # send steering command
     can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
                                                       frame, apply_steer, CS.cam_lkas))
-    return can_sends
+
+    new_actuators = c.actuators.copy()
+    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
+
+    return new_actuators, can_sends

selfdrive/car/mazda/interface.py

@@ -95,6 +95,6 @@ def update(self, c, can_strings):
     return self.CS.out
 
   def apply(self, c):
-    can_sends = self.CC.update(c, self.CS, self.frame)
+    ret = self.CC.update(c, self.CS, self.frame)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/mock/interface.py

@@ -88,4 +88,5 @@ def update(self, c, can_strings):
 
   def apply(self, c):
     # in mock no carcontrols
-    return []
+    actuators = car.CarControl.Actuators.new_message()
+    return actuators, []

selfdrive/car/nissan/carcontroller.py

@@ -87,4 +87,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
           self.packer, lkas_hud_info_msg, steer_hud_alert
         ))
 
-    return can_sends
+    new_actuators = actuators.copy()
+    new_actuators.steeringAngleDeg = apply_angle
+
+    return new_actuators, can_sends

selfdrive/car/nissan/interface.py

@@ -78,9 +78,9 @@ def update(self, c, can_strings):
     return self.CS.out
 
   def apply(self, c):
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.cruiseControl.cancel, c.hudControl.visualAlert,
-                               c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
-                               c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
+                         c.cruiseControl.cancel, c.hudControl.visualAlert,
+                         c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible,
+                         c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/subaru/carcontroller.py

@@ -72,4 +72,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
         can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
         self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
 
-    return can_sends
+    new_actuators = actuators.copy()
+    new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
+
+    return new_actuators, can_sends

selfdrive/car/subaru/interface.py

@@ -122,8 +122,8 @@ def update(self, c, can_strings):
     return self.CS.out
 
   def apply(self, c):
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.cruiseControl.cancel, c.hudControl.visualAlert,
-                               c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
+                         c.cruiseControl.cancel, c.hudControl.visualAlert,
+                         c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
     self.frame += 1
-    return can_sends
+    return ret

selfdrive/car/tesla/carcontroller.py

@@ -62,4 +62,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel):
 
     # TODO: HUD control
 
-    return can_sends
+    new_actuators = actuators.copy()
+    new_actuators.steeringAngleDeg = apply_angle
+
+    return actuators, can_sends

selfdrive/car/tesla/interface.py

@@ -71,6 +71,6 @@ def update(self, c, can_strings):
     return self.CS.out
 
   def apply(self, c):
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
+    ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
     self.frame += 1
-    return can_sends
+    return ret