car port: comma body

docs/CARS.md

@@ -67,6 +67,7 @@ How We Rate The Cars
 |Toyota|Prius Prime 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Toyota|RAV4 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Toyota|RAV4 Hybrid 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
+|comma|body|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 
 ## Silver Cars
 

selfdrive/car/body/bodycan.py

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

selfdrive/car/body/carcontroller.py

@@ -0,0 +1,101 @@
+import numpy as np
+
+from selfdrive.car.body import bodycan
+from opendbc.can.packer import CANPacker
+
+MAX_TORQUE = 500
+MAX_TORQUE_RATE = 50
+MAX_ANGLE_ERROR = 7
+
+class CarController():
+  def __init__(self, dbc_name, CP, VM):
+    self.packer = CANPacker(dbc_name)
+
+    self.i_speed = 0
+
+    self.i_balance = 0
+    self.d_balance = 0
+
+    self.i_torque = 0
+
+    self.speed_measured = 0.
+    self.speed_desired = 0.
+
+    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):
+    if len(CC.orientationNED) == 0 or len(CC.angularVelocity) == 0:
+      return [], CC.actuators.copy()
+
+    # ///////////////////////////////////////
+    # Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM)
+    # Mix steer into torque_diff
+    # self.steerRatio = 0.5
+    # torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000))
+    # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000))
+    # ////
+    # Setpoint speed PID
+    kp_speed = 0.001
+    ki_speed = 0.00001
+    alpha_speed = 1.0
+
+    self.speed_measured = (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
+    self.speed_desired = (1. - alpha_speed)*self.speed_desired
+    p_speed = (self.speed_desired - self.speed_measured)
+    self.i_speed += ki_speed * p_speed
+    self.i_speed = np.clip(self.i_speed, -0.1, 0.1)
+    set_point = p_speed * kp_speed + self.i_speed
+
+    # Balancing PID
+    kp_balance = 1300
+    ki_balance = 0
+    kd_balance = 280
+    alpha_d_balance = 1.0
+
+    # Clip angle error, this is enough to get up from stands
+    p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR))
+    self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr
+    self.d_balance =  np.clip(((1. - alpha_d_balance) * self.d_balance + alpha_d_balance * -CC.angularVelocity[1]), -1., 1.)
+    torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000))
+
+    # Positional recovery PID
+    kp_torque = 0.95
+    ki_torque = 0.1
+
+    p_torque = (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
+    self.i_torque += (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
+    torque_diff = int(np.clip(p_torque*kp_torque + self.i_torque*ki_torque, -100, 100))
+
+    # 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.copy()
+    new_actuators.accel = torque_l
+    new_actuators.steer = torque_r
+
+    return new_actuators, can_sends

selfdrive/car/body/carstate.py

@@ -0,0 +1,54 @@
+from cereal import car
+from opendbc.can.parser import CANParser
+from selfdrive.car.interfaces import CarStateBase
+from selfdrive.car.body.values import DBC
+
+
+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 = abs(ret.vEgo) < 1
+
+    # irrelevant for non-car
+    ret.doorOpen = False
+    ret.seatbeltUnlatched = False
+    ret.gearShifter = car.CarState.GearShifter.drive
+    ret.steeringTorque = 0
+    ret.steeringAngleDeg = 0
+    ret.steeringPressed = False
+    ret.cruiseState.enabled = True
+    ret.cruiseState.available = True
+    ret.cruiseState.speed = 0
+
+    return ret
+
+  @staticmethod
+  def get_can_parser(CP):
+    signals = [
+      # sig_name, sig_address
+      ("SPEED_L", "MOTORS_DATA"),
+      ("SPEED_R", "MOTORS_DATA"),
+      ("ELEC_ANGLE_L", "MOTORS_DATA"),
+      ("ELEC_ANGLE_R", "MOTORS_DATA"),
+      ("MOTOR_ERR_L", "MOTORS_DATA"),
+      ("MOTOR_ERR_R", "MOTORS_DATA"),
+      ("IGNITION", "VAR_VALUES"),
+      ("ENABLE_MOTORS", "VAR_VALUES"),
+      ("MCU_TEMP", "BODY_DATA"),
+      ("BATT_VOLTAGE", "BODY_DATA"),
+    ]
+
+    checks = [
+      ("MOTORS_DATA", 100),
+      ("VAR_VALUES", 10),
+      ("BODY_DATA", 1),
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)

selfdrive/car/body/interface.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+from cereal import car
+from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config
+from selfdrive.car.interfaces import CarInterfaceBase
+
+class CarInterface(CarInterfaceBase):
+  @staticmethod
+  def get_params(candidate, fingerprint=None, car_fw=None, disable_radar=False):
+
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
+
+    ret.carName = "body"
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)]
+
+    ret.steerRatio = 0.5
+    ret.steerRateCost = 0.5
+    ret.steerLimitTimer = 1.0
+    ret.steerActuatorDelay = 0.
+
+    ret.mass = 9
+    ret.wheelbase = 0.406
+    ret.wheelSpeedFactor = 0.008587
+    ret.centerToFront = ret.wheelbase * 0.44
+
+    ret.radarOffCan = True
+    ret.openpilotLongitudinalControl = True
+    ret.steerControlType = car.CarParams.SteerControlType.angle
+
+    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+
+    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
+
+    return ret
+
+  def update(self, c, can_strings):
+    self.cp.update_strings(can_strings)
+
+    ret = self.CS.update(self.cp)
+
+    ret.canValid = self.cp.can_valid
+
+    self.CS.out = ret.as_reader()
+    return self.CS.out
+
+  def apply(self, c):
+    return self.CC.update(c, self.CS)

selfdrive/car/body/radar_interface.py

@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+  pass

selfdrive/car/body/values.py

@@ -0,0 +1,33 @@
+from typing import Dict
+
+from cereal import car
+from selfdrive.car import dbc_dict
+from selfdrive.car.docs_definitions import CarInfo
+Ecu = car.CarParams.Ecu
+
+
+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
+
+class CAR:
+  BODY = "COMMA BODY"
+
+CAR_INFO: Dict[str, CarInfo] = {
+  CAR.BODY: CarInfo("comma body", package="All", good_torque=True),
+}
+
+FW_VERSIONS = {
+  CAR.BODY: {
+    (Ecu.engine, 0x720, None): [
+      b'02/27/2022'
+    ],
+  },
+}
+
+DBC = {
+  CAR.BODY: dbc_dict('comma_body', None),
+}

selfdrive/car/docs_definitions.py

@@ -76,6 +76,9 @@ def init(self, CP: car.CarParams, non_tested_cars: List[str], all_footnotes: Dic
       Column.MAINTAINED: CP.carFingerprint not in non_tested_cars,
     }
 
+    if self.name == "comma body":
+      self.row[Column.LONGITUDINAL] = True
+
     self.all_footnotes = all_footnotes
     for column in StarColumns:
       self.row[column] = Star.FULL if self.row[column] else Star.EMPTY

selfdrive/car/fw_versions.py

@@ -175,6 +175,13 @@ class Request:
     [NISSAN_VERSION_RESPONSE_STANDARD],
     rx_offset=NISSAN_RX_OFFSET,
   ),
+  # Body
+  Request(
+    "body",
+    [TESTER_PRESENT_REQUEST, UDS_VERSION_REQUEST],
+    [TESTER_PRESENT_RESPONSE, UDS_VERSION_RESPONSE],
+    bus=0,
+  ),
 ]
 
 

selfdrive/car/tests/routes.py

@@ -11,6 +11,7 @@
 from selfdrive.car.toyota.values import CAR as TOYOTA
 from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN
 from selfdrive.car.tesla.values import CAR as TESLA
+from selfdrive.car.body.values import CAR as COMMA
 
 # TODO: add routes for these cars
 non_tested_cars = [
@@ -25,6 +26,8 @@
 TestRoute = namedtuple('TestRoute', ['route', 'car_fingerprint'])
 
 routes = [
+  TestRoute("d6ac8ebdb47bc549|2022-03-31--13-10-06", COMMA.BODY),
+
   TestRoute("0c94aa1e1296d7c6|2021-05-05--19-48-37", CHRYSLER.JEEP_CHEROKEE),
   TestRoute("91dfedae61d7bd75|2021-05-22--20-07-52", CHRYSLER.JEEP_CHEROKEE_2019),
   TestRoute("420a8e183f1aed48|2020-03-05--07-15-29", CHRYSLER.PACIFICA_2017_HYBRID),

selfdrive/car/tests/test_models.py

@@ -122,6 +122,7 @@ def test_car_interface(self):
     # TODO: also check for checkusm and counter violations from can parser
     can_invalid_cnt = 0
     CC = car.CarControl.new_message()
+
     for i, msg in enumerate(self.can_msgs):
       CS = self.CI.update(CC, (msg.as_builder().to_bytes(),))
       self.CI.apply(CC)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-98d82b2da600755b12e9ecb2744bf4a8b0942ca1
+6c72ebf4ef60b895bde5f0700babf150fd97d523

selfdrive/test/process_replay/test_processes.py

@@ -12,6 +12,7 @@
 
 
 original_segments = [
+  ("BODY", "d6ac8ebdb47bc549|2022-03-31--13-10-06--4"),        # COMMA.BODY
   ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"),     # HYUNDAI.SONATA
   ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"),     # TOYOTA.PRIUS (INDI)
   ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"),     # TOYOTA.RAV4  (LQR)
@@ -30,6 +31,7 @@
 ]
 
 segments = [
+  ("BODY", "d6ac8ebdb47bc549|2022-03-31--13-10-06--4"),
   ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"),
   ("TOYOTA", "fakedata|2022-01-20--17-50-51--0"),
   ("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"),
@@ -67,6 +69,10 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None):
   # check to make sure openpilot is engaged in the route
   if cfg.proc_name == "controlsd":
     for msg in log_msgs:
+      if msg.which() == "carParams":
+        # body doesn't enable
+        if msg.carParams.carName == "body":
+          break
       if msg.which() == "controlsState":
         if msg.controlsState.active:
           break