mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-23 15:02:06 +08:00
car port: comma body (#24019)
* body FPv2 * .. * .. * .. * Temp, REVERT! * more cleanup * typo * .. * del eyes * should work? * fix * new dbc * .. * fixes * static analysis * cln balancing code * no test route * excluded_interfaces * THE DOCS! * comments on steer/speed mixin * switch to bus 0 * less UDS * FAKE bus 0 vin and fingerprint * FAKE locationd * Keep steady and remove handcoded offset * Improve startup sequence, get closer to stock openpilot * Forgot to define angle * lowercase * revert that * little cleanup * change safety model * update refs * body is gold * handle no llk * oops * litte more Co-authored-by: Comma Device <device@comma.ai> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
This commit is contained in:
Igor Biletskyy
@@ -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
|
||||
|
||||
|
||||
+1
-1
Submodule panda updated: 93863197dd...560bcc4063
@@ -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)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
@@ -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),
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -1 +1 @@
|
||||
98d82b2da600755b12e9ecb2744bf4a8b0942ca1
|
||||
6c72ebf4ef60b895bde5f0700babf150fd97d523
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user