move car interfaces to opendbc

car/body/bodycan.py

@@ -0,0 +1,7 @@
+def create_control(packer, torque_l, torque_r):
+  values = {
+    "TORQUE_L": torque_l,
+    "TORQUE_R": torque_r,
+  }
+
+  return packer.make_can_msg("TORQUE_CMD", 0, values)

car/body/carcontroller.py

@@ -0,0 +1,84 @@
+import numpy as np
+
+from openpilot.common.realtime import DT_CTRL
+from opendbc.can.packer import CANPacker
+from openpilot.selfdrive.car.body import bodycan
+from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM
+from openpilot.selfdrive.car.interfaces import CarControllerBase
+from openpilot.selfdrive.controls.lib.pid import PIDController
+
+
+MAX_TORQUE = 500
+MAX_TORQUE_RATE = 50
+MAX_ANGLE_ERROR = np.radians(7)
+MAX_POS_INTEGRATOR = 0.2   # meters
+MAX_TURN_INTEGRATOR = 0.1  # meters
+
+
+class CarController(CarControllerBase):
+  def __init__(self, dbc_name, CP, VM):
+    self.frame = 0
+    self.packer = CANPacker(dbc_name)
+
+    # PIDs
+    self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
+    self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
+
+    self.torque_r_filtered = 0.
+    self.torque_l_filtered = 0.
+
+  @staticmethod
+  def deadband_filter(torque, deadband):
+    if torque > 0:
+      torque += deadband
+    else:
+      torque -= deadband
+    return torque
+
+  def update(self, CC, CS, now_nanos):
+
+    torque_l = 0
+    torque_r = 0
+
+    llk_valid = len(CC.orientationNED) > 1 and len(CC.angularVelocity) > 1
+    if CC.enabled and llk_valid:
+      # Read these from the joystick
+      # TODO: this isn't acceleration, okay?
+      speed_desired = CC.actuators.accel / 5.
+      speed_diff_desired = -CC.actuators.steer / 2.
+
+      speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
+      speed_error = speed_desired - speed_measured
+
+      torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False)
+
+      speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
+      turn_error = speed_diff_measured - speed_diff_desired
+      freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or
+                           (turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR))
+      torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator)
+
+      # Combine 2 PIDs outputs
+      torque_r = torque + torque_diff
+      torque_l = torque - torque_diff
+
+      # Torque rate limits
+      self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10),
+                                       self.torque_r_filtered - MAX_TORQUE_RATE,
+                                       self.torque_r_filtered + MAX_TORQUE_RATE)
+      self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10),
+                                       self.torque_l_filtered - MAX_TORQUE_RATE,
+                                       self.torque_l_filtered + MAX_TORQUE_RATE)
+      torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE))
+      torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
+
+    can_sends = []
+    can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))
+
+    new_actuators = CC.actuators.as_builder()
+    new_actuators.accel = torque_l
+    new_actuators.steer = torque_r
+    new_actuators.steerOutputCan = torque_r
+
+    self.frame += 1
+    return new_actuators, can_sends

car/body/carstate.py

@@ -0,0 +1,40 @@
+from cereal import car
+from opendbc.can.parser import CANParser
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.body.values import DBC
+
+STARTUP_TICKS = 100
+
+class CarState(CarStateBase):
+  def update(self, cp):
+    ret = car.CarState.new_message()
+
+    ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
+    ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
+
+    ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor
+
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+    ret.standstill = False
+
+    ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'],
+                                   cp.vl['VAR_VALUES']['FAULT']])
+
+    ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1
+    ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100
+
+    # irrelevant for non-car
+    ret.gearShifter = car.CarState.GearShifter.drive
+    ret.cruiseState.enabled = True
+    ret.cruiseState.available = True
+
+    return ret
+
+  @staticmethod
+  def get_can_parser(CP):
+    messages = [
+      ("MOTORS_DATA", 100),
+      ("VAR_VALUES", 10),
+      ("BODY_DATA", 1),
+    ]
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)

car/body/fingerprints.py

@@ -0,0 +1,28 @@
+# ruff: noqa: E501
+from cereal import car
+from openpilot.selfdrive.car.body.values import CAR
+
+Ecu = car.CarParams.Ecu
+
+# debug ecu fw version is the git hash of the firmware
+
+
+FINGERPRINTS = {
+  CAR.COMMA_BODY: [{
+    513: 8, 516: 8, 514: 3, 515: 4
+  }],
+}
+
+FW_VERSIONS = {
+  CAR.COMMA_BODY: {
+    (Ecu.engine, 0x720, None): [
+      b'0.0.01',
+      b'0.3.00a',
+      b'02/27/2022',
+    ],
+    (Ecu.debug, 0x721, None): [
+      b'166bd860',
+      b'dc780f85',
+    ],
+  },
+}

car/body/interface.py

@@ -0,0 +1,39 @@
+import math
+from cereal import car
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM
+
+class CarInterface(CarInterfaceBase):
+  @staticmethod
+  def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
+    ret.notCar = True
+    ret.carName = "body"
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)]
+
+    ret.minSteerSpeed = -math.inf
+    ret.maxLateralAccel = math.inf  # TODO: set to a reasonable value
+    ret.steerLimitTimer = 1.0
+    ret.steerActuatorDelay = 0.
+
+    ret.wheelSpeedFactor = SPEED_FROM_RPM
+
+    ret.radarUnavailable = True
+    ret.openpilotLongitudinalControl = True
+    ret.steerControlType = car.CarParams.SteerControlType.angle
+
+    return ret
+
+  def _update(self, c):
+    ret = self.CS.update(self.cp)
+
+    # wait for everything to init first
+    if self.frame > int(5. / DT_CTRL):
+      # body always wants to enable
+      ret.init('events', 1)
+      ret.events[0].name = car.CarEvent.EventName.pcmEnable
+      ret.events[0].enable = True
+    self.frame += 1
+
+    return ret

car/body/radar_interface.py

@@ -0,0 +1,4 @@
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+  pass

car/body/values.py

@@ -0,0 +1,40 @@
+from cereal import car
+from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarDocs
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+
+Ecu = car.CarParams.Ecu
+
+SPEED_FROM_RPM = 0.008587
+
+
+class CarControllerParams:
+  ANGLE_DELTA_BP = [0., 5., 15.]
+  ANGLE_DELTA_V = [5., .8, .15]     # windup limit
+  ANGLE_DELTA_VU = [5., 3.5, 0.4]   # unwind limit
+  LKAS_MAX_TORQUE = 1               # A value of 1 is easy to overpower
+  STEER_THRESHOLD = 1.0
+
+  def __init__(self, CP):
+    pass
+
+
+class CAR(Platforms):
+  COMMA_BODY = PlatformConfig(
+    [CarDocs("comma body", package="All")],
+    CarSpecs(mass=9, wheelbase=0.406, steerRatio=0.5, centerToFrontRatio=0.44),
+    dbc_dict('comma_body', None),
+  )
+
+
+FW_QUERY_CONFIG = FwQueryConfig(
+  requests=[
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
+      bus=0,
+    ),
+  ],
+)
+
+DBC = CAR.create_dbc_map()

car/chrysler/carcontroller.py

@@ -0,0 +1,85 @@
+from opendbc.can.packer import CANPacker
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import apply_meas_steer_torque_limits
+from openpilot.selfdrive.car.chrysler import chryslercan
+from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
+from openpilot.selfdrive.car.interfaces import CarControllerBase
+
+
+class CarController(CarControllerBase):
+  def __init__(self, dbc_name, CP, VM):
+    self.CP = CP
+    self.apply_steer_last = 0
+    self.frame = 0
+
+    self.hud_count = 0
+    self.last_lkas_falling_edge = 0
+    self.lkas_control_bit_prev = False
+    self.last_button_frame = 0
+
+    self.packer = CANPacker(dbc_name)
+    self.params = CarControllerParams(CP)
+
+  def update(self, CC, CS, now_nanos):
+    can_sends = []
+
+    lkas_active = CC.latActive and self.lkas_control_bit_prev
+
+    # cruise buttons
+    if (self.frame - self.last_button_frame)*DT_CTRL > 0.05:
+      das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
+
+      # ACC cancellation
+      if CC.cruiseControl.cancel:
+        self.last_button_frame = self.frame
+        can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
+
+      # ACC resume from standstill
+      elif CC.cruiseControl.resume:
+        self.last_button_frame = self.frame
+        can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
+
+    # HUD alerts
+    if self.frame % 25 == 0:
+      if CS.lkas_car_model != -1:
+        can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
+                                                     self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
+        self.hud_count += 1
+
+    # steering
+    if self.frame % self.params.STEER_STEP == 0:
+
+      # TODO: can we make this more sane? why is it different for all the cars?
+      lkas_control_bit = self.lkas_control_bit_prev
+      if CS.out.vEgo > self.CP.minSteerSpeed:
+        lkas_control_bit = True
+      elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
+        if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
+          lkas_control_bit = False
+      elif self.CP.carFingerprint in RAM_CARS:
+        if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
+          lkas_control_bit = False
+
+      # EPS faults if LKAS re-enables too quickly
+      lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200)
+
+      if not lkas_control_bit and self.lkas_control_bit_prev:
+        self.last_lkas_falling_edge = self.frame
+      self.lkas_control_bit_prev = lkas_control_bit
+
+      # steer torque
+      new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
+      apply_steer = apply_meas_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params)
+      if not lkas_active or not lkas_control_bit:
+        apply_steer = 0
+      self.apply_steer_last = apply_steer
+
+      can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
+
+    self.frame += 1
+
+    new_actuators = CC.actuators.as_builder()
+    new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
+    new_actuators.steerOutputCan = self.apply_steer_last
+
+    return new_actuators, can_sends