diff --git a/docs/CARS.md b/docs/CARS.md
index 04f285b2e..024158ae2 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -67,6 +67,7 @@ How We Rate The Cars
|Toyota|Prius Prime 2021-22|All|
|||||
|Toyota|RAV4 2019-21|All||||||
|Toyota|RAV4 Hybrid 2019-21|All||||||
+|comma|body|All||||||
## Silver Cars
diff --git a/panda b/panda
index 93863197d..560bcc406 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit 93863197dd3228649e286074f79a0174821a8a81
+Subproject commit 560bcc4063f573e4d5d221235469334a4af14d50
diff --git a/selfdrive/car/body/__init__.py b/selfdrive/car/body/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/selfdrive/car/body/bodycan.py b/selfdrive/car/body/bodycan.py
new file mode 100644
index 000000000..9abf5d861
--- /dev/null
+++ b/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)
diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py
new file mode 100644
index 000000000..1c6adec51
--- /dev/null
+++ b/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
diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py
new file mode 100644
index 000000000..5f663ae95
--- /dev/null
+++ b/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)
diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py
new file mode 100644
index 000000000..325cb366d
--- /dev/null
+++ b/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)
diff --git a/selfdrive/car/body/radar_interface.py b/selfdrive/car/body/radar_interface.py
new file mode 100644
index 000000000..b2f765113
--- /dev/null
+++ b/selfdrive/car/body/radar_interface.py
@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+ pass
diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py
new file mode 100644
index 000000000..c6b0f9213
--- /dev/null
+++ b/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),
+}
diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py
index 635176678..8de7b1d26 100644
--- a/selfdrive/car/docs_definitions.py
+++ b/selfdrive/car/docs_definitions.py
@@ -76,6 +76,9 @@ class CarInfo:
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
diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 08fb21023..596a2128d 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -175,6 +175,13 @@ REQUESTS: List[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,
+ ),
]
diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py
index 2f72b4645..7f7c0c5dc 100644
--- a/selfdrive/car/tests/routes.py
+++ b/selfdrive/car/tests/routes.py
@@ -11,6 +11,7 @@ from selfdrive.car.subaru.values import CAR as SUBARU
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 @@ non_tested_cars = [
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),
diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py
index bbb797498..d885693bd 100755
--- a/selfdrive/car/tests/test_models.py
+++ b/selfdrive/car/tests/test_models.py
@@ -122,6 +122,7 @@ class TestCarModel(unittest.TestCase):
# 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)
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 33412d06f..ebb825c4f 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-98d82b2da600755b12e9ecb2744bf4a8b0942ca1
\ No newline at end of file
+6c72ebf4ef60b895bde5f0700babf150fd97d523
\ No newline at end of file
diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py
index f7dae5c9f..0387952e2 100755
--- a/selfdrive/test/process_replay/test_processes.py
+++ b/selfdrive/test/process_replay/test_processes.py
@@ -12,6 +12,7 @@ from tools.lib.logreader import LogReader
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 @@ original_segments = [
]
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