brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.9.6 release

Browse Source 
date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

238
selfdrive/car/__init__.py Normal file
View File

@@ -0,0 +1,238 @@
# functions common among cars
from collections import namedtuple
from typing import Dict, List, Optional
import capnp
from cereal import car
from openpilot.common.numpy_fast import clip, interp
# kg of standard extra cargo to count for drive, gas, etc...
STD_CARGO_KG = 136.
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])
def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
if val > val_steady + hyst_gap:
val_steady = val - hyst_gap
elif val < val_steady - hyst_gap:
val_steady = val + hyst_gap
return val_steady
def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule],
unpressed_btn: int = 0) -> List[capnp.lib.capnp._DynamicStructBuilder]:
events: List[capnp.lib.capnp._DynamicStructBuilder] = []
if cur_btn == prev_btn:
return events
# Add events for button presses, multiple when a button switches without going to unpressed
for pressed, btn in ((False, prev_btn), (True, cur_btn)):
if btn != unpressed_btn:
events.append(car.CarState.ButtonEvent(pressed=pressed,
type=buttons_dict.get(btn, ButtonType.unknown)))
return events
def gen_empty_fingerprint():
return {i: {} for i in range(8)}
# these params were derived for the Civic and used to calculate params for other cars
class VehicleDynamicsParams:
MASS = 1326. + STD_CARGO_KG
WHEELBASE = 2.70
CENTER_TO_FRONT = WHEELBASE * 0.4
CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT
ROTATIONAL_INERTIA = 2500
TIRE_STIFFNESS_FRONT = 192150
TIRE_STIFFNESS_REAR = 202500
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
def scale_rot_inertia(mass, wheelbase):
return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
center_to_rear = wheelbase - center_to_front
tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
(center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
(center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
return tire_stiffness_front, tire_stiffness_rear
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]:
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc}
def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS):
# limits due to driver torque
driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER
driver_min_torque = -LIMITS.STEER_MAX + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER
max_steer_allowed = max(min(LIMITS.STEER_MAX, driver_max_torque), 0)
min_steer_allowed = min(max(-LIMITS.STEER_MAX, driver_min_torque), 0)
apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed)
# slow rate if steer torque increases in magnitude
if apply_torque_last > 0:
apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP),
apply_torque_last + LIMITS.STEER_DELTA_UP)
else:
apply_torque = clip(apply_torque, apply_torque_last - LIMITS.STEER_DELTA_UP,
min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP))
return int(round(float(apply_torque)))
def apply_dist_to_meas_limits(val, val_last, val_meas,
STEER_DELTA_UP, STEER_DELTA_DOWN,
STEER_ERROR_MAX, STEER_MAX):
# limits due to comparison of commanded val VS measured val (torque/angle/curvature)
max_lim = min(max(val_meas + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
min_lim = max(min(val_meas - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)
val = clip(val, min_lim, max_lim)
# slow rate if val increases in magnitude
if val_last > 0:
val = clip(val,
max(val_last - STEER_DELTA_DOWN, -STEER_DELTA_UP),
val_last + STEER_DELTA_UP)
else:
val = clip(val,
val_last - STEER_DELTA_UP,
min(val_last + STEER_DELTA_DOWN, STEER_DELTA_UP))
return float(val)
def apply_meas_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS):
return int(round(apply_dist_to_meas_limits(apply_torque, apply_torque_last, motor_torque,
LIMITS.STEER_DELTA_UP, LIMITS.STEER_DELTA_DOWN,
LIMITS.STEER_ERROR_MAX, LIMITS.STEER_MAX)))
def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS):
# pick angle rate limits based on wind up/down
steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last)
rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN
angle_rate_lim = interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v)
return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim)
def common_fault_avoidance(fault_condition: bool, request: bool, above_limit_frames: int,
max_above_limit_frames: int, max_mismatching_frames: int = 1):
"""
Several cars have the ability to work around their EPS limits by cutting the
request bit of their LKAS message after a certain number of frames above the limit.
"""
# Count up to max_above_limit_frames, at which point we need to cut the request for above_limit_frames to avoid a fault
if request and fault_condition:
above_limit_frames += 1
else:
above_limit_frames = 0
# Once we cut the request bit, count additionally to max_mismatching_frames before setting the request bit high again.
# Some brands do not respect our workaround without multiple messages on the bus, for example
if above_limit_frames > max_above_limit_frames:
request = False
if above_limit_frames >= max_above_limit_frames + max_mismatching_frames:
above_limit_frames = 0
return above_limit_frames, request
def crc8_pedal(data):
crc = 0xFF # standard init value
poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1
size = len(data)
for i in range(size - 1, -1, -1):
crc ^= data[i]
for _ in range(8):
if ((crc & 0x80) != 0):
crc = ((crc << 1) ^ poly) & 0xFF
else:
crc <<= 1
return crc
def create_gas_interceptor_command(packer, gas_amount, idx):
# Common gas pedal msg generator
enable = gas_amount > 0.001
values = {
"ENABLE": enable,
"COUNTER_PEDAL": idx & 0xF,
}
if enable:
values["GAS_COMMAND"] = gas_amount * 255.
values["GAS_COMMAND2"] = gas_amount * 255.
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
checksum = crc8_pedal(dat[:-1])
values["CHECKSUM_PEDAL"] = checksum
return packer.make_can_msg("GAS_COMMAND", 0, values)
def make_can_msg(addr, dat, bus):
return [addr, 0, dat, bus]
def get_safety_config(safety_model, safety_param = None):
ret = car.CarParams.SafetyConfig.new_message()
ret.safetyModel = safety_model
if safety_param is not None:
ret.safetyParam = safety_param
return ret
class CanBusBase:
offset: int
def __init__(self, CP, fingerprint: Optional[Dict[int, Dict[int, int]]]) -> None:
if CP is None:
assert fingerprint is not None
num = max([k for k, v in fingerprint.items() if len(v)], default=0) // 4 + 1
else:
num = len(CP.safetyConfigs)
self.offset = 4 * (num - 1)
class CanSignalRateCalculator:
"""
Calculates the instantaneous rate of a CAN signal by using the counter
variable and the known frequency of the CAN message that contains it.
"""
def __init__(self, frequency):
self.frequency = frequency
self.previous_counter = 0
self.previous_value = 0
self.rate = 0
def update(self, current_value, current_counter):
if current_counter != self.previous_counter:
self.rate = (current_value - self.previous_value) * self.frequency
self.previous_counter = current_counter
self.previous_value = current_value
return self.rate

0
selfdrive/car/body/__init__.py Normal file
View File

7
selfdrive/car/body/bodycan.py Normal file
View File

@@ -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)

99
selfdrive/car/body/carcontroller.py Normal file
View File

@@ -0,0 +1,99 @@
import numpy as np
from openpilot.common.params import Params
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.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:
def __init__(self, dbc_name, CP, VM):
self.frame = 0
self.packer = CANPacker(dbc_name)
# Speed, balance and turn PIDs
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL)
self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL)
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.
params = Params()
self.wheeled_body = params.get("WheeledBody")
@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
if self.wheeled_body is None:
freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or
(speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
# Clip angle error, this is enough to get up from stands
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate)
else:
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.copy()
new_actuators.accel = torque_l
new_actuators.steer = torque_r
new_actuators.steerOutputCan = torque_r
self.frame += 1
return new_actuators, can_sends

40
selfdrive/car/body/carstate.py Normal file
View File

@@ -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)

28
selfdrive/car/body/fingerprints.py Normal file
View File

@@ -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.BODY: [{
513: 8, 516: 8, 514: 3, 515: 4
}],
}
FW_VERSIONS = {
CAR.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',
],
},
}

46
selfdrive/car/body/interface.py Normal file
View File

@@ -0,0 +1,46 @@
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.steerRatio = 0.5
ret.steerLimitTimer = 1.0
ret.steerActuatorDelay = 0.
ret.mass = 9
ret.wheelbase = 0.406
ret.wheelSpeedFactor = SPEED_FROM_RPM
ret.centerToFront = ret.wheelbase * 0.44
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
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

4
selfdrive/car/body/radar_interface.py Normal file
View File

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

47
selfdrive/car/body/values.py Normal file
View File

@@ -0,0 +1,47 @@
from enum import StrEnum
from typing import Dict
from cereal import car
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarInfo
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(StrEnum):
BODY = "COMMA BODY"
CAR_INFO: Dict[str, CarInfo] = {
CAR.BODY: CarInfo("comma body", package="All"),
}
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.BODY: dbc_dict('comma_body', None),
}

209
selfdrive/car/car_helpers.py Normal file
View File

@@ -0,0 +1,209 @@
import os
import time
from typing import Callable, Dict, List, Optional, Tuple
from cereal import car
from openpilot.common.params import Params
from openpilot.common.basedir import BASEDIR
from openpilot.system.version import is_comma_remote, is_tested_branch
from openpilot.selfdrive.car.interfaces import get_interface_attr
from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
from openpilot.common.swaglog import cloudlog
import cereal.messaging as messaging
from openpilot.selfdrive.car import gen_empty_fingerprint
FRAME_FINGERPRINT = 100 # 1s
EventName = car.CarEvent.EventName
def get_startup_event(car_recognized, controller_available, fw_seen):
if is_comma_remote() and is_tested_branch():
event = EventName.startup
else:
event = EventName.startupMaster
if not car_recognized:
if fw_seen:
event = EventName.startupNoCar
else:
event = EventName.startupNoFw
elif car_recognized and not controller_available:
event = EventName.startupNoControl
return event
def get_one_can(logcan):
while True:
can = messaging.recv_one_retry(logcan)
if len(can.can) > 0:
return can
def load_interfaces(brand_names):
ret = {}
for brand_name in brand_names:
path = f'openpilot.selfdrive.car.{brand_name}'
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'):
CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState
else:
CarState = None
if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'):
CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController
else:
CarController = None
for model_name in brand_names[brand_name]:
ret[model_name] = (CarInterface, CarController, CarState)
return ret
def _get_interface_names() -> Dict[str, List[str]]:
# returns a dict of brand name and its respective models
brand_names = {}
for brand_name, brand_models in get_interface_attr("CAR").items():
brand_names[brand_name] = [model.value for model in brand_models]
return brand_names
# imports from directory selfdrive/car/<name>/
interface_names = _get_interface_names()
interfaces = load_interfaces(interface_names)
def can_fingerprint(next_can: Callable) -> Tuple[Optional[str], Dict[int, dict]]:
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
frame = 0
car_fingerprint = None
done = False
while not done:
a = next_can()
for can in a.can:
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
for b in candidate_cars:
# Ignore extended messages and VIN query response.
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
return car_fingerprint, finger
# **** for use live only ****
def fingerprint(logcan, sendcan, num_pandas):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False)
ecu_rx_addrs = set()
params = Params()
start_time = time.monotonic()
if not skip_fw_query:
# Vin query only reliably works through OBDII
bus = 1
cached_params = params.get("CarParamsCache")
if cached_params is not None:
with car.CarParams.from_bytes(cached_params) as cached_params:
if cached_params.carName == "mock":
cached_params = None
if cached_params is not None and len(cached_params.carFw) > 0 and \
cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache:
cloudlog.warning("Using cached CarParams")
vin, vin_rx_addr = cached_params.carVin, 0
car_fw = list(cached_params.carFw)
cached = True
else:
cloudlog.warning("Getting VIN & FW versions")
set_obd_multiplexing(params, True)
vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas)
car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas)
cached = False
exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
else:
vin, vin_rx_addr = VIN_UNKNOWN, 0
exact_fw_match, fw_candidates, car_fw = True, set(), []
cached = False
if not is_valid_vin(vin):
cloudlog.event("Malformed VIN", vin=vin, error=True)
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
params.put("CarVin", vin)
# disable OBD multiplexing for potential ECU knockouts
set_obd_multiplexing(params, False)
params.put_bool("FirmwareQueryDone", True)
fw_query_time = time.monotonic() - start_time
# CAN fingerprint
# drain CAN socket so we get the latest messages
messaging.drain_sock_raw(logcan)
car_fingerprint, finger = can_fingerprint(lambda: get_one_can(logcan))
exact_match = True
source = car.CarParams.FingerprintSource.can
# If FW query returns exactly 1 candidate, use it
if len(fw_candidates) == 1:
car_fingerprint = list(fw_candidates)[0]
source = car.CarParams.FingerprintSource.fw
exact_match = exact_fw_match
if fixed_fingerprint:
car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed
cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, fingerprints=finger,
fw_query_time=fw_query_time, error=True)
return car_fingerprint, finger, vin, car_fw, source, exact_match
def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas)
if candidate is None:
cloudlog.event("car doesn't match any fingerprints", fingerprints=fingerprints, error=True)
candidate = "mock"
CarInterface, CarController, CarState = interfaces[candidate]
CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False)
CP.carVin = vin
CP.carFw = car_fw
CP.fingerprintSource = source
CP.fuzzyFingerprint = not exact_match
return CarInterface(CP, CarController, CarState), CP

0
selfdrive/car/chrysler/__init__.py Normal file
View File

84
selfdrive/car/chrysler/carcontroller.py Normal file
View File

@@ -0,0 +1,84 @@
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
class CarController:
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.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
return new_actuators, can_sends

150
selfdrive/car/chrysler/carstate.py Normal file
View File

@@ -0,0 +1,150 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.CP = CP
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.auto_high_beam = 0
self.button_counter = 0
self.lkas_car_model = -1
if CP.carFingerprint in RAM_CARS:
self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"]
else:
self.shifter_values = can_define.dv["GEAR"]["PRNDL"]
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
# lock info
ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"],
cp.vl["BCM_1"]["DOOR_OPEN_FR"],
cp.vl["BCM_1"]["DOOR_OPEN_RL"],
cp.vl["BCM_1"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["ORC_1"]["SEATBELT_DRIVER_UNLATCHED"] == 1
# brake pedal
ret.brake = 0
ret.brakePressed = cp.vl["ESP_1"]['Brake_Pedal_State'] == 1 # Physical brake pedal switch
# gas pedal
ret.gas = cp.vl["ECM_5"]["Accelerator_Position"]
ret.gasPressed = ret.gas > 1e-5
# car speed
if self.CP.carFingerprint in RAM_CARS:
ret.vEgoRaw = cp.vl["ESP_8"]["Vehicle_Speed"] * CV.KPH_TO_MS
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["Transmission_Status"]["Gear_State"], None))
else:
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None))
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["ESP_6"]["WHEEL_SPEED_FL"],
cp.vl["ESP_6"]["WHEEL_SPEED_FR"],
cp.vl["ESP_6"]["WHEEL_SPEED_RL"],
cp.vl["ESP_6"]["WHEEL_SPEED_RR"],
unit=1,
)
# button presses
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1,
cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2)
ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1
# steering wheel
ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"] + cp.vl["STEERING"]["STEERING_ANGLE_HP"]
ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"]
ret.steeringTorque = cp.vl["EPS_2"]["COLUMN_TORQUE"]
ret.steeringTorqueEps = cp.vl["EPS_2"]["EPS_TORQUE_MOTOR"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# cruise state
cp_cruise = cp_cam if self.CP.carFingerprint in RAM_CARS else cp
ret.cruiseState.available = cp_cruise.vl["DAS_3"]["ACC_AVAILABLE"] == 1
ret.cruiseState.enabled = cp_cruise.vl["DAS_3"]["ACC_ACTIVE"] == 1
ret.cruiseState.speed = cp_cruise.vl["DAS_4"]["ACC_SET_SPEED_KPH"] * CV.KPH_TO_MS
ret.cruiseState.nonAdaptive = cp_cruise.vl["DAS_4"]["ACC_STATE"] in (1, 2) # 1 NormalCCOn and 2 NormalCCSet
ret.cruiseState.standstill = cp_cruise.vl["DAS_3"]["ACC_STANDSTILL"] == 1
ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0
if self.CP.carFingerprint in RAM_CARS:
# Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message
self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON']
ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1
else:
ret.steerFaultTemporary = cp.vl["EPS_2"]["LKAS_TEMPORARY_FAULT"] == 1
ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4
# blindspot sensors
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1
ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
return ret
@staticmethod
def get_cruise_messages():
messages = [
("DAS_3", 50),
("DAS_4", 50),
]
return messages
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("ESP_1", 50),
("EPS_2", 100),
("ESP_6", 50),
("STEERING", 100),
("ECM_5", 50),
("CRUISE_BUTTONS", 50),
("STEERING_LEVERS", 10),
("ORC_1", 2),
("BCM_1", 1),
]
if CP.enableBsm:
messages.append(("BSM_1", 2))
if CP.carFingerprint in RAM_CARS:
messages += [
("ESP_8", 50),
("EPS_3", 50),
("Transmission_Status", 50),
]
else:
messages += [
("GEAR", 50),
("SPEED_1", 100),
]
messages += CarState.get_cruise_messages()
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
messages = [
("DAS_6", 4),
]
if CP.carFingerprint in RAM_CARS:
messages += CarState.get_cruise_messages()
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)

71
selfdrive/car/chrysler/chryslercan.py Normal file
View File

@@ -0,0 +1,71 @@
from cereal import car
from openpilot.selfdrive.car.chrysler.values import RAM_CARS
GearShifter = car.CarState.GearShifter
VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam):
# LKAS_HUD - Controls what lane-keeping icon is displayed
# == Color ==
# 0 hidden?
# 1 white
# 2 green
# 3 ldw
# == Lines ==
# 03 white Lines
# 04 grey lines
# 09 left lane close
# 0A right lane close
# 0B left Lane very close
# 0C right Lane very close
# 0D left cross cross
# 0E right lane cross
# == Alerts ==
# 7 Normal
# 6 lane departure place hands on wheel
color = 2 if lkas_active else 1
lines = 3 if lkas_active else 0
alerts = 7 if lkas_active else 0
if hud_count < (1 * 4): # first 3 seconds, 4Hz
alerts = 1
if hud_alert in (VisualAlert.ldw, VisualAlert.steerRequired):
color = 4
lines = 0
alerts = 6
values = {
"LKAS_ICON_COLOR": color,
"CAR_MODEL": car_model,
"LKAS_LANE_LINES": lines,
"LKAS_ALERTS": alerts,
}
if CP.carFingerprint in RAM_CARS:
values['AUTO_HIGH_BEAM_ON'] = auto_high_beam
return packer.make_can_msg("DAS_6", 0, values)
def create_lkas_command(packer, CP, apply_steer, lkas_control_bit):
# LKAS_COMMAND Lane-keeping signal to turn the wheel
enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1
values = {
"STEERING_TORQUE": apply_steer,
"LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0,
}
return packer.make_can_msg("LKAS_COMMAND", 0, values)
def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False):
values = {
"ACC_Cancel": cancel,
"ACC_Resume": resume,
"COUNTER": frame % 0x10,
}
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)

257
selfdrive/car/chrysler/fingerprints.py Normal file
View File

@@ -0,0 +1,257 @@
# ruff: noqa: E501
from cereal import car
from openpilot.selfdrive.car.chrysler.values import CAR
Ecu = car.CarParams.Ecu
# Unique CAN messages:
# Only the hybrids have 270: 8
# Only the gas have 55: 8, 416: 7
# For 564, All 2017 have length 4, whereas 2018-19 have length 8.
# For 924, Pacifica 2017 has length 3, whereas all 2018-19 have length 8.
# For 560, All 2019 have length 8, whereas all 2017-18 have length 4.
#
# Jeep Grand Cherokee unique messages:
# 2017 Trailhawk: 618: 8
# For 924, Trailhawk 2017 has length 3, whereas 2018 V6 has length 8.
FINGERPRINTS = {
CAR.PACIFICA_2017_HYBRID: [{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 840: 8, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8
}],
CAR.PACIFICA_2018: [{
55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
},
{
55: 8, 58: 6, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 956: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
}],
CAR.PACIFICA_2020: [{
55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.PACIFICA_2018_HYBRID: [{
68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8
},
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
}],
CAR.PACIFICA_2019_HYBRID: [{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8
},
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
},
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8
},
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1536: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
},
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 450: 8, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2018: 8, 2020: 8, 2026: 8, 2028: 8
}],
CAR.JEEP_GRAND_CHEROKEE: [{
55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 1576: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
},
{
257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 678: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1235: 8, 1242: 8, 1252: 8, 1792: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1892: 8, 1927: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.JEEP_GRAND_CHEROKEE_2019: [{
55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1538: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
}
FW_VERSIONS = {
CAR.JEEP_GRAND_CHEROKEE_2019: {
(Ecu.combinationMeter, 0x742, None): [
b'68402971AD',
],
(Ecu.srs, 0x744, None): [
b'68355363AB',
],
(Ecu.abs, 0x747, None): [
b'68408639AD',
],
(Ecu.fwdRadar, 0x753, None): [
b'68456722AC',
],
(Ecu.eps, 0x75a, None): [
b'68453431AA',
],
(Ecu.engine, 0x7e0, None): [
b'05035674AB ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035707AA',
],
},
CAR.RAM_1500: {
(Ecu.combinationMeter, 0x742, None): [
b'68294051AG',
b'68294051AI',
b'68294052AG',
b'68294063AG',
b'68294063AH',
b'68294063AI',
b'68434846AC',
b'68434858AC',
b'68434860AC',
b'68453503AC',
b'68453505AC',
b'68453511AC',
b'68453513AD',
b'68453514AD',
b'68510280AG',
b'68510283AG',
b'68527346AE',
b'68527375AD',
b'68527382AE',
],
(Ecu.srs, 0x744, None): [
b'68428609AB',
b'68441329AB',
b'68473844AB',
b'68490898AA',
b'68500728AA',
b'68615033AA',
b'68615034AA',
],
(Ecu.abs, 0x747, None): [
b'68292406AH',
b'68432418AB',
b'68432418AD',
b'68436004AD',
b'68436004AE',
b'68438454AC',
b'68438454AD',
b'68438456AE',
b'68438456AF',
b'68535469AB',
b'68535470AC',
b'68548900AB',
b'68586307AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672892AB',
b'04672932AB',
b'04672932AC',
b'22DTRHD_AA',
b'68320950AH',
b'68320950AI',
b'68320950AJ',
b'68320950AL',
b'68320950AM',
b'68454268AB',
b'68475160AE',
b'68475160AF',
b'68475160AG',
],
(Ecu.eps, 0x75a, None): [
b'21590101AA',
b'21590101AB',
b'68273275AF',
b'68273275AG',
b'68273275AH',
b'68312176AE',
b'68312176AG',
b'68440789AC',
b'68466110AB',
b'68469901AA',
b'68522583AB',
b'68522585AB',
b'68552788AA',
b'68552789AA',
b'68552790AA',
b'68585106AB',
b'68585109AB',
b'68585112AB',
],
(Ecu.engine, 0x7e0, None): [
b'05036065AE ',
b'05036066AE ',
b'05149591AD ',
b'05149592AE ',
b'05149846AA ',
b'05149848AA ',
b'68378701AI ',
b'68378748AL ',
b'68378758AM ',
b'68448163AJ',
b'68448165AK',
b'68500630AD',
b'68500630AE',
b'68539650AD',
],
(Ecu.transmission, 0x7e1, None): [
b'05149536AC',
b'68360078AL',
b'68360080AM',
b'68360081AM',
b'68360085AL',
b'68384328AD',
b'68384332AD',
b'68445533AB',
b'68484467AC',
b'68502994AD',
b'68520867AE',
b'68540431AB',
],
},
CAR.RAM_HD: {
(Ecu.combinationMeter, 0x742, None): [
b'68361606AH',
b'68437735AC',
b'68492693AD',
b'68525485AB',
b'68525487AB',
b'68525498AB',
b'68528791AF',
b'68628474AB',
],
(Ecu.srs, 0x744, None): [
b'68399794AC',
b'68428503AA',
b'68428505AA',
b'68428507AA',
],
(Ecu.abs, 0x747, None): [
b'68334977AH',
b'68455481AC',
b'68504022AA',
b'68504022AB',
b'68504022AC',
b'68530686AB',
b'68530686AC',
b'68544596AC',
b'68641704AA',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672895AB',
b'04672934AB',
b'56029827AG',
b'56029827AH',
b'68462657AE',
b'68484694AD',
b'68484694AE',
b'68615489AB',
],
(Ecu.eps, 0x761, None): [
b'68421036AC',
b'68507906AB',
b'68534023AC',
],
(Ecu.engine, 0x7e0, None): [
b'52370131AF',
b'52370231AF',
b'52370231AG',
b'52370491AA',
b'52370931CT',
b'52401032AE',
b'52421132AF',
b'52421332AF',
b'68527616AD ',
b'M2370131MB',
b'M2421132MB',
],
},
}

109
selfdrive/car/chrysler/interface.py Executable file
View File

@@ -0,0 +1,109 @@
#!/usr/bin/env python3
from cereal import car
from panda import Panda
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "chrysler"
ret.dashcamOnly = candidate in RAM_HD
# radar parsing needs some work, see https://github.com/commaai/openpilot/issues/26842
ret.radarUnavailable = True # DBC[candidate]['radar'] is None
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.4
# safety config
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler)]
if candidate in RAM_HD:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_HD
elif candidate in RAM_DT:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT
ret.minSteerSpeed = 3.8 # m/s
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate not in RAM_CARS:
# Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed.
new_eps_platform = candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019)
new_eps_firmware = any(fw.ecu == 'eps' and fw.fwVersion[:4] >= b"6841" for fw in car_fw)
if new_eps_platform or new_eps_firmware:
ret.flags |= ChryslerFlags.HIGHER_MIN_STEERING_SPEED.value
# Chrysler
if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020):
ret.mass = 2242.
ret.wheelbase = 3.089
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
ret.lateralTuning.pid.kf = 0.00006
# Jeep
elif candidate in (CAR.JEEP_GRAND_CHEROKEE, CAR.JEEP_GRAND_CHEROKEE_2019):
ret.mass = 1778
ret.wheelbase = 2.71
ret.steerRatio = 16.7
ret.steerActuatorDelay = 0.2
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
ret.lateralTuning.pid.kf = 0.00006
# Ram
elif candidate == CAR.RAM_1500:
ret.steerActuatorDelay = 0.2
ret.wheelbase = 3.88
ret.steerRatio = 16.3
ret.mass = 2493.
ret.minSteerSpeed = 14.5
# Older EPS FW allow steer to zero
if any(fw.ecu == 'eps' and b"68" < fw.fwVersion[:4] <= b"6831" for fw in car_fw):
ret.minSteerSpeed = 0.
elif candidate == CAR.RAM_HD:
ret.steerActuatorDelay = 0.2
ret.wheelbase = 3.785
ret.steerRatio = 15.61
ret.mass = 3405.
ret.minSteerSpeed = 16
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False)
else:
raise ValueError(f"Unsupported car: {candidate}")
if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
# TODO: allow these cars to steer down to 13 m/s if already engaged.
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
ret.centerToFront = ret.wheelbase * 0.44
ret.enableBsm = 720 in fingerprint[0]
return ret
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5):
self.low_speed_alert = True
elif ret.vEgo > (self.CP.minSteerSpeed + 1.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

86
selfdrive/car/chrysler/radar_interface.py Executable file
View File

@@ -0,0 +1,86 @@
#!/usr/bin/env python3
from opendbc.can.parser import CANParser
from cereal import car
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
from openpilot.selfdrive.car.chrysler.values import DBC
RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724
RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
def _create_radar_can_parser(car_fingerprint):
dbc = DBC[car_fingerprint]['radar']
if dbc is None:
return None
msg_n = len(RADAR_MSGS_C)
# list of [(signal name, message name or number), (...)]
# [('RADAR_STATE', 1024),
# ('LONG_DIST', 1072),
# ('LONG_DIST', 1073),
# ('LONG_DIST', 1074),
# ('LONG_DIST', 1075),
messages = list(zip(RADAR_MSGS_C +
RADAR_MSGS_D,
[20] * msg_n + # 20Hz (0.05s)
[20] * msg_n, strict=True)) # 20Hz (0.05s)
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
def _address_to_track(address):
if address in RADAR_MSGS_C:
return (address - RADAR_MSGS_C[0]) // 2
if address in RADAR_MSGS_D:
return (address - RADAR_MSGS_D[0]) // 2
raise ValueError("radar received unexpected address %d" % address)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.CP = CP
self.rcp = _create_radar_can_parser(CP.carFingerprint)
self.updated_messages = set()
self.trigger_msg = LAST_MSG
def update(self, can_strings):
if self.rcp is None or self.CP.radarUnavailable:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
for ii in self.updated_messages: # ii should be the message ID as a number
cpt = self.rcp.vl[ii]
trackId = _address_to_track(ii)
if trackId not in self.pts:
self.pts[trackId] = car.RadarData.RadarPoint.new_message()
self.pts[trackId].trackId = trackId
self.pts[trackId].aRel = float('nan')
self.pts[trackId].yvRel = float('nan')
self.pts[trackId].measured = True
if 'LONG_DIST' in cpt: # c_* message
self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car
# our lat_dist is positive to the right in car's frame.
# TODO what does yRel want?
self.pts[trackId].yRel = cpt['LAT_DIST'] # in car frame's y axis, left is positive
else: # d_* message
self.pts[trackId].vRel = cpt['REL_SPEED']
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
ret.points = [x for x in self.pts.values() if x.dRel != 0]
self.updated_messages.clear()
return ret

136
selfdrive/car/chrysler/values.py Normal file
View File

@@ -0,0 +1,136 @@
from enum import IntFlag, StrEnum
from dataclasses import dataclass, field
from typing import Dict, List, Optional, Union
from cereal import car
from panda.python import uds
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = car.CarParams.Ecu
class ChryslerFlags(IntFlag):
HIGHER_MIN_STEERING_SPEED = 1
class CAR(StrEnum):
# Chrysler
PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017"
PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018"
PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019"
PACIFICA_2018 = "CHRYSLER PACIFICA 2018"
PACIFICA_2020 = "CHRYSLER PACIFICA 2020"
# Jeep
JEEP_GRAND_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk
JEEP_GRAND_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk
# Ram
RAM_1500 = "RAM 1500 5TH GEN"
RAM_HD = "RAM HD 5TH GEN"
class CarControllerParams:
def __init__(self, CP):
self.STEER_STEP = 2 # 50 Hz
self.STEER_ERROR_MAX = 80
if CP.carFingerprint in RAM_HD:
self.STEER_DELTA_UP = 14
self.STEER_DELTA_DOWN = 14
self.STEER_MAX = 361 # higher than this faults the EPS
elif CP.carFingerprint in RAM_DT:
self.STEER_DELTA_UP = 6
self.STEER_DELTA_DOWN = 6
self.STEER_MAX = 261 # EPS allows more, up to 350?
else:
self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 3
self.STEER_MAX = 261 # higher than this faults the EPS
STEER_THRESHOLD = 120
RAM_DT = {CAR.RAM_1500, }
RAM_HD = {CAR.RAM_HD, }
RAM_CARS = RAM_DT | RAM_HD
@dataclass
class ChryslerCarInfo(CarInfo):
package: str = "Adaptive Cruise Control (ACC)"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca]))
CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"),
CAR.PACIFICA_2018_HYBRID: None, # same platforms
CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-23"),
CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"),
CAR.PACIFICA_2020: [
ChryslerCarInfo("Chrysler Pacifica 2019-20"),
ChryslerCarInfo("Chrysler Pacifica 2021", package="All"),
],
CAR.JEEP_GRAND_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"),
CAR.JEEP_GRAND_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"),
CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", car_parts=CarParts.common([CarHarness.ram])),
CAR.RAM_HD: [
ChryslerCarInfo("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])),
ChryslerCarInfo("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])),
],
}
CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf132)
CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(0xf132)
CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
CHRYSLER_RX_OFFSET = -0x280
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[CHRYSLER_VERSION_REQUEST],
[CHRYSLER_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.srs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter],
rx_offset=CHRYSLER_RX_OFFSET,
bus=0,
),
Request(
[CHRYSLER_VERSION_REQUEST],
[CHRYSLER_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.hybrid, Ecu.engine, Ecu.transmission],
bus=0,
),
Request(
[CHRYSLER_SOFTWARE_VERSION_REQUEST],
[CHRYSLER_SOFTWARE_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission],
bus=0,
),
],
extra_ecus=[
(Ecu.hybrid, 0x7e2, None), # manages transmission on hybrids
(Ecu.abs, 0x7e4, None), # alt address for abs on hybrids
],
)
DBC = {
CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.JEEP_GRAND_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.JEEP_GRAND_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.RAM_1500: dbc_dict('chrysler_ram_dt_generated', None),
CAR.RAM_HD: dbc_dict('chrysler_ram_hd_generated', None),
}

49
selfdrive/car/disable_ecu.py Executable file
View File

@@ -0,0 +1,49 @@
#!/usr/bin/env python3
from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
from openpilot.common.swaglog import cloudlog
EXT_DIAG_REQUEST = b'\x10\x03'
EXT_DIAG_RESPONSE = b'\x50\x03'
COM_CONT_RESPONSE = b''
def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
The ECU will stay silent as long as openpilot keeps sending Tester Present.
This is used to disable the radar in some cars. Openpilot will emulate the radar.
WARNING: THIS DISABLES AEB!"""
cloudlog.warning(f"ecu disable {hex(addr), sub_addr} ...")
for i in range(retry):
try:
query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
for _, _ in query.get_data(timeout).items():
cloudlog.warning("communication control disable tx/rx ...")
query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
query.get_data(0)
cloudlog.warning("ecu disabled")
return True
except Exception:
cloudlog.exception("ecu disable exception")
cloudlog.error(f"ecu disable retry ({i + 1}) ...")
cloudlog.error("ecu disable failed")
return False
if __name__ == "__main__":
import time
import cereal.messaging as messaging
sendcan = messaging.pub_sock('sendcan')
logcan = messaging.sub_sock('can')
time.sleep(1)
# honda bosch radar disable
disabled = disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03', timeout=0.5, debug=False)
print(f"disabled: {disabled}")

368
selfdrive/car/docs_definitions.py Normal file
View File

@@ -0,0 +1,368 @@
import re
from collections import namedtuple
import copy
from dataclasses import dataclass, field
from enum import Enum
from typing import Dict, List, Optional, Tuple, Union
from cereal import car
from openpilot.common.conversions import Conversions as CV
GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2
MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)"
class Column(Enum):
MAKE = "Make"
MODEL = "Model"
PACKAGE = "Supported Package"
LONGITUDINAL = "ACC"
FSR_LONGITUDINAL = "No ACC accel below"
FSR_STEERING = "No ALC below"
STEERING_TORQUE = "Steering Torque"
AUTO_RESUME = "Resume from stop"
HARDWARE = "Hardware Needed"
VIDEO = "Video"
class Star(Enum):
FULL = "full"
HALF = "half"
EMPTY = "empty"
# A part + its comprised parts
@dataclass
class BasePart:
name: str
parts: List[Enum] = field(default_factory=list)
def all_parts(self):
# Recursively get all parts
_parts = 'parts'
parts = []
parts.extend(getattr(self, _parts))
for part in getattr(self, _parts):
parts.extend(part.value.all_parts())
return parts
class EnumBase(Enum):
@property
def part_type(self):
return PartType(self.__class__)
class Mount(EnumBase):
mount = BasePart("mount")
angled_mount_8_degrees = BasePart("angled mount (8 degrees)")
class Cable(EnumBase):
rj45_cable_7ft = BasePart("RJ45 cable (7 ft)")
long_obdc_cable = BasePart("long OBD-C cable")
usb_a_2_a_cable = BasePart("USB A-A cable")
usbc_otg_cable = BasePart("USB C OTG cable")
usbc_coupler = BasePart("USB-C coupler")
obd_c_cable_1_5ft = BasePart("OBD-C cable (1.5 ft)")
right_angle_obd_c_cable_1_5ft = BasePart("right angle OBD-C cable (1.5 ft)")
class Accessory(EnumBase):
harness_box = BasePart("harness box")
comma_power_v2 = BasePart("comma power v2")
@dataclass
class BaseCarHarness(BasePart):
parts: List[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft])
has_connector: bool = True # without are hidden on the harness connector page
class CarHarness(EnumBase):
nidec = BaseCarHarness("Honda Nidec connector")
bosch_a = BaseCarHarness("Honda Bosch A connector")
bosch_b = BaseCarHarness("Honda Bosch B connector")
toyota_a = BaseCarHarness("Toyota A connector")
toyota_b = BaseCarHarness("Toyota B connector")
subaru_a = BaseCarHarness("Subaru A connector")
subaru_b = BaseCarHarness("Subaru B connector")
subaru_c = BaseCarHarness("Subaru C connector")
subaru_d = BaseCarHarness("Subaru D connector")
fca = BaseCarHarness("FCA connector")
ram = BaseCarHarness("Ram connector")
vw = BaseCarHarness("VW connector")
j533 = BaseCarHarness("J533 connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler])
hyundai_a = BaseCarHarness("Hyundai A connector")
hyundai_b = BaseCarHarness("Hyundai B connector")
hyundai_c = BaseCarHarness("Hyundai C connector")
hyundai_d = BaseCarHarness("Hyundai D connector")
hyundai_e = BaseCarHarness("Hyundai E connector")
hyundai_f = BaseCarHarness("Hyundai F connector")
hyundai_g = BaseCarHarness("Hyundai G connector")
hyundai_h = BaseCarHarness("Hyundai H connector")
hyundai_i = BaseCarHarness("Hyundai I connector")
hyundai_j = BaseCarHarness("Hyundai J connector")
hyundai_k = BaseCarHarness("Hyundai K connector")
hyundai_l = BaseCarHarness("Hyundai L connector")
hyundai_m = BaseCarHarness("Hyundai M connector")
hyundai_n = BaseCarHarness("Hyundai N connector")
hyundai_o = BaseCarHarness("Hyundai O connector")
hyundai_p = BaseCarHarness("Hyundai P connector")
hyundai_q = BaseCarHarness("Hyundai Q connector")
hyundai_r = BaseCarHarness("Hyundai R connector")
custom = BaseCarHarness("Developer connector")
obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable, Cable.long_obdc_cable], has_connector=False)
gm = BaseCarHarness("GM connector", parts=[Accessory.harness_box])
nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler])
nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler])
mazda = BaseCarHarness("Mazda connector")
ford_q3 = BaseCarHarness("Ford Q3 connector")
ford_q4 = BaseCarHarness("Ford Q4 connector")
class Device(EnumBase):
threex = BasePart("comma 3X", parts=[Mount.mount, Cable.right_angle_obd_c_cable_1_5ft])
# variant of comma 3X with angled mounts
threex_angled_mount = BasePart("comma 3X", parts=[Mount.angled_mount_8_degrees, Cable.right_angle_obd_c_cable_1_5ft])
red_panda = BasePart("red panda")
class Kit(EnumBase):
red_panda_kit = BasePart("CAN FD panda kit", parts=[Device.red_panda, Accessory.harness_box,
Cable.usb_a_2_a_cable, Cable.usbc_otg_cable, Cable.obd_c_cable_1_5ft])
class Tool(EnumBase):
socket_8mm_deep = BasePart("Socket Wrench 8mm or 5/16\" (deep)")
pry_tool = BasePart("Pry Tool")
class PartType(Enum):
accessory = Accessory
cable = Cable
connector = CarHarness
device = Device
kit = Kit
mount = Mount
tool = Tool
DEFAULT_CAR_PARTS: List[EnumBase] = [Device.threex]
@dataclass
class CarParts:
parts: List[EnumBase] = field(default_factory=list)
def __call__(self):
return copy.deepcopy(self)
@classmethod
def common(cls, add: Optional[List[EnumBase]] = None, remove: Optional[List[EnumBase]] = None):
p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])]
return cls(p)
def all_parts(self):
parts = []
for part in self.parts:
parts.extend(part.value.all_parts())
return self.parts + parts
CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_footnote"], defaults=(False, False))
class CommonFootnote(Enum):
EXP_LONG_AVAIL = CarFootnote(
"openpilot Longitudinal Control (Alpha) is available behind a toggle; " +
"the toggle is only available in non-release branches such as `devel` or `master-ci`.",
Column.LONGITUDINAL, docs_only=True)
EXP_LONG_DSU = CarFootnote(
"By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. " +
"If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace " +
"stock ACC. <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b>",
Column.LONGITUDINAL)
def get_footnotes(footnotes: List[Enum], column: Column) -> List[Enum]:
# Returns applicable footnotes given current column
return [fn for fn in footnotes if fn.value.column == column]
# TODO: store years as a list
def get_year_list(years):
years_list = []
if len(years) == 0:
return years_list
for year in years.split(','):
year = year.strip()
if len(year) == 4:
years_list.append(str(year))
elif "-" in year and len(year) == 7:
start, end = year.split("-")
years_list.extend(map(str, range(int(start), int(f"20{end}") + 1)))
else:
raise Exception(f"Malformed year string: {years}")
return years_list
def split_name(name: str) -> Tuple[str, str, str]:
make, model = name.split(" ", 1)
years = ""
match = re.search(MODEL_YEARS_RE, model)
if match is not None:
years = model[match.start():]
model = model[:match.start() - 1]
return make, model, years
@dataclass
class CarInfo:
# make + model + model years
name: str
# Example for Toyota Corolla MY20
# requirements: Lane Tracing Assist (LTA) and Dynamic Radar Cruise Control (DRCC)
# US Market reference: "All", since all Corolla in the US come standard with LTA and DRCC
# the simplest description of the requirements for the US market
package: str
# the minimum compatibility requirements for this model, regardless
# of market. can be a package, trim, or list of features
requirements: Optional[str] = None
video_link: Optional[str] = None
footnotes: List[Enum] = field(default_factory=list)
min_steer_speed: Optional[float] = None
min_enable_speed: Optional[float] = None
auto_resume: Optional[bool] = None
# all the parts needed for the supported car
car_parts: CarParts = field(default_factory=CarParts)
def __post_init__(self):
self.make, self.model, self.years = split_name(self.name)
self.year_list = get_year_list(self.years)
def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]):
self.car_name = CP.carName
self.car_fingerprint = CP.carFingerprint
# longitudinal column
op_long = "Stock"
if CP.experimentalLongitudinalAvailable or CP.enableDsu:
op_long = "openpilot available"
if CP.enableDsu:
self.footnotes.append(CommonFootnote.EXP_LONG_DSU)
else:
self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL)
elif CP.openpilotLongitudinalControl and not CP.enableDsu:
op_long = "openpilot"
# min steer & enable speed columns
# TODO: set all the min steer speeds in carParams and remove this
if self.min_steer_speed is not None:
assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams"
else:
self.min_steer_speed = CP.minSteerSpeed
# TODO: set all the min enable speeds in carParams correctly and remove this
if self.min_enable_speed is None:
self.min_enable_speed = CP.minEnableSpeed
if self.auto_resume is None:
self.auto_resume = CP.autoResumeSng
# hardware column
hardware_col = "None"
if self.car_parts.parts:
model_years = self.model + (' ' + self.years if self.years else '')
buy_link = f'<a href="https://comma.ai/shop/comma-3x.html?make={self.make}&model={model_years}">Buy Here</a>'
tools_docs = [part for part in self.car_parts.all_parts() if isinstance(part, Tool)]
parts_docs = [part for part in self.car_parts.all_parts() if not isinstance(part, Tool)]
def display_func(parts):
return '<br>'.join([f"- {parts.count(part)} {part.value.name}" for part in sorted(set(parts), key=lambda part: str(part.value.name))])
hardware_col = f'<details><summary>Parts</summary><sub>{display_func(parts_docs)}<br>{buy_link}</sub></details>'
if len(tools_docs):
hardware_col += f'<details><summary>Tools</summary><sub>{display_func(tools_docs)}</sub></details>'
self.row: Dict[Enum, Union[str, Star]] = {
Column.MAKE: self.make,
Column.MODEL: self.model,
Column.PACKAGE: self.package,
Column.LONGITUDINAL: op_long,
Column.FSR_LONGITUDINAL: f"{max(self.min_enable_speed * CV.MS_TO_MPH, 0):.0f} mph",
Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph",
Column.STEERING_TORQUE: Star.EMPTY,
Column.AUTO_RESUME: Star.FULL if self.auto_resume else Star.EMPTY,
Column.HARDWARE: hardware_col,
Column.VIDEO: self.video_link if self.video_link is not None else "", # replaced with an image and link from template in get_column
}
# Set steering torque star from max lateral acceleration
assert CP.maxLateralAccel > 0.1
if CP.maxLateralAccel >= GOOD_TORQUE_THRESHOLD:
self.row[Column.STEERING_TORQUE] = Star.FULL
self.all_footnotes = all_footnotes
self.detail_sentence = self.get_detail_sentence(CP)
return self
def init_make(self, CP: car.CarParams):
"""CarInfo subclasses can add make-specific logic for harness selection, footnotes, etc."""
def get_detail_sentence(self, CP):
if not CP.notCar:
sentence_builder = "openpilot upgrades your <strong>{car_model}</strong> with automated lane centering{alc} and adaptive cruise control{acc}."
if self.min_steer_speed > self.min_enable_speed:
alc = f" <strong>above {self.min_steer_speed * CV.MS_TO_MPH:.0f} mph</strong>," if self.min_steer_speed > 0 else " <strong>at all speeds</strong>,"
else:
alc = ""
# Exception for cars which do not auto-resume yet
acc = ""
if self.min_enable_speed > 0:
acc = f" <strong>while driving above {self.min_enable_speed * CV.MS_TO_MPH:.0f} mph</strong>"
elif self.auto_resume:
acc = " <strong>that automatically resumes from a stop</strong>"
if self.row[Column.STEERING_TORQUE] != Star.FULL:
sentence_builder += " This car may not be able to take tight turns on its own."
# experimental mode
exp_link = "<a href='https://blog.comma.ai/090release/#experimental-mode' target='_blank' class='link-light-new-regular-text'>Experimental mode</a>"
if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable:
sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}."
return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc)
else:
if CP.carFingerprint == "COMMA BODY":
return "The body is a robotics dev kit that can run openpilot. <a href='https://www.commabody.com'>Learn more.</a>"
else:
raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}")
def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str:
item: Union[str, Star] = self.row[column]
if isinstance(item, Star):
item = star_icon.format(item.value)
elif column == Column.MODEL and len(self.years):
item += f" {self.years}"
elif column == Column.VIDEO and len(item) > 0:
item = video_icon.format(item)
footnotes = get_footnotes(self.footnotes, column)
if len(footnotes):
sups = sorted([self.all_footnotes[fn] for fn in footnotes])
item += footnote_tag.format(f'{",".join(map(str, sups))}')
return item

96
selfdrive/car/ecu_addrs.py Executable file
View File

@@ -0,0 +1,96 @@
#!/usr/bin/env python3
import capnp
import time
from typing import Optional, Set
import cereal.messaging as messaging
from panda.python.uds import SERVICE_TYPE
from openpilot.selfdrive.car import make_can_msg
from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
from openpilot.common.swaglog import cloudlog
def make_tester_present_msg(addr, bus, subaddr=None):
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
if subaddr is not None:
dat.insert(0, subaddr)
dat.extend([0x0] * (8 - len(dat)))
return make_can_msg(addr, bytes(dat), bus)
def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: Optional[int] = None) -> bool:
# ISO-TP messages are always padded to 8 bytes
# tester present response is always a single frame
dat_offset = 1 if subaddr is not None else 0
if len(msg.dat) == 8 and 1 <= msg.dat[dat_offset] <= 7:
# success response
if msg.dat[dat_offset + 1] == (SERVICE_TYPE.TESTER_PRESENT + 0x40):
return True
# error response
if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == SERVICE_TYPE.TESTER_PRESENT:
return True
return False
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[EcuAddrBusType]:
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
queries: Set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list}
responses = queries
return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug)
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[EcuAddrBusType],
responses: Set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> Set[EcuAddrBusType]:
ecu_responses: Set[EcuAddrBusType] = set() # set((addr, subaddr, bus),)
try:
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
messaging.drain_sock_raw(logcan)
sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan'))
start_time = time.monotonic()
while time.monotonic() - start_time < timeout:
can_packets = messaging.drain_sock(logcan, wait_for_one=True)
for packet in can_packets:
for msg in packet.can:
if not len(msg.dat):
cloudlog.warning("ECU addr scan: skipping empty remote frame")
continue
subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0]
if (msg.address, subaddr, msg.src) in responses and is_tester_present_response(msg, subaddr):
if debug:
print(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}")
if (msg.address, subaddr, msg.src) in ecu_responses:
print(f"Duplicate ECU address: {hex(msg.address)}")
ecu_responses.add((msg.address, subaddr, msg.src))
except Exception:
cloudlog.exception("ECU addr scan exception")
return ecu_responses
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description='Get addresses of all ECUs')
parser.add_argument('--debug', action='store_true')
parser.add_argument('--bus', type=int, default=1)
parser.add_argument('--timeout', type=float, default=1.0)
args = parser.parse_args()
logcan = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
time.sleep(1.0)
print("Getting ECU addresses ...")
ecu_addrs = get_all_ecu_addrs(logcan, sendcan, args.bus, args.timeout, debug=args.debug)
print()
print("Found ECUs on addresses:")
for addr, subaddr, _ in ecu_addrs:
msg = f" 0x{hex(addr)}"
if subaddr is not None:
msg += f" (sub-address: {hex(subaddr)})"
print(msg)

46
selfdrive/car/fingerprints.py Normal file
View File

@@ -0,0 +1,46 @@
from openpilot.selfdrive.car.interfaces import get_interface_attr
FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True)
_FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True)
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
def is_valid_for_fingerprint(msg, car_fingerprint: dict[int, int]):
adr = msg.address
# ignore addresses that are more than 11 bits
return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800
def eliminate_incompatible_cars(msg, candidate_cars):
"""Removes cars that could not have sent msg.
Inputs:
msg: A cereal/log CanData message from the car.
candidate_cars: A list of cars to consider.
Returns:
A list containing the subset of candidate_cars that could have sent msg.
"""
compatible_cars = []
for car_name in candidate_cars:
car_fingerprints = _FINGERPRINTS[car_name]
for fingerprint in car_fingerprints:
# add alien debug address
if is_valid_for_fingerprint(msg, fingerprint | _DEBUG_ADDRESS):
compatible_cars.append(car_name)
break
return compatible_cars
def all_known_cars():
"""Returns a list of all known car strings."""
return list({*FW_VERSIONS.keys(), *_FINGERPRINTS.keys()})
def all_legacy_fingerprint_cars():
"""Returns a list of all known car strings, FPv1 only."""
return list(_FINGERPRINTS.keys())

0
selfdrive/car/ford/__init__.py Normal file
View File

114
selfdrive/car/ford/carcontroller.py Normal file
View File

@@ -0,0 +1,114 @@
from cereal import car
from openpilot.common.numpy_fast import clip
from openpilot.common.conversions import Conversions as CV
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_std_steer_angle_limits
from openpilot.selfdrive.car.ford import fordcan
from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams
LongCtrlState = car.CarControl.Actuators.LongControlState
VisualAlert = car.CarControl.HUDControl.VisualAlert
def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw):
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
if v_ego_raw > 9:
apply_curvature = clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR)
# Curvature rate limit after driver torque limit
apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams)
return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.VM = VM
self.packer = CANPacker(dbc_name)
self.CAN = fordcan.CanBus(CP)
self.frame = 0
self.apply_curvature_last = 0
self.main_on_last = False
self.lkas_enabled_last = False
self.steer_alert_last = False
def update(self, CC, CS, now_nanos):
can_sends = []
actuators = CC.actuators
hud_control = CC.hudControl
main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
### acc buttons ###
if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
# if stock lane centering isn't off, send a button press to toggle it off
# the stock system checks for steering pressed, and eventually disengages cruise control
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
### lateral control ###
# send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0:
if CC.latActive:
# apply rate limits, curvature error limit, and clip to signal range
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
apply_curvature = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw)
else:
apply_curvature = 0.
self.apply_curvature_last = apply_curvature
if self.CP.carFingerprint in CANFD_CAR:
# TODO: extended mode
mode = 1 if CC.latActive else 0
counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF
can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
else:
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
### longitudinal control ###
# send acc msg at 50Hz
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
# Both gas and accel are in m/s^2, accel is used solely for braking
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
gas = accel
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
gas = CarControllerParams.INACTIVE_GAS
stopping = CC.actuators.longControlState == LongCtrlState.stopping
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=40 * CV.MPH_TO_KPH))
### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
# send lkas ui msg at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
# send acc ui msg at 5Hz or if ui state changes
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
fcw_alert, CS.out.cruiseState.standstill, hud_control,
CS.acc_tja_status_stock_values))
self.main_on_last = main_on
self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert
new_actuators = actuators.copy()
new_actuators.curvature = self.apply_curvature_last
self.frame += 1
return new_actuators, can_sends

171
selfdrive/car/ford/carstate.py Normal file
View File

@@ -0,0 +1,171 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.ford.fordcan import CanBus
from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
self.vehicle_sensors_valid = False
self.unsupported_platform = False
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
# Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums,
# this must be root-caused before enabling support. Ford Q4 hybrids do not have this problem.
# TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants
self.unsupported_platform = (cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 and
self.CP.carFingerprint not in CANFD_CAR)
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
# The vehicle usually recovers out of this state within a minute of normal driving
self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3
# car speed
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
# gas pedal
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
ret.gasPressed = ret.gas > 1e-6
# brake pedal
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
# steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
# ret.espDisabled = False # TODO: find traction control signal
if self.CP.carFingerprint in CANFD_CAR:
# this signal is always 0 on non-CAN FD cars
ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
# cruise state
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
if not self.CP.openpilotLongitudinalControl:
ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
# gear
if self.CP.transmissionType == TransmissionType.automatic:
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
ret.gearShifter = self.parse_gear_shifter(gear)
elif self.CP.transmissionType == TransmissionType.manual:
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.drive
# safety
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
# button presses
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
# TODO: block this going to the camera otherwise it will enable stock TJA
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
# lock info
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
# blindspot sensors
if self.CP.enableBsm:
cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp
ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
# Stock steering buttons so that we can passthru blinkers etc.
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
# Stock values from IPMA so that we can retain some stock functionality
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
return ret
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("VehicleOperatingModes", 100),
("BrakeSysFeatures", 50),
("Yaw_Data_FD1", 100),
("DesiredTorqBrk", 50),
("EngVehicleSpThrottle", 100),
("BrakeSnData_4", 50),
("EngBrakeData", 10),
("Cluster_Info1_FD1", 10),
("SteeringPinion_Data", 100),
("EPAS_INFO", 50),
("Steering_Data_FD1", 10),
("BodyInfo_3_FD1", 2),
("RCMStatusMessage2_FD1", 10),
]
if CP.carFingerprint in CANFD_CAR:
messages += [
("Lane_Assist_Data3_FD1", 33),
]
if CP.transmissionType == TransmissionType.automatic:
messages += [
("Gear_Shift_by_Wire_FD1", 10),
]
elif CP.transmissionType == TransmissionType.manual:
messages += [
("Engine_Clutch_Data", 33),
("BCM_Lamp_Stat_FD1", 1),
]
if CP.enableBsm and CP.carFingerprint not in CANFD_CAR:
messages += [
("Side_Detect_L_Stat", 5),
("Side_Detect_R_Stat", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).main)
@staticmethod
def get_cam_can_parser(CP):
messages = [
# sig_address, frequency
("ACCDATA", 50),
("ACCDATA_2", 50),
("ACCDATA_3", 5),
("IPMA_Data", 1),
]
if CP.enableBsm and CP.carFingerprint in CANFD_CAR:
messages += [
("Side_Detect_L_Stat", 5),
("Side_Detect_R_Stat", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).camera)

194
selfdrive/car/ford/fingerprints.py Normal file
View File

@@ -0,0 +1,194 @@
from cereal import car
from openpilot.selfdrive.car.ford.values import CAR
Ecu = car.CarParams.Ecu
FW_VERSIONS = {
CAR.BRONCO_SPORT_MK1: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'M1PA-14C204-GF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1PA-14C204-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'N1PA-14C204-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'N1PA-14C204-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.ESCAPE_MK4: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'LX6A-14C204-BJV\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-BJX\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-CNG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-DPK\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-BEJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MX6A-14C204-CAB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NX6A-14C204-BLE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.EXPLORER_MK6: {
(Ecu.eps, 0x730, None): [
b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'LB5A-14C204-ATJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-ATS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-AUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-AZL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-BUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MB5A-14C204-RC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NB5A-14C204-AZD\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB5A-14C204-DA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.F_150_MK14: {
(Ecu.eps, 0x730, None): [
b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'PL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PL3A-14C204-BRB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.F_150_LIGHTNING_MK1: {
(Ecu.abs, 0x760, None): [
b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'NL3A-14C204-BAR\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MUSTANG_MACH_E_MK1: {
(Ecu.eps, 0x730, None): [
b'LJ9C-14D003-AM\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ9C-14D003-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LK9C-2D053-CK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'MJ98-14C204-BBS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NJ98-14C204-VH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FOCUS_MK4: {
(Ecu.eps, 0x730, None): [
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAVERICK_MK1: {
(Ecu.eps, 0x730, None): [
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'NZ6A-14C204-AAA\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NZ6A-14C204-PA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NZ6A-14C204-ZA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NZ6A-14C204-ZC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6A-14C204-BE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6A-14C204-JC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6A-14C204-JE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
}

341
selfdrive/car/ford/fordcan.py Normal file
View File

@@ -0,0 +1,341 @@
from cereal import car
from openpilot.selfdrive.car import CanBusBase
HUDControl = car.CarControl.HUDControl
class CanBus(CanBusBase):
def __init__(self, CP=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
@property
def main(self) -> int:
return self.offset
@property
def radar(self) -> int:
return self.offset + 1
@property
def camera(self) -> int:
return self.offset + 2
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
checksum = mode + counter
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
checksum += sig_val + (sig_val >> 8)
return 0xFF - (checksum & 0xFF)
def create_lka_msg(packer, CAN: CanBus):
"""
Creates an empty CAN message for the Ford LKA Command.
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
Frequency is 33Hz.
"""
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float):
"""
Creates a CAN message for the Ford TJA/LCA Command.
This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam assist and highway
driving. It is not subject to the PSCM lockout.
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
by the following coefficients:
c0: lateral offset between the vehicle and the centerline (positive is right)
c1: heading angle between the vehicle and the centerline (positive is right)
c2: curvature of the centerline (positive is left)
c3: rate of change of curvature of the centerline
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
angle cannot be easily controlled.
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
using tools such as Forscan.
Frequency is 20Hz.
"""
values = {
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
# 3=InterventionRight, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
# Makes no difference with curvature control
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
# The stock system always uses comfortable
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float, counter: int):
"""
Create a CAN message for the new Ford Lane Centering command.
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
additional signals for a counter and checksum.
Frequency is 20Hz.
"""
values = {
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
# 3=SafeRampOut, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
"LatCtlPath_No_Cnt": counter, # [0|15]
"LatCtlPath_No_Cs": 0, # [0|255]
}
# calculate checksum
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[2]
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, v_ego_kph: float):
"""
Creates a CAN message for the Ford ACC Command.
This command can be used to enable ACC, to set the ACC gas/brake/decel values
and to disable ACC.
Frequency is 50Hz.
"""
decel = accel < 0 and long_active
values = {
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
"AccPrpl_A_Pred": gas, # Acceleration request: [-5|5.23] m/s^2
"AccResumEnbl_B_Rq": 1 if long_active else 0,
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
# TODO: we may be able to improve braking response by utilizing pre-charging better
"AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes
"AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active
"AccStopStat_B_Rq": 1 if stopping else 0,
}
return packer.make_can_msg("ACCDATA", CAN.main, values)
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
hud_control, stock_values: dict):
"""
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
assist status.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 5Hz.
"""
# Tja_D_Stat
if enabled:
if hud_control.leftLaneDepart:
status = 3 # ActiveInterventionLeft
elif hud_control.rightLaneDepart:
status = 4 # ActiveInterventionRight
else:
status = 2 # Active
elif main_on:
if hud_control.leftLaneDepart:
status = 5 # ActiveWarningLeft
elif hud_control.rightLaneDepart:
status = 6 # ActiveWarningRight
else:
status = 1 # Standby
else:
status = 0 # Off
values = {s: stock_values[s] for s in [
"HaDsply_No_Cs",
"HaDsply_No_Cnt",
"AccStopStat_D_Dsply", # ACC stopped status message
"AccTrgDist2_D_Dsply", # ACC target distance
"AccStopRes_B_Dsply",
"TjaWarn_D_Rq", # TJA warning
"TjaMsgTxt_D_Dsply", # TJA text
"IaccLamp_D_Rq", # iACC status icon
"AccMsgTxt_D2_Rq", # ACC text
"FcwDeny_B_Dsply", # FCW disabled
"FcwMemStat_B_Actl", # FCW enabled setting
"AccTGap_B_Dsply", # ACC time gap display setting
"CadsAlignIncplt_B_Actl",
"AccFllwMde_B_Dsply", # ACC follow mode display setting
"CadsRadrBlck_B_Actl",
"CmbbPostEvnt_B_Dsply", # AEB event status
"AccStopMde_B_Dsply", # ACC stop mode display setting
"FcwMemSens_D_Actl", # FCW sensitivity setting
"FcwMsgTxt_D_Rq", # FCW text
"AccWarn_D_Dsply", # ACC warning
"FcwVisblWarn_B_Rq", # FCW visible alert
"FcwAudioWarn_B_Rq", # FCW audio alert
"AccTGap_D_Dsply", # ACC time gap
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
"FdaMem_B_Stat", # FDA enabled setting
]}
values.update({
"Tja_D_Stat": status, # TJA status
})
if CP.openpilotLongitudinalControl:
values.update({
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
"AccMsgTxt_D2_Rq": 0, # ACC text
"AccTGap_B_Dsply": 0, # Show time gap control UI
"AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator
"AccStopMde_B_Dsply": 1 if standstill else 0,
"AccWarn_D_Dsply": 0, # ACC warning
"AccTGap_D_Dsply": 4, # Fixed time gap in UI
})
# Forwards FCW alert from IPMA
if fcw_alert:
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
stock_values: dict):
"""
Creates a CAN message for the Ford IPC IPMA/LKAS status.
Show the LKAS status with the "driver assist" lines in the IPC.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 1Hz.
"""
# LaActvStats_D_Dsply
# R Intvn Warn Supprs Avail No
# L
# Intvn 24 19 14 9 4
# Warn 23 18 13 8 3
# Supprs 22 17 12 7 2
# Avail 21 16 11 6 1
# No 20 15 10 5 0
#
# TODO: test suppress state
if enabled:
lines = 0 # NoLeft_NoRight
if hud_control.leftLaneDepart:
lines += 4
elif hud_control.leftLaneVisible:
lines += 1
if hud_control.rightLaneDepart:
lines += 20
elif hud_control.rightLaneVisible:
lines += 5
elif main_on:
lines = 0
else:
if hud_control.leftLaneDepart:
lines = 3 # WarnLeft_NoRight
elif hud_control.rightLaneDepart:
lines = 15 # NoLeft_WarnRight
else:
lines = 30 # LA_Off
hands_on_wheel_dsply = 1 if steer_alert else 0
values = {s: stock_values[s] for s in [
"FeatConfigIpmaActl",
"FeatNoIpmaActl",
"PersIndexIpma_D_Actl",
"AhbcRampingV_D_Rq", # AHB ramping
"LaDenyStats_B_Dsply", # LKAS error
"CamraDefog_B_Req", # Windshield heater?
"CamraStats_D_Dsply", # Camera status
"DasAlrtLvl_D_Dsply", # DAS alert level
"DasStats_D_Dsply", # DAS status
"DasWarn_D_Dsply", # DAS warning
"AhbHiBeam_D_Rq", # AHB status
"Passthru_63",
"Passthru_48",
]}
values.update({
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
})
return packer.make_can_msg("IPMA_Data", CAN.main, values)
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
"""
Creates a CAN message for the Ford SCCM buttons/switches.
Includes cruise control buttons, turn lights and more.
Frequency is 10Hz.
"""
values = {s: stock_values[s] for s in [
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
"WiprFront_D_Stat",
"LghtAmb_D_Sns",
"AccButtnGapDecPress",
"AccButtnGapIncPress",
"AslButtnOnOffCnclPress",
"AslButtnOnOffPress",
"LaSwtchPos_D_Stat",
"CcAslButtnCnclResPress",
"CcAslButtnDeny_B_Actl",
"CcAslButtnIndxDecPress",
"CcAslButtnIndxIncPress",
"CcAslButtnOffCnclPress",
"CcAslButtnOnOffCncl",
"CcAslButtnOnPress",
"CcAslButtnResDecPress",
"CcAslButtnResIncPress",
"CcAslButtnSetDecPress",
"CcAslButtnSetIncPress",
"CcAslButtnSetPress",
"CcButtnOffPress",
"CcButtnOnOffCnclPress",
"CcButtnOnOffPress",
"CcButtnOnPress",
"HeadLghtHiFlash_D_Actl",
"HeadLghtHiOn_B_StatAhb",
"AhbStat_B_Dsply",
"AccButtnGapTogglePress",
"WiprFrontSwtch_D_Stat",
"HeadLghtHiCtrl_D_RqAhb",
]}
values.update({
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
})
return packer.make_can_msg("Steering_Data_FD1", bus, values)

116
selfdrive/car/ford/interface.py Normal file
View File

@@ -0,0 +1,116 @@
from cereal import car
from panda import Panda
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.ford.fordcan import CanBus
from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "ford"
ret.dashcamOnly = candidate in CANFD_CAR
ret.radarUnavailable = True
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.2
ret.steerLimitTimer = 1.0
CAN = CanBus(fingerprint=fingerprint)
cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)]
if CAN.main >= 4:
cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = cfgs
ret.experimentalLongitudinalAvailable = True
if experimental_long:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL
ret.openpilotLongitudinalControl = True
if candidate in CANFD_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD
if candidate == CAR.BRONCO_SPORT_MK1:
ret.wheelbase = 2.67
ret.steerRatio = 17.7
ret.mass = 1625
elif candidate == CAR.ESCAPE_MK4:
ret.wheelbase = 2.71
ret.steerRatio = 16.7
ret.mass = 1750
elif candidate == CAR.EXPLORER_MK6:
ret.wheelbase = 3.025
ret.steerRatio = 16.8
ret.mass = 2050
elif candidate == CAR.F_150_MK14:
# required trim only on SuperCrew
ret.wheelbase = 3.69
ret.steerRatio = 17.0
ret.mass = 2000
elif candidate == CAR.F_150_LIGHTNING_MK1:
# required trim only on SuperCrew
ret.wheelbase = 3.70
ret.steerRatio = 16.9
ret.mass = 2948
elif candidate == CAR.MUSTANG_MACH_E_MK1:
ret.wheelbase = 2.984
ret.steerRatio = 17.0 # guess
ret.mass = 2200
elif candidate == CAR.FOCUS_MK4:
ret.wheelbase = 2.7
ret.steerRatio = 15.0
ret.mass = 1350
elif candidate == CAR.MAVERICK_MK1:
ret.wheelbase = 3.076
ret.steerRatio = 17.0
ret.mass = 1650
else:
raise ValueError(f"Unsupported car: {candidate}")
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
found_ecus = [fw.ecu for fw in car_fw]
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
ret.transmissionType = TransmissionType.automatic
else:
ret.transmissionType = TransmissionType.manual
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
# TODO: detect bsm in car_fw?
ret.enableBsm = 0x3A6 in fingerprint[CAN.main] and 0x3A7 in fingerprint[CAN.main]
# LCA can steer down to zero
ret.minSteerSpeed = 0.
ret.autoResumeSng = ret.minEnableSpeed == -1.
ret.centerToFront = ret.wheelbase * 0.44
return ret
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
if not self.CS.vehicle_sensors_valid:
events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
if self.CS.unsupported_platform:
events.add(car.CarEvent.EventName.startupNoControl)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

143
selfdrive/car/ford/radar_interface.py Normal file
View File

@@ -0,0 +1,143 @@
from math import cos, sin
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.ford.fordcan import CanBus
from openpilot.selfdrive.car.ford.values import DBC, RADAR
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
DELPHI_MRR_RADAR_START_ADDR = 0x120
DELPHI_MRR_RADAR_MSG_COUNT = 64
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
msg_n = len(DELPHI_ESR_RADAR_MSGS)
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
messages = []
for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
msg = f"MRR_Detection_{i:03d}"
messages += [(msg, 20)]
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.updated_messages = set()
self.track_id = 0
self.radar = DBC[CP.carFingerprint]['radar']
if self.radar is None or CP.radarUnavailable:
self.rcp = None
elif self.radar == RADAR.DELPHI_ESR:
self.rcp = _create_delphi_esr_radar_can_parser(CP)
self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1]
self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS}
elif self.radar == RADAR.DELPHI_MRR:
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1
else:
raise ValueError(f"Unsupported radar: {self.radar}")
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
if self.radar == RADAR.DELPHI_ESR:
self._update_delphi_esr()
elif self.radar == RADAR.DELPHI_MRR:
self._update_delphi_mrr()
ret.points = list(self.pts.values())
self.updated_messages.clear()
return ret
def _update_delphi_esr(self):
for ii in sorted(self.updated_messages):
cpt = self.rcp.vl[ii]
if cpt['X_Rel'] > 0.00001:
self.valid_cnt[ii] = 0 # reset counter
if cpt['X_Rel'] > 0.00001:
self.valid_cnt[ii] += 1
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
#print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
# radar point only valid if there have been enough valid measurements
if self.valid_cnt[ii] > 0:
if ii not in self.pts:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['V_Rel']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
def _update_delphi_mrr(self):
for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
# SCAN_INDEX rotates through 0..3 on each message
# treat these as separate points
scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
i = (ii - 1) * 4 + scanIndex
if i not in self.pts:
self.pts[i] = car.RadarData.RadarPoint.new_message()
self.pts[i].trackId = self.track_id
self.pts[i].aRel = float('nan')
self.pts[i].yvRel = float('nan')
self.track_id += 1
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
if valid:
azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964]
dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984]
distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984]
dRel = cos(azimuth) * dist # m from front of car
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
# delphi doesn't notify of track switches, so do it manually
# TODO: refactor this to radard if more radars behave this way
if abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5:
self.track_id += 1
self.pts[i].trackId = self.track_id
self.pts[i].dRel = dRel
self.pts[i].yRel = yRel
self.pts[i].vRel = distRate
self.pts[i].measured = True
else:
del self.pts[i]

131
selfdrive/car/ford/values.py Normal file
View File

@@ -0,0 +1,131 @@
from collections import defaultdict
from dataclasses import dataclass
from enum import Enum, StrEnum
from typing import Dict, List, Union
from cereal import car
from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \
Device
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
class CarControllerParams:
STEER_STEP = 5 # LateralMotionControl, 20Hz
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
# Curvature rate limits
# The curvature signal is limited to 0.003 to 0.009 m^-1/sec by the EPS depending on speed and direction
# Limit to ~2 m/s^3 up, ~3 m/s^3 down at 75 mph
# Worst case, the low speed limits will allow 4.3 m/s^3 up, 4.9 m/s^3 down at 75 mph
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.0002, 0.0001])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.000225, 0.00015])
CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
ACCEL_MAX = 2.0 # m/s^2 max acceleration
ACCEL_MIN = -3.5 # m/s^2 max deceleration
MIN_GAS = -0.5
INACTIVE_GAS = -5.0
def __init__(self, CP):
pass
class CAR(StrEnum):
BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN"
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
F_150_MK14 = "FORD F-150 14TH GEN"
FOCUS_MK4 = "FORD FOCUS 4TH GEN"
MAVERICK_MK1 = "FORD MAVERICK 1ST GEN"
F_150_LIGHTNING_MK1 = "FORD F-150 LIGHTNING 1ST GEN"
MUSTANG_MACH_E_MK1 = "FORD MUSTANG MACH-E 1ST GEN"
CANFD_CAR = {CAR.F_150_MK14, CAR.F_150_LIGHTNING_MK1, CAR.MUSTANG_MACH_E_MK1}
class RADAR:
DELPHI_ESR = 'ford_fusion_2018_adas'
DELPHI_MRR = 'FORD_CADS'
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR))
# F-150 radar is not yet supported
DBC[CAR.F_150_MK14] = dbc_dict("ford_lincoln_base_pt", None)
DBC[CAR.F_150_LIGHTNING_MK1] = dbc_dict("ford_lincoln_base_pt", None)
DBC[CAR.MUSTANG_MACH_E_MK1] = dbc_dict("ford_lincoln_base_pt", None)
class Footnote(Enum):
FOCUS = CarFootnote(
"Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " +
"North and South America/Southeast Asia.",
Column.MODEL,
)
@dataclass
class FordCarInfo(CarInfo):
package: str = "Co-Pilot360 Assist+"
def init_make(self, CP: car.CarParams):
harness = CarHarness.ford_q4 if CP.carFingerprint in CANFD_CAR else CarHarness.ford_q3
if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14):
self.car_parts = CarParts([Device.threex_angled_mount, harness])
else:
self.car_parts = CarParts([Device.threex, harness])
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"),
CAR.ESCAPE_MK4: [
FordCarInfo("Ford Escape 2020-22"),
FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"),
],
CAR.EXPLORER_MK6: [
FordCarInfo("Ford Explorer 2020-23"),
FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"),
],
CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"),
CAR.F_150_LIGHTNING_MK1: FordCarInfo("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0"),
CAR.MUSTANG_MACH_E_MK1: FordCarInfo("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0"),
CAR.FOCUS_MK4: FordCarInfo("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS]),
CAR.MAVERICK_MK1: [
FordCarInfo("Ford Maverick 2022", "LARIAT Luxury"),
FordCarInfo("Ford Maverick 2023", "Co-Pilot360 Assist"),
],
}
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# CAN and CAN FD queries are combined.
# FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
# TODO: properly handle auxiliary requests to separate queries and add back whitelists
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
# whitelist_ecus=[Ecu.engine],
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
# whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
bus=0,
auxiliary=True,
),
],
extra_ecus=[
(Ecu.shiftByWire, 0x732, None),
],
)

92
selfdrive/car/fw_query_definitions.py Executable file
View File

@@ -0,0 +1,92 @@
#!/usr/bin/env python3
import capnp
import copy
from dataclasses import dataclass, field
import struct
from typing import Callable, Dict, List, Optional, Set, Tuple
import panda.python.uds as uds
AddrType = Tuple[int, Optional[int]]
EcuAddrBusType = Tuple[int, Optional[int], int]
EcuAddrSubAddr = Tuple[int, int, Optional[int]]
LiveFwVersions = Dict[AddrType, Set[bytes]]
OfflineFwVersions = Dict[str, Dict[EcuAddrSubAddr, List[bytes]]]
def p16(val):
return struct.pack("!H", val)
class StdQueries:
# FW queries
TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0])
TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0])
SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT])
SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40])
DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
uds.SESSION_TYPE.DEFAULT])
DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4])
MANUFACTURER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
OBD_VERSION_REQUEST = b'\x09\x04'
OBD_VERSION_RESPONSE = b'\x49\x04'
# VIN queries
OBD_VIN_REQUEST = b'\x09\x02'
OBD_VIN_RESPONSE = b'\x49\x02\x01'
UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN)
UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN)
@dataclass
class Request:
request: List[bytes]
response: List[bytes]
whitelist_ecus: List[int] = field(default_factory=list)
rx_offset: int = 0x8
bus: int = 1
# Whether this query should be run on the first auxiliary panda (CAN FD cars for example)
auxiliary: bool = False
# FW responses from these queries will not be used for fingerprinting
logging: bool = False
# boardd toggles OBD multiplexing on/off as needed
obd_multiplexing: bool = True
@dataclass
class FwQueryConfig:
requests: List[Request]
# TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus
# Overrides and removes from essential ecus for specific models and ecus (exact matching)
non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict)
# Ecus added for data collection, not to be fingerprinted on
extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list)
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions,
# returns set of candidates. Only will match if one candidate is returned
match_fw_to_car_fuzzy: Optional[Callable[[LiveFwVersions, OfflineFwVersions], Set[str]]] = None
def __post_init__(self):
for i in range(len(self.requests)):
if self.requests[i].auxiliary:
new_request = copy.deepcopy(self.requests[i])
new_request.bus += 4
self.requests.append(new_request)

388
selfdrive/car/fw_versions.py Executable file
View File

@@ -0,0 +1,388 @@
#!/usr/bin/env python3
from collections import defaultdict
from typing import Any, DefaultDict, Dict, List, Optional, Set
from tqdm import tqdm
import capnp
import panda.python.uds as uds
from cereal import car
from openpilot.common.params import Params
from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs
from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType
from openpilot.selfdrive.car.interfaces import get_interface_attr
from openpilot.selfdrive.car.fingerprints import FW_VERSIONS
from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
from openpilot.common.swaglog import cloudlog
Ecu = car.CarParams.Ecu
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
FUZZY_EXCLUDE_ECUS = [Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps, Ecu.debug]
FW_QUERY_CONFIGS = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True)
VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True)
MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e}
REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests]
def chunks(l, n=128):
for i in range(0, len(l), n):
yield l[i:i + n]
def is_brand(brand: str, filter_brand: Optional[str]) -> bool:
"""Returns if brand matches filter_brand or no brand filter is specified"""
return filter_brand is None or brand == filter_brand
def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder],
filter_brand: Optional[str] = None) -> Dict[AddrType, Set[bytes]]:
fw_versions_dict: DefaultDict[AddrType, Set[bytes]] = defaultdict(set)
for fw in fw_versions:
if is_brand(fw.brand, filter_brand) and not fw.logging:
sub_addr = fw.subAddress if fw.subAddress != 0 else None
fw_versions_dict[(fw.address, sub_addr)].add(fw.fwVersion)
return dict(fw_versions_dict)
def get_brand_addrs() -> Dict[str, Set[AddrType]]:
brand_addrs: DefaultDict[str, Set[AddrType]] = defaultdict(set)
for brand, cars in VERSIONS.items():
# Add ecus in database + extra ecus to match against
brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in FW_QUERY_CONFIGS[brand].extra_ecus}
for fw in cars.values():
brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in fw.keys()}
return dict(brand_addrs)
def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude=None):
"""Do a fuzzy FW match. This function will return a match, and the number of firmware version
that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars
the match is rejected."""
# Build lookup table from (addr, sub_addr, fw) to list of candidate cars
all_fw_versions = defaultdict(list)
for candidate, fw_by_addr in FW_VERSIONS.items():
if not is_brand(MODEL_TO_BRAND[candidate], match_brand):
continue
if candidate == exclude:
continue
for addr, fws in fw_by_addr.items():
# These ECUs are known to be shared between models (EPS only between hybrid/ICE version)
# Getting this exactly right isn't crucial, but excluding camera and radar makes it almost
# impossible to get 3 matching versions, even if two models with shared parts are released at the same
# time and only one is in our database.
if addr[0] in FUZZY_EXCLUDE_ECUS:
continue
for f in fws:
all_fw_versions[(addr[1], addr[2], f)].append(candidate)
matched_ecus = set()
candidate = None
for addr, versions in live_fw_versions.items():
ecu_key = (addr[0], addr[1])
for version in versions:
# All cars that have this FW response on the specified address
candidates = all_fw_versions[(*ecu_key, version)]
if len(candidates) == 1:
matched_ecus.add(ecu_key)
if candidate is None:
candidate = candidates[0]
# We uniquely matched two different cars. No fuzzy match possible
elif candidate != candidates[0]:
return set()
# Note that it is possible to match to a candidate without all its ECUs being present
# if there are enough matches. FIXME: parameterize this or require all ECUs to exist like exact matching
if len(matched_ecus) >= 2:
if log:
cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {len(matched_ecus)} matching ECUs")
return {candidate}
else:
return set()
def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True) -> Set[str]:
"""Do an exact FW match. Returns all cars that match the given
FW versions for a list of "essential" ECUs. If an ECU is not considered
essential the FW version can be missing to get a fingerprint, but if it's present it
needs to match the database."""
invalid = set()
candidates = {c: f for c, f in FW_VERSIONS.items() if
is_brand(MODEL_TO_BRAND[c], match_brand)}
for candidate, fws in candidates.items():
config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]]
for ecu, expected_versions in fws.items():
ecu_type = ecu[0]
addr = ecu[1:]
found_versions = live_fw_versions.get(addr, set())
if not len(found_versions):
# Some models can sometimes miss an ecu, or show on two different addresses
if candidate in config.non_essential_ecus.get(ecu_type, []):
continue
# Ignore non essential ecus
if ecu_type not in ESSENTIAL_ECUS:
continue
# Virtual debug ecu doesn't need to match the database
if ecu_type == Ecu.debug:
continue
if not any(found_version in expected_versions for found_version in found_versions):
invalid.add(candidate)
break
return set(candidates.keys()) - invalid
def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True):
# Try exact matching first
exact_matches = []
if allow_exact:
exact_matches = [(True, match_fw_to_car_exact)]
if allow_fuzzy:
exact_matches.append((False, match_fw_to_car_fuzzy))
for exact_match, match_func in exact_matches:
# For each brand, attempt to fingerprint using all FW returned from its queries
matches = set()
for brand in VERSIONS.keys():
fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand)
matches |= match_func(fw_versions_dict, match_brand=brand, log=log)
# If specified and no matches so far, fall back to brand's fuzzy fingerprinting function
config = FW_QUERY_CONFIGS[brand]
if not exact_match and not len(matches) and config.match_fw_to_car_fuzzy is not None:
matches |= config.match_fw_to_car_fuzzy(fw_versions_dict, VERSIONS[brand])
if len(matches):
return exact_match, matches
return True, set()
def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
params = Params()
# queries are split by OBD multiplexing mode
queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []}
parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []}
responses = set()
for brand, config, r in REQUESTS:
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
for brand_versions in VERSIONS[brand].values():
for ecu_type, addr, sub_addr in list(brand_versions) + config.extra_ecus:
# Only query ecus in whitelist if whitelist is not empty
if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus:
a = (addr, sub_addr, r.bus)
# Build set of queries
if sub_addr is None:
if a not in parallel_queries[r.obd_multiplexing]:
parallel_queries[r.obd_multiplexing].append(a)
else: # subaddresses must be queried one by one
if [a] not in queries[r.obd_multiplexing]:
queries[r.obd_multiplexing].append([a])
# Build set of expected responses to filter
response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
responses.add((response_addr, sub_addr, r.bus))
for obd_multiplexing in queries:
queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing])
ecu_responses = set()
for obd_multiplexing in queries:
set_obd_multiplexing(params, obd_multiplexing)
for query in queries[obd_multiplexing]:
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
return ecu_responses
def get_brand_ecu_matches(ecu_rx_addrs):
"""Returns dictionary of brands and matches with ECUs in their FW versions"""
brand_addrs = get_brand_addrs()
brand_matches = {brand: set() for brand, _, _ in REQUESTS}
brand_rx_offsets = {(brand, r.rx_offset) for brand, _, r in REQUESTS}
for addr, sub_addr, _ in ecu_rx_addrs:
# Since we can't know what request an ecu responded to, add matches for all possible rx offsets
for brand, rx_offset in brand_rx_offsets:
a = (uds.get_rx_addr_for_tx_addr(addr, -rx_offset), sub_addr)
if a in brand_addrs[brand]:
brand_matches[brand].add(a)
return brand_matches
def set_obd_multiplexing(params: Params, obd_multiplexing: bool):
if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing:
cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}")
params.remove("ObdMultiplexingChanged")
params.put_bool("ObdMultiplexingEnabled", obd_multiplexing)
params.get_bool("ObdMultiplexingChanged", block=True)
cloudlog.warning("OBD multiplexing set successfully")
def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False) -> \
List[capnp.lib.capnp._DynamicStructBuilder]:
"""Queries for FW versions ordering brands by likelihood, breaks when exact match is found"""
all_car_fw = []
brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True):
# Skip this brand if there are no matching present ECUs
if not len(brand_matches[brand]):
continue
car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress)
all_car_fw.extend(car_fw)
# If there is a match using this brand's FW alone, finish querying early
_, matches = match_fw_to_car(car_fw, log=False)
if len(matches) == 1:
break
return all_car_fw
def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False) -> \
List[capnp.lib.capnp._DynamicStructBuilder]:
versions = VERSIONS.copy()
params = Params()
if query_brand is not None:
versions = {query_brand: versions[query_brand]}
if extra is not None:
versions.update(extra)
# Extract ECU addresses to query from fingerprints
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
ecu_types = {}
for brand, brand_versions in versions.items():
config = FW_QUERY_CONFIGS[brand]
for ecu in brand_versions.values():
# Each brand can define extra ECUs to query for data collection
for ecu_type, addr, sub_addr in list(ecu) + config.extra_ecus:
a = (brand, addr, sub_addr)
if a not in ecu_types:
ecu_types[a] = ecu_type
if sub_addr is None:
if a not in parallel_addrs:
parallel_addrs.append(a)
else:
if [a] not in addrs:
addrs.append([a])
addrs.insert(0, parallel_addrs)
# Get versions and build capnp list to put into CarParams
car_fw = []
requests = [(brand, config, r) for brand, config, r in REQUESTS if is_brand(brand, query_brand)]
for addr in tqdm(addrs, disable=not progress):
for addr_chunk in chunks(addr):
for brand, config, r in requests:
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
# Toggle OBD multiplexing for each request
if r.bus % 4 == 1:
set_obd_multiplexing(params, r.obd_multiplexing)
try:
query_addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and
(len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)]
if query_addrs:
query = IsoTpParallelQuery(sendcan, logcan, r.bus, query_addrs, r.request, r.response, r.rx_offset, debug=debug)
for (tx_addr, sub_addr), version in query.get_data(timeout).items():
f = car.CarParams.CarFw.new_message()
f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown)
f.fwVersion = version
f.address = tx_addr
f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset)
f.request = r.request
f.brand = brand
f.bus = r.bus
f.logging = r.logging or (f.ecu, tx_addr, sub_addr) in config.extra_ecus
f.obdMultiplexing = r.obd_multiplexing
if sub_addr is not None:
f.subAddress = sub_addr
car_fw.append(f)
except Exception:
cloudlog.exception("FW query exception")
return car_fw
if __name__ == "__main__":
import time
import argparse
import cereal.messaging as messaging
from openpilot.selfdrive.car.vin import get_vin
parser = argparse.ArgumentParser(description='Get firmware version of ECUs')
parser.add_argument('--scan', action='store_true')
parser.add_argument('--debug', action='store_true')
parser.add_argument('--brand', help='Only query addresses/with requests for this brand')
args = parser.parse_args()
logcan = messaging.sub_sock('can')
pandaStates_sock = messaging.sub_sock('pandaStates')
sendcan = messaging.pub_sock('sendcan')
extra: Any = None
if args.scan:
extra = {}
# Honda
for i in range(256):
extra[(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = []
extra[(Ecu.unknown, 0x700 + i, None)] = []
extra[(Ecu.unknown, 0x750, i)] = []
extra = {"any": {"debug": extra}}
time.sleep(1.)
num_pandas = len(messaging.recv_one_retry(pandaStates_sock).pandaStates)
t = time.time()
print("Getting vin...")
vin_rx_addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
print(f'RX: {hex(vin_rx_addr)}, VIN: {vin}')
print(f"Getting VIN took {time.time() - t:.3f} s")
print()
t = time.time()
fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True)
_, candidates = match_fw_to_car(fw_vers)
print()
print("Found FW versions")
print("{")
padding = max([len(fw.brand) for fw in fw_vers] or [0])
for version in fw_vers:
subaddr = None if version.subAddress == 0 else hex(version.subAddress)
print(f" Brand: {version.brand:{padding}}, bus: {version.bus} - (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]")
print("}")
print()
print("Possible matches:", candidates)
print(f"Getting fw took {time.time() - t:.3f} s")

0
selfdrive/car/gm/__init__.py Normal file
View File

164
selfdrive/car/gm/carcontroller.py Normal file
View File

@@ -0,0 +1,164 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import interp
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
from openpilot.selfdrive.car.gm import gmcan
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
VisualAlert = car.CarControl.HUDControl.VisualAlert
NetworkLocation = car.CarParams.NetworkLocation
LongCtrlState = car.CarControl.Actuators.LongControlState
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
CAMERA_CANCEL_DELAY_FRAMES = 10
# Enforce a minimum interval between steering messages to avoid a fault
MIN_STEER_MSG_INTERVAL_MS = 15
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.start_time = 0.
self.apply_steer_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.frame = 0
self.last_steer_frame = 0
self.last_button_frame = 0
self.cancel_counter = 0
self.lka_steering_cmd_counter = 0
self.lka_icon_status_last = (False, False)
self.params = CarControllerParams(self.CP)
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt'])
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis'])
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
hud_alert = hud_control.visualAlert
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
# Send CAN commands.
can_sends = []
# Steering (Active: 50Hz, inactive: 10Hz)
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Also send at 50Hz:
# - on startup, first few msgs are blocked
# - until we're in sync with camera so counters align when relay closes, preventing a fault.
# openpilot can subtly drift, so this is activated throughout a drive to stay synced
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4
if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync:
steer_step = self.params.STEER_STEP
self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
# received the ASCMLKASteeringCmd loopback confirmation too recently
last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
if CS.loopback_lka_steering_cmd_ts_nanos == 0:
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1
if CC.latActive:
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
else:
apply_steer = 0
self.last_steer_frame = self.frame
self.apply_steer_last = apply_steer
idx = self.lka_steering_cmd_counter % 4
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive))
if self.CP.openpilotLongitudinalControl:
# Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
if not CC.longActive:
# ASCM sends max regen when not enabled
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = 0
else:
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
# Don't allow any gas above inactive regen while stopping
# FIXME: brakes aren't applied immediately when enabling at a stop
if stopping:
self.apply_gas = self.params.INACTIVE_REGEN
idx = (self.frame // 4) % 4
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
friction_brake_bus = CanBus.CHASSIS
# GM Camera exceptions
# TODO: can we always check the longControlState?
if self.CP.networkLocation == NetworkLocation.fwdCamera:
at_full_stop = at_full_stop and stopping
friction_brake_bus = CanBus.POWERTRAIN
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop))
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
idx, CC.enabled, near_stop, at_full_stop, self.CP))
# Send dashboard UI commands (ACC status)
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
if not self.CP.radarUnavailable:
tt = self.frame * DT_CTRL
time_and_headlights_step = 10
if self.frame % time_and_headlights_step == 0:
idx = (self.frame // time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if self.frame % speed_and_accelerometer_step == 0:
idx = (self.frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
else:
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
# A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
# Stock longitudinal, integrated at camera
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
self.last_button_frame = self.frame
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
if self.frame % 10 == 0:
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
self.frame += 1
return new_actuators, can_sends

166
selfdrive/car/gm/carstate.py Normal file
View File

@@ -0,0 +1,166 @@
import copy
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import mean
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD
TransmissionType = car.CarParams.TransmissionType
NetworkLocation = car.CarParams.NetworkLocation
STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
self.cluster_min_speed = CV.KPH_TO_MS / 2.
self.loopback_lka_steering_cmd_updated = False
self.loopback_lka_steering_cmd_ts_nanos = 0
self.pt_lka_steering_cmd_counter = 0
self.cam_lka_steering_cmd_counter = 0
self.buttons_counter = 0
def update(self, pt_cp, cam_cp, loopback_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
# Variables used for avoiding LKAS faults
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
if self.loopback_lka_steering_cmd_updated:
self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
if self.CP.networkLocation == NetworkLocation.fwdCamera:
self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
ret.wheelSpeeds = self.get_wheel_speeds(
pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
)
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
# sample rear wheel speeds, standstill=True if ECM allows engagement with brake
ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
ret.gearShifter = self.parse_gear_shifter("T")
else:
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
if self.CP.networkLocation == NetworkLocation.fwdCamera:
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
else:
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
# that the brake is being intermittently pressed without user interaction.
# To avoid a cruise fault we need to use a conservative brake position threshold
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
ret.brakePressed = ret.brake >= 8
# Regen braking is braking
if self.CP.transmissionType == TransmissionType.direct:
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
ret.gasPressed = ret.gas > 1e-5
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
ret.steerFaultTemporary = self.lkas_status == 2
ret.steerFaultPermanent = self.lkas_status == 3
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1)
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
if self.CP.networkLocation == NetworkLocation.fwdCamera:
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
# openpilot controls nonAdaptive when not pcmCruise
if self.CP.pcmCruise:
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
return ret
@staticmethod
def get_cam_can_parser(CP):
messages = []
if CP.networkLocation == NetworkLocation.fwdCamera:
messages += [
("AEBCmd", 10),
("ASCMLKASteeringCmd", 10),
("ASCMActiveCruiseControlStatus", 25),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.CAMERA)
@staticmethod
def get_can_parser(CP):
messages = [
("BCMTurnSignals", 1),
("ECMPRDNL2", 10),
("PSCMStatus", 10),
("ESPStatus", 10),
("BCMDoorBeltStatus", 10),
("BCMGeneralPlatformStatus", 10),
("EBCMWheelSpdFront", 20),
("EBCMWheelSpdRear", 20),
("EBCMFrictionBrakeStatus", 20),
("AcceleratorPedal2", 33),
("ASCMSteeringButton", 33),
("ECMEngineStatus", 100),
("PSCMSteeringAngle", 100),
("ECMAcceleratorPos", 80),
]
# Used to read back last counter sent to PT by camera
if CP.networkLocation == NetworkLocation.fwdCamera:
messages += [
("ASCMLKASteeringCmd", 0),
]
if CP.transmissionType == TransmissionType.direct:
messages.append(("EBCMRegenPaddle", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN)
@staticmethod
def get_loopback_can_parser(CP):
messages = [
("ASCMLKASteeringCmd", 0),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.LOOPBACK)

56
selfdrive/car/gm/fingerprints.py Normal file
View File

@@ -0,0 +1,56 @@
# ruff: noqa: E501
from openpilot.selfdrive.car.gm.values import CAR
# Trailblazer also matches as a SILVERADO, TODO: split with fw verisions
FINGERPRINTS = {
CAR.HOLDEN_ASTRA: [{
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7
}],
CAR.VOLT: [{
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
},
{
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 578: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1516: 8, 1601: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2018: 8, 2020: 8, 2024: 8, 2028: 8
},
{
170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 8, 381: 2, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 5, 844: 8, 853: 8, 866: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7
}],
CAR.BUICK_LACROSSE: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
CAR.BUICK_REGAL: [{
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8
}],
CAR.CADILLAC_ATS: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
}],
CAR.MALIBU: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8
}],
CAR.ACADIA: [{
190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7
},
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
}],
CAR.ESCALADE: [{
170: 8, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 4, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
CAR.ESCALADE_ESV: [{
309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
}],
CAR.ESCALADE_ESV_2019: [{
715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7
}],
CAR.BOLT_EUV: [{
189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
}],
CAR.SILVERADO: [{
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
}],
CAR.EQUINOX: [{
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
}],
}

173
selfdrive/car/gm/gmcan.py Normal file
View File

@@ -0,0 +1,173 @@
from openpilot.selfdrive.car import make_can_msg
from openpilot.selfdrive.car.gm.values import CAR
def create_buttons(packer, bus, idx, button):
values = {
"ACCButtons": button,
"RollingCounter": idx,
"ACCAlwaysOne": 1,
"DistanceButton": 0,
}
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
checksum -= 2 * values["DistanceButton"]
values["SteeringButtonChecksum"] = checksum
return packer.make_can_msg("ASCMSteeringButton", bus, values)
def create_pscm_status(packer, bus, pscm_status):
values = {s: pscm_status[s] for s in [
"HandsOffSWDetectionMode",
"HandsOffSWlDetectionStatus",
"LKATorqueDeliveredStatus",
"LKADriverAppldTrq",
"LKATorqueDelivered",
"LKATotalTorqueDelivered",
"RollingCounter",
"PSCMStatusChecksum",
]}
checksum_mod = int(1 - values["HandsOffSWlDetectionStatus"]) << 5
values["HandsOffSWlDetectionStatus"] = 1
values["PSCMStatusChecksum"] += checksum_mod
return packer.make_can_msg("PSCMStatus", bus, values)
def create_steering_control(packer, bus, apply_steer, idx, lkas_active):
values = {
"LKASteeringCmdActive": lkas_active,
"LKASteeringCmd": apply_steer,
"RollingCounter": idx,
"LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_steer & 0x7ff) - idx
}
return packer.make_can_msg("ASCMLKASteeringCmd", bus, values)
def create_adas_keepalive(bus):
dat = b"\x00\x00\x00\x00\x00\x00\x00"
return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)]
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
values = {
"GasRegenCmdActive": enabled,
"RollingCounter": idx,
"GasRegenCmdActiveInv": 1 - enabled,
"GasRegenCmd": throttle,
"GasRegenFullStopActive": at_full_stop,
"GasRegenAlwaysOne": 1,
"GasRegenAlwaysOne2": 1,
"GasRegenAlwaysOne3": 1,
}
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \
(((0xff - dat[2]) & 0xff) << 8) | \
((0x100 - dat[3] - idx) & 0xff)
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP):
mode = 0x1
# TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake
if enabled and CP.carFingerprint in (CAR.BOLT_EUV,):
mode = 0x9
if apply_brake > 0:
mode = 0xa
if at_full_stop:
mode = 0xd
# TODO: this is to have GM bringing the car to complete stop,
# but currently it conflicts with OP controls, so turned off. Not set by all cars
#elif near_stop:
# mode = 0xb
brake = (0x1000 - apply_brake) & 0xfff
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
values = {
"RollingCounter": idx,
"FrictionBrakeMode": mode,
"FrictionBrakeChecksum": checksum,
"FrictionBrakeCmd": -apply_brake
}
return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw):
target_speed = min(target_speed_kph, 255)
values = {
"ACCAlwaysOne": 1,
"ACCResumeButton": 0,
"ACCSpeedSetpoint": target_speed,
"ACCGapLevel": 3 * enabled, # 3 "far", 0 "inactive"
"ACCCmdActive": enabled,
"ACCAlwaysOne2": 1,
"ACCLeadCar": lead_car_in_sight,
"FCWAlert": 0x3 if fcw else 0
}
return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values)
def create_adas_time_status(bus, tt, idx):
dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff,
((tt & 0xf) << 4) + (idx << 2)]
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3]
chksum = chksum & 0xfff
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12]
return make_can_msg(0xa1, bytes(dat), bus)
def create_adas_steering_status(bus, idx):
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
chksum = 0x60 + sum(dat)
dat += [chksum >> 8, chksum & 0xff]
return make_can_msg(0x306, bytes(dat), bus)
def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
spd = int(speed_ms * 16) & 0xfff
accel = 0 & 0xfff
# 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L
#stick = 0x08
near_range_cutoff = 0x27
near_range_mode = 1 if spd <= near_range_cutoff else 0
far_range_mode = 1 - near_range_mode
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0]
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4]
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff]
return make_can_msg(0x308, bytes(dat), bus)
def create_adas_headlights_status(packer, bus):
values = {
"Always42": 0x42,
"Always4": 0x4,
}
return packer.make_can_msg("ASCMHeadlight", bus, values)
def create_lka_icon_command(bus, active, critical, steer):
if active and steer == 1:
if critical:
dat = b"\x50\xc0\x14"
else:
dat = b"\x50\x40\x18"
elif active:
if critical:
dat = b"\x40\xc0\x14"
else:
dat = b"\x40\x40\x18"
else:
dat = b"\x00\x00\x00"
return make_can_msg(0x104c006c, dat, bus)

290
selfdrive/car/gm/interface.py Executable file
View File

@@ -0,0 +1,290 @@
#!/usr/bin/env python3
from cereal import car
from math import fabs, exp
from panda import Panda
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
NetworkLocation = car.CarParams.NetworkLocation
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
NON_LINEAR_TORQUE_PARAMS = {
CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
CAR.SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
}
class CarInterface(CarInterfaceBase):
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
@staticmethod
def get_steer_feedforward_volt(desired_angle, v_ego):
desired_angle *= 0.02904609
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
def get_steer_feedforward_function(self):
if self.CP.carFingerprint == CAR.VOLT:
return self.get_steer_feedforward_volt
else:
return CarInterfaceBase.get_steer_feedforward_default
def torque_from_lateral_accel_siglin(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
def sig(val):
return 1 / (1 + exp(-val)) - 0.5
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params
steer_torque = (sig(lateral_accel_value * a) * b) + (lateral_accel_value * c)
return float(steer_torque) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
return self.torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "gm"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
if candidate in EV_CAR:
ret.transmissionType = TransmissionType.direct
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.15]
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kiBP = [0.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = True
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = True # no radar
ret.pcmCruise = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
ret.minEnableSpeed = 5 * CV.KPH_TO_MS
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
# Tuning for experimental long
ret.longitudinalTuning.kpV = [2.0, 1.5]
ret.longitudinalTuning.kiV = [0.72]
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
if experimental_long:
ret.pcmCruise = False
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
else: # ASCM, OBD-II harness
ret.openpilotLongitudinalControl = True
ret.networkLocation = NetworkLocation.gateway
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
# supports stop and go, but initial engage must (conservatively) be above 18mph
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} or \
(ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable)
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.steerLimitTimer = 0.4
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
if candidate == CAR.VOLT:
ret.mass = 1607.
ret.wheelbase = 2.69
ret.steerRatio = 17.7 # Stock 15.7, LiveParameters
ret.tireStiffnessFactor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh
ret.lateralTuning.pid.kpBP = [0., 40.]
ret.lateralTuning.pid.kpV = [0., 0.17]
ret.lateralTuning.pid.kiBP = [0.]
ret.lateralTuning.pid.kiV = [0.]
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
ret.steerActuatorDelay = 0.2
elif candidate == CAR.MALIBU:
ret.mass = 1496.
ret.wheelbase = 2.83
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
elif candidate == CAR.HOLDEN_ASTRA:
ret.mass = 1363.
ret.wheelbase = 2.662
# Remaining parameters copied from Volt for now
ret.centerToFront = ret.wheelbase * 0.4
ret.steerRatio = 15.7
elif candidate == CAR.ACADIA:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 4353. * CV.LB_TO_KG
ret.wheelbase = 2.86
ret.steerRatio = 14.4 # end to end is 13.46
ret.centerToFront = ret.wheelbase * 0.4
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_LACROSSE:
ret.mass = 1712.
ret.wheelbase = 2.91
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_REGAL:
ret.mass = 3779. * CV.LB_TO_KG # (3849+3708)/2
ret.wheelbase = 2.83 # 111.4 inches in meters
ret.steerRatio = 14.4 # guess for tourx
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
elif candidate == CAR.CADILLAC_ATS:
ret.mass = 1601.
ret.wheelbase = 2.78
ret.steerRatio = 15.3
ret.centerToFront = ret.wheelbase * 0.5
elif candidate == CAR.ESCALADE:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 5653. * CV.LB_TO_KG # (5552+5815)/2
ret.wheelbase = 2.95 # 116 inches in meters
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019):
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 2739.
ret.wheelbase = 3.302
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
ret.tireStiffnessFactor = 1.0
if candidate == CAR.ESCALADE_ESV:
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
else:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BOLT_EUV:
ret.mass = 1669.
ret.wheelbase = 2.63779
ret.steerRatio = 16.8
ret.centerToFront = ret.wheelbase * 0.4
ret.tireStiffnessFactor = 1.0
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.SILVERADO:
ret.mass = 2450.
ret.wheelbase = 3.75
ret.steerRatio = 16.3
ret.centerToFront = ret.wheelbase * 0.5
ret.tireStiffnessFactor = 1.0
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
# with foot on brake to allow engagement, but this platform only has that check in the camera.
# TODO: check if this is split by EV/ICE with more platforms in the future
if ret.openpilotLongitudinalControl:
ret.minEnableSpeed = -1.
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.EQUINOX:
ret.mass = 3500. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 14.4
ret.centerToFront = ret.wheelbase * 0.4
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.TRAILBLAZER:
ret.mass = 1345.
ret.wheelbase = 2.64
ret.steerRatio = 16.8
ret.centerToFront = ret.wheelbase * 0.4
ret.tireStiffnessFactor = 1.0
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
return ret
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
# Don't add event if transitioning from INIT, unless it's to an actual button
if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
ret.buttonEvents = create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT,
unpressed_btn=CruiseButtons.UNPRESS)
# The ECM allows enabling on falling edge of set, but only rising edge of resume
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
GearShifter.eco, GearShifter.manumatic],
pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
if not self.CP.pcmCruise:
if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
events.add(EventName.buttonEnable)
# Enabling at a standstill with brake is allowed
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward
if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and
self.CP.networkLocation == NetworkLocation.fwdCamera):
events.add(EventName.belowEngageSpeed)
if ret.cruiseState.standstill:
events.add(EventName.resumeRequired)
if ret.vEgo < self.CP.minSteerSpeed:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

101
selfdrive/car/gm/radar_interface.py Executable file
View File

@@ -0,0 +1,101 @@
#!/usr/bin/env python3
import math
from cereal import car
from openpilot.common.conversions import Conversions as CV
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.gm.values import DBC, CanBus
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
RADAR_HEADER_MSG = 1120
SLOT_1_MSG = RADAR_HEADER_MSG + 1
NUM_SLOTS = 20
# Actually it's 0x47f, but can parser only reports
# messages that are present in DBC
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
def create_radar_can_parser(car_fingerprint):
# C1A-ARS3-A by Continental
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
signals = list(zip(['FLRRNumValidTargets',
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6, strict=True))
messages = list({(s[1], 14) for s in signals})
return CANParser(DBC[car_fingerprint]['radar'], messages, CanBus.OBSTACLE)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint)
self.trigger_msg = LAST_RADAR_MSG
self.updated_messages = set()
self.radar_ts = CP.radarTimeStep
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
ret = car.RadarData.new_message()
header = self.rcp.vl[RADAR_HEADER_MSG]
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \
header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt']
errors = []
if not self.rcp.can_valid:
errors.append("canError")
if fault:
errors.append("fault")
ret.errors = errors
currentTargets = set()
num_targets = header['FLRRNumValidTargets']
# Not all radar messages describe targets,
# no need to monitor all of the self.rcp.msgs_upd
for ii in self.updated_messages:
if ii == RADAR_HEADER_MSG:
continue
if num_targets == 0:
break
cpt = self.rcp.vl[ii]
# Zero distance means it's an empty target slot
if cpt['TrkRange'] > 0.0:
targetId = cpt['TrkObjectID']
currentTargets.add(targetId)
if targetId not in self.pts:
self.pts[targetId] = car.RadarData.RadarPoint.new_message()
self.pts[targetId].trackId = targetId
distance = cpt['TrkRange']
self.pts[targetId].dRel = distance # from front of car
# From driver's pov, left is positive
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
self.pts[targetId].vRel = cpt['TrkRangeRate']
self.pts[targetId].aRel = float('nan')
self.pts[targetId].yvRel = float('nan')
for oldTarget in list(self.pts.keys()):
if oldTarget not in currentTargets:
del self.pts[oldTarget]
ret.points = list(self.pts.values())
self.updated_messages.clear()
return ret

155
selfdrive/car/gm/values.py Normal file
View File

@@ -0,0 +1,155 @@
from collections import defaultdict
from dataclasses import dataclass
from enum import Enum, StrEnum
from typing import Dict, List, Union
from cereal import car
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
Ecu = car.CarParams.Ecu
class CarControllerParams:
STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output
STEER_STEP = 3 # Active control frames per command (~33hz)
INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz)
STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness
STEER_DELTA_DOWN = 15
STEER_DRIVER_ALLOWANCE = 65
STEER_DRIVER_MULTIPLIER = 4
STEER_DRIVER_FACTOR = 100
NEAR_STOP_BRAKE_PHASE = 0.5 # m/s
# Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera"
ADAS_KEEPALIVE_STEP = 100
CAMERA_KEEPALIVE_STEP = 100
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
# perform the closed loop control, and might need some
# to apply some more braking if we're on a downhill slope.
# Our controller should still keep the 2 second average above
# -3.5 m/s^2 as per planner limits
ACCEL_MAX = 2. # m/s^2
ACCEL_MIN = -4. # m/s^2
def __init__(self, CP):
# Gas/brake lookups
self.ZERO_GAS = 2048 # Coasting
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
if CP.carFingerprint in CAMERA_ACC_CAR:
self.MAX_GAS = 3400
self.MAX_ACC_REGEN = 1514
self.INACTIVE_REGEN = 1554
# Camera ACC vehicles have no regen while enabled.
# Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly
max_regen_acceleration = 0.
else:
self.MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill.
self.MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen
self.INACTIVE_REGEN = 1404
# ICE has much less engine braking force compared to regen in EVs,
# lower threshold removes some braking deadzone
max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX]
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS]
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration]
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
class CAR(StrEnum):
HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
VOLT = "CHEVROLET VOLT PREMIER 2017"
CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018"
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
ACADIA = "GMC ACADIA DENALI 2018"
BUICK_LACROSSE = "BUICK LACROSSE 2017"
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
ESCALADE = "CADILLAC ESCALADE 2017"
ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
ESCALADE_ESV_2019 = "CADILLAC ESCALADE ESV 2019"
BOLT_EUV = "CHEVROLET BOLT EUV 2022"
SILVERADO = "CHEVROLET SILVERADO 1500 2020"
EQUINOX = "CHEVROLET EQUINOX 2019"
TRAILBLAZER = "CHEVROLET TRAILBLAZER 2021"
class Footnote(Enum):
OBD_II = CarFootnote(
'Requires a <a href="https://github.com/commaai/openpilot/wiki/GM#hardware" target="_blank">community built ASCM harness</a>. ' +
'<b><i>NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).</i></b>',
Column.MODEL)
@dataclass
class GMCarInfo(CarInfo):
package: str = "Adaptive Cruise Control (ACC)"
def init_make(self, CP: car.CarParams):
if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera:
self.car_parts = CarParts.common([CarHarness.gm])
else:
self.car_parts = CarParts.common([CarHarness.obd_ii])
self.footnotes.append(Footnote.OBD_II)
CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"),
CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ"),
CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"),
CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"),
CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"),
CAR.BUICK_LACROSSE: GMCarInfo("Buick LaCrosse 2017-19", "Driver Confidence Package 2"),
CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"),
CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"),
CAR.ESCALADE_ESV_2019: GMCarInfo("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS"),
CAR.BOLT_EUV: [
GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"),
GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"),
],
CAR.SILVERADO: [
GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"),
GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"),
],
CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22"),
CAR.TRAILBLAZER: GMCarInfo("Chevrolet Trailblazer 2021-22"),
}
class CruiseButtons:
INIT = 0
UNPRESS = 1
RES_ACCEL = 2
DECEL_SET = 3
MAIN = 5
CANCEL = 6
class AccState:
OFF = 0
ACTIVE = 1
FAULTED = 3
STANDSTILL = 4
class CanBus:
POWERTRAIN = 0
OBSTACLE = 1
CAMERA = 2
CHASSIS = 2
LOOPBACK = 128
DROPPED = 192
GM_RX_OFFSET = 0x400
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'))
EV_CAR = {CAR.VOLT, CAR.BOLT_EUV}
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX, CAR.TRAILBLAZER}
STEER_THRESHOLD = 1.0

0
selfdrive/car/honda/__init__.py Normal file
View File

270
selfdrive/car/honda/carcontroller.py Normal file
View File

@@ -0,0 +1,270 @@
from collections import namedtuple
from cereal import car
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import create_gas_interceptor_command
from openpilot.selfdrive.car.honda import hondacan
from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
def compute_gb_honda_bosch(accel, speed):
# TODO returns 0s, is unused
return 0.0, 0.0
def compute_gb_honda_nidec(accel, speed):
creep_brake = 0.0
creep_speed = 2.3
creep_brake_value = 0.15
if speed < creep_speed:
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
gb = float(accel) / 4.8 - creep_brake
return clip(gb, 0.0, 1.0), clip(-gb, 0.0, 1.0)
def compute_gas_brake(accel, speed, fingerprint):
if fingerprint in HONDA_BOSCH:
return compute_gb_honda_bosch(accel, speed)
else:
return compute_gb_honda_nidec(accel, speed)
# TODO not clear this does anything useful
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
# *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
brake = 0.
braking = brake > 0.
# for small brake oscillations within brake_hyst_gap, don't change the brake command
if brake == 0.:
brake_steady = 0.
elif brake > brake_steady + brake_hyst_gap:
brake_steady = brake - brake_hyst_gap
elif brake < brake_steady - brake_hyst_gap:
brake_steady = brake + brake_hyst_gap
brake = brake_steady
return brake, braking, brake_steady
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts):
pump_on = False
# reset pump timer if:
# - there is an increment in brake request
# - we are applying steady state brakes and we haven't been running the pump
# for more than 20s (to prevent pressure bleeding)
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0):
last_pump_ts = ts
# once the pump is on, run it for at least 0.2s
if ts - last_pump_ts < 0.2 and apply_brake > 0:
pump_on = True
return pump_on, last_pump_ts
def process_hud_alert(hud_alert):
# initialize to no alert
fcw_display = 0
steer_required = 0
acc_alert = 0
# priority is: FCW, steer required, all others
if hud_alert == VisualAlert.fcw:
fcw_display = VISUAL_HUD[hud_alert.raw]
elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
steer_required = VISUAL_HUD[hud_alert.raw]
else:
acc_alert = VISUAL_HUD[hud_alert.raw]
return fcw_display, steer_required, acc_alert
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "lead_visible",
"lanes_visible", "fcw", "acc_alert", "steer_required"])
def rate_limit_steer(new_steer, last_steer):
# TODO just hardcoded ramp to min/max in 0.33s for all Honda
MAX_DELTA = 3 * DT_CTRL
return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA)
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.packer = CANPacker(dbc_name)
self.params = CarControllerParams(CP)
self.frame = 0
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0.
self.stopping_counter = 0
self.accel = 0.0
self.speed = 0.0
self.gas = 0.0
self.brake = 0.0
self.last_steer = 0.0
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric)
hud_v_cruise = hud_control.setSpeed / conversion if hud_control.speedVisible else 255
pcm_cancel_cmd = CC.cruiseControl.cancel
if CC.longActive:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
else:
accel = 0.0
gas, brake = 0.0, 0.0
# *** rate limit steer ***
limited_steer = rate_limit_steer(actuators.steer, self.last_steer)
self.last_steer = limited_steer
# *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
CS.out.vEgo, self.CP.carFingerprint)
# *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(interp(-limited_steer * self.params.STEER_MAX,
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
# Send CAN commands
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and self.CP.openpilotLongitudinalControl:
if self.frame % 10 == 0:
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# Send steering command.
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint,
CS.CP.openpilotLongitudinalControl))
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
# all of this is only relevant for HONDA NIDEC
max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V)
# TODO this 1.44 is just to maintain previous behavior
pcm_speed_BP = [-wind_brake,
-wind_brake * (3 / 4),
0.0,
0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if self.CP.enableGasInterceptor or not CC.longActive:
pcm_speed = 0.0
pcm_accel = int(0.0)
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
pcm_speed_V = [0.0,
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX)
else:
pcm_speed_V = [0.0,
clip(CS.out.vEgo - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX)
if not self.CP.openpilotLongitudinalControl:
if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
else:
# Send gas and brake commands.
if self.frame % 2 == 0:
ts = self.frame * DT_CTRL
if self.CP.carFingerprint in HONDA_BOSCH:
self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
stopping = actuators.longControlState == LongCtrlState.stopping
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas,
self.stopping_counter, self.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
pcm_override = True
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, fcw_display,
self.CP.carFingerprint, CS.stock_brake))
self.apply_brake_last = apply_brake
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
if self.CP.enableGasInterceptor:
# way too aggressive at low speed without this
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
# 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 CC.longActive:
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
else:
self.gas = 0.0
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2))
# Send dashboard UI commands.
if self.frame % 10 == 0:
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud))
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed
if not self.CP.enableGasInterceptor:
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
new_actuators = actuators.copy()
new_actuators.speed = self.speed
new_actuators.accel = self.accel
new_actuators.gas = self.gas
new_actuators.brake = self.brake
new_actuators.steer = self.last_steer
new_actuators.steerOutputCan = apply_steer
self.frame += 1
return new_actuators, can_sends

301
selfdrive/car/honda/carstate.py Normal file
View File

@@ -0,0 +1,301 @@
from collections import defaultdict
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import interp
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus
from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, \
HONDA_BOSCH_RADARLESS
from openpilot.selfdrive.car.interfaces import CarStateBase
TransmissionType = car.CarParams.TransmissionType
def get_can_messages(CP, gearbox_msg):
messages = [
("ENGINE_DATA", 100),
("WHEEL_SPEEDS", 50),
("STEERING_SENSORS", 100),
("SEATBELT_STATUS", 10),
("CRUISE", 10),
("POWERTRAIN_DATA", 100),
("CAR_SPEED", 10),
("VSA_STATUS", 50),
("STEER_STATUS", 100),
("STEER_MOTOR_TORQUE", 0), # TODO: not on every car
]
if CP.carFingerprint == CAR.ODYSSEY_CHN:
messages += [
("SCM_FEEDBACK", 25),
("SCM_BUTTONS", 50),
]
else:
messages += [
("SCM_FEEDBACK", 10),
("SCM_BUTTONS", 25),
]
if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E):
messages.append((gearbox_msg, 50))
else:
messages.append((gearbox_msg, 100))
if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
messages.append(("BRAKE_MODULE", 50))
if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
messages.append(("EPB_STATUS", 50))
if CP.carFingerprint in HONDA_BOSCH:
# these messages are on camera bus on radarless cars
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
messages += [
("ACC_HUD", 10),
("ACC_CONTROL", 50),
]
else: # Nidec signals
if CP.carFingerprint == CAR.ODYSSEY_CHN:
messages.append(("CRUISE_PARAMS", 10))
else:
messages.append(("CRUISE_PARAMS", 50))
# TODO: clean this up
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
pass
elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
pass
else:
messages.append(("DOORS_STATUS", 3))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
messages.append(("GAS_SENSOR", 50))
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
messages.append(("CRUISE_FAULT_STATUS", 50))
elif CP.openpilotLongitudinalControl:
messages.append(("STANDSTILL", 50))
return messages
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.gearbox_msg = "GEARBOX"
if CP.carFingerprint == CAR.ACCORD and CP.transmissionType == TransmissionType.cvt:
self.gearbox_msg = "GEARBOX_15T"
self.main_on_sig_msg = "SCM_FEEDBACK"
if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES:
self.main_on_sig_msg = "SCM_BUTTONS"
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
self.brake_switch_prev = False
self.brake_switch_active = False
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
self.dash_speed_seen = False
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
# car params
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
# used for car hud message
self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"]
# ******************* parse out can *******************
# STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED
# panda checks if the signal is non-zero
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
# TODO: find a common signal across all cars
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
else:
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
# LOW_SPEED_LOCKOUT is not worth a warning
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
ret.accFaulted = bool(cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"])
else:
# On some cars, these two signals are always 1, this flag is masking a bug in release
# FIXME: find and set the ACC faulted signals on more platforms
if self.CP.openpilotLongitudinalControl:
ret.accFaulted = bool(cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"])
# Log non-critical stock ACC/LKAS faults if Nidec (camera)
if self.CP.carFingerprint not in HONDA_BOSCH:
ret.carFaultedNonCritical = bool(cp_cam.vl["ACC_HUD"]["ACC_PROBLEM"] or cp_cam.vl["LKAS_HUD"]["LKAS_PROBLEM"])
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
)
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0
# blend in transmission speed at low speed, since it has more low speed accuracy
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3
if self.dash_speed_seen:
conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
# TODO: set for all cars
if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
if self.CP.enableGasInterceptor:
# Same threshold as panda, equivalent to 1e-5 with previous DBC scaling
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 492
else:
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200)
if self.CP.carFingerprint in HONDA_BOSCH:
if not self.CP.openpilotLongitudinalControl:
# ACC_HUD is on camera bus on radarless cars
acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"]
ret.cruiseState.nonAdaptive = acc_hud["CRUISE_CONTROL_LABEL"] != 0
ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252.
conversion = get_cruise_speed_conversion(self.CP.carFingerprint, self.is_metric)
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * conversion
self.v_cruise_pcm_prev = ret.cruiseState.speed
else:
ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
if self.CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
else:
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
# brake switch rises earlier than brake pressed but is never 1 when in park
brake_switch_vals = cp.vl_all["POWERTRAIN_DATA"]["BRAKE_SWITCH"]
if len(brake_switch_vals):
brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
if len(brake_switch_vals) > 1:
self.brake_switch_prev = brake_switch_vals[-2] != 0
self.brake_switch_active = brake_switch and self.brake_switch_prev
self.brake_switch_prev = brake_switch
ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.RIDGELINE):
if ret.brake > 0.1:
ret.brakePressed = True
if self.CP.carFingerprint in HONDA_BOSCH:
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
self.acc_hud = False
self.lkas_hud = False
if self.CP.carFingerprint not in HONDA_BOSCH:
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
self.acc_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
self.lkas_hud = cp_cam.vl["LKAS_HUD"]
if self.CP.enableBsm:
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
# more info here: https://github.com/commaai/openpilot/pull/1867
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
return ret
def get_can_parser(self, CP):
messages = get_can_messages(CP, self.gearbox_msg)
return CANParser(DBC[CP.carFingerprint]["pt"], messages, get_pt_bus(CP.carFingerprint))
@staticmethod
def get_cam_can_parser(CP):
messages = [
("STEERING_CONTROL", 100),
]
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
messages.append(("LKAS_HUD", 10))
if not CP.openpilotLongitudinalControl:
messages.append(("ACC_HUD", 10))
elif CP.carFingerprint not in HONDA_BOSCH:
messages += [
("ACC_HUD", 10),
("LKAS_HUD", 10),
("BRAKE_COMMAND", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
@staticmethod
def get_body_can_parser(CP):
if CP.enableBsm:
messages = [
("BSM_STATUS_LEFT", 3),
("BSM_STATUS_RIGHT", 3),
]
bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
return CANParser(DBC[CP.carFingerprint]["body"], messages, bus_body)
return None

1377
selfdrive/car/honda/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

191
selfdrive/car/honda/hondacan.py Normal file
View File

@@ -0,0 +1,191 @@
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams
# CAN bus layout with relay
# 0 = ACC-CAN - radar side
# 1 = F-CAN B - powertrain
# 2 = ACC-CAN - camera side
# 3 = F-CAN A - OBDII port
def get_pt_bus(car_fingerprint):
return 1 if car_fingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) else 0
def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
no_radar = car_fingerprint in HONDA_BOSCH_RADARLESS
if radar_disabled or no_radar:
# when radar is disabled, steering commands are sent directly to powertrain bus
return get_pt_bus(car_fingerprint)
# normally steering commands are sent to radar, which forwards them to powertrain bus
return 0
def get_cruise_speed_conversion(car_fingerprint: str, is_metric: bool) -> float:
# on certain cars, CRUISE_SPEED changes to imperial with car's unit setting
return CV.MPH_TO_MS if car_fingerprint in HONDA_BOSCH_RADARLESS and not is_metric else CV.KPH_TO_MS
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake):
# TODO: do we loose pressure if we keep pump off for long?
brakelights = apply_brake > 0
brake_rq = apply_brake > 0
pcm_fault_cmd = False
values = {
"COMPUTER_BRAKE": apply_brake,
"BRAKE_PUMP_REQUEST": pump_on,
"CRUISE_OVERRIDE": pcm_override,
"CRUISE_FAULT_CMD": pcm_fault_cmd,
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
"COMPUTER_BRAKE_REQUEST": brake_rq,
"SET_ME_1": 1,
"BRAKE_LIGHTS": brakelights,
"CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw
"FCW": fcw << 1, # TODO: Why are there two bits for fcw?
"AEB_REQ_1": 0,
"AEB_REQ_2": 0,
"AEB_STATUS": 0,
}
bus = get_pt_bus(car_fingerprint)
return packer.make_can_msg("BRAKE_COMMAND", bus, values)
def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, car_fingerprint):
commands = []
bus = get_pt_bus(car_fingerprint)
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
control_on = 5 if enabled else 0
gas_command = gas if active and accel > min_gas_accel else -30000
accel_command = accel if active else 0
braking = 1 if active and accel < min_gas_accel else 0
standstill = 1 if active and stopping_counter > 0 else 0
standstill_release = 1 if active and stopping_counter == 0 else 0
# common ACC_CONTROL values
acc_control_values = {
'ACCEL_COMMAND': accel_command,
'STANDSTILL': standstill,
}
if car_fingerprint in HONDA_BOSCH_RADARLESS:
acc_control_values.update({
"CONTROL_ON": enabled,
"IDLESTOP_ALLOW": stopping_counter > 200, # allow idle stop after 4 seconds (50 Hz)
})
else:
acc_control_values.update({
# setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
"CONTROL_ON": control_on,
"GAS_COMMAND": gas_command, # used for gas
"BRAKE_LIGHTS": braking,
"BRAKE_REQUEST": braking,
"STANDSTILL_RELEASE": standstill_release,
})
acc_control_on_values = {
"SET_TO_3": 0x03,
"CONTROL_ON": enabled,
"SET_TO_FF": 0xff,
"SET_TO_75": 0x75,
"SET_TO_30": 0x30,
}
commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values))
commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values))
return commands
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, radar_disabled):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
"STEER_TORQUE_REQUEST": lkas_active,
}
bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled)
return packer.make_can_msg("STEERING_CONTROL", bus, values)
def create_bosch_supplemental_1(packer, car_fingerprint):
# non-active params
values = {
"SET_ME_X04": 0x04,
"SET_ME_X80": 0x80,
"SET_ME_X10": 0x10,
}
bus = get_lkas_cmd_bus(car_fingerprint)
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud):
commands = []
bus_pt = get_pt_bus(CP.carFingerprint)
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled)
if CP.openpilotLongitudinalControl:
acc_hud_values = {
'CRUISE_SPEED': hud.v_cruise,
'ENABLE_MINI_CAR': 1 if enabled else 0,
'HUD_DISTANCE': 0, # max distance setting on display
'IMPERIAL_UNIT': int(not is_metric),
'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0,
'SET_ME_X01_2': 1,
}
if CP.carFingerprint in HONDA_BOSCH:
acc_hud_values['ACC_ON'] = int(enabled)
acc_hud_values['FCM_OFF'] = 1
acc_hud_values['FCM_OFF_2'] = 1
else:
acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH
acc_hud_values['PCM_GAS'] = hud.pcm_accel
acc_hud_values['SET_ME_X01'] = 1
acc_hud_values['FCM_OFF'] = acc_hud['FCM_OFF']
acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2']
acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM']
acc_hud_values['ICONS'] = acc_hud['ICONS']
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values))
lkas_hud_values = {
'SET_ME_X41': 0x41,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes_visible,
'BEEP': 0,
}
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
lkas_hud_values['LANE_LINES'] = 3
lkas_hud_values['DASHED_LANES'] = hud.lanes_visible
# car likely needs to see LKAS_PROBLEM fall within a specific time frame, so forward from camera
lkas_hud_values['LKAS_PROBLEM'] = lkas_hud['LKAS_PROBLEM']
if not (CP.flags & HondaFlags.BOSCH_EXT_HUD):
lkas_hud_values['SET_ME_X48'] = 0x48
if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl:
commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values))
commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values))
else:
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values))
if radar_disabled:
radar_hud_values = {
'CMBS_OFF': 0x01,
'SET_TO_1': 0x01,
}
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values))
if CP.carFingerprint == CAR.CIVIC_BOSCH:
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {}))
return commands
def spam_buttons_command(packer, button_val, car_fingerprint):
values = {
'CRUISE_BUTTONS': button_val,
'CRUISE_SETTING': 0,
}
# send buttons to camera on radarless cars
bus = 2 if car_fingerprint in HONDA_BOSCH_RADARLESS else get_pt_bus(car_fingerprint)
return packer.make_can_msg("SCM_BUTTONS", bus, values)

346
selfdrive/car/honda/interface.py Executable file
View File

@@ -0,0 +1,346 @@
#!/usr/bin/env python3
from cereal import car
from panda import Panda
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import interp
from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, \
HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.disable_ecu import disable_ecu
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
TransmissionType = car.CarParams.TransmissionType
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
class CarInterface(CarInterfaceBase):
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
if CP.carFingerprint in HONDA_BOSCH:
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
elif CP.enableGasInterceptor:
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX
else:
# NIDECs don't allow acceleration near cruise_speed,
# so limit limits of pid to prevent windup
ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2]
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "honda"
if candidate in HONDA_BOSCH:
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)]
ret.radarUnavailable = True
# Disable the radar and let openpilot control longitudinal
# WARNING: THIS DISABLES AEB!
# If Bosch radarless, this blocks ACC messages from the camera
ret.experimentalLongitudinalAvailable = True
ret.openpilotLongitudinalControl = experimental_long
ret.pcmCruise = not ret.openpilotLongitudinalControl
else:
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)]
ret.enableGasInterceptor = 0x201 in fingerprint[0]
ret.openpilotLongitudinalControl = True
ret.pcmCruise = not ret.enableGasInterceptor
if candidate == CAR.CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
# Detect Bosch cars with new HUD msgs
if any(0x33DA in f for f in fingerprint.values()):
ret.flags |= HondaFlags.BOSCH_EXT_HUD.value
# Accord 1.5T CVT has different gearbox message
if candidate == CAR.ACCORD and 0x191 in fingerprint[1]:
ret.transmissionType = TransmissionType.cvt
# Certain Hondas have an extra steering sensor at the bottom of the steering rack,
# which improves controls quality as it removes the steering column torsion from feedback.
# Tire stiffness factor fictitiously lower if it includes the steering column torsion effect.
# For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani"
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
if candidate in HONDA_BOSCH:
ret.longitudinalTuning.kpV = [0.25]
ret.longitudinalTuning.kiV = [0.05]
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
if candidate in HONDA_BOSCH_RADARLESS:
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
else:
# default longitudinal tuning for all hondas
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
eps_modified = False
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
eps_modified = True
if candidate == CAR.CIVIC:
ret.mass = 1326.
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.4
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
if eps_modified:
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
# stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
# modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
# stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
# modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
# note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022):
ret.mass = 1326.
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.4
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate in (CAR.ACCORD, CAR.ACCORDH):
ret.mass = 3279. * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.8467
if eps_modified:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
else:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.ACURA_ILX:
ret.mass = 3095. * CV.LB_TO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.37
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.72
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate in (CAR.CRV, CAR.CRV_EU):
ret.mass = 3572. * CV.LB_TO_KG
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.89 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_5G:
ret.mass = 3410. * CV.LB_TO_KG
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
if eps_modified:
# stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F
# stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
# modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
ret.tireStiffnessFactor = 0.677
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_HYBRID:
ret.mass = 1667. # mean of 4 models in kg
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.677
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.FIT:
ret.mass = 2644. * CV.LB_TO_KG
ret.wheelbase = 2.53
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 13.06
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.75
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
elif candidate == CAR.FREED:
ret.mass = 3086. * CV.LB_TO_KG
ret.wheelbase = 2.74
# the remaining parameters were copied from FIT
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 13.06
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
ret.tireStiffnessFactor = 0.75
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
elif candidate in (CAR.HRV, CAR.HRV_3G):
ret.mass = 3125 * CV.LB_TO_KG
ret.wheelbase = 2.61
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.2
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
ret.tireStiffnessFactor = 0.5
if candidate == CAR.HRV:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
ret.wheelSpeedFactor = 1.025
else:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning
elif candidate == CAR.ACURA_RDX:
ret.mass = 3935. * CV.LB_TO_KG
ret.wheelbase = 2.68
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.0 # as spec
ret.tireStiffnessFactor = 0.444
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate == CAR.ACURA_RDX_3G:
ret.mass = 4068. * CV.LB_TO_KG
ret.wheelbase = 2.75
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 11.95 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
ret.tireStiffnessFactor = 0.677
elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
ret.mass = 1900.
ret.wheelbase = 3.00
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 14.35 # as spec
ret.tireStiffnessFactor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
if candidate == CAR.ODYSSEY_CHN:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
elif candidate == CAR.PILOT:
ret.mass = 4278. * CV.LB_TO_KG # average weight
ret.wheelbase = 2.86
ret.centerToFront = ret.wheelbase * 0.428
ret.steerRatio = 16.0 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate == CAR.RIDGELINE:
ret.mass = 4515. * CV.LB_TO_KG
ret.wheelbase = 3.18
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.59 # as spec
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate == CAR.INSIGHT:
ret.mass = 2987. * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 15.0 # 12.58 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.HONDA_E:
ret.mass = 3338.8 * CV.LB_TO_KG
ret.wheelbase = 2.5
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 16.71
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.tireStiffnessFactor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning
else:
raise ValueError(f"unsupported car {candidate}")
# These cars use alternate user brake msg (0x1BE)
if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
# These cars use alternate SCM messages (SCM_FEEDBACK AND SCM_BUTTON)
if candidate in HONDA_NIDEC_ALT_SCM_MESSAGES:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_NIDEC_ALT
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
if ret.enableGasInterceptor and candidate not in HONDA_BOSCH:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_GAS_INTERCEPTOR
if candidate in HONDA_BOSCH_RADARLESS:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
return ret
@staticmethod
def init(CP, logcan, sendcan):
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl:
disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03')
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.buttonEvents = [
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}),
]
# events
events = self.create_common_events(ret, pcm_enable=False)
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
if self.CP.pcmCruise:
# we engage when pcm is active (rising edge)
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
events.add(EventName.pcmEnable)
elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl):
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
if ret.vEgo < self.CP.minEnableSpeed + 2.:
# non loud alert if cruise disables below 25mph as expected (+ a little margin)
events.add(EventName.speedTooLow)
else:
events.add(EventName.cruiseDisabled)
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.add(EventName.manualRestart)
ret.events = events.to_msg()
return ret
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

84
selfdrive/car/honda/radar_interface.py Executable file
View File

@@ -0,0 +1,84 @@
#!/usr/bin/env python3
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
from openpilot.selfdrive.car.honda.values import DBC
def _create_nidec_can_parser(car_fingerprint):
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
messages = [(m, 20) for m in radar_messages]
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.track_id = 0
self.radar_fault = False
self.radar_wrong_config = False
self.radar_off_can = CP.radarUnavailable
self.radar_ts = CP.radarTimeStep
self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar
# Nidec
if self.radar_off_can:
self.rcp = None
else:
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445
self.updated_messages = set()
def update(self, can_strings):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = car.RadarData.new_message()
for ii in sorted(updated_messages):
cpt = self.rcp.vl[ii]
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
errors = []
if not self.rcp.can_valid:
errors.append("canError")
if self.radar_fault:
errors.append("fault")
if self.radar_wrong_config:
errors.append("wrongConfig")
ret.errors = errors
ret.points = list(self.pts.values())
return ret

240
selfdrive/car/honda/values.py Normal file
View File

@@ -0,0 +1,240 @@
from dataclasses import dataclass
from enum import Enum, IntFlag, StrEnum
from typing import Dict, List, Optional, Union
from cereal import car
from openpilot.common.conversions import Conversions as CV
from panda.python import uds
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Ecu = car.CarParams.Ecu
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarControllerParams:
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
# perform the closed loop control, and might need some
# to apply some more braking if we're on a downhill slope.
# Our controller should still keep the 2 second average above
# -3.5 m/s^2 as per planner limits
NIDEC_ACCEL_MIN = -4.0 # m/s^2
NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons
NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6]
NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0]
NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
NIDEC_GAS_MAX = 198 # 0xc6
NIDEC_BRAKE_MAX = 1024 // 4
BOSCH_ACCEL_MIN = -3.5 # m/s^2
BOSCH_ACCEL_MAX = 2.0 # m/s^2
BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0] # 2m/s^2
BOSCH_GAS_LOOKUP_V = [0, 1600]
def __init__(self, CP):
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
# mirror of list (assuming first item is zero) for interp of signed request values
assert(CP.lateralParams.torqueBP[0] == 0)
assert(CP.lateralParams.torqueBP[0] == 0)
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
class HondaFlags(IntFlag):
# Bosch models with alternate set of LKAS_HUD messages
BOSCH_EXT_HUD = 1
# Car button codes
class CruiseButtons:
RES_ACCEL = 4
DECEL_SET = 3
CANCEL = 2
MAIN = 1
# See dbc files for info on values
VISUAL_HUD = {
VisualAlert.none: 0,
VisualAlert.fcw: 1,
VisualAlert.steerRequired: 1,
VisualAlert.ldw: 1,
VisualAlert.brakePressed: 10,
VisualAlert.wrongGear: 6,
VisualAlert.seatbeltUnbuckled: 5,
VisualAlert.speedTooHigh: 8
}
class CAR(StrEnum):
ACCORD = "HONDA ACCORD 2018"
ACCORDH = "HONDA ACCORD HYBRID 2018"
CIVIC = "HONDA CIVIC 2016"
CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019"
CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019"
CIVIC_2022 = "HONDA CIVIC 2022"
ACURA_ILX = "ACURA ILX 2016"
CRV = "HONDA CR-V 2016"
CRV_5G = "HONDA CR-V 2017"
CRV_EU = "HONDA CR-V EU 2016"
CRV_HYBRID = "HONDA CR-V HYBRID 2019"
FIT = "HONDA FIT 2018"
FREED = "HONDA FREED 2020"
HRV = "HONDA HRV 2019"
HRV_3G = "HONDA HR-V 2023"
ODYSSEY = "HONDA ODYSSEY 2018"
ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019"
ACURA_RDX = "ACURA RDX 2018"
ACURA_RDX_3G = "ACURA RDX 2020"
PILOT = "HONDA PILOT 2017"
RIDGELINE = "HONDA RIDGELINE 2017"
INSIGHT = "HONDA INSIGHT 2019"
HONDA_E = "HONDA E 2020"
class Footnote(Enum):
CIVIC_DIESEL = CarFootnote(
"2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.",
Column.FSR_STEERING)
@dataclass
class HondaCarInfo(CarInfo):
package: str = "Honda Sensing"
def init_make(self, CP: car.CarParams):
if CP.carFingerprint in HONDA_BOSCH:
self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a])
else:
self.car_parts = CarParts.common([CarHarness.nidec])
CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
CAR.ACCORD: [
HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS),
HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS),
],
CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"),
CAR.CIVIC_BOSCH: [
HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8",
footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS),
],
CAR.CIVIC_BOSCH_DIESEL: None, # same platform
CAR.CIVIC_2022: [
HondaCarInfo("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
HondaCarInfo("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
],
CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS),
CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring
CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"),
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"),
CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.PILOT: [
HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS),
HondaCarInfo("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS),
],
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-23", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS),
}
HONDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xF112)
HONDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(0xF112)
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# Currently used to fingerprint
Request(
[StdQueries.UDS_VERSION_REQUEST],
[StdQueries.UDS_VERSION_RESPONSE],
bus=1,
),
# Data collection requests:
# Log extra identifiers for current ECUs
Request(
[HONDA_VERSION_REQUEST],
[HONDA_VERSION_RESPONSE],
bus=1,
logging=True,
),
# Nidec PT bus
Request(
[StdQueries.UDS_VERSION_REQUEST],
[StdQueries.UDS_VERSION_RESPONSE],
bus=0,
logging=True,
),
# Bosch PT bus
Request(
[StdQueries.UDS_VERSION_REQUEST],
[StdQueries.UDS_VERSION_RESPONSE],
bus=1,
logging=True,
obd_multiplexing=False,
),
],
extra_ecus=[
# The only other ECU on PT bus accessible by camera on radarless Civic
(Ecu.unknown, 0x18DAB3F1, None),
],
)
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None),
CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None),
CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_accord_2018_can_generated', None),
CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'),
CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.CRV_HYBRID: dbc_dict('honda_accord_2018_can_generated', None),
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None),
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None),
CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None),
CAR.CIVIC_2022: dbc_dict('honda_civic_ex_2022_can_generated', None),
}
STEER_THRESHOLD = {
# default is 1200, overrides go here
CAR.ACURA_RDX: 400,
CAR.CRV_EU: 400,
}
HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.PILOT, CAR.RIDGELINE}
HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G}
HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G, CAR.HRV_3G}
HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G}

0
selfdrive/car/hyundai/__init__.py Normal file
View File

207
selfdrive/car/hyundai/carcontroller.py Normal file
View File

@@ -0,0 +1,207 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
# All slightly below EPS thresholds to avoid fault
MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
# initialize to no line visible
# TODO: this is not accurate for all cars
sys_state = 1
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
sys_state = 3 if enabled or sys_warning else 4
elif hud_control.leftLaneVisible:
sys_state = 5
elif hud_control.rightLaneVisible:
sys_state = 6
# initialize to no warnings
left_lane_warning = 0
right_lane_warning = 0
if hud_control.leftLaneDepart:
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
if hud_control.rightLaneDepart:
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
self.angle_limit_counter = 0
self.frame = 0
self.accel_last = 0
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
# steering torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, MAX_ANGLE_FRAMES,
MAX_ANGLE_CONSECUTIVE_FRAMES)
if not CC.latActive:
apply_steer = 0
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
self.apply_steer_last = apply_steer
# accel + longitudinal
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units))
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool):
can_sends = []
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# TODO: resume for alt button cars
pass
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
return can_sends

366
selfdrive/car/hyundai/carstate.py Normal file
View File

@@ -0,0 +1,366 @@
from collections import deque
import copy
import math
from cereal import car
from openpilot.common.conversions import Conversions as CV
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
from openpilot.selfdrive.car.interfaces import CarStateBase
PREV_BUTTON_SAMPLES = 8
CLUSTER_SAMPLE_RATE = 20 # frames
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
if CP.carFingerprint in CANFD_CAR:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
self.cruise_info = {}
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
self.cluster_speed = 0
self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
self.params = CarControllerParams(CP)
def update(self, cp, cp_cam):
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam)
ret = car.CarState.new_message()
cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
self.cluster_speed_counter = 0
# Mimic how dash converts to imperial.
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
ret.vEgoCluster = self.cluster_speed * speed_conv
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
else:
ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if not self.CP.openpilotLongitudinalControl:
aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 # sometimes only SCC system shows an FCW
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
return ret
def update_canfd(self, cp, cp_cam):
ret = car.CarState.new_message()
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR):
offset = 255. if self.CP.carFingerprint in EV_CAR else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR"] == 1
ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT"] == 0
gear = cp.vl[self.gear_msg_canfd]["GEAR"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
# TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP"
if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN:
left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT"
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig],
cp.vl["BLINKERS"][right_blinker_sig])
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
# cruise state
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = cp.vl["TCS"]["ACCEnable"] == 0
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
else:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
# Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
# It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
# The car will brake, but does not respect positive acceleration commands in this mode
# TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
if self.CP.carFingerprint in EV_CAR:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.flags & HyundaiFlags.CANFD_HDA2:
self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
else cp_cam.vl["CAM_0x2a4"])
return ret
def get_can_parser(self, CP):
if CP.carFingerprint in CANFD_CAR:
return self.get_can_parser_canfd(CP)
messages = [
# address, frequency
("MDPS12", 50),
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("CLU15", 5),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("CGW4", 5),
("WHL_SPD11", 50),
("SAS11", 100),
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
if CP.enableBsm:
messages.append(("LCA11", 50))
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("ELECT_GEAR", 20))
elif CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
pass
elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
messages.append(("TCU12", 100))
else:
messages.append(("LVR12", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
if CP.carFingerprint in CANFD_CAR:
return CarState.get_cam_can_parser_canfd(CP)
messages = [
("LKAS11", 100)
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
def get_can_parser_canfd(self, CP):
messages = [
(self.gear_msg_canfd, 100),
(self.accelerator_msg_canfd, 100),
("WHEEL_SPEEDS", 100),
("STEERING_SENSORS", 100),
("MDPS", 100),
("TCS", 50),
("CRUISE_BUTTONS_ALT", 50),
("BLINKERS", 4),
("DOORS_SEATBELTS", 4),
]
if CP.carFingerprint in EV_CAR:
messages += [
("MANUAL_SPEED_LIMIT_ASSIST", 10),
]
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
messages += [
("CRUISE_BUTTONS", 50)
]
if CP.enableBsm:
messages += [
("BLINDSPOTS_REAR_CORNERS", 20),
]
if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
messages += [
("SCC_CONTROL", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
@staticmethod
def get_cam_can_parser_canfd(CP):
messages = []
if CP.flags & HyundaiFlags.CANFD_HDA2:
block_lfa_msg = "CAM_0x362" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "CAM_0x2a4"
messages += [(block_lfa_msg, 20)]
elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
messages += [
("SCC_CONTROL", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)

1710
selfdrive/car/hyundai/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

213
selfdrive/car/hyundai/hyundaican.py Normal file
View File

@@ -0,0 +1,213 @@
import crcmod
from openpilot.selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
torque_fault, lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane,
left_lane_depart, right_lane_depart):
values = {s: lkas11[s] for s in [
"CF_Lkas_LdwsActivemode",
"CF_Lkas_LdwsSysState",
"CF_Lkas_SysWarning",
"CF_Lkas_LdwsLHWarning",
"CF_Lkas_LdwsRHWarning",
"CF_Lkas_HbaLamp",
"CF_Lkas_FcwBasReq",
"CF_Lkas_HbaSysState",
"CF_Lkas_FcwOpt",
"CF_Lkas_HbaOpt",
"CF_Lkas_FcwSysState",
"CF_Lkas_FcwCollisionWarning",
"CF_Lkas_FusionState",
"CF_Lkas_FcwOpt_USM",
"CF_Lkas_LdwsOpt_USM",
]}
values["CF_Lkas_LdwsSysState"] = sys_state
values["CF_Lkas_SysWarning"] = 3 if sys_warning else 0
values["CF_Lkas_LdwsLHWarning"] = left_lane_depart
values["CF_Lkas_LdwsRHWarning"] = right_lane_depart
values["CR_Lkas_StrToqReq"] = apply_steer
values["CF_Lkas_ActToi"] = steer_req
values["CF_Lkas_ToiFlt"] = torque_fault # seems to allow actuation on CR_Lkas_StrToqReq
values["CF_Lkas_MsgCount"] = frame % 0x10
if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022,
CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022,
CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED,
CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN):
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
# FcwOpt_USM 5 = Orange blinking car + lanes
# FcwOpt_USM 4 = Orange car + lanes
# FcwOpt_USM 3 = Green blinking car + lanes
# FcwOpt_USM 2 = Green car + lanes
# FcwOpt_USM 1 = White car + lanes
# FcwOpt_USM 0 = No car + lanes
values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1
# SysWarning 4 = keep hands on wheel
# SysWarning 5 = keep hands on wheel (red)
# SysWarning 6 = keep hands on wheel (red) + beep
# Note: the warning is hidden while the blinkers are on
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
# Likely cars lacking the ability to show individual lane lines in the dash
elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL):
# SysWarning 4 = keep hands on wheel + beep
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
# SysState 0 = no icons
# SysState 1-2 = white car + lanes
# SysState 3 = green car + lanes, green steering wheel
# SysState 4 = green car + lanes
values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1
values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition
# these have no effect
values["CF_Lkas_LdwsActivemode"] = 0
values["CF_Lkas_FcwOpt_USM"] = 0
elif car_fingerprint == CAR.HYUNDAI_GENESIS:
# This field is actually LdwsActivemode
# Genesis and Optima fault when forwarding while engaged
values["CF_Lkas_LdwsActivemode"] = 2
dat = packer.make_can_msg("LKAS11", 0, values)[2]
if car_fingerprint in CHECKSUM["crc8"]:
# CRC Checksum as seen on 2019 Hyundai Santa Fe
dat = dat[:6] + dat[7:8]
checksum = hyundai_checksum(dat)
elif car_fingerprint in CHECKSUM["6B"]:
# Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
checksum = sum(dat[:6]) % 256
else:
# Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger
checksum = (sum(dat[:6]) + dat[7]) % 256
values["CF_Lkas_Chksum"] = checksum
return packer.make_can_msg("LKAS11", 0, values)
def create_clu11(packer, frame, clu11, button, car_fingerprint):
values = {s: clu11[s] for s in [
"CF_Clu_CruiseSwState",
"CF_Clu_CruiseSwMain",
"CF_Clu_SldMainSW",
"CF_Clu_ParityBit1",
"CF_Clu_VanzDecimal",
"CF_Clu_Vanz",
"CF_Clu_SPEED_UNIT",
"CF_Clu_DetentOut",
"CF_Clu_RheostatLevel",
"CF_Clu_CluInfo",
"CF_Clu_AmpInfo",
"CF_Clu_AliveCnt1",
]}
values["CF_Clu_CruiseSwState"] = button
values["CF_Clu_AliveCnt1"] = frame % 0x10
# send buttons to camera on camera-scc based cars
bus = 2 if car_fingerprint in CAMERA_SCC_CAR else 0
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
values = {
"LFA_Icon_State": 2 if enabled else 0,
"HDA_Active": 1 if hda_set_speed else 0,
"HDA_Icon_State": 2 if hda_set_speed else 0,
"HDA_VSetReq": hda_set_speed,
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca):
commands = []
scc11_values = {
"MainMode_ACC": 1,
"TauGapSet": 4,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
"ObjValid": 1, # close lead makes controls tighter
"ACC_ObjStatus": 1, # close lead makes controls tighter
"ACC_ObjLatPos": 0,
"ACC_ObjRelSpd": 0,
"ACC_ObjDist": 1, # close lead makes controls tighter
}
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
scc12_values = {
"ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
"StopReq": 1 if stopping else 0,
"aReqRaw": accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"CR_VSM_Alive": idx % 0xF,
}
# show AEB disabled indicator on dash with SCC12 if not sending FCA messages.
# these signals also prevent a TCS fault on non-FCA cars with alpha longitudinal
if not use_fca:
scc12_values["CF_VSM_ConfMode"] = 1
scc12_values["AEB_Status"] = 1 # AEB disabled
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
scc14_values = {
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
}
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
# Only send FCA11 on cars where it exists on the bus
if use_fca:
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands
def create_acc_opt(packer):
commands = []
scc13_values = {
"SCCDrvModeRValue": 2,
"SCC_Equip": 1,
"Lead_Veh_Dep_Alert_USM": 2,
}
commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
# TODO: this needs to be detected and conditionally sent on unsupported long cars
fca12_values = {
"FCA_DrvSetState": 2,
"FCA_USM": 1, # AEB disabled
}
commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
return commands
def create_frt_radar_opt(packer):
frt_radar11_values = {
"CF_FCA_Equip_Front_Radar": 1,
}
return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)

223
selfdrive/car/hyundai/hyundaicanfd.py Normal file
View File

@@ -0,0 +1,223 @@
from openpilot.common.numpy_fast import clip
from openpilot.selfdrive.car import CanBusBase
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
class CanBus(CanBusBase):
def __init__(self, CP, hda2=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
if hda2 is None:
assert CP is not None
hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
# have a different harness than the HDA1 and non-HDA variants in order to split
# a different bus, since the steering is done by different ECUs.
self._a, self._e = 1, 0
if hda2:
self._a, self._e = 0, 1
self._a += self.offset
self._e += self.offset
self._cam = 2 + self.offset
@property
def ECAN(self):
return self._e
@property
def ACAN(self):
return self._a
@property
def CAM(self):
return self._cam
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"NEW_SIGNAL_1": 0,
"NEW_SIGNAL_2": 0,
}
if CP.flags & HyundaiFlags.CANFD_HDA2:
hda2_lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
ret.append(packer.make_can_msg(hda2_lkas_msg, CAN.ACAN, values))
else:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_suppress_lfa(packer, CAN, hda2_lfa_block_msg, hda2_alt_steering):
suppress_msg = "CAM_0x362" if hda2_alt_steering else "CAM_0x2a4"
msg_bytes = 32 if hda2_alt_steering else 24
values = {f"BYTE{i}": hda2_lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
values["COUNTER"] = hda2_lfa_block_msg["COUNTER"]
values["SET_ME_0"] = 0
values["SET_ME_0_2"] = 0
values["LEFT_LANE_LINE"] = 0
values["RIGHT_LANE_LINE"] = 0
return packer.make_can_msg(suppress_msg, CAN.ACAN, values)
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
"COUNTER": cnt,
"SET_ME_1": 1,
"CRUISE_BUTTONS": btn,
}
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 2 if enabled else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = clip(accel, accel_last - jn, accel_last + jn)
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"StopReq": 1 if stopping else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
"JerkLowerLimit": jerk if enabled else 1,
"JerkUpperLimit": 3.0,
"ACC_ObjDist": 1,
"ObjValid": 0,
"OBJ_STATUS": 2,
"SET_ME_2": 0x4,
"SET_ME_3": 0x3,
"SET_ME_TMP_64": 0x64,
"DISTANCE_SETTING": 4,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
return ret
def create_adrv_messages(packer, CAN, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
values = {
}
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
}
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
if frame % 5 == 0:
values = {
'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
'SET_ME_TMP_F': 0xf,
'SET_ME_TMP_F_2': 0xf,
}
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
values = {
'SET_ME_E1': 0xe1,
'SET_ME_3A': 0x3a,
}
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
if frame % 20 == 0:
values = {
'SET_ME_15': 0x15,
}
ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
return ret

366
selfdrive/car/hyundai/interface.py Normal file
View File

@@ -0,0 +1,366 @@
from cereal import car
from panda import Panda
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \
UNSUPPORTED_LONGITUDINAL_CAR, Buttons
from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.disable_ecu import disable_ecu
Ecu = car.CarParams.Ecu
SafetyModel = car.CarParams.SafetyModel
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
def set_safety_config_hyundai(candidate, CAN, can_fd=False):
platform = SafetyModel.hyundaiCanfd if can_fd else \
SafetyModel.hyundaiLegacy if candidate in LEGACY_SAFETY_MODE_CAR else \
SafetyModel.hyundai
cfgs = [get_safety_config(platform), ]
if CAN.ECAN >= 4:
cfgs.insert(0, get_safety_config(SafetyModel.noOutput))
return cfgs
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "hyundai"
ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
# FIXME: the Optima Hybrid 2017 uses a different SCC12 checksum
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
CAN = CanBus(None, hda2, fingerprint)
if candidate in CANFD_CAR:
# detect HDA2 with ADAS Driving ECU
if hda2:
if 0x110 in fingerprint[CAN.CAM]:
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
else:
# non-HDA2
if 0x1cf not in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
# ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead
if 0x130 not in fingerprint[CAN.ECAN]:
if 0x40 not in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value
else:
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
if candidate not in CANFD_RADAR_SCC_CAR:
ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
else:
# Send LFA message on cars with HDA
if 0x485 in fingerprint[2]:
ret.flags |= HyundaiFlags.SEND_LFA.value
# These cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
if 0x38d in fingerprint[0] or 0x38d in fingerprint[2]:
ret.flags |= HyundaiFlags.USE_FCA.value
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerLimitTimer = 0.4
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN):
ret.mass = 1600. if candidate == CAR.AZERA_6TH_GEN else 1675. # ICE is ~average of 2.5L and 3.5L
ret.wheelbase = 2.885
ret.steerRatio = 14.5
elif candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
ret.mass = 3982. * CV.LB_TO_KG
ret.wheelbase = 2.766
# Values from optimizer
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
ret.tireStiffnessFactor = 0.82
elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID):
ret.mass = 1513.
ret.wheelbase = 2.84
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.SONATA_LF:
ret.mass = 1536.
ret.wheelbase = 2.804
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.PALISADE:
ret.mass = 1999.
ret.wheelbase = 2.90
ret.steerRatio = 15.6 * 1.15
ret.tireStiffnessFactor = 0.63
elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30):
ret.mass = 1275.
ret.wheelbase = 2.7
ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
ret.tireStiffnessFactor = 0.385 # stiffnessFactor settled on 1.0081302973865127
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.ELANTRA_2021:
ret.mass = 2800. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.ELANTRA_HEV_2021:
ret.mass = 3017. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.HYUNDAI_GENESIS:
ret.mass = 2060.
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_EV_2ND_GEN):
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743., CAR.KONA_EV_2ND_GEN: 1740.}.get(candidate, 1275.)
ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6)
ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec
ret.tireStiffnessFactor = 0.385
elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV):
ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
ret.tireStiffnessFactor = 0.385
if candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6):
ret.mass = 1948
ret.wheelbase = 2.97
ret.steerRatio = 14.26
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.VELOSTER:
ret.mass = 2917. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.TUCSON:
ret.mass = 3520. * CV.LB_TO_KG
ret.wheelbase = 2.67
ret.steerRatio = 14.00 * 1.15
ret.tireStiffnessFactor = 0.385
elif candidate in (CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN):
ret.mass = 1630. # average
ret.wheelbase = 2.756
ret.steerRatio = 16.
ret.tireStiffnessFactor = 0.385
elif candidate == CAR.SANTA_CRUZ_1ST_GEN:
ret.mass = 1870. # weight from Limited trim - the only supported trim
ret.wheelbase = 3.000
# steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf
ret.steerRatio = 14.2
elif candidate == CAR.CUSTIN_1ST_GEN:
ret.mass = 1690. # from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0
ret.wheelbase = 3.055
ret.steerRatio = 17.0 # from learner
elif candidate == CAR.STARIA_4TH_GEN:
ret.mass = 2205.
ret.wheelbase = 3.273
ret.steerRatio = 11.94 # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf
# Kia
elif candidate == CAR.KIA_SORENTO:
ret.mass = 1985.
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_PHEV_2022):
ret.mass = 3543. * CV.LB_TO_KG # average of all the cars
ret.wheelbase = 2.7
ret.steerRatio = 13.6 # average of all the cars
ret.tireStiffnessFactor = 0.385
if candidate == CAR.KIA_NIRO_PHEV:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.KIA_SELTOS:
ret.mass = 1337.
ret.wheelbase = 2.63
ret.steerRatio = 14.56
elif candidate == CAR.KIA_SPORTAGE_5TH_GEN:
ret.mass = 1700. # weight from SX and above trims, average of FWD and AWD versions
ret.wheelbase = 2.756
ret.steerRatio = 13.6 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications
elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL):
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
ret.tireStiffnessFactor = 0.5
if candidate == CAR.KIA_OPTIMA_G4:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022):
ret.mass = 1825.
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.KIA_FORTE:
ret.mass = 2878. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.KIA_CEED:
ret.mass = 1450.
ret.wheelbase = 2.65
ret.steerRatio = 13.75
ret.tireStiffnessFactor = 0.5
elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020):
ret.mass = 3381. * CV.LB_TO_KG
ret.wheelbase = 2.85
ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims)
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.KIA_EV6:
ret.mass = 2055
ret.wheelbase = 2.9
ret.steerRatio = 16.
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.KIA_SPORTAGE_HYBRID_5TH_GEN:
ret.mass = 1767. # SX Prestige trim support only
ret.wheelbase = 2.756
ret.steerRatio = 13.6
elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN):
ret.wheelbase = 2.81
ret.steerRatio = 13.5 # average of the platforms
if candidate == CAR.KIA_SORENTO_4TH_GEN:
ret.mass = 3957 * CV.LB_TO_KG
elif candidate == CAR.KIA_SORENTO_HEV_4TH_GEN:
ret.mass = 4255 * CV.LB_TO_KG
else:
ret.mass = 4537 * CV.LB_TO_KG
elif candidate == CAR.KIA_CARNIVAL_4TH_GEN:
ret.mass = 2087.
ret.wheelbase = 3.09
ret.steerRatio = 14.23
elif candidate == CAR.KIA_K8_HEV_1ST_GEN:
ret.mass = 1630. # https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid
ret.wheelbase = 2.895
ret.steerRatio = 13.27 # guesstimate from K5 platform
# Genesis
elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN:
ret.mass = 2205
ret.wheelbase = 2.9
# https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1.
ret.steerRatio = 12.6
elif candidate == CAR.GENESIS_G70:
ret.steerActuatorDelay = 0.1
ret.mass = 1640.0
ret.wheelbase = 2.84
ret.steerRatio = 13.56
elif candidate == CAR.GENESIS_G70_2020:
ret.mass = 3673.0 * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.steerRatio = 12.9
elif candidate == CAR.GENESIS_GV70_1ST_GEN:
ret.mass = 1950.
ret.wheelbase = 2.87
ret.steerRatio = 14.6
elif candidate == CAR.GENESIS_G80:
ret.mass = 2060.
ret.wheelbase = 3.01
ret.steerRatio = 16.5
elif candidate == CAR.GENESIS_G90:
ret.mass = 2200.
ret.wheelbase = 3.15
ret.steerRatio = 12.069
elif candidate == CAR.GENESIS_GV80:
ret.mass = 2258.
ret.wheelbase = 2.95
ret.steerRatio = 14.14
# *** longitudinal control ***
if candidate in CANFD_CAR:
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
else:
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stoppingControl = True
ret.startingState = True
ret.vEgoStarting = 0.1
ret.startAccel = 1.0
ret.longitudinalActuatorDelayLowerBound = 0.5
ret.longitudinalActuatorDelayUpperBound = 0.5
# *** feature detection ***
if candidate in CANFD_CAR:
ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN]
else:
ret.enableBsm = 0x58b in fingerprint[0]
# *** panda safety config ***
ret.safetyConfigs = set_safety_config_hyundai(candidate, CAN, can_fd=(candidate in CANFD_CAR))
if hda2:
ret.flags |= HyundaiFlags.CANFD_HDA2.value
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
if candidate in CANFD_CAR:
if hda2 and ret.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC or candidate in CAMERA_SCC_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
if candidate in HYBRID_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS
elif candidate in EV_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS
if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
ret.flags |= HyundaiFlags.ALT_LIMITS.value
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS
ret.centerToFront = ret.wheelbase * 0.4
return ret
@staticmethod
def init(CP, logcan, sendcan):
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value):
addr, bus = 0x7d0, 0
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, CanBus(CP).ECAN
disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
# for blinkers
if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
if self.CS.CP.openpilotLongitudinalControl:
ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
# Main button also can trigger an engagement on these cars
allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

79
selfdrive/car/hyundai/radar_interface.py Normal file
View File

@@ -0,0 +1,79 @@
import math
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
from openpilot.selfdrive.car.hyundai.values import DBC
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
def get_radar_can_parser(CP):
if DBC[CP.carFingerprint]['radar'] is None:
return None
messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)]
return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.updated_messages = set()
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
self.rcp = get_radar_can_parser(CP)
def update(self, can_strings):
if self.radar_off_can or (self.rcp is None):
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = car.RadarData.new_message()
if self.rcp is None:
return ret
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]
if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1
valid = msg['STATE'] in (3, 4)
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[addr].vRel = msg['REL_SPEED']
self.pts[addr].aRel = msg['REL_ACCEL']
self.pts[addr].yvRel = float('nan')
else:
del self.pts[addr]
ret.points = list(self.pts.values())
return ret

590
selfdrive/car/hyundai/values.py Normal file
View File

@@ -0,0 +1,590 @@
import re
from dataclasses import dataclass
from enum import Enum, IntFlag, StrEnum
from typing import Dict, List, Optional, Set, Tuple, Union
from cereal import car
from panda.python import uds
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = car.CarParams.Ecu
class CarControllerParams:
ACCEL_MIN = -3.5 # m/s
ACCEL_MAX = 2.0 # m/s
def __init__(self, CP):
self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 7
self.STEER_DRIVER_ALLOWANCE = 50
self.STEER_DRIVER_MULTIPLIER = 2
self.STEER_DRIVER_FACTOR = 1
self.STEER_THRESHOLD = 150
self.STEER_STEP = 1 # 100 Hz
if CP.carFingerprint in CANFD_CAR:
self.STEER_MAX = 270
self.STEER_DRIVER_ALLOWANCE = 250
self.STEER_DRIVER_MULTIPLIER = 2
self.STEER_THRESHOLD = 250
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
# If the max stock LKAS request is <384, add your car to this list.
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ,
CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV,
CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO):
self.STEER_MAX = 255
# these cars have significantly more torque than most HKG; limit to 70% of max
elif CP.flags & HyundaiFlags.ALT_LIMITS:
self.STEER_MAX = 270
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
# Default for most HKG
else:
self.STEER_MAX = 384
class HyundaiFlags(IntFlag):
CANFD_HDA2 = 1
CANFD_ALT_BUTTONS = 2
CANFD_ALT_GEARS = 4
CANFD_CAMERA_SCC = 8
ALT_LIMITS = 16
ENABLE_BLINKERS = 32
CANFD_ALT_GEARS_2 = 64
SEND_LFA = 128
USE_FCA = 256
CANFD_HDA2_ALT_STEERING = 512
class CAR(StrEnum):
# Hyundai
AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN"
AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN"
ELANTRA = "HYUNDAI ELANTRA 2017"
ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
ELANTRA_2021 = "HYUNDAI ELANTRA 2021"
ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021"
HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019"
IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022"
IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020"
IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
KONA = "HYUNDAI KONA 2020"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022"
KONA_EV_2ND_GEN = "HYUNDAI KONA ELECTRIC 2ND GEN"
KONA_HEV = "HYUNDAI KONA HYBRID 2020"
SANTA_FE = "HYUNDAI SANTA FE 2019"
SANTA_FE_2022 = "HYUNDAI SANTA FE 2022"
SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022"
SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022"
SONATA = "HYUNDAI SONATA 2020"
SONATA_LF = "HYUNDAI SONATA 2019"
STARIA_4TH_GEN = "HYUNDAI STARIA 4TH GEN"
TUCSON = "HYUNDAI TUCSON 2019"
PALISADE = "HYUNDAI PALISADE 2020"
VELOSTER = "HYUNDAI VELOSTER 2019"
SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021"
IONIQ_5 = "HYUNDAI IONIQ 5 2022"
IONIQ_6 = "HYUNDAI IONIQ 6 2023"
TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN"
TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN"
SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN"
CUSTIN_1ST_GEN = "HYUNDAI CUSTIN 1ST GEN"
# Kia
KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
KIA_K5_2021 = "KIA K5 2021"
KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020"
KIA_K8_HEV_1ST_GEN = "KIA K8 HYBRID 1ST GEN"
KIA_NIRO_EV = "KIA NIRO EV 2020"
KIA_NIRO_EV_2ND_GEN = "KIA NIRO EV 2ND GEN"
KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019"
KIA_NIRO_PHEV_2022 = "KIA NIRO PLUG-IN HYBRID 2022"
KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
KIA_NIRO_HEV_2ND_GEN = "KIA NIRO HYBRID 2ND GEN"
KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_OPTIMA_H_G4_FL = "KIA OPTIMA HYBRID 4TH GEN FACELIFT"
KIA_SELTOS = "KIA SELTOS 2021"
KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN"
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
KIA_SORENTO_4TH_GEN = "KIA SORENTO 4TH GEN"
KIA_SORENTO_HEV_4TH_GEN = "KIA SORENTO HYBRID 4TH GEN"
KIA_SORENTO_PHEV_4TH_GEN = "KIA SORENTO PLUG-IN HYBRID 4TH GEN"
KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN"
KIA_STINGER = "KIA STINGER GT2 2018"
KIA_STINGER_2022 = "KIA STINGER 2022"
KIA_CEED = "KIA CEED INTRO ED 2019"
KIA_EV6 = "KIA EV6 2022"
KIA_CARNIVAL_4TH_GEN = "KIA CARNIVAL 4TH GEN"
# Genesis
GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN"
GENESIS_G70 = "GENESIS G70 2018"
GENESIS_G70_2020 = "GENESIS G70 2020"
GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN"
GENESIS_G80 = "GENESIS G80 2017"
GENESIS_G90 = "GENESIS G90 2017"
GENESIS_GV80 = "GENESIS GV80 2023"
class Footnote(Enum):
CANFD = CarFootnote(
"Requires a <a href=\"https://comma.ai/shop/can-fd-panda-kit\" target=\"_blank\">CAN FD panda kit</a> if not using " +
"comma 3X for this <a href=\"https://en.wikipedia.org/wiki/CAN_FD\" target=\"_blank\">CAN FD car</a>.",
Column.MODEL, shop_footnote=False)
@dataclass
class HyundaiCarInfo(CarInfo):
package: str = "Smart Cruise Control (SCC)"
def init_make(self, CP: car.CarParams):
if CP.carFingerprint in CANFD_CAR:
self.footnotes.insert(0, Footnote.CANFD)
CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.AZERA_HEV_6TH_GEN: [
HyundaiCarInfo("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])),
HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
],
CAR.ELANTRA: [
# TODO: 2017-18 could be Hyundai G
HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])),
HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])),
],
CAR.ELANTRA_GT_I30: [
HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c",
car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.HYUNDAI_GENESIS: [
# TODO: check 2015 packages
HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),
HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),
],
CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h])), # TODO: confirm 2020-21 harness
CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b])),
CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o])),
CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i])), # TODO: check packages
# TODO: this is the 2024 US MY, not yet released
CAR.KONA_EV_2ND_GEN: HyundaiCarInfo("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw",
car_parts=CarParts.common([CarHarness.hyundai_r])),
CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s",
car_parts=CarParts.common([CarHarness.hyundai_d])),
CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4",
car_parts=CarParts.common([CarHarness.hyundai_l])),
CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw",
car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.STARIA_4TH_GEN: HyundaiCarInfo("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.TUCSON: [
HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])),
],
CAR.PALISADE: [
HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])),
HyundaiCarInfo("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
],
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.IONIQ_5: [
HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])),
HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])),
HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
],
CAR.IONIQ_6: [
HyundaiCarInfo("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])),
],
CAR.TUCSON_4TH_GEN: [
HyundaiCarInfo("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])),
HyundaiCarInfo("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
],
CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.CUSTIN_1ST_GEN: HyundaiCarInfo("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
# Kia
CAR.KIA_FORTE: [
HyundaiCarInfo("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
HyundaiCarInfo("Kia Forte 2023", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_K8_HEV_1ST_GEN: HyundaiCarInfo("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
CAR.KIA_NIRO_EV: [
HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])),
HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])),
HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])),
HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])),
],
CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_NIRO_PHEV: [
HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])),
HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_d])),
],
CAR.KIA_NIRO_PHEV_2022: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_f])),
CAR.KIA_NIRO_HEV_2021: [
HyundaiCarInfo("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])),
HyundaiCarInfo("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])),
],
CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control",
car_parts=CarParts.common([CarHarness.hyundai_b])), # TODO: may support 2016, 2018
CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g])),
# TODO: may support adjacent years. may have a non-zero minimum steering speed
CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.KIA_OPTIMA_H_G4_FL: HyundaiCarInfo("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.KIA_SORENTO: [
HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8",
car_parts=CarParts.common([CarHarness.hyundai_e])),
HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.KIA_SORENTO_HEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0",
car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.KIA_EV6: [
HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])),
HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))
],
CAR.KIA_CARNIVAL_4TH_GEN: [
HyundaiCarInfo("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarInfo("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k]))
],
# Genesis
CAR.GENESIS_GV60_EV_1ST_GEN: [
HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
],
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_f])),
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_f])),
CAR.GENESIS_GV70_1ST_GEN: [
HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])),
],
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m])),
}
class Buttons:
NONE = 0
RES_ACCEL = 1
SET_DECEL = 2
GAP_DIST = 3
CANCEL = 4 # on newer models, this is a pause/resume button
def get_platform_codes(fw_versions: List[bytes]) -> Set[Tuple[bytes, Optional[bytes]]]:
# Returns unique, platform-specific identification codes for a set of versions
codes = set() # (code-Optional[part], date)
for fw in fw_versions:
code_match = PLATFORM_CODE_FW_PATTERN.search(fw)
part_match = PART_NUMBER_FW_PATTERN.search(fw)
date_match = DATE_FW_PATTERN.search(fw)
if code_match is not None:
code: bytes = code_match.group()
part = part_match.group() if part_match else None
date = date_match.group() if date_match else None
if part is not None:
# part number starts with generic ECU part type, add what is specific to platform
code += b"-" + part[-5:]
codes.add((code, date))
return codes
def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> Set[str]:
# Non-electric CAN FD platforms often do not have platform code specifiers needed
# to distinguish between hybrid and ICE. All EVs so far are either exclusively
# electric or specify electric in the platform code.
# TODO: whitelist platforms that we've seen hybrid and ICE versions of that have these specifiers
fuzzy_platform_blacklist = {str(c) for c in set(CANFD_CAR - EV_CAR)}
candidates: Set[str] = set()
for candidate, fws in offline_fw_versions.items():
# Keep track of ECUs which pass all checks (platform codes, within date range)
valid_found_ecus = set()
valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
for ecu, expected_versions in fws.items():
addr = ecu[1:]
# Only check ECUs expected to have platform codes
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
# Expected platform codes & dates
codes = get_platform_codes(expected_versions)
expected_platform_codes = {code for code, _ in codes}
expected_dates = {date for _, date in codes if date is not None}
# Found platform codes & dates
codes = get_platform_codes(live_fw_versions.get(addr, set()))
found_platform_codes = {code for code, _ in codes}
found_dates = {date for _, date in codes if date is not None}
# Check platform code + part number matches for any found versions
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
break
if ecu[0] in DATE_FW_ECUS:
# If ECU can have a FW date, require it to exist
# (this excludes candidates in the database without dates)
if not len(expected_dates) or not len(found_dates):
break
# Check any date within range in the database, format is %y%m%d
if not any(min(expected_dates) <= found_date <= max(expected_dates) for found_date in found_dates):
break
valid_found_ecus.add(addr)
# If all live ECUs pass all checks for candidate, add it as a match
if valid_expected_ecus.issubset(valid_found_ecus):
candidates.add(candidate)
return candidates - fuzzy_platform_blacklist
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf100) # Long description
HYUNDAI_VERSION_REQUEST_ALT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf110) # Alt long description
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
p16(0xf100)
HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
# Regex patterns for parsing platform code, FW date, and part number from FW versions
PLATFORM_CODE_FW_PATTERN = re.compile(b'((?<=' + HYUNDAI_VERSION_REQUEST_LONG[1:] +
b')[A-Z]{2}[A-Za-z0-9]{0,2})')
DATE_FW_PATTERN = re.compile(b'(?<=[ -])([0-9]{6}$)')
PART_NUMBER_FW_PATTERN = re.compile(b'(?<=[0-9][.,][0-9]{2} )([0-9]{5}[-/]?[A-Z][A-Z0-9]{3}[0-9])')
# List of ECUs expected to have platform codes, camera and radar should exist on all cars
# TODO: use abs, it has the platform code and part number on many platforms
PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps]
# So far we've only seen dates in fwdCamera
# TODO: there are date codes in the ABS firmware versions in hex
DATE_FW_ECUS = [Ecu.fwdCamera]
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# TODO: minimize shared whitelists for CAN and cornerRadar for CAN-FD
# CAN queries (OBD-II port)
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera],
),
Request(
[HYUNDAI_VERSION_REQUEST_MULTI],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar],
),
# CAN-FD queries (from camera)
# TODO: combine shared whitelists with CAN requests
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
# CAN-FD debugging queries
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
],
extra_ecus=[
(Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms
(Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms)
(Ecu.hvac, 0x7b3, None), # HVAC Control Assembly
(Ecu.cornerRadar, 0x7b7, None),
],
# Custom fuzzy fingerprinting function using platform codes, part numbers + FW dates:
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
CHECKSUM = {
"crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021,
CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022,
CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
}
CAN_GEARS = {
# which message has the gear. hybrid and EV use ELECT_GEAR
"use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
"use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
}
CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN,
CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN,
CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN,
CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.KIA_K8_HEV_1ST_GEN,
CAR.STARIA_4TH_GEN}
# The radar does SCC on these cars when HDA I, rather than the camera
CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80,
CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN}
# These CAN FD cars do not accept communication control to disable the ADAS ECU,
# responds with 0x7F2822 - 'conditions not correct'
CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN}
# The camera does SCC on these cars, rather than the radar
CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
# these cars use a different gas signal
HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ,
CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN,
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN,
CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_K8_HEV_1ST_GEN,
CAR.AZERA_HEV_6TH_GEN, CAR.KIA_NIRO_PHEV_2022}
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022,
CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KONA_EV_2ND_GEN}
# these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4,
CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022,
CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc.
UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL,
CAR.KIA_OPTIMA_H_G4_FL}
# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py
DBC = {
CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None),
CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_K5_HEV_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H_G4_FL: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KONA: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
CAR.TUCSON: dbc_dict('hyundai_kia_generic', None),
CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_EV6: dbc_dict('hyundai_canfd', None),
CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.IONIQ_5: dbc_dict('hyundai_canfd', None),
CAR.IONIQ_6: dbc_dict('hyundai_canfd', None),
CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SORENTO_PHEV_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None),
CAR.KIA_CARNIVAL_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SORENTO_HEV_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KONA_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_K8_HEV_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.CUSTIN_1ST_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_NIRO_PHEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.STARIA_4TH_GEN: dbc_dict('hyundai_canfd', None),
}

467
selfdrive/car/interfaces.py Normal file
View File

@@ -0,0 +1,467 @@
import os
import time
import numpy as np
import tomllib
from abc import abstractmethod, ABC
from enum import StrEnum
from typing import Any, Dict, Optional, Tuple, List, Callable
from cereal import car
from openpilot.common.basedir import BASEDIR
from openpilot.common.conversions import Conversions as CV
from openpilot.common.simple_kalman import KF1D, get_kalman_gain
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
from openpilot.selfdrive.controls.lib.events import Events
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
EventName = car.CarEvent.EventName
TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, bool], float]
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
ACCEL_MAX = 2.0
ACCEL_MIN = -3.5
FRICTION_THRESHOLD = 0.3
TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.toml')
TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.toml')
TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.toml')
def get_torque_params(candidate):
with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f:
sub = tomllib.load(f)
if candidate in sub:
candidate = sub[candidate]
with open(TORQUE_PARAMS_PATH, 'rb') as f:
params = tomllib.load(f)
with open(TORQUE_OVERRIDE_PATH, 'rb') as f:
override = tomllib.load(f)
# Ensure no overlap
if sum([candidate in x for x in [sub, params, override]]) > 1:
raise RuntimeError(f'{candidate} is defined twice in torque config')
if candidate in override:
out = override[candidate]
elif candidate in params:
out = params[candidate]
else:
raise NotImplementedError(f"Did not find torque params for {candidate}")
return {key: out[i] for i, key in enumerate(params['legend'])}
# generic car and radar interfaces
class CarInterfaceBase(ABC):
def __init__(self, CP, CarController, CarState):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.steering_unpressed = 0
self.low_speed_alert = False
self.no_steer_warning = False
self.silent_steer_warning = True
self.v_ego_cluster_seen = False
self.CS = None
self.can_parsers = []
if CarState is not None:
self.CS = CarState(CP)
self.cp = self.CS.get_can_parser(CP)
self.cp_cam = self.CS.get_cam_can_parser(CP)
self.cp_adas = self.CS.get_adas_can_parser(CP)
self.cp_body = self.CS.get_body_can_parser(CP)
self.cp_loopback = self.CS.get_loopback_can_parser(CP)
self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback]
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return ACCEL_MIN, ACCEL_MAX
@classmethod
def get_non_essential_params(cls, candidate: str):
"""
Parameters essential to controlling the car may be incomplete or wrong without FW versions or fingerprints.
"""
return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False)
@classmethod
def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
ret = CarInterfaceBase.get_std_params(candidate)
ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)
# Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
if not ret.notCar:
ret.mass = ret.mass + STD_CARGO_KG
# Set params dependent on values set by the car interface
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor)
return ret
@staticmethod
@abstractmethod
def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]],
car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
raise NotImplementedError
@staticmethod
def init(CP, logcan, sendcan):
pass
@staticmethod
def get_steer_feedforward_default(desired_angle, v_ego):
# Proportional to realigning tire momentum: lateral acceleration.
# TODO: something with lateralPlan.curvatureRates
return desired_angle * (v_ego**2)
def get_steer_feedforward_function(self):
return self.get_steer_feedforward_default
def torque_from_lateral_accel_linear(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
return (lateral_accel_value / float(torque_params.latAccelFactor)) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
return self.torque_from_lateral_accel_linear
# returns a set of default params to avoid repetition in car specific params
@staticmethod
def get_std_params(candidate):
ret = car.CarParams.new_message()
ret.carFingerprint = candidate
# Car docs fields
ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
ret.autoResumeSng = True # describes whether car can resume from a stop automatically
# standard ALC params
ret.tireStiffnessFactor = 1.0
ret.steerControlType = car.CarParams.SteerControlType.torque
ret.minSteerSpeed = 0.
ret.wheelSpeedFactor = 1.0
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA
ret.openpilotLongitudinalControl = False
ret.stopAccel = -2.0
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
ret.vEgoStopping = 0.5
ret.vEgoStarting = 0.5
ret.stoppingControl = True
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.longitudinalTuning.kf = 1.
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [1.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [1.]
# TODO estimate car specific lag, use .15s for now
ret.longitudinalActuatorDelayLowerBound = 0.15
ret.longitudinalActuatorDelayUpperBound = 0.15
ret.steerLimitTimer = 1.0
return ret
@staticmethod
def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
params = get_torque_params(candidate)
tune.init('torque')
tune.torque.useSteeringAngle = use_steering_angle
tune.torque.kp = 1.0
tune.torque.kf = 1.0
tune.torque.ki = 0.1
tune.torque.friction = params['FRICTION']
tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
tune.torque.latAccelOffset = 0.0
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
@abstractmethod
def _update(self, c: car.CarControl) -> car.CarState:
pass
def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
# parse can
for cp in self.can_parsers:
if cp is not None:
cp.update_strings(can_strings)
# get CarState
ret = self._update(c)
ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None)
ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None)
if ret.vEgoCluster == 0.0 and not self.v_ego_cluster_seen:
ret.vEgoCluster = ret.vEgo
else:
self.v_ego_cluster_seen = True
# Many cars apply hysteresis to the ego dash speed
if self.CS is not None:
ret.vEgoCluster = apply_hysteresis(ret.vEgoCluster, self.CS.out.vEgoCluster, self.CS.cluster_speed_hyst_gap)
if abs(ret.vEgo) < self.CS.cluster_min_speed:
ret.vEgoCluster = 0.0
if ret.cruiseState.speedCluster == 0:
ret.cruiseState.speedCluster = ret.cruiseState.speed
# copy back for next iteration
reader = ret.as_reader()
if self.CS is not None:
self.CS.out = reader
return reader
@abstractmethod
def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]:
pass
def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
events = Events()
if cs_out.doorOpen:
events.add(EventName.doorOpen)
if cs_out.seatbeltUnlatched:
events.add(EventName.seatbeltNotLatched)
if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
cs_out.gearShifter not in extra_gears):
events.add(EventName.wrongGear)
if cs_out.gearShifter == GearShifter.reverse:
events.add(EventName.reverseGear)
if not cs_out.cruiseState.available:
events.add(EventName.wrongCarMode)
if cs_out.espDisabled:
events.add(EventName.espDisabled)
if cs_out.stockFcw:
events.add(EventName.stockFcw)
if cs_out.stockAeb:
events.add(EventName.stockAeb)
if cs_out.vEgo > MAX_CTRL_SPEED:
events.add(EventName.speedTooHigh)
if cs_out.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if cs_out.parkingBrake:
events.add(EventName.parkBrake)
if cs_out.accFaulted:
events.add(EventName.accFaulted)
if cs_out.steeringPressed:
events.add(EventName.steerOverride)
# Handle button presses
for b in cs_out.buttonEvents:
# Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
events.add(EventName.buttonEnable)
# Disable on rising and falling edge of cancel for both stock and OP long
if b.type == ButtonType.cancel:
events.add(EventName.buttonCancel)
# Handle permanent and temporary steering faults
self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
if cs_out.steerFaultTemporary:
if cs_out.steeringPressed and (not self.CS.out.steerFaultTemporary or self.no_steer_warning):
self.no_steer_warning = True
else:
self.no_steer_warning = False
# if the user overrode recently, show a less harsh alert
if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
self.silent_steer_warning = True
events.add(EventName.steerTempUnavailableSilent)
else:
events.add(EventName.steerTempUnavailable)
else:
self.no_steer_warning = False
self.silent_steer_warning = False
if cs_out.steerFaultPermanent:
events.add(EventName.steerUnavailable)
# we engage when pcm is active (rising edge)
# enabling can optionally be blocked by the car interface
if pcm_enable:
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
events.add(EventName.pcmEnable)
elif not cs_out.cruiseState.enabled:
events.add(EventName.pcmDisable)
return events
class RadarInterfaceBase(ABC):
def __init__(self, CP):
self.rcp = None
self.pts = {}
self.delay = 0
self.radar_ts = CP.radarTimeStep
self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ
def update(self, can_strings):
ret = car.RadarData.new_message()
if not self.no_radar_sleep:
time.sleep(self.radar_ts) # radard runs on RI updates
return ret
class CarStateBase(ABC):
def __init__(self, CP):
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.out = car.CarState.new_message()
self.cruise_buttons = 0
self.left_blinker_cnt = 0
self.right_blinker_cnt = 0
self.steering_pressed_cnt = 0
self.left_blinker_prev = False
self.right_blinker_prev = False
self.cluster_speed_hyst_gap = 0.0
self.cluster_min_speed = 0.0 # min speed before dropping to 0
Q = [[0.0, 0.0], [0.0, 100.0]]
R = 0.3
A = [[1.0, DT_CTRL], [0.0, 1.0]]
C = [[1.0, 0.0]]
x0=[[0.0], [0.0]]
K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
def update_speed_kf(self, v_ego_raw):
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.set_x([[v_ego_raw], [0.0]])
v_ego_x = self.v_ego_kf.update(v_ego_raw)
return float(v_ego_x[0]), float(v_ego_x[1])
def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS):
factor = unit * self.CP.wheelSpeedFactor
wheelSpeeds = car.CarState.WheelSpeeds.new_message()
wheelSpeeds.fl = fl * factor
wheelSpeeds.fr = fr * factor
wheelSpeeds.rl = rl * factor
wheelSpeeds.rr = rr * factor
return wheelSpeeds
def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
"""Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
iterations"""
# TODO: Handle case when switching direction. Now both blinkers can be on at the same time
self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0)
self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0)
return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0
def update_steering_pressed(self, steering_pressed, steering_pressed_min_count):
"""Applies filtering on steering pressed for noisy driver torque signals."""
self.steering_pressed_cnt += 1 if steering_pressed else -1
self.steering_pressed_cnt = clip(self.steering_pressed_cnt, 0, steering_pressed_min_count * 2)
return self.steering_pressed_cnt > steering_pressed_min_count
def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool):
"""Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time,
or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker
is forced to the other side. On a rising edge of the stalk the timeout is reset."""
if left_blinker_stalk:
self.right_blinker_cnt = 0
if not self.left_blinker_prev:
self.left_blinker_cnt = blinker_time
if right_blinker_stalk:
self.left_blinker_cnt = 0
if not self.right_blinker_prev:
self.right_blinker_cnt = blinker_time
self.left_blinker_cnt = max(self.left_blinker_cnt - 1, 0)
self.right_blinker_cnt = max(self.right_blinker_cnt - 1, 0)
self.left_blinker_prev = left_blinker_stalk
self.right_blinker_prev = right_blinker_stalk
return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
@staticmethod
def parse_gear_shifter(gear: Optional[str]) -> car.CarState.GearShifter:
if gear is None:
return GearShifter.unknown
d: Dict[str, car.CarState.GearShifter] = {
'P': GearShifter.park, 'PARK': GearShifter.park,
'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse,
'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral,
'E': GearShifter.eco, 'ECO': GearShifter.eco,
'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic,
'D': GearShifter.drive, 'DRIVE': GearShifter.drive,
'S': GearShifter.sport, 'SPORT': GearShifter.sport,
'L': GearShifter.low, 'LOW': GearShifter.low,
'B': GearShifter.brake, 'BRAKE': GearShifter.brake,
}
return d.get(gear.upper(), GearShifter.unknown)
@staticmethod
def get_cam_can_parser(CP):
return None
@staticmethod
def get_adas_can_parser(CP):
return None
@staticmethod
def get_body_can_parser(CP):
return None
@staticmethod
def get_loopback_can_parser(CP):
return None
INTERFACE_ATTR_FILE = {
"FINGERPRINTS": "fingerprints",
"FW_VERSIONS": "fingerprints",
}
# interface-specific helpers
def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str | StrEnum, Any]:
# read all the folders in selfdrive/car and return a dict where:
# - keys are all the car models or brand names
# - values are attr values from all car folders
result = {}
for car_folder in sorted([x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]):
try:
brand_name = car_folder.split('/')[-1]
brand_values = __import__(f'openpilot.selfdrive.car.{brand_name}.{INTERFACE_ATTR_FILE.get(attr, "values")}', fromlist=[attr])
if hasattr(brand_values, attr) or not ignore_none:
attr_data = getattr(brand_values, attr, None)
else:
continue
if combine_brands:
if isinstance(attr_data, dict):
for f, v in attr_data.items():
result[f] = v
else:
result[brand_name] = attr_data
except (ImportError, OSError):
pass
return result

170
selfdrive/car/isotp_parallel_query.py Normal file
View File

@@ -0,0 +1,170 @@
import time
from collections import defaultdict
from functools import partial
import cereal.messaging as messaging
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr
class IsoTpParallelQuery:
def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addrs=None, debug=False, response_pending_timeout=10):
self.sendcan = sendcan
self.logcan = logcan
self.bus = bus
self.request = request
self.response = response
self.functional_addrs = functional_addrs or []
self.debug = debug
self.response_pending_timeout = response_pending_timeout
real_addrs = [a if isinstance(a, tuple) else (a, None) for a in addrs]
for tx_addr, _ in real_addrs:
assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}"
self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in real_addrs}
self.msg_buffer = defaultdict(list)
def rx(self):
"""Drain can socket and sort messages into buffers based on address"""
can_packets = messaging.drain_sock(self.logcan, wait_for_one=True)
for packet in can_packets:
for msg in packet.can:
if msg.src == self.bus and msg.address in self.msg_addrs.values():
self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
def _can_tx(self, tx_addr, dat, bus):
"""Helper function to send single message"""
msg = [tx_addr, 0, dat, bus]
self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan'))
def _can_rx(self, addr, sub_addr=None):
"""Helper function to retrieve message with specified address and subadress from buffer"""
keep_msgs = []
if sub_addr is None:
msgs = self.msg_buffer[addr]
else:
# Filter based on subadress
msgs = []
for m in self.msg_buffer[addr]:
first_byte = m[2][0]
if first_byte == sub_addr:
msgs.append(m)
else:
keep_msgs.append(m)
self.msg_buffer[addr] = keep_msgs
return msgs
def _drain_rx(self):
messaging.drain_sock_raw(self.logcan)
self.msg_buffer = defaultdict(list)
def _create_isotp_msg(self, tx_addr, sub_addr, rx_addr):
can_client = CanClient(self._can_tx, partial(self._can_rx, rx_addr, sub_addr=sub_addr), tx_addr, rx_addr,
self.bus, sub_addr=sub_addr, debug=self.debug)
max_len = 8 if sub_addr is None else 7
# uses iso-tp frame separation time of 10 ms
# TODO: use single_frame_mode so ECUs can send as fast as they want,
# as well as reduces chances we process messages from previous queries
return IsoTpMessage(can_client, timeout=0, separation_time=0.01, debug=self.debug, max_len=max_len)
def get_data(self, timeout, total_timeout=60.):
self._drain_rx()
# Create message objects
msgs = {}
request_counter = {}
request_done = {}
for tx_addr, rx_addr in self.msg_addrs.items():
msgs[tx_addr] = self._create_isotp_msg(*tx_addr, rx_addr)
request_counter[tx_addr] = 0
request_done[tx_addr] = False
# Send first request to functional addrs, subsequent responses are handled on physical addrs
if len(self.functional_addrs):
for addr in self.functional_addrs:
self._create_isotp_msg(addr, None, -1).send(self.request[0])
# Send first frame (single or first) to all addresses and receive asynchronously in the loop below.
# If querying functional addrs, only set up physical IsoTpMessages to send consecutive frames
for msg in msgs.values():
msg.send(self.request[0], setup_only=len(self.functional_addrs) > 0)
results = {}
start_time = time.monotonic()
addrs_responded = set() # track addresses that have ever sent a valid iso-tp frame for timeout logging
response_timeouts = {tx_addr: start_time + timeout for tx_addr in self.msg_addrs}
while True:
self.rx()
for tx_addr, msg in msgs.items():
try:
dat, rx_in_progress = msg.recv()
except Exception:
cloudlog.exception(f"Error processing UDS response: {tx_addr}")
request_done[tx_addr] = True
continue
# Extend timeout for each consecutive ISO-TP frame to avoid timing out on long responses
if rx_in_progress:
addrs_responded.add(tx_addr)
response_timeouts[tx_addr] = time.monotonic() + timeout
if dat is None:
continue
# Log unexpected empty responses
if len(dat) == 0:
cloudlog.error(f"iso-tp query empty response: {tx_addr}")
request_done[tx_addr] = True
continue
counter = request_counter[tx_addr]
expected_response = self.response[counter]
response_valid = dat.startswith(expected_response)
if response_valid:
if counter + 1 < len(self.request):
response_timeouts[tx_addr] = time.monotonic() + timeout
msg.send(self.request[counter + 1])
request_counter[tx_addr] += 1
else:
results[tx_addr] = dat[len(expected_response):]
request_done[tx_addr] = True
else:
error_code = dat[2] if len(dat) > 2 else -1
if error_code == 0x78:
response_timeouts[tx_addr] = time.monotonic() + self.response_pending_timeout
cloudlog.error(f"iso-tp query response pending: {tx_addr}")
else:
request_done[tx_addr] = True
cloudlog.error(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}")
# Mark request done if address timed out
cur_time = time.monotonic()
for tx_addr in response_timeouts:
if cur_time - response_timeouts[tx_addr] > 0:
if not request_done[tx_addr]:
if request_counter[tx_addr] > 0:
cloudlog.error(f"iso-tp query timeout after receiving partial response: {tx_addr}")
elif tx_addr in addrs_responded:
cloudlog.error(f"iso-tp query timeout while receiving response: {tx_addr}")
# TODO: handle functional addresses
# else:
# cloudlog.error(f"iso-tp query timeout with no response: {tx_addr}")
request_done[tx_addr] = True
# Break if all requests are done (finished or timed out)
if all(request_done.values()):
break
if cur_time - start_time > total_timeout:
cloudlog.error("iso-tp query timeout while receiving data")
break
return results

0
selfdrive/car/mazda/__init__.py Normal file
View File

65
selfdrive/car/mazda/carcontroller.py Normal file
View File

@@ -0,0 +1,65 @@
from cereal import car
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
from openpilot.selfdrive.car.mazda import mazdacan
from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.packer = CANPacker(dbc_name)
self.brake_counter = 0
self.frame = 0
def update(self, CC, CS, now_nanos):
can_sends = []
apply_steer = 0
if CC.latActive:
# calculate steer and also set limits due to driver torque
new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, CarControllerParams)
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
# a race condition with the stock system, where the second cancel from openpilot
# will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
# read 3 messages and most likely sync state before we attempt cancel.
self.brake_counter = self.brake_counter + 1
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
if CC.cruiseControl.resume and self.frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button when planner wants car to move
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
self.apply_steer_last = apply_steer
# send HUD alerts
if self.frame % 50 == 0:
ldw = CC.hudControl.visualAlert == VisualAlert.ldw
steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
# TODO: find a way to silence audible warnings so we can add more hud alerts
steer_required = steer_required and CS.lkas_allowed_speed
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
self.frame, apply_steer, CS.cam_lkas))
new_actuators = CC.actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.steerOutputCan = apply_steer
self.frame += 1
return new_actuators, can_sends

146
selfdrive/car/mazda/carstate.py Normal file
View File

@@ -0,0 +1,146 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["GEAR"]["GEAR"]
self.crz_btns_counter = 0
self.acc_active_last = False
self.low_speed_alert = False
self.lkas_allowed_speed = False
self.lkas_disabled = False
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["FL"],
cp.vl["WHEEL_SPEEDS"]["FR"],
cp.vl["WHEEL_SPEEDS"]["RL"],
cp.vl["WHEEL_SPEEDS"]["RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
# Match panda speed reading
speed_kph = cp.vl["ENGINE_DATA"]["SPEED"]
ret.standstill = speed_kph < .1
can_gear = int(cp.vl["GEAR"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"])
ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS_STATUS"] != 0
ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS_STATUS"] != 0
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"]
ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"]
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"]
ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"]
# TODO: this should be from 0 - 1.
ret.brakePressed = cp.vl["PEDALS"]["BRAKE_ON"] == 1
ret.brake = cp.vl["BRAKE"]["BRAKE_PRESSURE"]
ret.seatbeltUnlatched = cp.vl["SEATBELT"]["DRIVER_SEATBELT"] == 0
ret.doorOpen = any([cp.vl["DOORS"]["FL"], cp.vl["DOORS"]["FR"],
cp.vl["DOORS"]["BL"], cp.vl["DOORS"]["BR"]])
# TODO: this should be from 0 - 1.
ret.gas = cp.vl["ENGINE_DATA"]["PEDAL_GAS"]
ret.gasPressed = ret.gas > 0
# Either due to low speed or hands off
lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1
if self.CP.minSteerSpeed > 0:
# LKAS is enabled at 52kph going up and disabled at 45kph going down
# wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes
if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked:
self.lkas_allowed_speed = True
elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
self.lkas_allowed_speed = False
else:
self.lkas_allowed_speed = True
# TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on
# it should be used for carState.cruiseState.nonAdaptive instead
ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1
ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
if ret.cruiseState.enabled:
if not self.lkas_allowed_speed and self.acc_active_last:
self.low_speed_alert = True
else:
self.low_speed_alert = False
# Check if LKAS is disabled due to lack of driver torque when all other states indicate
# it should be enabled (steer lockout). Don't warn until we actually get lkas active
# and lose it again, i.e, after initial lkas activation
ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
self.acc_active_last = ret.cruiseState.enabled
self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
# camera signals
self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
return ret
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("BLINK_INFO", 10),
("STEER", 67),
("STEER_RATE", 83),
("STEER_TORQUE", 83),
("WHEEL_SPEEDS", 100),
]
if CP.carFingerprint in GEN1:
messages += [
("ENGINE_DATA", 100),
("CRZ_CTRL", 50),
("CRZ_EVENTS", 50),
("CRZ_BTNS", 10),
("PEDALS", 50),
("BRAKE", 50),
("SEATBELT", 10),
("DOORS", 10),
("GEAR", 20),
("BSM", 10),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
messages = []
if CP.carFingerprint in GEN1:
messages += [
# sig_address, frequency
("CAM_LANEINFO", 2),
("CAM_LKAS", 16),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)

258
selfdrive/car/mazda/fingerprints.py Normal file
View File

@@ -0,0 +1,258 @@
from cereal import car
from openpilot.selfdrive.car.mazda.values import CAR
Ecu = car.CarParams.Ecu
FW_VERSIONS = {
CAR.CX5_2022: {
(Ecu.eps, 0x730, None): [
b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PEW5-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX85-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH54-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'KGWD-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-U\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PG69-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.CX5: {
(Ecu.eps, 0x730, None): [
b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KCB8-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-M-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PA53-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PAR4-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2E-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2F-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2K-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX68-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFA-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SHKT-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KL2K-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KN0W-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PA66-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX39-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX39-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.CX9: {
(Ecu.eps, 0x730, None): [
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-L-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PX23-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX24-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM4-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXN8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXN8-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFM-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PXM4-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD6-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFM-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA3: {
(Ecu.eps, 0x730, None): [
b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYJW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'B63C-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'B45A-437AS-0-08\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-Q\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'P52G-21PS1-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PY2S-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKA-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA6: {
(Ecu.eps, 0x730, None): [
b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GBEF-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GFBC-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PA34-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX4F-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYH7-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GBVH-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GDDM-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PA28-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYH3-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.CX9_2021: {
(Ecu.eps, 0x730, None): [
b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PXGW-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXGW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM4-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM6-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
}

68
selfdrive/car/mazda/interface.py Executable file
View File

@@ -0,0 +1,68 @@
#!/usr/bin/env python3
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "mazda"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
ret.radarUnavailable = True
ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021)
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
ret.tireStiffnessFactor = 0.70 # not optimized yet
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.CX5, CAR.CX5_2022):
ret.mass = 3655 * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.steerRatio = 15.5
elif candidate in (CAR.CX9, CAR.CX9_2021):
ret.mass = 4217 * CV.LB_TO_KG
ret.wheelbase = 3.1
ret.steerRatio = 17.6
elif candidate == CAR.MAZDA3:
ret.mass = 2875 * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.steerRatio = 14.0
elif candidate == CAR.MAZDA6:
ret.mass = 3443 * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.steerRatio = 15.5
if candidate not in (CAR.CX5_2022, ):
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
ret.centerToFront = ret.wheelbase * 0.41
return ret
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
# events
events = self.create_common_events(ret)
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
elif self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

128
selfdrive/car/mazda/mazdacan.py Normal file
View File

@@ -0,0 +1,128 @@
from openpilot.selfdrive.car.mazda.values import GEN1, Buttons
def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
tmp = apply_steer + 2048
lo = tmp & 0xFF
hi = tmp >> 8
# copy values from camera
b1 = int(lkas["BIT_1"])
er1 = int(lkas["ERR_BIT_1"])
lnv = 0
ldw = 0
er2 = int(lkas["ERR_BIT_2"])
# Some older models do have these, newer models don't.
# Either way, they all work just fine if set to zero.
steering_angle = 0
b2 = 0
tmp = steering_angle + 2048
ahi = tmp >> 10
amd = (tmp & 0x3FF) >> 2
amd = (amd >> 4) | (( amd & 0xF) << 4)
alo = (tmp & 0x3) << 2
ctr = frame % 16
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
# bytes [ 5 ] [ 6 ] [ 7 ]
csum = csum - ahi - amd - alo - b2
if ahi == 1:
csum = csum + 15
if csum < 0:
if csum < -256:
csum = csum + 512
else:
csum = csum + 256
csum = csum % 256
values = {}
if car_fingerprint in GEN1:
values = {
"LKAS_REQUEST": apply_steer,
"CTR": ctr,
"ERR_BIT_1": er1,
"LINE_NOT_VISIBLE" : lnv,
"LDW": ldw,
"BIT_1": b1,
"ERR_BIT_2": er2,
"STEERING_ANGLE": steering_angle,
"ANGLE_ENABLED": b2,
"CHKSUM": csum
}
return packer.make_can_msg("CAM_LKAS", 0, values)
def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
values = {s: cam_msg[s] for s in [
"LINE_VISIBLE",
"LINE_NOT_VISIBLE",
"LANE_LINES",
"BIT1",
"BIT2",
"BIT3",
"NO_ERR_BIT",
"S1",
"S1_HBEAM",
]}
values.update({
# TODO: what's the difference between all these? do we need to send all?
"HANDS_WARN_3_BITS": 0b111 if steer_required else 0,
"HANDS_ON_STEER_WARN": steer_required,
"HANDS_ON_STEER_WARN_2": steer_required,
# TODO: right lane works, left doesn't
# TODO: need to do something about L/R
"LDW_WARN_LL": 0,
"LDW_WARN_RL": 0,
})
return packer.make_can_msg("CAM_LANEINFO", 0, values)
def create_button_cmd(packer, car_fingerprint, counter, button):
can = int(button == Buttons.CANCEL)
res = int(button == Buttons.RESUME)
if car_fingerprint in GEN1:
values = {
"CAN_OFF": can,
"CAN_OFF_INV": (can + 1) % 2,
"SET_P": 0,
"SET_P_INV": 1,
"RES": res,
"RES_INV": (res + 1) % 2,
"SET_M": 0,
"SET_M_INV": 1,
"DISTANCE_LESS": 0,
"DISTANCE_LESS_INV": 1,
"DISTANCE_MORE": 0,
"DISTANCE_MORE_INV": 1,
"MODE_X": 0,
"MODE_X_INV": 1,
"MODE_Y": 0,
"MODE_Y_INV": 1,
"BIT1": 1,
"BIT2": 1,
"BIT3": 1,
"CTR": (counter + 1) % 16,
}
return packer.make_can_msg("CRZ_BTNS", 0, values)

5
selfdrive/car/mazda/radar_interface.py Executable file
View File

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

95
selfdrive/car/mazda/values.py Normal file
View File

@@ -0,0 +1,95 @@
from dataclasses import dataclass, field
from enum import StrEnum
from typing import Dict, List, Union
from cereal import car
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
# Steer torque limits
class CarControllerParams:
STEER_MAX = 800 # theoretical max_steer 2047
STEER_DELTA_UP = 10 # torque increase per refresh
STEER_DELTA_DOWN = 25 # torque decrease per refresh
STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting
STEER_DRIVER_MULTIPLIER = 1 # weight driver torque
STEER_DRIVER_FACTOR = 1 # from dbc
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
STEER_STEP = 1 # 100 Hz
def __init__(self, CP):
pass
class CAR(StrEnum):
CX5 = "MAZDA CX-5"
CX9 = "MAZDA CX-9"
MAZDA3 = "MAZDA 3"
MAZDA6 = "MAZDA 6"
CX9_2021 = "MAZDA CX-9 2021"
CX5_2022 = "MAZDA CX-5 2022"
@dataclass
class MazdaCarInfo(CarInfo):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.mazda]))
CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = {
CAR.CX5: MazdaCarInfo("Mazda CX-5 2017-21"),
CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"),
CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"),
CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"),
CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"),
CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"),
}
class LKAS_LIMITS:
STEER_THRESHOLD = 15
DISABLE_SPEED = 45 # kph
ENABLE_SPEED = 52 # kph
class Buttons:
NONE = 0
SET_PLUS = 1
SET_MINUS = 2
RESUME = 3
CANCEL = 4
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
),
# Log responses on powertrain bus
Request(
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
bus=0,
logging=True,
),
],
)
DBC = {
CAR.CX5: dbc_dict('mazda_2017', None),
CAR.CX9: dbc_dict('mazda_2017', None),
CAR.MAZDA3: dbc_dict('mazda_2017', None),
CAR.MAZDA6: dbc_dict('mazda_2017', None),
CAR.CX9_2021: dbc_dict('mazda_2017', None),
CAR.CX5_2022: dbc_dict('mazda_2017', None),
}
# Gen 1 hardware: same CAN messages and same camera
GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022}

0
selfdrive/car/mock/__init__.py Normal file
View File

35
selfdrive/car/mock/interface.py Executable file
View File

@@ -0,0 +1,35 @@
#!/usr/bin/env python3
from cereal import car
import cereal.messaging as messaging
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
# mocked car interface for dashcam mode
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
self.speed = 0.
self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "mock"
ret.mass = 1700.
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 13.
return ret
def _update(self, c):
self.sm.update(0)
gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
ret = car.CarState.new_message()
ret.vEgo = self.sm[gps_sock].speed
ret.vEgoRaw = self.sm[gps_sock].speed
return ret
def apply(self, c, now_nanos):
actuators = car.CarControl.Actuators.new_message()
return actuators, []

4
selfdrive/car/mock/radar_interface.py Normal file
View File

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

13
selfdrive/car/mock/values.py Normal file
View File

@@ -0,0 +1,13 @@
from enum import StrEnum
from typing import Dict, List, Optional, Union
from openpilot.selfdrive.car.docs_definitions import CarInfo
class CAR(StrEnum):
MOCK = 'mock'
CAR_INFO: Dict[str, Optional[Union[CarInfo, List[CarInfo]]]] = {
CAR.MOCK: None,
}

0
selfdrive/car/nissan/__init__.py Normal file
View File

81
selfdrive/car/nissan/carcontroller.py Normal file
View File

@@ -0,0 +1,81 @@
from cereal import car
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_std_steer_angle_limits
from openpilot.selfdrive.car.nissan import nissancan
from openpilot.selfdrive.car.nissan.values import CAR, CarControllerParams
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.frame = 0
self.lkas_max_torque = 0
self.apply_angle_last = 0
self.packer = CANPacker(dbc_name)
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
can_sends = []
### STEER ###
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
if CC.latActive:
# windup slower
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CarControllerParams)
# Max torque from driver before EPS will give up and not apply torque
if not bool(CS.out.steeringPressed):
self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE
else:
# Scale max torque based on how much torque the driver is applying to the wheel
self.lkas_max_torque = max(
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum
CarControllerParams.LKAS_MAX_TORQUE * 0.5,
# Start scaling torque at STEER_THRESHOLD
CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD)
)
else:
apply_angle = CS.out.steeringAngleDeg
self.lkas_max_torque = 0
self.apply_angle_last = apply_angle
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd:
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg))
# TODO: Find better way to cancel!
# For some reason spamming the cancel button is unreliable on the Leaf
# We now cancel by making propilot think the seatbelt is unlatched,
# this generates a beep and a warning message every time you disengage
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0:
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd))
can_sends.append(nissancan.create_steering_control(
self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque))
# Below are the HUD messages. We copy the stock message and modify
if self.CP.carFingerprint != CAR.ALTIMA:
if self.frame % 2 == 0:
can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
if self.frame % 50 == 0:
can_sends.append(nissancan.create_lkas_hud_info_msg(
self.packer, CS.lkas_hud_info_msg, steer_hud_alert
))
new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_angle
self.frame += 1
return new_actuators, can_sends

191
selfdrive/car/nissan/carstate.py Normal file
View File

@@ -0,0 +1,191 @@
import copy
from collections import deque
from cereal import car
from opendbc.can.can_define import CANDefine
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.common.conversions import Conversions as CV
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
TORQUE_SAMPLES = 12
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.lkas_hud_msg = {}
self.lkas_hud_info_msg = {}
self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES)
self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
def update(self, cp, cp_adas, cp_cam):
ret = car.CarState.new_message()
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"]
ret.gasPressed = bool(ret.gas > 3)
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"])
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"])
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"],
cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"],
cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"],
cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0
if self.CP.carFingerprint == CAR.ALTIMA:
ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"])
else:
ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"])
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL):
ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0
ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"])
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
if self.CP.carFingerprint == CAR.LEAF:
ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0
elif self.CP.carFingerprint == CAR.LEAF_IC:
ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1
ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"])
elif self.CP.carFingerprint == CAR.ALTIMA:
ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0
ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"])
if self.CP.carFingerprint == CAR.ALTIMA:
speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"]
else:
speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"]
if speed != 255:
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS
else:
conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS
ret.cruiseState.speed = speed * conversion
ret.cruiseState.speedCluster = (speed - 1) * conversion # Speed on HUD is always 1 lower than actually sent on can bus
if self.CP.carFingerprint == CAR.ALTIMA:
ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
else:
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
self.steeringTorqueSamples.append(ret.steeringTorque)
# Filtering driver torque to prevent steeringPressed false positives
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD)
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])
ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"],
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"],
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FR"],
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FL"]])
ret.espDisabled = bool(cp.vl["ESP"]["ESP_DISABLED"])
can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
if self.CP.carFingerprint == CAR.ALTIMA:
self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"])
else:
self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"])
self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"])
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"])
if self.CP.carFingerprint != CAR.ALTIMA:
self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"])
self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"])
return ret
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("STEER_ANGLE_SENSOR", 100),
("WHEEL_SPEEDS_REAR", 50),
("WHEEL_SPEEDS_FRONT", 50),
("ESP", 25),
("GEARBOX", 25),
("DOORS_LIGHTS", 10),
("LIGHTS", 10),
]
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
messages += [
("GAS_PEDAL", 100),
("CRUISE_THROTTLE", 50),
("HUD", 25),
]
elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
messages += [
("BRAKE_PEDAL", 100),
("CRUISE_THROTTLE", 50),
("CANCEL_MSG", 50),
("HUD_SETTINGS", 25),
("SEATBELT", 10),
]
if CP.carFingerprint == CAR.ALTIMA:
messages += [
("CRUISE_STATE", 10),
("LKAS_SETTINGS", 10),
("PROPILOT_HUD", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1)
messages.append(("STEER_TORQUE_SENSOR", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_adas_can_parser(CP):
# this function generates lists for signal, messages and initial values
if CP.carFingerprint == CAR.ALTIMA:
messages = [
("LKAS", 100),
("PRO_PILOT", 100),
]
else:
messages = [
("PROPILOT_HUD_INFO_MSG", 2),
("LKAS_SETTINGS", 10),
("CRUISE_STATE", 50),
("PROPILOT_HUD", 50),
("LKAS", 100),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
@staticmethod
def get_cam_can_parser(CP):
messages = []
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL):
messages.append(("PRO_PILOT", 100))
elif CP.carFingerprint == CAR.ALTIMA:
messages.append(("STEER_TORQUE_SENSOR", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1)

104
selfdrive/car/nissan/fingerprints.py Normal file
View File

@@ -0,0 +1,104 @@
# ruff: noqa: E501
from cereal import car
from openpilot.selfdrive.car.nissan.values import CAR
Ecu = car.CarParams.Ecu
FINGERPRINTS = {
CAR.XTRAIL: [{
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837: 8, 2015: 8, 2016: 8, 2024: 8
},
{
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3, 1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8
}],
CAR.LEAF: [{
2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8
},
{
2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8
}],
CAR.LEAF_IC: [{
2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8
}],
CAR.ROGUE: [{
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.ALTIMA: [{
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 438: 8, 451: 8, 517: 8, 520: 2, 522: 8, 523: 6, 539: 8, 541: 7, 542: 8, 543: 8, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 570: 8, 576: 8, 577: 8, 582: 8, 583: 8, 584: 8, 586: 8, 587: 8, 588: 8, 589: 8, 590: 8, 591: 8, 592: 8, 600: 8, 601: 8, 610: 8, 611: 8, 612: 8, 614: 8, 615: 8, 616: 8, 617: 8, 622: 8, 623: 8, 634: 7, 638: 8, 645: 8, 648: 5, 654: 6, 658: 8, 659: 8, 660: 8, 661: 8, 665: 8, 666: 8, 674: 2, 675: 8, 676: 8, 682: 8, 683: 8, 684: 8, 685: 8, 686: 8, 687: 8, 689: 8, 690: 8, 703: 8, 708: 7, 709: 7, 711: 7, 712: 7, 713: 7, 714: 8, 715: 8, 716: 8, 717: 7, 718: 7, 719: 7, 720: 7, 723: 8, 726: 7, 727: 7, 728: 7, 735: 8, 746: 8, 748: 6, 749: 6, 750: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 779: 7, 781: 7, 782: 7, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1144: 7, 1145: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1258: 8, 1259: 8, 1266: 8, 1273: 7, 1306: 1, 1314: 8, 1323: 8, 1324: 8, 1342: 1, 1376: 8, 1401: 8, 1454: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
}
FW_VERSIONS = {
CAR.ALTIMA: {
(Ecu.fwdCamera, 0x707, None): [
b'284N86CA1D',
],
(Ecu.eps, 0x742, None): [
b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80',
],
(Ecu.engine, 0x7e0, None): [
b'237109HE2B',
],
(Ecu.gateway, 0x18dad0f1, None): [
b'284U29HE0A',
],
},
CAR.LEAF: {
(Ecu.abs, 0x740, None): [
b'476606WK9B',
],
(Ecu.eps, 0x742, None): [
b'5SN2A\xb7A\x05\x02N126F\x15\xb2\x00\x00\x00\x00\x00\x00\x00\x80',
],
(Ecu.fwdCamera, 0x707, None): [
b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80',
],
(Ecu.gateway, 0x18dad0f1, None): [
b'284U26WK0C',
],
},
CAR.LEAF_IC: {
(Ecu.fwdCamera, 0x707, None): [
b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80',
b'5SH4BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80',
b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80',
],
(Ecu.abs, 0x740, None): [
b'476605SD2E',
b'476605SH1D',
b'476605SK2A',
],
(Ecu.eps, 0x742, None): [
b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80',
b'5SH2C\xb7A\x05\x02N123F\x15\xa3\x00\x00\x00\x00\x00\x00\x00\x80',
b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80',
],
(Ecu.gateway, 0x18dad0f1, None): [
b'284U25SF0C',
b'284U25SH3A',
b'284U25SK2D',
],
},
CAR.XTRAIL: {
(Ecu.fwdCamera, 0x707, None): [
b'284N86FR2A',
],
(Ecu.abs, 0x740, None): [
b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80',
b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80',
],
(Ecu.eps, 0x742, None): [
b'6FP2A\x99A\x05\x02N123F\x18\x02\x00\x00\x00\x00\x00\x00\x00\x80',
],
(Ecu.combinationMeter, 0x743, None): [
b'6FR2A\x18B\x05\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
],
(Ecu.engine, 0x7e0, None): [
b'6FR9A\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
b'6FU9B\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
],
(Ecu.gateway, 0x18dad0f1, None): [
b'284U26FR0E',
],
},
}

60
selfdrive/car/nissan/interface.py Normal file
View File

@@ -0,0 +1,60 @@
from cereal import car
from panda import Panda
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.nissan.values import CAR
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "nissan"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
ret.autoResumeSng = False
ret.steerLimitTimer = 1.0
ret.steerActuatorDelay = 0.1
ret.steerRatio = 17
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.radarUnavailable = True
if candidate in (CAR.ROGUE, CAR.XTRAIL):
ret.mass = 1610
ret.wheelbase = 2.705
ret.centerToFront = ret.wheelbase * 0.44
elif candidate in (CAR.LEAF, CAR.LEAF_IC):
ret.mass = 1610
ret.wheelbase = 2.705
ret.centerToFront = ret.wheelbase * 0.44
elif candidate == CAR.ALTIMA:
# Altima has EPS on C-CAN unlike the others that have it on V-CAN
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS
ret.mass = 1492
ret.wheelbase = 2.824
ret.centerToFront = ret.wheelbase * 0.44
return ret
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
buttonEvents = []
be = car.CarState.ButtonEvent.new_message()
be.type = car.CarState.ButtonEvent.Type.accelCruise
buttonEvents.append(be)
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake])
if self.CS.lkas_enabled:
events.add(car.CarEvent.EventName.invalidLkasSetting)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

154
selfdrive/car/nissan/nissancan.py Normal file
View File

@@ -0,0 +1,154 @@
import crcmod
from openpilot.selfdrive.car.nissan.values import CAR
# TODO: add this checksum to the CANPacker
nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque):
values = {
"COUNTER": frame % 0x10,
"DESIRED_ANGLE": apply_steer,
"SET_0x80_2": 0x80,
"SET_0x80": 0x80,
"MAX_TORQUE": lkas_max_torque if steer_on else 0,
"LKA_ACTIVE": steer_on,
}
dat = packer.make_can_msg("LKAS", 0, values)[2]
values["CHECKSUM"] = nissan_checksum(dat[:7])
return packer.make_can_msg("LKAS", 0, values)
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg):
values = {s: cruise_throttle_msg[s] for s in [
"COUNTER",
"PROPILOT_BUTTON",
"CANCEL_BUTTON",
"GAS_PEDAL_INVERTED",
"SET_BUTTON",
"RES_BUTTON",
"FOLLOW_DISTANCE_BUTTON",
"NO_BUTTON_PRESSED",
"GAS_PEDAL",
"USER_BRAKE_PRESSED",
"NEW_SIGNAL_2",
"GAS_PRESSED_INVERTED",
"unsure1",
"unsure2",
"unsure3",
]}
can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2
values["CANCEL_BUTTON"] = 1
values["NO_BUTTON_PRESSED"] = 0
values["PROPILOT_BUTTON"] = 0
values["SET_BUTTON"] = 0
values["RES_BUTTON"] = 0
values["FOLLOW_DISTANCE_BUTTON"] = 0
return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values)
def create_cancel_msg(packer, cancel_msg, cruise_cancel):
values = {s: cancel_msg[s] for s in [
"CANCEL_SEATBELT",
"NEW_SIGNAL_1",
"NEW_SIGNAL_2",
"NEW_SIGNAL_3",
]}
if cruise_cancel:
values["CANCEL_SEATBELT"] = 1
return packer.make_can_msg("CANCEL_MSG", 2, values)
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart):
values = {s: lkas_hud_msg[s] for s in [
"LARGE_WARNING_FLASHING",
"SIDE_RADAR_ERROR_FLASHING1",
"SIDE_RADAR_ERROR_FLASHING2",
"LEAD_CAR",
"LEAD_CAR_ERROR",
"FRONT_RADAR_ERROR",
"FRONT_RADAR_ERROR_FLASHING",
"SIDE_RADAR_ERROR_FLASHING3",
"LKAS_ERROR_FLASHING",
"SAFETY_SHIELD_ACTIVE",
"RIGHT_LANE_GREEN_FLASH",
"LEFT_LANE_GREEN_FLASH",
"FOLLOW_DISTANCE",
"AUDIBLE_TONE",
"SPEED_SET_ICON",
"SMALL_STEERING_WHEEL_ICON",
"unknown59",
"unknown55",
"unknown26",
"unknown28",
"unknown31",
"SET_SPEED",
"unknown43",
"unknown08",
"unknown05",
"unknown02",
]}
values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0
values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0
values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0
return packer.make_can_msg("PROPILOT_HUD", 0, values)
def create_lkas_hud_info_msg(packer, lkas_hud_info_msg, steer_hud_alert):
values = {s: lkas_hud_info_msg[s] for s in [
"NA_HIGH_ACCEL_TEMP",
"SIDE_RADAR_NA_HIGH_CABIN_TEMP",
"SIDE_RADAR_MALFUNCTION",
"LKAS_MALFUNCTION",
"FRONT_RADAR_MALFUNCTION",
"SIDE_RADAR_NA_CLEAN_REAR_CAMERA",
"NA_POOR_ROAD_CONDITIONS",
"CURRENTLY_UNAVAILABLE",
"SAFETY_SHIELD_OFF",
"FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION",
"PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED",
"SIDE_IMPACT_NA_RADAR_OBSTRUCTION",
"WARNING_DO_NOT_ENTER",
"SIDE_IMPACT_SYSTEM_OFF",
"SIDE_IMPACT_MALFUNCTION",
"FRONT_COLLISION_MALFUNCTION",
"SIDE_RADAR_MALFUNCTION2",
"LKAS_MALFUNCTION2",
"FRONT_RADAR_MALFUNCTION2",
"PROPILOT_NA_MSGS",
"BOTTOM_MSG",
"HANDS_ON_WHEEL_WARNING",
"WARNING_STEP_ON_BRAKE_NOW",
"PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED",
"PROPILOT_NA_HIGH_CABIN_TEMP",
"WARNING_PROPILOT_MALFUNCTION",
"ACC_UNAVAILABLE_HIGH_CABIN_TEMP",
"ACC_NA_FRONT_CAMERA_IMPARED",
"unknown07",
"unknown10",
"unknown15",
"unknown23",
"unknown19",
"unknown31",
"unknown32",
"unknown46",
"unknown61",
"unknown55",
"unknown50",
]}
if steer_hud_alert:
values["HANDS_ON_WHEEL_WARNING"] = 1
return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values)

4
selfdrive/car/nissan/radar_interface.py Normal file
View File

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

91
selfdrive/car/nissan/values.py Normal file
View File

@@ -0,0 +1,91 @@
from dataclasses import dataclass, field
from enum import StrEnum
from typing import Dict, List, Optional, Union
from cereal import car
from panda.python import uds
from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarInfo, CarHarness, CarParts
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
class CarControllerParams:
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
STEER_THRESHOLD = 1.0
def __init__(self, CP):
pass
class CAR(StrEnum):
XTRAIL = "NISSAN X-TRAIL 2017"
LEAF = "NISSAN LEAF 2018"
# Leaf with ADAS ECU found behind instrument cluster instead of glovebox
# Currently the only known difference between them is the inverted seatbelt signal.
LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster"
ROGUE = "NISSAN ROGUE 2019"
ALTIMA = "NISSAN ALTIMA 2020"
@dataclass
class NissanCarInfo(CarInfo):
package: str = "ProPILOT Assist"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.nissan_a]))
CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = {
CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"),
CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"),
CAR.LEAF_IC: None, # same platforms
CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"),
CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b])),
}
# Default diagnostic session
NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81])
NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0x81])
# Manufacturer specific
NISSAN_DIAGNOSTIC_REQUEST_KWP_2 = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xda])
NISSAN_DIAGNOSTIC_RESPONSE_KWP_2 = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xda])
NISSAN_VERSION_REQUEST_KWP = b'\x21\x83'
NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83'
NISSAN_RX_OFFSET = 0x20
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
),
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
rx_offset=NISSAN_RX_OFFSET,
),
# Rogue's engine solely responds to this
Request(
[NISSAN_DIAGNOSTIC_REQUEST_KWP_2, NISSAN_VERSION_REQUEST_KWP],
[NISSAN_DIAGNOSTIC_RESPONSE_KWP_2, NISSAN_VERSION_RESPONSE_KWP],
),
Request(
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
rx_offset=NISSAN_RX_OFFSET,
),
],
)
DBC = {
CAR.XTRAIL: dbc_dict('nissan_x_trail_2017_generated', None),
CAR.LEAF: dbc_dict('nissan_leaf_2018_generated', None),
CAR.LEAF_IC: dbc_dict('nissan_leaf_2018_generated', None),
CAR.ROGUE: dbc_dict('nissan_x_trail_2017_generated', None),
CAR.ALTIMA: dbc_dict('nissan_x_trail_2017_generated', None),
}

0
selfdrive/car/subaru/__init__.py Normal file
View File

144
selfdrive/car/subaru/carcontroller.py Normal file
View File

@@ -0,0 +1,144 @@
from openpilot.common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
from openpilot.selfdrive.car.subaru import subarucan
from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, \
CanBus, CarControllerParams, SubaruFlags
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
# involves the total steering angle change rather than rate, but these limits work well for now
MAX_STEER_RATE = 25 # deg/s
MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.frame = 0
self.cruise_button_prev = 0
self.steer_rate_counter = 0
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
can_sends = []
# *** steering ***
if (self.frame % self.p.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
if not CC.latActive:
apply_steer = 0
if self.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive))
else:
apply_steer_req = CC.latActive
if self.CP.carFingerprint in STEER_RATE_LIMITED:
# Steering rate fault prevention
self.steer_rate_counter, apply_steer_req = \
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req))
self.apply_steer_last = apply_steer
# *** longitudinal ***
if CC.longActive:
apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V)))
apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V)))
apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V)))
# limit min and max values
cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
else:
cruise_throttle = CarControllerParams.THROTTLE_INACTIVE
cruise_rpm = CarControllerParams.RPM_MIN
cruise_brake = CarControllerParams.BRAKE_MIN
# *** alerts and pcm cancel ***
if self.CP.carFingerprint in PREGLOBAL_CARS:
if self.frame % 5 == 0:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
cruise_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
cruise_button = 1
else:
cruise_button = CS.cruise_button
# unstick previous mocked button press
if cruise_button == 1 and self.cruise_button_prev == 1:
cruise_button = 0
self.cruise_button_prev = cruise_button
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
else:
if self.frame % 10 == 0:
can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled,
self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible))
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
if self.CP.openpilotLongitudinalControl:
if self.frame % 5 == 0:
can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg,
self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
can_sends.append(subarucan.create_es_brake(self.packer, self.frame // 5, CS.es_brake_msg,
self.CP.openpilotLongitudinalControl, CC.longActive, cruise_brake))
can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd,
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
else:
if pcm_cancel_cmd:
if self.CP.carFingerprint not in HYBRID_CARS:
bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT:
# Tester present (keeps eyesight disabled)
if self.frame % 100 == 0:
can_sends.append([GLOBAL_ES_ADDR, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", CanBus.camera])
# Create all of the other eyesight messages to keep the rest of the car happy when eyesight is disabled
if self.frame % 5 == 0:
can_sends.append(subarucan.create_es_highbeamassist(self.packer))
if self.frame % 10 == 0:
can_sends.append(subarucan.create_es_static_1(self.packer))
if self.frame % 2 == 0:
can_sends.append(subarucan.create_es_static_2(self.packer))
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
self.frame += 1
return new_actuators, can_sends

220
selfdrive/car/subaru/carstate.py Normal file
View File

@@ -0,0 +1,220 @@
import copy
from cereal import car
from opendbc.can.can_define import CANDefine
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags
from openpilot.selfdrive.car import CanSignalRateCalculator
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["Transmission"]["Gear"]
self.angle_rate_calulator = CanSignalRateCalculator(50)
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"]
ret.gas = throttle_msg["Throttle_Pedal"] / 255.
ret.gasPressed = ret.gas > 1e-5
if self.car_fingerprint in PREGLOBAL_CARS:
ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 0
else:
cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1
cp_wheels = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
ret.wheelSpeeds = self.get_wheel_speeds(
cp_wheels.vl["Wheel_Speeds"]["FL"],
cp_wheels.vl["Wheel_Speeds"]["FR"],
cp_wheels.vl["Wheel_Speeds"]["RL"],
cp_wheels.vl["Wheel_Speeds"]["RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw == 0
# continuous blinker signals for assisted lane change
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
cp.vl["Dashlights"]["RIGHT_BLINKER"])
if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
cp_transmission = cp_body if self.car_fingerprint in HYBRID_CARS else cp
can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
if self.car_fingerprint not in PREGLOBAL_CARS:
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"])
ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
if self.car_fingerprint in HYBRID_CARS:
ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
else:
ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \
(self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1):
ret.cruiseState.speed *= CV.MPH_TO_KPH
ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1
ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"],
cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam
if self.car_fingerprint in PREGLOBAL_CARS:
self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
else:
ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
(cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2)
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
# TODO: Hybrid cars don't have ES_Distance, need a replacement
if self.car_fingerprint not in HYBRID_CARS:
# 8 is known AEB, there are a few other values related to AEB we ignore
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
if self.car_fingerprint not in HYBRID_CARS:
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
return ret
@staticmethod
def get_common_global_body_messages(CP):
messages = [
("Wheel_Speeds", 50),
("Brake_Status", 50),
]
if CP.carFingerprint not in HYBRID_CARS:
messages.append(("CruiseControl", 20))
return messages
@staticmethod
def get_common_global_es_messages(CP):
messages = [
("ES_Brake", 20),
]
if CP.carFingerprint not in HYBRID_CARS:
messages += [
("ES_Distance", 20),
("ES_Status", 20)
]
return messages
@staticmethod
def get_common_preglobal_body_messages():
messages = [
("CruiseControl", 50),
("Wheel_Speeds", 50),
("Dash_State2", 1),
]
return messages
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("Dashlights", 10),
("Steering_Torque", 50),
("BodyInfo", 1),
("Brake_Pedal", 50),
]
if CP.carFingerprint not in HYBRID_CARS:
messages += [
("Throttle", 100),
("Transmission", 100)
]
if CP.enableBsm:
messages.append(("BSD_RCTA", 17))
if CP.carFingerprint not in PREGLOBAL_CARS:
if CP.carFingerprint not in GLOBAL_GEN2:
messages += CarState.get_common_global_body_messages(CP)
else:
messages += CarState.get_common_preglobal_body_messages()
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.main)
@staticmethod
def get_cam_can_parser(CP):
if CP.carFingerprint in PREGLOBAL_CARS:
messages = [
("ES_DashStatus", 20),
("ES_Distance", 20),
]
else:
messages = [
("ES_DashStatus", 10),
("ES_LKAS_State", 10),
]
if CP.carFingerprint not in GLOBAL_GEN2:
messages += CarState.get_common_global_es_messages(CP)
if CP.flags & SubaruFlags.SEND_INFOTAINMENT:
messages.append(("ES_Infotainment", 10))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.camera)
@staticmethod
def get_body_can_parser(CP):
messages = []
if CP.carFingerprint in GLOBAL_GEN2:
messages += CarState.get_common_global_body_messages(CP)
messages += CarState.get_common_global_es_messages(CP)
if CP.carFingerprint in HYBRID_CARS:
messages += [
("Throttle_Hybrid", 40),
("Transmission", 100)
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt)

563
selfdrive/car/subaru/fingerprints.py Normal file
View File

@@ -0,0 +1,563 @@
from cereal import car
from openpilot.selfdrive.car.subaru.values import CAR
Ecu = car.CarParams.Ecu
FW_VERSIONS = {
CAR.ASCENT: {
(Ecu.abs, 0x7b0, None): [
b'\xa5 \x19\x02\x00',
b'\xa5 !\x02\x00',
b'\xf1\x82\xa5 \x19\x02\x00',
],
(Ecu.eps, 0x746, None): [
b'\x05\xc0\xd0\x00',
b'\x85\xc0\xd0\x00',
b'\x95\xc0\xd0\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00d\xb9\x00\x00\x00\x00',
b'\x00\x00d\xb9\x1f@ \x10',
b'\x00\x00e@\x00\x00\x00\x00',
b'\x00\x00e@\x1f@ $',
b"\x00\x00e~\x1f@ '",
],
(Ecu.engine, 0x7e0, None): [
b'\xbb,\xa0t\x07',
b'\xd1,\xa0q\x07',
b'\xf1\x82\xbb,\xa0t\x07',
b'\xf1\x82\xbb,\xa0t\x87',
b'\xf1\x82\xd1,\xa0q\x07',
b'\xf1\x82\xd9,\xa0@\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\x00\xfe\xf7\x00\x00',
b'\x01\xfe\xf7\x00\x00',
b'\x01\xfe\xf9\x00\x00',
b'\x01\xfe\xfa\x00\x00',
],
},
CAR.ASCENT_2023: {
(Ecu.abs, 0x7b0, None): [
b'\xa5 #\x03\x00',
],
(Ecu.eps, 0x746, None): [
b'%\xc0\xd0\x11',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x05!\x08\x1dK\x05!\x08\x01/',
],
(Ecu.engine, 0x7a2, None): [
b'\xe5,\xa0P\x07',
],
(Ecu.transmission, 0x7a3, None): [
b'\x04\xfe\xf3\x00\x00',
],
},
CAR.LEGACY: {
(Ecu.abs, 0x7b0, None): [
b'\xa1 \x02\x01',
b'\xa1 \x02\x02',
b'\xa1 \x03\x03',
b'\xa1\\ x04\x01',
],
(Ecu.eps, 0x746, None): [
b'\x9b\xc0\x11\x00',
b'\x9b\xc0\x11\x02',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00e\x80\x00\x1f@ \x19\x00',
b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\xde"a0\x07',
b'\xde,\xa0@\x07',
b'\xe2"aq\x07',
b'\xe2,\xa0@\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xa5\xf6\x05@\x00',
b'\xa5\xfe\xc7@\x00',
b'\xa7\xf6\x04@\x00',
b'\xa7\xfe\xc4@\x00',
],
},
CAR.IMPREZA: {
(Ecu.abs, 0x7b0, None): [
b'z\x84\x19\x90\x00',
b'z\x94\x08\x90\x00',
b'z\x94\x08\x90\x01',
b'z\x94\x0c\x90\x00',
b'z\x94\x0c\x90\x01',
b'z\x94.\x90\x00',
b'z\x94?\x90\x00',
b'z\x9c\x19\x80\x01',
b'\xa2 \x185\x00',
b'\xa2 \x193\x00',
b'\xa2 \x194\x00',
b'\xa2 \x19`\x00',
b'\xf1\x00\xb2\x06\x04',
],
(Ecu.eps, 0x746, None): [
b'z\xc0\x00\x00',
b'z\xc0\x04\x00',
b'z\xc0\x08\x00',
b'z\xc0\n\x00',
b'z\xc0\x0c\x00',
b'\x8a\xc0\x00\x00',
b'\x8a\xc0\x10\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00c\xf4\x00\x00\x00\x00',
b'\x00\x00c\xf4\x1f@ \x07',
b'\x00\x00d)\x00\x00\x00\x00',
b'\x00\x00d)\x1f@ \x07',
b'\x00\x00dd\x00\x00\x00\x00',
b'\x00\x00dd\x1f@ \x0e',
b'\x00\x00d\xb5\x1f@ \x0e',
b'\x00\x00d\xdc\x00\x00\x00\x00',
b'\x00\x00d\xdc\x1f@ \x0e',
b'\x00\x00e\x02\x1f@ \x14',
b'\x00\x00e\x1c\x00\x00\x00\x00',
b'\x00\x00e\x1c\x1f@ \x14',
b'\x00\x00e+\x00\x00\x00\x00',
b'\x00\x00e+\x1f@ \x14',
],
(Ecu.engine, 0x7e0, None): [
b'\xaa\x00Bu\x07',
b'\xaa\x01bt\x07',
b'\xaa!`u\x07',
b'\xaa!au\x07',
b'\xaa!av\x07',
b'\xaa!aw\x07',
b'\xaa!dq\x07',
b'\xaa!ds\x07',
b'\xaa!dt\x07',
b'\xaaafs\x07',
b'\xbe!as\x07',
b'\xbe!at\x07',
b'\xbeacr\x07',
b'\xc5!`r\x07',
b'\xc5!`s\x07',
b'\xc5!ap\x07',
b'\xc5!ar\x07',
b'\xc5!as\x07',
b'\xc5!dr\x07',
b'\xc5!ds\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xe3\xd0\x081\x00',
b'\xe3\xd5\x161\x00',
b'\xe3\xe5F1\x00',
b'\xe3\xf5\x06\x00\x00',
b'\xe3\xf5\x07\x00\x00',
b'\xe3\xf5C\x00\x00',
b'\xe3\xf5F\x00\x00',
b'\xe3\xf5G\x00\x00',
b'\xe4\xe5\x061\x00',
b'\xe4\xf5\x02\x00\x00',
b'\xe4\xf5\x07\x00\x00',
b'\xe5\xf5\x04\x00\x00',
b'\xe5\xf5$\x00\x00',
b'\xe5\xf5B\x00\x00',
],
},
CAR.IMPREZA_2020: {
(Ecu.abs, 0x7b0, None): [
b'\xa2 \x193\x00',
b'\xa2 \x194\x00',
b'\xa2 `\x00',
b'\xa2 !3\x00',
b'\xa2 !`\x00',
b'\xa2 !i\x00',
b'\xf1\x00\xb2\x06\x04',
],
(Ecu.eps, 0x746, None): [
b'\n\xc0\x04\x00',
b'\n\xc0\x04\x01',
b'\x9a\xc0\x00\x00',
b'\x9a\xc0\x04\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00eb\x1f@ "',
b'\x00\x00eq\x00\x00\x00\x00',
b'\x00\x00eq\x1f@ "',
b'\x00\x00e\x8f\x00\x00\x00\x00',
b'\x00\x00e\x8f\x1f@ )',
b'\x00\x00e\x92\x00\x00\x00\x00',
b'\x00\x00e\xa4\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\xca!`0\x07',
b'\xca!`p\x07',
b'\xca!ap\x07',
b'\xca!f@\x07',
b'\xca!fp\x07',
b'\xcc!`p\x07',
b'\xcc!fp\x07',
b'\xcc"f0\x07',
b'\xe6!`@\x07',
b'\xe6!fp\x07',
b'\xe6"f0\x07',
b'\xe6"fp\x07',
b'\xf3"f@\x07',
b'\xf3"fp\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xe6\xf5\x04\x00\x00',
b'\xe6\xf5$\x00\x00',
b'\xe6\xf5D0\x00',
b'\xe7\xf5\x04\x00\x00',
b'\xe7\xf5D0\x00',
b'\xe7\xf6B0\x00',
b'\xe9\xf5"\x00\x00',
b'\xe9\xf5B0\x00',
b'\xe9\xf6B0\x00',
b'\xe9\xf6F0\x00',
b'\xf1\x00\xd7\x10@',
],
},
CAR.CROSSTREK_HYBRID: {
(Ecu.abs, 0x7b0, None): [
b'\xa2 \x19e\x01',
b'\xa2 !e\x01',
],
(Ecu.eps, 0x746, None): [
b'\n\xc2\x01\x00',
b'\x9a\xc2\x01\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00el\x1f@ #',
],
(Ecu.engine, 0x7e0, None): [
b'\xd7!`@\x07',
b'\xd7!`p\x07',
b'\xf4!`0\x07',
],
},
CAR.FORESTER: {
(Ecu.abs, 0x7b0, None): [
b'\xa3 \x18\x14\x00',
b'\xa3 \x18&\x00',
b'\xa3 \x19\x14\x00',
b'\xa3 \x19&\x00',
b'\xa3 \x14\x00',
b'\xa3 \x14\x01',
b'\xf1\x00\xbb\r\x05',
],
(Ecu.eps, 0x746, None): [
b'\x8d\xc0\x00\x00',
b'\x8d\xc0\x04\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00e!\x00\x00\x00\x00',
b'\x00\x00e!\x1f@ \x11',
b'\x00\x00e^\x00\x00\x00\x00',
b'\x00\x00e^\x1f@ !',
b'\x00\x00e`\x1f@ ',
b'\x00\x00e\x97\x00\x00\x00\x00',
b'\x00\x00e\x97\x1f@ 0',
b'\xf1\x00\xac\x02\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\xb6"`A\x07',
b'\xb6\xa2`A\x07',
b'\xcb"`@\x07',
b'\xcb"`p\x07',
b'\xcf"`0\x07',
b'\xcf"`p\x07',
b'\xf1\x00\xa2\x10\n',
],
(Ecu.transmission, 0x7e1, None): [
b'\x1a\xe6B1\x00',
b'\x1a\xe6F1\x00',
b'\x1a\xf6B0\x00',
b'\x1a\xf6B`\x00',
b'\x1a\xf6F`\x00',
b'\x1a\xf6b0\x00',
b'\x1a\xf6b`\x00',
],
},
CAR.FORESTER_HYBRID: {
(Ecu.abs, 0x7b0, None): [
b'\xa3 \x19T\x00',
],
(Ecu.eps, 0x746, None): [
b'\x8d\xc2\x00\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00eY\x1f@ !',
],
(Ecu.engine, 0x7e0, None): [
b'\xd2\xa1`r\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\x1b\xa7@a\x00',
],
},
CAR.FORESTER_PREGLOBAL: {
(Ecu.abs, 0x7b0, None): [
b'm\x97\x14@',
b'}\x97\x14@',
b'\xf1\x00\xbb\x0c\x04',
],
(Ecu.eps, 0x746, None): [
b'm\xc0\x10\x00',
b'}\xc0\x10\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00c\xe9\x00\x00\x00\x00',
b'\x00\x00c\xe9\x1f@ \x03',
b'\x00\x00d5\x1f@ \t',
b'\x00\x00d\xd3\x1f@ \t',
],
(Ecu.engine, 0x7e0, None): [
b'\xa7"@p\x07',
b'\xa7)\xa0q\x07',
b'\xba"@@\x07',
b'\xba"@p\x07',
b'\xf1\x82\xa7)\xa0q\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\x1a\xf6F`\x00',
b'\xda\xf2`\x80\x00',
b'\xda\xfd\xe0\x80\x00',
b'\xdc\xf2@`\x00',
b'\xdc\xf2``\x00',
b'\xdc\xf2`\x80\x00',
b'\xdc\xf2`\x81\x00',
],
},
CAR.LEGACY_PREGLOBAL: {
(Ecu.abs, 0x7b0, None): [
b'[\x97D\x00',
b'[\xba\xc4\x03',
b'k\x97D\x00',
b'k\x9aD\x00',
b'{\x97D\x00',
],
(Ecu.eps, 0x746, None): [
b'K\xb0\x00\x01',
b'[\xb0\x00\x01',
b'k\xb0\x00\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00c\x94\x1f@\x10\x08',
b'\x00\x00c\xb7\x1f@\x10\x16',
b'\x00\x00c\xec\x1f@ \x04',
],
(Ecu.engine, 0x7e0, None): [
b'\xa0"@q\x07',
b'\xa0+@p\x07',
b'\xab*@r\x07',
b'\xab+@p\x07',
b'\xb4"@0\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xbd\xf2\x00`\x00',
b'\xbe\xf2\x00p\x00',
b'\xbe\xfb\xc0p\x00',
b'\xbf\xf2\x00\x80\x00',
b'\xbf\xfb\xc0\x80\x00',
],
},
CAR.OUTBACK_PREGLOBAL: {
(Ecu.abs, 0x7b0, None): [
b'[\xba\xac\x03',
b'[\xf7\xac\x00',
b'[\xf7\xac\x03',
b'[\xf7\xbc\x03',
b'k\x97\xac\x00',
b'k\x9a\xac\x00',
b'{\x97\xac\x00',
b'{\x9a\xac\x00',
],
(Ecu.eps, 0x746, None): [
b'K\xb0\x00\x00',
b'K\xb0\x00\x02',
b'[\xb0\x00\x00',
b'k\xb0\x00\x00',
b'{\xb0\x00\x01',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00c\x90\x1f@\x10\x0e',
b'\x00\x00c\x94\x00\x00\x00\x00',
b'\x00\x00c\x94\x1f@\x10\x08',
b'\x00\x00c\xb7\x1f@\x10\x16',
b'\x00\x00c\xd1\x1f@\x10\x17',
b'\x00\x00c\xec\x1f@ \x04',
b'\x00\x00c\xec7@\x04',
b'\xf1\x00\xf0\xe0\x0e',
],
(Ecu.engine, 0x7e0, None): [
b'\xa0"@\x80\x07',
b'\xa0*@q\x07',
b'\xa0*@u\x07',
b'\xa0+@@\x07',
b'\xa0bAq\x07',
b'\xab"@@\x07',
b'\xab"@s\x07',
b'\xab*@@\x07',
b'\xab+@@\x07',
b'\xb4"@0\x07',
b'\xb4"@p\x07',
b'\xb4"@r\x07',
b'\xb4+@p\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xbd\xf2@`\x00',
b'\xbd\xf2@\x81\x00',
b'\xbd\xfb\xe0\x80\x00',
b'\xbe\xf2@p\x00',
b'\xbe\xf2@\x80\x00',
b'\xbe\xfb\xe0p\x00',
b'\xbf\xe2@\x80\x00',
b'\xbf\xf2@\x80\x00',
b'\xbf\xfb\xe0b\x00',
],
},
CAR.OUTBACK_PREGLOBAL_2018: {
(Ecu.abs, 0x7b0, None): [
b'\x8b\x97\xac\x00',
b'\x8b\x97\xbc\x00',
b'\x8b\x99\xac\x00',
b'\x8b\x9a\xac\x00',
b'\x9b\x97\xac\x00',
b'\x9b\x97\xbe\x10',
b'\x9b\x9a\xac\x00',
],
(Ecu.eps, 0x746, None): [
b'{\xb0\x00\x00',
b'{\xb0\x00\x01',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00df\x1f@ \n',
b'\x00\x00d\x95\x00\x00\x00\x00',
b'\x00\x00d\x95\x1f@ \x0f',
b'\x00\x00d\xfe\x00\x00\x00\x00',
b'\x00\x00d\xfe\x1f@ \x15',
b'\x00\x00e\x19\x1f@ \x15',
],
(Ecu.engine, 0x7e0, None): [
b'\xb5"@P\x07',
b'\xb5"@p\x07',
b'\xb5+@@\x07',
b'\xb5b@1\x07',
b'\xb5q\xe0@\x07',
b'\xc4"@0\x07',
b'\xc4+@0\x07',
b'\xc4b@p\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xbb\xf2@`\x00',
b'\xbb\xfb\xe0`\x00',
b'\xbc\xaf\xe0`\x00',
b'\xbc\xe2@\x80\x00',
b'\xbc\xf2@\x80\x00',
b'\xbc\xf2@\x81\x00',
b'\xbc\xfb\xe0`\x00',
b'\xbc\xfb\xe0\x80\x00',
],
},
CAR.OUTBACK: {
(Ecu.abs, 0x7b0, None): [
b'\xa1 \x06\x02',
b'\xa1 \x06\x00',
b'\xa1 \x06\x01',
b'\xa1 \x07\x00',
b'\xa1 \x07\x02',
b'\xa1 \x08\x00',
b'\xa1 \x08\x01',
b'\xa1 \x08\x02',
b'\xa1 "\t\x00',
b'\xa1 "\t\x01',
],
(Ecu.eps, 0x746, None): [
b'\x1b\xc0\x10\x00',
b'\x9b\xc0\x10\x00',
b'\x9b\xc0 \x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00eJ\x00\x00\x00\x00\x00\x00',
b'\x00\x00eJ\x00\x1f@ \x19\x00',
b'\x00\x00e\x80\x00\x1f@ \x19\x00',
b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00',
b'\x00\x00e\x9a\x00\x1f@ 1\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\xbc"`@\x07',
b'\xbc"`q\x07',
b'\xbc,\xa0q\x07',
b'\xbc,\xa0u\x07',
b'\xde"`0\x07',
b'\xde,\xa0@\x07',
b'\xe2"`0\x07',
b'\xe2"`p\x07',
b'\xe3,\xa0@\x07',
b'\xf1\x82\xbc,\xa0q\x07',
b'\xf1\x82\xe2,\xa0@\x07',
b'\xf1\x82\xe3,\xa0@\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xa5\xf6D@\x00',
b'\xa5\xfe\xf6@\x00',
b'\xa5\xfe\xf7@\x00',
b'\xa5\xfe\xf8@\x00',
b'\xa7\x8e\xf40\x00',
b'\xa7\xf6D@\x00',
b'\xa7\xfe\xf4@\x00',
b'\xf1\x82\xa7\xf6D@\x00',
],
},
CAR.FORESTER_2022: {
(Ecu.abs, 0x7b0, None): [
b'\xa3 !v\x00',
b'\xa3 !x\x00',
b'\xa3 "v\x00',
b'\xa3 "x\x00',
],
(Ecu.eps, 0x746, None): [
b'-\xc0\x040',
b'-\xc0%0',
b'=\xc0%\x02',
b'=\xc04\x02',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x04!\x01\x1eD\x07!\x00\x04,',
b'\x04!\x08\x01.\x07!\x08\x022',
],
(Ecu.engine, 0x7e0, None): [
b'\xd5"`0\x07',
b'\xd5"a0\x07',
b'\xf1"`q\x07',
b'\xf1"aq\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\x1d\x86B0\x00',
b'\x1d\xf6B0\x00',
b'\x1e\x86B0\x00',
b'\x1e\xf6D0\x00',
],
},
CAR.OUTBACK_2023: {
(Ecu.abs, 0x7b0, None): [
b'\xa1 #\x14\x00',
b'\xa1 #\x17\x00',
],
(Ecu.eps, 0x746, None): [
b'+\xc0\x10\x11\x00',
b'+\xc0\x12\x11\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\t!\x08\x046\x05!\x08\x01/',
],
(Ecu.engine, 0x7a2, None): [
b'\xed,\xa0q\x07',
b'\xed,\xa2q\x07',
],
(Ecu.transmission, 0x7a3, None): [
b'\xa8\x8e\xf41\x00',
b'\xa8\xfe\xf41\x00',
],
},
}

153
selfdrive/car/subaru/interface.py Normal file
View File

@@ -0,0 +1,153 @@
from cereal import car
from panda import Panda
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.disable_ecu import disable_ecu
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "subaru"
ret.radarUnavailable = True
# for HYBRID CARS to be upstreamed, we need:
# - replacement for ES_Distance so we can cancel the cruise control
# - to find the Cruise_Activated bit from the car
# - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS)
ret.autoResumeSng = False
# Detect infotainment message sent from the camera
if candidate not in PREGLOBAL_CARS and 0x323 in fingerprint[2]:
ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value
if candidate in PREGLOBAL_CARS:
ret.enableBsm = 0x25c in fingerprint[0]
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)]
else:
ret.enableBsm = 0x228 in fingerprint[0]
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)]
if candidate in GLOBAL_GEN2:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2
ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
if candidate in LKAS_ANGLE:
ret.steerControlType = car.CarParams.SteerControlType.angle
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.ASCENT, CAR.ASCENT_2023):
ret.mass = 2031.
ret.wheelbase = 2.89
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 13.5
ret.steerActuatorDelay = 0.3 # end-to-end angle controller
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.00003
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
elif candidate == CAR.IMPREZA:
ret.mass = 1568.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 15
ret.steerActuatorDelay = 0.4 # end-to-end angle controller
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
elif candidate == CAR.IMPREZA_2020:
ret.mass = 1480.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17 # learned, 14 stock
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
elif candidate == CAR.CROSSTREK_HYBRID:
ret.mass = 1668.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17
ret.steerActuatorDelay = 0.1
elif candidate in (CAR.FORESTER, CAR.FORESTER_2022, CAR.FORESTER_HYBRID):
ret.mass = 1568.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17 # learned, 14 stock
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.000038
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
elif candidate in (CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023):
ret.mass = 1568.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17
ret.steerActuatorDelay = 0.1
elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018):
ret.safetyConfigs[0].safetyParam = Panda.FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE # Outback 2018-2019 and Forester have reversed driver torque signal
ret.mass = 1568
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
elif candidate == CAR.LEGACY_PREGLOBAL:
ret.mass = 1568
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 12.5 # 14.5 stock
ret.steerActuatorDelay = 0.15
elif candidate == CAR.OUTBACK_PREGLOBAL:
ret.mass = 1568
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
else:
raise ValueError(f"unknown car: {candidate}")
ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
if candidate in GLOBAL_GEN2 and ret.openpilotLongitudinalControl:
ret.flags |= SubaruFlags.DISABLE_EYESIGHT.value
if ret.openpilotLongitudinalControl:
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
ret.stoppingControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
return ret
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.events = self.create_common_events(ret).to_msg()
return ret
@staticmethod
def init(CP, logcan, sendcan):
if CP.flags & SubaruFlags.DISABLE_EYESIGHT:
disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01')
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

4
selfdrive/car/subaru/radar_interface.py Normal file
View File

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

312
selfdrive/car/subaru/subarucan.py Normal file
View File

@@ -0,0 +1,312 @@
from cereal import car
from openpilot.selfdrive.car.subaru.values import CanBus
VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_steering_control(packer, apply_steer, steer_req):
values = {
"LKAS_Output": apply_steer,
"LKAS_Request": steer_req,
"SET_1": 1
}
return packer.make_can_msg("ES_LKAS", 0, values)
def create_steering_status(packer):
return packer.make_can_msg("ES_LKAS_State", 0, {})
def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
values = {s: es_distance_msg[s] for s in [
"CHECKSUM",
"Signal1",
"Cruise_Fault",
"Cruise_Throttle",
"Signal2",
"Car_Follow",
"Low_Speed_Follow",
"Cruise_Soft_Disable",
"Signal7",
"Cruise_Brake_Active",
"Distance_Swap",
"Cruise_EPB",
"Signal4",
"Close_Distance",
"Signal5",
"Cruise_Cancel",
"Cruise_Set",
"Cruise_Resume",
"Signal6",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_Throttle"] = cruise_throttle
# Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
values["Cruise_Soft_Disable"] = 0
values["Cruise_Fault"] = 0
values["Cruise_Brake_Active"] = brake_cmd
if pcm_cancel_cmd:
values["Cruise_Cancel"] = 1
values["Cruise_Throttle"] = 1818 # inactive throttle
return packer.make_can_msg("ES_Distance", bus, values)
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
values = {s: es_lkas_state_msg[s] for s in [
"CHECKSUM",
"LKAS_Alert_Msg",
"Signal1",
"LKAS_ACTIVE",
"LKAS_Dash_State",
"Signal2",
"Backward_Speed_Limit_Menu",
"LKAS_Left_Line_Enable",
"LKAS_Left_Line_Light_Blink",
"LKAS_Right_Line_Enable",
"LKAS_Right_Line_Light_Blink",
"LKAS_Left_Line_Visible",
"LKAS_Right_Line_Visible",
"LKAS_Alert",
"Signal3",
]}
values["COUNTER"] = frame % 0x10
# Filter the stock LKAS "Keep hands on wheel" alert
if values["LKAS_Alert_Msg"] == 1:
values["LKAS_Alert_Msg"] = 0
# Filter the stock LKAS sending an audible alert when it turns off LKAS
if values["LKAS_Alert"] == 27:
values["LKAS_Alert"] = 0
# Filter the stock LKAS sending an audible alert when "Keep hands on wheel" alert is active (2020+ models)
if values["LKAS_Alert"] == 28 and values["LKAS_Alert_Msg"] == 7:
values["LKAS_Alert"] = 0
# Filter the stock LKAS sending an audible alert when "Keep hands on wheel OFF" alert is active (2020+ models)
if values["LKAS_Alert"] == 30:
values["LKAS_Alert"] = 0
# Filter the stock LKAS sending "Keep hands on wheel OFF" alert (2020+ models)
if values["LKAS_Alert_Msg"] == 7:
values["LKAS_Alert_Msg"] = 0
# Show Keep hands on wheel alert for openpilot steerRequired alert
if visual_alert == VisualAlert.steerRequired:
values["LKAS_Alert_Msg"] = 1
# Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW)
if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0:
if left_lane_depart:
values["LKAS_Alert"] = 12 # Left lane departure dash alert
elif right_lane_depart:
values["LKAS_Alert"] = 11 # Right lane departure dash alert
if enabled:
values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
values["LKAS_Dash_State"] = 2 # Green enabled indicator
else:
values["LKAS_Dash_State"] = 0 # LKAS Not enabled
values["LKAS_Left_Line_Visible"] = int(left_line)
values["LKAS_Right_Line_Visible"] = int(right_line)
return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
values = {s: dashstatus_msg[s] for s in [
"CHECKSUM",
"PCB_Off",
"LDW_Off",
"Signal1",
"Cruise_State_Msg",
"LKAS_State_Msg",
"Signal2",
"Cruise_Soft_Disable",
"Cruise_Status_Msg",
"Signal3",
"Cruise_Distance",
"Signal4",
"Conventional_Cruise",
"Signal5",
"Cruise_Disengaged",
"Cruise_Activated",
"Signal6",
"Cruise_Set_Speed",
"Cruise_Fault",
"Cruise_On",
"Display_Own_Car",
"Brake_Lights",
"Car_Follow",
"Signal7",
"Far_Distance",
"Cruise_State",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_State"] = 0
values["Cruise_Activated"] = enabled
values["Cruise_Disengaged"] = 0
values["Car_Follow"] = int(lead_visible)
values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
values["LDW_Off"] = 0
values["Cruise_Fault"] = 0
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
if values["LKAS_State_Msg"] in (2, 3):
values["LKAS_State_Msg"] = 0
return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value):
values = {s: es_brake_msg[s] for s in [
"CHECKSUM",
"Signal1",
"Brake_Pressure",
"AEB_Status",
"Cruise_Brake_Lights",
"Cruise_Brake_Fault",
"Cruise_Brake_Active",
"Cruise_Activated",
"Signal3",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_Brake_Fault"] = 0
values["Cruise_Activated"] = long_active
values["Brake_Pressure"] = brake_value
values["Cruise_Brake_Active"] = brake_value > 0
values["Cruise_Brake_Lights"] = brake_value >= 70
return packer.make_can_msg("ES_Brake", CanBus.main, values)
def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm):
values = {s: es_status_msg[s] for s in [
"CHECKSUM",
"Signal1",
"Cruise_Fault",
"Cruise_RPM",
"Cruise_Activated",
"Brake_Lights",
"Cruise_Hold",
"Signal3",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_RPM"] = cruise_rpm
values["Cruise_Fault"] = 0
values["Cruise_Activated"] = long_active
return packer.make_can_msg("ES_Status", CanBus.main, values)
def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
values = {s: es_infotainment_msg[s] for s in [
"CHECKSUM",
"LKAS_State_Infotainment",
"LKAS_Blue_Lines",
"Signal1",
"Signal2",
]}
values["COUNTER"] = frame % 0x10
if values["LKAS_State_Infotainment"] in (3, 4):
values["LKAS_State_Infotainment"] = 0
# Show Keep hands on wheel alert for openpilot steerRequired alert
if visual_alert == VisualAlert.steerRequired:
values["LKAS_State_Infotainment"] = 3
# Show Obstacle Detected for fcw
if visual_alert == VisualAlert.fcw:
values["LKAS_State_Infotainment"] = 2
return packer.make_can_msg("ES_Infotainment", CanBus.main, values)
def create_es_highbeamassist(packer):
values = {
"HBA_Available": False,
}
return packer.make_can_msg("ES_HighBeamAssist", CanBus.main, values)
def create_es_static_1(packer):
values = {
"SET_3": 3,
}
return packer.make_can_msg("ES_STATIC_1", CanBus.main, values)
def create_es_static_2(packer):
values = {
"SET_3": 3,
}
return packer.make_can_msg("ES_STATIC_2", CanBus.main, values)
# *** Subaru Pre-global ***
def subaru_preglobal_checksum(packer, values, addr, checksum_byte=7):
dat = packer.make_can_msg(addr, 0, values)[2]
return (sum(dat[:checksum_byte]) + sum(dat[checksum_byte+1:])) % 256
def create_preglobal_steering_control(packer, frame, apply_steer, steer_req):
values = {
"COUNTER": frame % 0x08,
"LKAS_Command": apply_steer,
"LKAS_Active": steer_req,
}
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS")
return packer.make_can_msg("ES_LKAS", CanBus.main, values)
def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
values = {s: es_distance_msg[s] for s in [
"Cruise_Throttle",
"Signal1",
"Car_Follow",
"Signal2",
"Cruise_Brake_Active",
"Distance_Swap",
"Standstill",
"Signal3",
"Close_Distance",
"Signal4",
"Standstill_2",
"Cruise_Fault",
"Signal5",
"COUNTER",
"Signal6",
"Cruise_Button",
"Signal7",
]}
values["Cruise_Button"] = cruise_button
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance")
return packer.make_can_msg("ES_Distance", CanBus.main, values)

203
selfdrive/car/subaru/values.py Normal file
View File

@@ -0,0 +1,203 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag, StrEnum
from typing import Dict, List, Union
from cereal import car
from panda.python import uds
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Tool, Column
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Ecu = car.CarParams.Ecu
class CarControllerParams:
def __init__(self, CP):
self.STEER_STEP = 2 # how often we update the steer cmd
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 1 # from dbc
if CP.carFingerprint in GLOBAL_GEN2:
self.STEER_MAX = 1000
self.STEER_DELTA_UP = 40
self.STEER_DELTA_DOWN = 40
elif CP.carFingerprint == CAR.IMPREZA_2020:
self.STEER_MAX = 1439
else:
self.STEER_MAX = 2047
THROTTLE_MIN = 808
THROTTLE_MAX = 3400
THROTTLE_INACTIVE = 1818 # corresponds to zero acceleration
THROTTLE_ENGINE_BRAKE = 808 # while braking, eyesight sets throttle to this, probably for engine braking
BRAKE_MIN = 0
BRAKE_MAX = 600 # about -3.5m/s2 from testing
RPM_MIN = 0
RPM_MAX = 2400
RPM_INACTIVE = 600 # a good base rpm for zero acceleration
THROTTLE_LOOKUP_BP = [0, 2]
THROTTLE_LOOKUP_V = [THROTTLE_INACTIVE, THROTTLE_MAX]
RPM_LOOKUP_BP = [0, 2]
RPM_LOOKUP_V = [RPM_INACTIVE, RPM_MAX]
BRAKE_LOOKUP_BP = [-3.5, 0]
BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN]
class SubaruFlags(IntFlag):
SEND_INFOTAINMENT = 1
DISABLE_EYESIGHT = 2
GLOBAL_ES_ADDR = 0x787
GEN2_ES_BUTTONS_DID = b'\x11\x30'
class CanBus:
main = 0
alt = 1
camera = 2
class CAR(StrEnum):
# Global platform
ASCENT = "SUBARU ASCENT LIMITED 2019"
ASCENT_2023 = "SUBARU ASCENT 2023"
IMPREZA = "SUBARU IMPREZA LIMITED 2019"
IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
FORESTER = "SUBARU FORESTER 2019"
OUTBACK = "SUBARU OUTBACK 6TH GEN"
CROSSTREK_HYBRID = "SUBARU CROSSTREK HYBRID 2020"
FORESTER_HYBRID = "SUBARU FORESTER HYBRID 2020"
LEGACY = "SUBARU LEGACY 7TH GEN"
FORESTER_2022 = "SUBARU FORESTER 2022"
OUTBACK_2023 = "SUBARU OUTBACK 7TH GEN"
# Pre-global
FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018"
LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018"
OUTBACK_PREGLOBAL = "SUBARU OUTBACK 2015 - 2017"
OUTBACK_PREGLOBAL_2018 = "SUBARU OUTBACK 2018 - 2019"
class Footnote(Enum):
GLOBAL = CarFootnote(
"In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.",
Column.PACKAGE)
EXP_LONG = CarFootnote(
"Enabling longitudinal control (alpha) will disable all EyeSight functionality, including AEB, LDW, and RAB.",
Column.LONGITUDINAL)
@dataclass
class SubaruCarInfo(CarInfo):
package: str = "EyeSight Driver Assistance"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.subaru_a]))
footnotes: List[Enum] = field(default_factory=lambda: [Footnote.GLOBAL])
def init_make(self, CP: car.CarParams):
self.car_parts.parts.extend([Tool.socket_8mm_deep, Tool.pry_tool])
if CP.experimentalLongitudinalAvailable:
self.footnotes.append(Footnote.EXP_LONG)
CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"),
CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b])),
CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b])),
CAR.IMPREZA: [
SubaruCarInfo("Subaru Impreza 2017-19"),
SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"),
SubaruCarInfo("Subaru XV 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"),
],
CAR.IMPREZA_2020: [
SubaruCarInfo("Subaru Impreza 2020-22"),
SubaruCarInfo("Subaru Crosstrek 2020-23"),
SubaruCarInfo("Subaru XV 2020-21"),
],
# TODO: is there an XV and Impreza too?
CAR.CROSSTREK_HYBRID: SubaruCarInfo("Subaru Crosstrek Hybrid 2020", car_parts=CarParts.common([CarHarness.subaru_b])),
CAR.FORESTER_HYBRID: SubaruCarInfo("Subaru Forester Hybrid 2020"),
CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"),
CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"),
CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"),
CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"),
CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"),
CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022-23", "All", car_parts=CarParts.common([CarHarness.subaru_c])),
CAR.OUTBACK_2023: SubaruCarInfo("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])),
CAR.ASCENT_2023: SubaruCarInfo("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])),
}
SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission],
),
# Some Eyesight modules fail on TESTER_PRESENT_REQUEST
# TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars
Request(
[SUBARU_VERSION_REQUEST],
[SUBARU_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera],
),
# Non-OBD requests
Request(
[StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission],
bus=0,
logging=True,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission],
bus=1,
logging=True,
obd_multiplexing=False,
),
],
)
DBC = {
CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None),
CAR.ASCENT_2023: dbc_dict('subaru_global_2017_generated', None),
CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None),
CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None),
CAR.CROSSTREK_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None),
CAR.OUTBACK_2023: dbc_dict('subaru_global_2017_generated', None),
CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
CAR.OUTBACK_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None),
}
LKAS_ANGLE = {CAR.FORESTER_2022, CAR.OUTBACK_2023, CAR.ASCENT_2023}
GLOBAL_GEN2 = {CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023, CAR.ASCENT_2023}
PREGLOBAL_CARS = {CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018}
HYBRID_CARS = {CAR.CROSSTREK_HYBRID, CAR.FORESTER_HYBRID}
# Cars that temporarily fault when steering angle rate is greater than some threshold.
# Appears to be all torque-based cars produced around 2019 - present
STEER_RATE_LIMITED = GLOBAL_GEN2 | {CAR.IMPREZA_2020, CAR.FORESTER}

0
selfdrive/car/tesla/__init__.py Normal file
View File

66
selfdrive/car/tesla/carcontroller.py Normal file
View File

@@ -0,0 +1,66 @@
from openpilot.common.numpy_fast import clip
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_std_steer_angle_limits
from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN
from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.frame = 0
self.apply_angle_last = 0
self.packer = CANPacker(dbc_name)
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
can_sends = []
# Temp disable steering on a hands_on_fault, and allow for user override
hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3
lkas_enabled = CC.latActive and not hands_on_fault
if self.frame % 2 == 0:
if lkas_enabled:
# Angular rate limit based on speed
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams)
# To not fault the EPS
apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20)
else:
apply_angle = CS.out.steeringAngleDeg
self.apply_angle_last = apply_angle
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, (self.frame // 2) % 16))
# Longitudinal control (in sync with stock message, about 40Hz)
if self.CP.openpilotLongitudinalControl:
target_accel = actuators.accel
target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
max_accel = 0 if target_accel < 0 else target_accel
min_accel = 0 if target_accel > 0 else target_accel
while len(CS.das_control_counters) > 0:
can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft()))
# Cancel on user steering override, since there is no steering torque blending
if hands_on_fault:
pcm_cancel_cmd = True
if self.frame % 10 == 0 and pcm_cancel_cmd:
# Spam every possible counter value, otherwise it might not be accepted
for counter in range(16):
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter))
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter))
# TODO: HUD control
new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = self.apply_angle_last
self.frame += 1
return new_actuators, can_sends

125
selfdrive/car/tesla/carstate.py Normal file
View File

@@ -0,0 +1,125 @@
import copy
from collections import deque
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
from openpilot.selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.button_states = {button.event_type: False for button in BUTTONS}
self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis'])
# Needed by carcontroller
self.msg_stw_actn_req = None
self.hands_on_level = 0
self.steer_warning = None
self.acc_state = 0
self.das_control_counters = deque(maxlen=32)
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
# Vehicle speed
ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = (ret.vEgo < 0.1)
# Gas pedal
ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0
ret.gasPressed = (ret.gas > 0)
# Brake pedal
ret.brake = 0
ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1)
# Steering wheel
self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"]
self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None)
steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None)
ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"]
ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate
ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"]
ret.steeringPressed = (self.hands_on_level > 0)
ret.steerFaultPermanent = steer_status == "EAC_FAULT"
ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON"))
# Cruise state
cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None)
acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"))
ret.cruiseState.enabled = acc_enabled
if speed_units == "KPH":
ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
elif speed_units == "MPH":
ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled)
ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special
# Gear
ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
# Buttons
buttonEvents = []
for button in BUTTONS:
state = (cp.vl[button.can_addr][button.can_msg] in button.values)
if self.button_states[button.event_type] != state:
event = car.CarState.ButtonEvent.new_message()
event.type = button.event_type
event.pressed = state
buttonEvents.append(event)
self.button_states[button.event_type] = state
ret.buttonEvents = buttonEvents
# Doors
ret.doorOpen = any((self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS)
# Blinkers
ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1)
ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1)
# Seatbelt
ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1)
# TODO: blindspot
# AEB
ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1)
# Messages needed by carcontroller
self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"])
self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"]
self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"])
return ret
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("ESP_B", 50),
("DI_torque1", 100),
("DI_torque2", 100),
("STW_ANGLHP_STAT", 100),
("EPAS_sysStatus", 25),
("DI_state", 10),
("STW_ACTN_RQ", 10),
("GTW_carState", 10),
("SDM1", 10),
("BrakeMessage", 50),
]
return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis)
@staticmethod
def get_cam_can_parser(CP):
messages = [
# sig_address, frequency
("DAS_control", 40),
]
return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis)

28
selfdrive/car/tesla/fingerprints.py Normal file
View File

@@ -0,0 +1,28 @@
# ruff: noqa: E501
from cereal import car
from openpilot.selfdrive.car.tesla.values import CAR
Ecu = car.CarParams.Ecu
FINGERPRINTS = {
CAR.AP1_MODELS: [{
1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4
}],
}
FW_VERSIONS = {
CAR.AP2_MODELS: {
(Ecu.adas, 0x649, None): [
b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11',
],
(Ecu.electricBrakeBooster, 0x64d, None): [
b'1037123-00-A',
],
(Ecu.fwdRadar, 0x671, None): [
b'\x01\x00W\x00\x00\x00\x07\x00\x00\x00\x00\x08\x01\x00\x00\x00\x07\xff\xfe',
],
(Ecu.eps, 0x730, None): [
b'\x10#\x01',
],
},
}

62
selfdrive/car/tesla/interface.py Executable file
View File

@@ -0,0 +1,62 @@
#!/usr/bin/env python3
from cereal import car
from panda import Panda
from openpilot.selfdrive.car.tesla.values import CANBUS, CAR
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "tesla"
# There is no safe way to do steer blending with user torque,
# so the steering behaves like autopilot. This is not
# how openpilot should be, hence dashcamOnly
ret.dashcamOnly = True
ret.steerControlType = car.CarParams.SteerControlType.angle
# Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command
ret.longitudinalTuning.kpBP = [0]
ret.longitudinalTuning.kpV = [0]
ret.longitudinalTuning.kiBP = [0]
ret.longitudinalTuning.kiV = [0]
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
ret.radarTimeStep = (1.0 / 8) # 8Hz
# Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus
# If so, we assume that it is connected to the longitudinal harness.
if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()):
ret.openpilotLongitudinalControl = True
ret.safetyConfigs = [
get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL),
get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN),
]
else:
ret.openpilotLongitudinalControl = False
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
ret.steerLimitTimer = 1.0
ret.steerActuatorDelay = 0.25
if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS):
ret.mass = 2100.
ret.wheelbase = 2.959
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 15.0
else:
raise ValueError(f"Unsupported car: {candidate}")
return ret
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.events = self.create_common_events(ret).to_msg()
return ret
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

89
selfdrive/car/tesla/radar_interface.py Executable file
View File

@@ -0,0 +1,89 @@
#!/usr/bin/env python3
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.tesla.values import DBC, CANBUS
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
RADAR_MSGS_A = list(range(0x310, 0x36E, 3))
RADAR_MSGS_B = list(range(0x311, 0x36F, 3))
NUM_POINTS = len(RADAR_MSGS_A)
def get_radar_can_parser(CP):
# Status messages
messages = [
('TeslaRadarSguInfo', 10),
]
# Radar tracks. There are also raw point clouds available,
# we don't use those.
for i in range(NUM_POINTS):
msg_id_a = RADAR_MSGS_A[i]
msg_id_b = RADAR_MSGS_B[i]
messages.extend([
(msg_id_a, 8),
(msg_id_b, 8),
])
return CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.rcp = get_radar_can_parser(CP)
self.updated_messages = set()
self.track_id = 0
self.trigger_msg = RADAR_MSGS_B[-1]
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
values = self.rcp.update_strings(can_strings)
self.updated_messages.update(values)
if self.trigger_msg not in self.updated_messages:
return None
ret = car.RadarData.new_message()
# Errors
errors = []
sgu_info = self.rcp.vl['TeslaRadarSguInfo']
if not self.rcp.can_valid:
errors.append('canError')
if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']:
errors.append('fault')
ret.errors = errors
# Radar tracks
for i in range(NUM_POINTS):
msg_a = self.rcp.vl[RADAR_MSGS_A[i]]
msg_b = self.rcp.vl[RADAR_MSGS_B[i]]
# Make sure msg A and B are together
if msg_a['Index'] != msg_b['Index2']:
continue
# Check if it's a valid track
if not msg_a['Tracked']:
if i in self.pts:
del self.pts[i]
continue
# New track!
if i not in self.pts:
self.pts[i] = car.RadarData.RadarPoint.new_message()
self.pts[i].trackId = self.track_id
self.track_id += 1
# Parse track data
self.pts[i].dRel = msg_a['LongDist']
self.pts[i].yRel = msg_a['LatDist']
self.pts[i].vRel = msg_a['LongSpeed']
self.pts[i].aRel = msg_a['LongAccel']
self.pts[i].yvRel = msg_b['LatSpeed']
self.pts[i].measured = bool(msg_a['Meas'])
ret.points = list(self.pts.values())
self.updated_messages.clear()
return ret

94
selfdrive/car/tesla/teslacan.py Normal file
View File

@@ -0,0 +1,94 @@
import crcmod
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.tesla.values import CANBUS, CarControllerParams
class TeslaCAN:
def __init__(self, packer, pt_packer):
self.packer = packer
self.pt_packer = pt_packer
self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
@staticmethod
def checksum(msg_id, dat):
# TODO: get message ID from name instead
ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF)
ret += sum(dat)
return ret & 0xFF
def create_steering_control(self, angle, enabled, counter):
values = {
"DAS_steeringAngleRequest": -angle,
"DAS_steeringHapticRequest": 0,
"DAS_steeringControlType": 1 if enabled else 0,
"DAS_steeringControlCounter": counter,
}
data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2]
values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)
def create_action_request(self, msg_stw_actn_req, cancel, bus, counter):
# We copy this whole message when spamming cancel
values = {s: msg_stw_actn_req[s] for s in [
"SpdCtrlLvr_Stat",
"VSL_Enbl_Rq",
"SpdCtrlLvrStat_Inv",
"DTR_Dist_Rq",
"TurnIndLvr_Stat",
"HiBmLvr_Stat",
"WprWashSw_Psd",
"WprWash_R_Sw_Posn_V2",
"StW_Lvr_Stat",
"StW_Cond_Flt",
"StW_Cond_Psd",
"HrnSw_Psd",
"StW_Sw00_Psd",
"StW_Sw01_Psd",
"StW_Sw02_Psd",
"StW_Sw03_Psd",
"StW_Sw04_Psd",
"StW_Sw05_Psd",
"StW_Sw06_Psd",
"StW_Sw07_Psd",
"StW_Sw08_Psd",
"StW_Sw09_Psd",
"StW_Sw10_Psd",
"StW_Sw11_Psd",
"StW_Sw12_Psd",
"StW_Sw13_Psd",
"StW_Sw14_Psd",
"StW_Sw15_Psd",
"WprSw6Posn",
"MC_STW_ACTN_RQ",
"CRC_STW_ACTN_RQ",
]}
if cancel:
values["SpdCtrlLvr_Stat"] = 1
values["MC_STW_ACTN_RQ"] = counter
data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2]
values["CRC_STW_ACTN_RQ"] = self.crc(data[:7])
return self.packer.make_can_msg("STW_ACTN_RQ", bus, values)
def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt):
messages = []
values = {
"DAS_setSpeed": speed * CV.MS_TO_KPH,
"DAS_accState": acc_state,
"DAS_aebEvent": 0,
"DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,
"DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX,
"DAS_accelMin": min_accel,
"DAS_accelMax": max_accel,
"DAS_controlCounter": cnt,
"DAS_controlChecksum": 0,
}
for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]:
data = packer.make_can_msg("DAS_control", bus, values)[2]
values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
messages.append(packer.make_can_msg("DAS_control", bus, values))
return messages

91
selfdrive/car/tesla/values.py Normal file
View File

@@ -0,0 +1,91 @@
from collections import namedtuple
from enum import StrEnum
from typing import Dict, List, Union
from cereal import car
from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarInfo
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
class CAR(StrEnum):
AP1_MODELS = 'TESLA AP1 MODEL S'
AP2_MODELS = 'TESLA AP2 MODEL S'
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"),
CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"),
}
DBC = {
CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
}
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
whitelist_ecus=[Ecu.eps],
rx_offset=0x08,
bus=0,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar],
rx_offset=0x10,
bus=0,
),
]
)
class CANBUS:
# Lateral harness
chassis = 0
radar = 1
autopilot_chassis = 2
# Longitudinal harness
powertrain = 4
private = 5
autopilot_powertrain = 6
GEAR_MAP = {
"DI_GEAR_INVALID": car.CarState.GearShifter.unknown,
"DI_GEAR_P": car.CarState.GearShifter.park,
"DI_GEAR_R": car.CarState.GearShifter.reverse,
"DI_GEAR_N": car.CarState.GearShifter.neutral,
"DI_GEAR_D": car.CarState.GearShifter.drive,
"DI_GEAR_SNA": car.CarState.GearShifter.unknown,
}
DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"]
# Make sure the message and addr is also in the CAN parser!
BUTTONS = [
Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]),
Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]),
Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]),
Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]),
Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]),
Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]),
]
class CarControllerParams:
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 1.6, .3])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 7.0, 0.8])
JERK_LIMIT_MAX = 8
JERK_LIMIT_MIN = -8
ACCEL_TO_SPEED_MULTIPLIER = 3
def __init__(self, CP):
pass

0
selfdrive/car/tests/__init__.py Normal file
View File

152
selfdrive/car/tests/test_car_interfaces.py Executable file
View File

@@ -0,0 +1,152 @@
#!/usr/bin/env python3
import os
import math
import unittest
import hypothesis.strategies as st
from hypothesis import Phase, given, settings
import importlib
from parameterized import parameterized
from cereal import car, messaging
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car import gen_empty_fingerprint
from openpilot.selfdrive.car.car_helpers import interfaces
from openpilot.selfdrive.car.fingerprints import all_known_cars
from openpilot.selfdrive.car.fw_versions import FW_VERSIONS
from openpilot.selfdrive.car.interfaces import get_interface_attr
from openpilot.selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator
ALL_ECUS = list({ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()})
MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '20'))
def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
# Fuzzy CAN fingerprints and FW versions to test more states of the CarInterface
fingerprint_strategy = st.fixed_dictionaries({key: st.dictionaries(st.integers(min_value=0, max_value=0x800),
st.integers(min_value=0, max_value=64)) for key in
gen_empty_fingerprint()})
# only pick from possible ecus to reduce search space
car_fw_strategy = st.lists(st.sampled_from(ALL_ECUS))
params_strategy = st.fixed_dictionaries({
'fingerprints': fingerprint_strategy,
'car_fw': car_fw_strategy,
'experimental_long': st.booleans(),
})
params: dict = draw(params_strategy)
params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']]
return params
class TestCarInterfaces(unittest.TestCase):
@classmethod
def setUpClass(cls):
os.environ['NO_RADAR_SLEEP'] = '1'
# FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause
# many generated examples to overrun when max_examples > ~20, don't use it
@parameterized.expand([(car,) for car in sorted(all_known_cars())])
@settings(max_examples=MAX_EXAMPLES, deadline=None,
phases=(Phase.reuse, Phase.generate, Phase.shrink))
@given(data=st.data())
def test_car_interfaces(self, car_name, data):
CarInterface, CarController, CarState = interfaces[car_name]
args = get_fuzzy_car_interface_args(data.draw)
car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
experimental_long=args['experimental_long'], docs=False)
car_interface = CarInterface(car_params, CarController, CarState)
assert car_params
assert car_interface
self.assertGreater(car_params.mass, 1)
self.assertGreater(car_params.wheelbase, 0)
# centerToFront is center of gravity to front wheels, assert a reasonable range
self.assertTrue(car_params.wheelbase * 0.3 < car_params.centerToFront < car_params.wheelbase * 0.7)
self.assertGreater(car_params.maxLateralAccel, 0)
# Longitudinal sanity checks
self.assertEqual(len(car_params.longitudinalTuning.kpV), len(car_params.longitudinalTuning.kpBP))
self.assertEqual(len(car_params.longitudinalTuning.kiV), len(car_params.longitudinalTuning.kiBP))
self.assertEqual(len(car_params.longitudinalTuning.deadzoneV), len(car_params.longitudinalTuning.deadzoneBP))
# Lateral sanity checks
if car_params.steerControlType != car.CarParams.SteerControlType.angle:
tune = car_params.lateralTuning
if tune.which() == 'pid':
self.assertTrue(not math.isnan(tune.pid.kf) and tune.pid.kf > 0)
self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP))
self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP))
elif tune.which() == 'torque':
self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0)
self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0)
cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True)
# Run car interface
now_nanos = 0
CC = car.CarControl.new_message(**cc_msg)
for _ in range(10):
car_interface.update(CC, [])
car_interface.apply(CC, now_nanos)
car_interface.apply(CC, now_nanos)
now_nanos += DT_CTRL * 1e9 # 10 ms
CC = car.CarControl.new_message(**cc_msg)
CC.enabled = True
for _ in range(10):
car_interface.update(CC, [])
car_interface.apply(CC, now_nanos)
car_interface.apply(CC, now_nanos)
now_nanos += DT_CTRL * 1e9 # 10ms
# Test radar interface
RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface
radar_interface = RadarInterface(car_params)
assert radar_interface
# Run radar interface once
radar_interface.update([])
if not car_params.radarUnavailable and radar_interface.rcp is not None and \
hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'):
radar_interface._update([radar_interface.trigger_msg])
# Test radar fault
if not car_params.radarUnavailable and radar_interface.rcp is not None:
cans = [messaging.new_message('can', 1).to_bytes() for _ in range(5)]
rr = radar_interface.update(cans)
self.assertTrue(rr is None or len(rr.errors) > 0)
def test_interface_attrs(self):
"""Asserts basic behavior of interface attribute getter"""
num_brands = len(get_interface_attr('CAR'))
self.assertGreaterEqual(num_brands, 13)
# Should return value for all brands when not combining, even if attribute doesn't exist
ret = get_interface_attr('FAKE_ATTR')
self.assertEqual(len(ret), num_brands)
# Make sure we can combine dicts
ret = get_interface_attr('DBC', combine_brands=True)
self.assertGreaterEqual(len(ret), 160)
# We don't support combining non-dicts
ret = get_interface_attr('CAR', combine_brands=True)
self.assertEqual(len(ret), 0)
# If brand has None value, it shouldn't return when ignore_none=True is specified
none_brands = {b for b, v in get_interface_attr('FINGERPRINTS').items() if v is None}
self.assertGreaterEqual(len(none_brands), 1)
ret = get_interface_attr('FINGERPRINTS', ignore_none=True)
none_brands_in_ret = none_brands.intersection(ret)
self.assertEqual(len(none_brands_in_ret), 0, f'Brands with None values in ignore_none=True result: {none_brands_in_ret}')
if __name__ == "__main__":
unittest.main()

76
selfdrive/car/torque_data/override.toml Normal file
View File

@@ -0,0 +1,76 @@
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
### angle control
# Nissan appears to have torque
"NISSAN X-TRAIL 2017" = [nan, 1.5, nan]
"NISSAN ALTIMA 2020" = [nan, 1.5, nan]
"NISSAN LEAF 2018 Instrument Cluster" = [nan, 1.5, nan]
"NISSAN LEAF 2018" = [nan, 1.5, nan]
"NISSAN ROGUE 2019" = [nan, 1.5, nan]
# New subarus angle based controllers
"SUBARU FORESTER 2022" = [nan, 3.0, nan]
"SUBARU OUTBACK 7TH GEN" = [nan, 3.0, nan]
"SUBARU ASCENT 2023" = [nan, 3.0, nan]
# Toyota LTA also has torque
"TOYOTA RAV4 2023" = [nan, 3.0, nan]
# Tesla has high torque
"TESLA AP1 MODEL S" = [nan, 2.5, nan]
"TESLA AP2 MODEL S" = [nan, 2.5, nan]
# Guess
"FORD BRONCO SPORT 1ST GEN" = [nan, 1.5, nan]
"FORD ESCAPE 4TH GEN" = [nan, 1.5, nan]
"FORD EXPLORER 6TH GEN" = [nan, 1.5, nan]
"FORD F-150 14TH GEN" = [nan, 1.5, nan]
"FORD FOCUS 4TH GEN" = [nan, 1.5, nan]
"FORD MAVERICK 1ST GEN" = [nan, 1.5, nan]
"FORD F-150 LIGHTNING 1ST GEN" = [nan, 1.5, nan]
"FORD MUSTANG MACH-E 1ST GEN" = [nan, 1.5, nan]
###
# No steering wheel
"COMMA BODY" = [nan, 1000, nan]
# Totally new cars
"RAM 1500 5TH GEN" = [2.0, 2.0, 0.05]
"RAM HD 5TH GEN" = [1.4, 1.4, 0.05]
"SUBARU OUTBACK 6TH GEN" = [2.0, 2.0, 0.2]
"CADILLAC ESCALADE 2017" = [1.899999976158142, 1.842270016670227, 0.1120000034570694]
"CADILLAC ESCALADE ESV 2019" = [1.15, 1.3, 0.2]
"CHEVROLET BOLT EUV 2022" = [2.0, 2.0, 0.05]
"CHEVROLET SILVERADO 1500 2020" = [1.9, 1.9, 0.112]
"CHEVROLET TRAILBLAZER 2021" = [1.33, 1.9, 0.16]
"CHEVROLET EQUINOX 2019" = [2.0, 2.0, 0.05]
"VOLKSWAGEN PASSAT NMS" = [2.5, 2.5, 0.1]
"VOLKSWAGEN SHARAN 2ND GEN" = [2.5, 2.5, 0.1]
"HYUNDAI SANTA CRUZ 1ST GEN" = [2.7, 2.7, 0.1]
"KIA SPORTAGE 5TH GEN" = [2.7, 2.7, 0.1]
"KIA SPORTAGE HYBRID 5TH GEN" = [2.5, 2.5, 0.1]
"GENESIS GV70 1ST GEN" = [2.42, 2.42, 0.1]
"KIA SORENTO PLUG-IN HYBRID 4TH GEN" = [2.5, 2.5, 0.1]
"GENESIS GV60 ELECTRIC 1ST GEN" = [2.5, 2.5, 0.1]
"KIA SORENTO 4TH GEN" = [2.5, 2.5, 0.1]
"KIA NIRO HYBRID 2ND GEN" = [2.42, 2.5, 0.12]
"KIA NIRO EV 2ND GEN" = [2.05, 2.5, 0.14]
"GENESIS GV80 2023" = [2.5, 2.5, 0.1]
"KIA CARNIVAL 4TH GEN" = [1.75, 1.75, 0.15]
"GMC ACADIA DENALI 2018" = [1.6, 1.6, 0.2]
"LEXUS IS 2023" = [2.0, 2.0, 0.1]
"KIA SORENTO HYBRID 4TH GEN" = [2.5, 2.5, 0.1]
"HYUNDAI KONA ELECTRIC 2ND GEN" = [2.5, 2.5, 0.1]
"HYUNDAI IONIQ 6 2023" = [2.5, 2.5, 0.1]
"HYUNDAI AZERA 6TH GEN" = [1.8, 1.8, 0.1]
"HYUNDAI AZERA HYBRID 6TH GEN" = [1.8, 1.8, 0.1]
"KIA K8 HYBRID 1ST GEN" = [2.5, 2.5, 0.1]
"HYUNDAI CUSTIN 1ST GEN" = [2.5, 2.5, 0.1]
"LEXUS GS F 2016" = [2.5, 2.5, 0.08]
"HYUNDAI STARIA 4TH GEN" = [1.8, 2.0, 0.15]
# Dashcam or fallback configured as ideal car
"mock" = [10.0, 10, 0.0]
# Manually checked
"HONDA CIVIC 2022" = [2.5, 1.2, 0.15]
"HONDA HR-V 2023" = [2.5, 1.2, 0.2]

88
selfdrive/car/torque_data/params.toml Normal file
View File

@@ -0,0 +1,88 @@
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"ACURA ILX 2016" = [1.524988973896102, 0.519011053086259, 0.34236219253028]
"ACURA RDX 2018" = [0.9987728568686902, 0.5323765166196301, 0.303218805715844]
"ACURA RDX 2020" = [1.4314459806646749, 0.33874701282109954, 0.18048847083897598]
"AUDI A3 3RD GEN" = [1.5122414863077502, 1.7443517531719404, 0.15194151892450905]
"AUDI Q3 2ND GEN" = [1.4439223359448605, 1.2254955789112076, 0.1413798895978097]
"CHEVROLET VOLT PREMIER 2017" = [1.5961527626411784, 1.8422651988094612, 0.1572393918005158]
"CHRYSLER PACIFICA 2018" = [2.07140, 1.3366521181047952, 0.13776367250652022]
"CHRYSLER PACIFICA 2020" = [1.86206, 1.509076559398423, 0.14328246159386085]
"CHRYSLER PACIFICA HYBRID 2017" = [1.79422, 1.06831764583744, 0.116237]
"CHRYSLER PACIFICA HYBRID 2018" = [2.08887, 1.2943025830995154, 0.114818]
"CHRYSLER PACIFICA HYBRID 2019" = [1.90120, 1.1958788168371808, 0.131520]
"GENESIS G70 2018" = [3.8520195946707947, 2.354697063349854, 0.06830285485626221]
"HONDA ACCORD 2018" = [1.7135052593468778, 0.3461280068322071, 0.21579936052863807]
"HONDA ACCORD HYBRID 2018" = [1.6651615004829625, 0.30322180951193245, 0.2083000440586149]
"HONDA CIVIC (BOSCH) 2019" = [1.691708637466905, 0.40132900729454185, 0.25460295304024094]
"HONDA CIVIC 2016" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544]
"HONDA CR-V 2016" = [0.7667141440182675, 0.5927571534745969, 0.40909087636157127]
"HONDA CR-V 2017" = [2.01323205142022, 0.2700612209345081, 0.2238412881331528]
"HONDA CR-V HYBRID 2019" = [2.072034634644233, 0.7152085160516978, 0.20237105008376083]
"HONDA FIT 2018" = [1.5719981427109775, 0.5712761407108976, 0.110773383324281]
"HONDA HRV 2019" = [2.0661212805710205, 0.7521343418694775, 0.17760375789242094]
"HONDA INSIGHT 2019" = [1.5201671214069354, 0.5660229120683284, 0.25808042580281876]
"HONDA ODYSSEY 2018" = [1.8774809275211801, 0.8394431662987996, 0.2096978613792822]
"HONDA PILOT 2017" = [1.7262026201812795, 0.9470005614967523, 0.21351430733218763]
"HONDA RIDGELINE 2017" = [1.4146525028237624, 0.7356572861629564, 0.23307177552211328]
"HYUNDAI ELANTRA 2021" = [3.169, 2.1259108157250735, 0.0819]
"HYUNDAI GENESIS 2015-2016" = [2.7807965280270794, 2.325, 0.0984484465421171]
"HYUNDAI IONIQ 5 2022" = [3.172929, 2.713050, 0.096019]
"HYUNDAI IONIQ ELECTRIC LIMITED 2019" = [1.7662975472852054, 1.613755614526594, 0.17087579756306276]
"HYUNDAI IONIQ PHEV 2020" = [3.2928700076638537, 2.1193482926455656, 0.12463700961468778]
"HYUNDAI IONIQ PLUG-IN HYBRID 2019" = [2.970807902012267, 1.6312321830002083, 0.1088964990357482]
"HYUNDAI KONA ELECTRIC 2019" = [3.078814714619148, 2.307336938253934, 0.12359762054065548]
"HYUNDAI PALISADE 2020" = [2.544642494803999, 1.8721703683337008, 0.1301424599248651]
"HYUNDAI SANTA FE 2019" = [3.0787027729757632, 2.6173437483495565, 0.1207019341823945]
"HYUNDAI SANTA FE HYBRID 2022" = [3.501877602644835, 2.729064118456137, 0.10384068104538963]
"HYUNDAI SANTA FE PlUG-IN HYBRID 2022" = [1.6953050513611045, 1.5837614296206861, 0.12672855941458458]
"HYUNDAI SONATA 2019" = [2.2200457811703953, 1.2967330275895228, 0.14039920986586393]
"HYUNDAI SONATA 2020" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593]
"HYUNDAI SONATA HYBRID 2021" = [2.8990264092395734, 2.061410192222139, 0.0899805488717382]
"HYUNDAI TUCSON HYBRID 4TH GEN" = [2.960174, 2.860284, 0.108745]
"JEEP GRAND CHEROKEE 2019" = [2.30972, 1.289689569171081, 0.117048]
"JEEP GRAND CHEROKEE V6 2018" = [2.27116, 1.4057367824262523, 0.11725947414922003]
"KIA EV6 2022" = [3.2, 2.093457, 0.05]
"KIA K5 2021" = [2.405339728085138, 1.460032270828705, 0.11650989850813716]
"KIA NIRO EV 2020" = [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
"KIA SORENTO GT LINE 2018" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
"KIA STINGER GT2 2018" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
"LEXUS ES 2019" = [2.0357564999999997, 1.999082295195227, 0.101533]
"LEXUS NX 2018" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927]
"LEXUS NX 2020" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
"LEXUS RX 2016" = [1.5876816543130423, 1.0427699298523752, 0.21334066732397142]
"LEXUS RX 2020" = [1.5375561442049257, 1.343166476215164, 0.1931062001527557]
"LEXUS RX HYBRID 2017" = [1.6984261557042386, 1.3211501880159107, 0.1820354534928893]
"MAZDA CX-9 2021" = [1.7601682915983443, 1.0889677335154337, 0.17713792194297195]
"SKODA SUPERB 3RD GEN" = [1.166437404652981, 1.1686163012668165, 0.12194533036948708]
"SUBARU FORESTER 2019" = [3.6617001649776793, 2.342197172531713, 0.11075960785398745]
"SUBARU IMPREZA LIMITED 2019" = [1.0670704910352047, 0.8234374840709592, 0.20986563268614938]
"SUBARU IMPREZA SPORT 2020" = [2.6068223389108303, 2.134872342760203, 0.15261513193561627]
"TOYOTA AVALON 2016" = [2.5185770183845646, 1.7153346784214922, 0.10603968787111022]
"TOYOTA AVALON 2019" = [1.7036141952825095, 1.239619084240008, 0.08459830394899492]
"TOYOTA AVALON 2022" = [2.3154403649717357, 2.7777922854327124, 0.11453999639164605]
"TOYOTA C-HR 2018" = [1.5591084333664578, 1.271271459066948, 0.20259087058453193]
"TOYOTA C-HR 2021" = [1.7678810166088303, 1.3742176337919942, 0.2319674583741509]
"TOYOTA CAMRY 2018" = [2.0568162685952505, 1.7576185169559122, 0.108878753]
"TOYOTA CAMRY 2021" = [2.3548324999999997, 2.368900128946771, 0.118436]
"TOYOTA COROLLA 2017" = [3.117154369115421, 1.8438132575043773, 0.12289685869250652]
"TOYOTA COROLLA TSS2 2019" = [1.991132339206426, 1.868866242720403, 0.19570063298031432]
"TOYOTA HIGHLANDER 2017" = [1.8696367437248915, 1.626293990451463, 0.17485372210240796]
"TOYOTA HIGHLANDER 2020" = [1.9617570834136164, 1.8611643317268927, 0.14519673256119725]
"TOYOTA HIGHLANDER HYBRID 2018" = [1.752033, 1.6433903296845025, 0.144600]
"TOYOTA MIRAI 2021" = [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
"TOYOTA PRIUS 2017" = [1.60, 1.5023147650693636, 0.151515]
"TOYOTA PRIUS TSS2 2021" = [1.972600, 1.9104337425537743, 0.170968]
"TOYOTA RAV4 2017" = [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
"TOYOTA RAV4 2019" = [2.279239424615458, 2.087101966779332, 0.13682208413446817]
"TOYOTA RAV4 2019 8965" = [2.3080951748210854, 2.1189367835820603, 0.12942102328134028]
"TOYOTA RAV4 2019 x02" = [2.762293266024922, 2.243615865975329, 0.11113568178327986]
"TOYOTA RAV4 HYBRID 2017" = [1.9796257271652042, 1.7503987331707576, 0.14628860048885406]
"TOYOTA RAV4 2022" = [2.241883248393209, 1.9304407208090029, 0.112174]
"TOYOTA RAV4 2022 x02" = [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
"TOYOTA SIENNA 2018" = [1.689726, 1.3208264576110418, 0.140456]
"VOLKSWAGEN ARTEON 1ST GEN" = [1.45136518053819, 1.3639364049316804, 0.23806361745695032]
"VOLKSWAGEN ATLAS 1ST GEN" = [1.4677006726964945, 1.6733266634075656, 0.12959584092073367]
"VOLKSWAGEN GOLF 7TH GEN" = [1.3750394140491293, 1.5814743077200641, 0.2018321939386586]
"VOLKSWAGEN JETTA 7TH GEN" = [1.2271623034089392, 1.216955117387, 0.19437384688370712]
"VOLKSWAGEN PASSAT 8TH GEN" = [1.3432120736752917, 1.7087275587362314, 0.19444383787326647]
"VOLKSWAGEN TIGUAN 2ND GEN" = [0.9711965500094828, 1.0001565939459098, 0.1465626137072916]

84
selfdrive/car/torque_data/substitute.toml Normal file
View File

@@ -0,0 +1,84 @@
legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"MAZDA 3" = "MAZDA CX-9 2021"
"MAZDA 6" = "MAZDA CX-9 2021"
"MAZDA CX-5" = "MAZDA CX-9 2021"
"MAZDA CX-5 2022" = "MAZDA CX-9 2021"
"MAZDA CX-9" = "MAZDA CX-9 2021"
"TOYOTA ALPHARD 2020" = "TOYOTA SIENNA 2018"
"TOYOTA PRIUS v 2017" = "TOYOTA PRIUS 2017"
"LEXUS IS 2018" = "LEXUS NX 2018"
"LEXUS CT HYBRID 2018" = "LEXUS NX 2018"
"LEXUS ES 2018" = "TOYOTA CAMRY 2018"
"LEXUS ES HYBRID 2018" = "TOYOTA CAMRY 2018"
"LEXUS RC 2020" = "LEXUS NX 2020"
"KIA OPTIMA 4TH GEN" = "HYUNDAI SONATA 2020"
"KIA OPTIMA 4TH GEN FACELIFT" = "HYUNDAI SONATA 2020"
"KIA OPTIMA HYBRID 2017 & SPORTS 2019" = "HYUNDAI SONATA 2020"
"KIA OPTIMA HYBRID 4TH GEN FACELIFT" = "HYUNDAI SONATA 2020"
"KIA FORTE E 2018 & GT 2021" = "HYUNDAI SONATA 2020"
"KIA CEED INTRO ED 2019" = "HYUNDAI SONATA 2020"
"KIA SELTOS 2021" = "HYUNDAI SONATA 2020"
"KIA NIRO HYBRID 2019" = "KIA NIRO EV 2020"
"KIA NIRO PLUG-IN HYBRID 2022" = "KIA NIRO EV 2020"
"KIA NIRO HYBRID 2021" = "KIA NIRO EV 2020"
"HYUNDAI VELOSTER 2019" = "HYUNDAI SONATA 2019"
"HYUNDAI KONA 2020" = "HYUNDAI KONA ELECTRIC 2019"
"HYUNDAI KONA HYBRID 2020" = "HYUNDAI KONA ELECTRIC 2019"
"HYUNDAI KONA ELECTRIC 2022" = "HYUNDAI KONA ELECTRIC 2019"
"HYUNDAI IONIQ HYBRID 2017-2019" = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
"HYUNDAI IONIQ HYBRID 2020-2022" = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
"HYUNDAI IONIQ ELECTRIC 2020" = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
"HYUNDAI ELANTRA 2017" = "HYUNDAI SONATA 2019"
"HYUNDAI I30 N LINE 2019 & GT 2018 DCT" = "HYUNDAI SONATA 2019"
"HYUNDAI ELANTRA HYBRID 2021" = "HYUNDAI SONATA 2020"
"HYUNDAI TUCSON 2019" = "HYUNDAI SANTA FE 2019"
"HYUNDAI TUCSON 4TH GEN" = "HYUNDAI TUCSON HYBRID 4TH GEN"
"HYUNDAI SANTA FE 2022" = "HYUNDAI SANTA FE HYBRID 2022"
"KIA K5 HYBRID 2020" = "KIA K5 2021"
"KIA STINGER 2022" = "KIA STINGER GT2 2018"
"GENESIS G90 2017" = "GENESIS G70 2018"
"GENESIS G80 2017" = "GENESIS G70 2018"
"GENESIS G70 2020" = "HYUNDAI SONATA 2020"
"HONDA FREED 2020" = "HONDA ODYSSEY 2018"
"HONDA CR-V EU 2016" = "HONDA CR-V 2016"
"HONDA CIVIC SEDAN 1.6 DIESEL 2019" = "HONDA CIVIC (BOSCH) 2019"
"HONDA E 2020" = "HONDA CIVIC (BOSCH) 2019"
"HONDA ODYSSEY CHN 2019" = "HONDA ODYSSEY 2018"
"BUICK LACROSSE 2017" = "CHEVROLET VOLT PREMIER 2017"
"BUICK REGAL ESSENCE 2018" = "CHEVROLET VOLT PREMIER 2017"
"CADILLAC ESCALADE ESV 2016" = "CHEVROLET VOLT PREMIER 2017"
"CADILLAC ATS Premium Performance 2018" = "CHEVROLET VOLT PREMIER 2017"
"CHEVROLET MALIBU PREMIER 2017" = "CHEVROLET VOLT PREMIER 2017"
"HOLDEN ASTRA RS-V BK 2017" = "CHEVROLET VOLT PREMIER 2017"
"SKODA FABIA 4TH GEN" = "VOLKSWAGEN GOLF 7TH GEN"
"SKODA OCTAVIA 3RD GEN" = "SKODA SUPERB 3RD GEN"
"SKODA SCALA 1ST GEN" = "SKODA SUPERB 3RD GEN"
"SKODA KODIAQ 1ST GEN" = "SKODA SUPERB 3RD GEN"
"SKODA KAROQ 1ST GEN" = "SKODA SUPERB 3RD GEN"
"SKODA KAMIQ 1ST GEN" = "SKODA SUPERB 3RD GEN"
"VOLKSWAGEN CRAFTER 2ND GEN" = "VOLKSWAGEN TIGUAN 2ND GEN"
"VOLKSWAGEN T-ROC 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN"
"VOLKSWAGEN T-CROSS 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN"
"VOLKSWAGEN TOURAN 2ND GEN" = "VOLKSWAGEN TIGUAN 2ND GEN"
"VOLKSWAGEN TRANSPORTER T6.1" = "VOLKSWAGEN TIGUAN 2ND GEN"
"AUDI Q2 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN"
"VOLKSWAGEN TAOS 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN"
"VOLKSWAGEN POLO 6TH GEN" = "VOLKSWAGEN GOLF 7TH GEN"
"SEAT LEON 3RD GEN" = "VOLKSWAGEN GOLF 7TH GEN"
"SEAT ATECA 1ST GEN" = "VOLKSWAGEN GOLF 7TH GEN"
"SUBARU CROSSTREK HYBRID 2020" = "SUBARU IMPREZA SPORT 2020"
"SUBARU FORESTER HYBRID 2020" = "SUBARU IMPREZA SPORT 2020"
"SUBARU LEGACY 7TH GEN" = "SUBARU OUTBACK 6TH GEN"
# Old subarus don't have much data guessing it's like low torque impreza"
"SUBARU OUTBACK 2018 - 2019" = "SUBARU IMPREZA LIMITED 2019"
"SUBARU OUTBACK 2015 - 2017" = "SUBARU IMPREZA LIMITED 2019"
"SUBARU FORESTER 2017 - 2018" = "SUBARU IMPREZA LIMITED 2019"
"SUBARU LEGACY 2015 - 2018" = "SUBARU IMPREZA LIMITED 2019"
"SUBARU ASCENT LIMITED 2019" = "SUBARU FORESTER 2019"

Some files were not shown because too many files have changed in this diff Show More