move selfdrive/car to opendbc (#32630)
* move most of /car * move some car tests move some car tests * fix selfdrive/car/tests * fix selfdrive/controls tests * fix the rest of the selfdrive tests * bump opendbc * fix all tests * few more non-test references * remove opcar and move docs to car fix these debugging scripts fix docs * bump opendbc and panda forgot pandapull/33322/head
Shane Smiskol
8 months ago
committed by
GitHub
parent
bf68eeb596
commit
e735a7f379
192 changed files with 141 additions and 22938 deletions
@ -1 +1 @@ |
|||||||
Subproject commit f4d077b832d46ac149c2b07dc37d777dc21237ea |
Subproject commit 9a53f0808029b34c997d2999e2d7df4280a3ce32 |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit 1cbcc13c35429b06e9d36c55b9ab46df73a8b51a |
Subproject commit e7a04b54b039307c33f542efd21ce936c45756be |
||||||
@ -1,323 +0,0 @@ |
|||||||
# functions common among cars |
|
||||||
import logging |
|
||||||
from collections import namedtuple |
|
||||||
from dataclasses import dataclass |
|
||||||
from enum import IntFlag, ReprEnum, EnumType |
|
||||||
from dataclasses import replace |
|
||||||
|
|
||||||
from panda.python.uds import SERVICE_TYPE |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.can_definitions import CanData |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarDocs |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip, interp |
|
||||||
|
|
||||||
# set up logging |
|
||||||
carlog = logging.getLogger('carlog') |
|
||||||
carlog.setLevel(logging.INFO) |
|
||||||
carlog.propagate = False |
|
||||||
|
|
||||||
DT_CTRL = 0.01 # car state and control loop timestep (s) |
|
||||||
|
|
||||||
# kg of standard extra cargo to count for drive, gas, etc... |
|
||||||
STD_CARGO_KG = 136. |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
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, structs.CarState.ButtonEvent.Type], |
|
||||||
unpressed_btn: int = 0) -> list[structs.CarState.ButtonEvent]: |
|
||||||
events: list[structs.CarState.ButtonEvent] = [] |
|
||||||
|
|
||||||
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(structs.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 |
|
||||||
|
|
||||||
|
|
||||||
DbcDict = dict[str, str] |
|
||||||
|
|
||||||
|
|
||||||
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> DbcDict: |
|
||||||
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 apply_center_deadzone(error, deadzone): |
|
||||||
if (error > - deadzone) and (error < deadzone): |
|
||||||
error = 0. |
|
||||||
return error |
|
||||||
|
|
||||||
|
|
||||||
def rate_limit(new_value, last_value, dw_step, up_step): |
|
||||||
return clip(new_value, last_value + dw_step, last_value + up_step) |
|
||||||
|
|
||||||
|
|
||||||
def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, |
|
||||||
torque_params: structs.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float: |
|
||||||
friction_interp = interp( |
|
||||||
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), |
|
||||||
[-friction_threshold, friction_threshold], |
|
||||||
[-torque_params.friction, torque_params.friction] |
|
||||||
) |
|
||||||
friction = float(friction_interp) if friction_compensation else 0.0 |
|
||||||
return friction |
|
||||||
|
|
||||||
|
|
||||||
def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False): |
|
||||||
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT] |
|
||||||
if subaddr is not None: |
|
||||||
dat.insert(0, subaddr) |
|
||||||
dat.append(0x80 if suppress_response else 0x0) # sub-function |
|
||||||
|
|
||||||
dat.extend([0x0] * (8 - len(dat))) |
|
||||||
return CanData(addr, bytes(dat), bus) |
|
||||||
|
|
||||||
|
|
||||||
def get_safety_config(safety_model: structs.CarParams.SafetyModel, safety_param: int = None) -> structs.CarParams.SafetyConfig: |
|
||||||
ret = structs.CarParams.SafetyConfig() |
|
||||||
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: dict[int, dict[int, int]] | None) -> 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 |
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True, kw_only=True) |
|
||||||
class CarSpecs: |
|
||||||
mass: float # kg, curb weight |
|
||||||
wheelbase: float # meters |
|
||||||
steerRatio: float |
|
||||||
centerToFrontRatio: float = 0.5 |
|
||||||
minSteerSpeed: float = 0.0 # m/s |
|
||||||
minEnableSpeed: float = -1.0 # m/s |
|
||||||
tireStiffnessFactor: float = 1.0 |
|
||||||
|
|
||||||
def override(self, **kwargs): |
|
||||||
return replace(self, **kwargs) |
|
||||||
|
|
||||||
|
|
||||||
class Freezable: |
|
||||||
_frozen: bool = False |
|
||||||
|
|
||||||
def freeze(self): |
|
||||||
if not self._frozen: |
|
||||||
self._frozen = True |
|
||||||
|
|
||||||
def __setattr__(self, *args, **kwargs): |
|
||||||
if self._frozen: |
|
||||||
raise Exception("cannot modify frozen object") |
|
||||||
super().__setattr__(*args, **kwargs) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass(order=True) |
|
||||||
class PlatformConfig(Freezable): |
|
||||||
car_docs: list[CarDocs] |
|
||||||
specs: CarSpecs |
|
||||||
|
|
||||||
dbc_dict: DbcDict |
|
||||||
|
|
||||||
flags: int = 0 |
|
||||||
|
|
||||||
platform_str: str | None = None |
|
||||||
|
|
||||||
def __hash__(self) -> int: |
|
||||||
return hash(self.platform_str) |
|
||||||
|
|
||||||
def override(self, **kwargs): |
|
||||||
return replace(self, **kwargs) |
|
||||||
|
|
||||||
def init(self): |
|
||||||
pass |
|
||||||
|
|
||||||
def __post_init__(self): |
|
||||||
self.init() |
|
||||||
|
|
||||||
|
|
||||||
class PlatformsType(EnumType): |
|
||||||
def __new__(metacls, cls, bases, classdict, *, boundary=None, _simple=False, **kwds): |
|
||||||
for key in classdict._member_names.keys(): |
|
||||||
cfg: PlatformConfig = classdict[key] |
|
||||||
cfg.platform_str = key |
|
||||||
cfg.freeze() |
|
||||||
return super().__new__(metacls, cls, bases, classdict, boundary=boundary, _simple=_simple, **kwds) |
|
||||||
|
|
||||||
|
|
||||||
class Platforms(str, ReprEnum, metaclass=PlatformsType): |
|
||||||
config: PlatformConfig |
|
||||||
|
|
||||||
def __new__(cls, platform_config: PlatformConfig): |
|
||||||
member = str.__new__(cls, platform_config.platform_str) |
|
||||||
member.config = platform_config |
|
||||||
member._value_ = platform_config.platform_str |
|
||||||
return member |
|
||||||
|
|
||||||
def __repr__(self): |
|
||||||
return f"<{self.__class__.__name__}.{self.name}>" |
|
||||||
|
|
||||||
@classmethod |
|
||||||
def create_dbc_map(cls) -> dict[str, DbcDict]: |
|
||||||
return {p: p.config.dbc_dict for p in cls} |
|
||||||
|
|
||||||
@classmethod |
|
||||||
def with_flags(cls, flags: IntFlag) -> set['Platforms']: |
|
||||||
return {p for p in cls if p.config.flags & flags} |
|
||||||
@ -1,7 +0,0 @@ |
|||||||
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) |
|
||||||
@ -1,142 +0,0 @@ |
|||||||
import copy |
|
||||||
import numpy as np |
|
||||||
from numbers import Number |
|
||||||
|
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import DT_CTRL |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip, interp |
|
||||||
from openpilot.selfdrive.car.body import bodycan |
|
||||||
from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
|
|
||||||
|
|
||||||
class PIController: |
|
||||||
def __init__(self, k_p, k_i, pos_limit=1e308, neg_limit=-1e308, rate=100): |
|
||||||
self._k_p = k_p |
|
||||||
self._k_i = k_i |
|
||||||
if isinstance(self._k_p, Number): |
|
||||||
self._k_p = [[0], [self._k_p]] |
|
||||||
if isinstance(self._k_i, Number): |
|
||||||
self._k_i = [[0], [self._k_i]] |
|
||||||
|
|
||||||
self.pos_limit = pos_limit |
|
||||||
self.neg_limit = neg_limit |
|
||||||
|
|
||||||
self.i_unwind_rate = 0.3 / rate |
|
||||||
self.i_rate = 1.0 / rate |
|
||||||
self.speed = 0.0 |
|
||||||
|
|
||||||
self.reset() |
|
||||||
|
|
||||||
@property |
|
||||||
def k_p(self): |
|
||||||
return interp(self.speed, self._k_p[0], self._k_p[1]) |
|
||||||
|
|
||||||
@property |
|
||||||
def k_i(self): |
|
||||||
return interp(self.speed, self._k_i[0], self._k_i[1]) |
|
||||||
|
|
||||||
@property |
|
||||||
def error_integral(self): |
|
||||||
return self.i/self.k_i |
|
||||||
|
|
||||||
def reset(self): |
|
||||||
self.p = 0.0 |
|
||||||
self.i = 0.0 |
|
||||||
self.control = 0 |
|
||||||
|
|
||||||
def update(self, error, speed=0.0, freeze_integrator=False): |
|
||||||
self.speed = speed |
|
||||||
|
|
||||||
self.p = float(error) * self.k_p |
|
||||||
|
|
||||||
i = self.i + error * self.k_i * self.i_rate |
|
||||||
control = self.p + i |
|
||||||
|
|
||||||
# Update when changing i will move the control away from the limits |
|
||||||
# or when i will move towards the sign of the error |
|
||||||
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or |
|
||||||
(error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ |
|
||||||
not freeze_integrator: |
|
||||||
self.i = i |
|
||||||
|
|
||||||
control = self.p + self.i |
|
||||||
|
|
||||||
self.control = clip(control, self.neg_limit, self.pos_limit) |
|
||||||
return self.control |
|
||||||
|
|
||||||
|
|
||||||
MAX_TORQUE = 500 |
|
||||||
MAX_TORQUE_RATE = 50 |
|
||||||
MAX_ANGLE_ERROR = np.radians(7) |
|
||||||
MAX_POS_INTEGRATOR = 0.2 # meters |
|
||||||
MAX_TURN_INTEGRATOR = 0.1 # meters |
|
||||||
|
|
||||||
|
|
||||||
class CarController(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.packer = CANPacker(dbc_name) |
|
||||||
|
|
||||||
# PIDs |
|
||||||
self.turn_pid = PIController(110, k_i=11.5, rate=1/DT_CTRL) |
|
||||||
self.wheeled_speed_pid = PIController(110, k_i=11.5, rate=1/DT_CTRL) |
|
||||||
|
|
||||||
self.torque_r_filtered = 0. |
|
||||||
self.torque_l_filtered = 0. |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def deadband_filter(torque, deadband): |
|
||||||
if torque > 0: |
|
||||||
torque += deadband |
|
||||||
else: |
|
||||||
torque -= deadband |
|
||||||
return torque |
|
||||||
|
|
||||||
def update(self, CC, CS, now_nanos): |
|
||||||
|
|
||||||
torque_l = 0 |
|
||||||
torque_r = 0 |
|
||||||
|
|
||||||
llk_valid = len(CC.orientationNED) > 1 and len(CC.angularVelocity) > 1 |
|
||||||
if CC.enabled and llk_valid: |
|
||||||
# Read these from the joystick |
|
||||||
# TODO: this isn't acceleration, okay? |
|
||||||
speed_desired = CC.actuators.accel / 5. |
|
||||||
speed_diff_desired = -CC.actuators.steer / 2. |
|
||||||
|
|
||||||
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. |
|
||||||
speed_error = speed_desired - speed_measured |
|
||||||
|
|
||||||
torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False) |
|
||||||
|
|
||||||
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) |
|
||||||
turn_error = speed_diff_measured - speed_diff_desired |
|
||||||
freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or |
|
||||||
(turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR)) |
|
||||||
torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator) |
|
||||||
|
|
||||||
# Combine 2 PIDs outputs |
|
||||||
torque_r = torque + torque_diff |
|
||||||
torque_l = torque - torque_diff |
|
||||||
|
|
||||||
# Torque rate limits |
|
||||||
self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), |
|
||||||
self.torque_r_filtered - MAX_TORQUE_RATE, |
|
||||||
self.torque_r_filtered + MAX_TORQUE_RATE) |
|
||||||
self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), |
|
||||||
self.torque_l_filtered - MAX_TORQUE_RATE, |
|
||||||
self.torque_l_filtered + MAX_TORQUE_RATE) |
|
||||||
torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) |
|
||||||
torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) |
|
||||||
|
|
||||||
can_sends = [] |
|
||||||
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) |
|
||||||
|
|
||||||
new_actuators = copy.copy(CC.actuators) |
|
||||||
new_actuators.accel = torque_l |
|
||||||
new_actuators.steer = torque_r |
|
||||||
new_actuators.steerOutputCan = torque_r |
|
||||||
|
|
||||||
self.frame += 1 |
|
||||||
return new_actuators, can_sends |
|
||||||
@ -1,39 +0,0 @@ |
|||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
from openpilot.selfdrive.car.body.values import DBC |
|
||||||
|
|
||||||
|
|
||||||
class CarState(CarStateBase): |
|
||||||
def update(self, cp, *_) -> structs.CarState: |
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
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 = structs.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) |
|
||||||
@ -1,28 +0,0 @@ |
|||||||
# ruff: noqa: E501 |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.body.values import CAR |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
# debug ecu fw version is the git hash of the firmware |
|
||||||
|
|
||||||
|
|
||||||
FINGERPRINTS = { |
|
||||||
CAR.COMMA_BODY: [{ |
|
||||||
513: 8, 516: 8, 514: 3, 515: 4 |
|
||||||
}], |
|
||||||
} |
|
||||||
|
|
||||||
FW_VERSIONS = { |
|
||||||
CAR.COMMA_BODY: { |
|
||||||
(Ecu.engine, 0x720, None): [ |
|
||||||
b'0.0.01', |
|
||||||
b'0.3.00a', |
|
||||||
b'02/27/2022', |
|
||||||
], |
|
||||||
(Ecu.debug, 0x721, None): [ |
|
||||||
b'166bd860', |
|
||||||
b'dc780f85', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
@ -1,25 +0,0 @@ |
|||||||
import math |
|
||||||
from openpilot.selfdrive.car import get_safety_config, structs |
|
||||||
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: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
ret.notCar = True |
|
||||||
ret.carName = "body" |
|
||||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.body)] |
|
||||||
|
|
||||||
ret.minSteerSpeed = -math.inf |
|
||||||
ret.maxLateralAccel = math.inf # TODO: set to a reasonable value |
|
||||||
ret.steerLimitTimer = 1.0 |
|
||||||
ret.steerActuatorDelay = 0. |
|
||||||
|
|
||||||
ret.wheelSpeedFactor = SPEED_FROM_RPM |
|
||||||
|
|
||||||
ret.radarUnavailable = True |
|
||||||
ret.openpilotLongitudinalControl = True |
|
||||||
ret.steerControlType = structs.CarParams.SteerControlType.angle |
|
||||||
|
|
||||||
return ret |
|
||||||
@ -1,4 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase |
|
||||||
|
|
||||||
class RadarInterface(RadarInterfaceBase): |
|
||||||
pass |
|
||||||
@ -1,40 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarDocs |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
SPEED_FROM_RPM = 0.008587 |
|
||||||
|
|
||||||
|
|
||||||
class CarControllerParams: |
|
||||||
ANGLE_DELTA_BP = [0., 5., 15.] |
|
||||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit |
|
||||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit |
|
||||||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower |
|
||||||
STEER_THRESHOLD = 1.0 |
|
||||||
|
|
||||||
def __init__(self, CP): |
|
||||||
pass |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
COMMA_BODY = PlatformConfig( |
|
||||||
[CarDocs("comma body", package="All")], |
|
||||||
CarSpecs(mass=9, wheelbase=0.406, steerRatio=0.5, centerToFrontRatio=0.44), |
|
||||||
dbc_dict('comma_body', None), |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
FW_QUERY_CONFIG = FwQueryConfig( |
|
||||||
requests=[ |
|
||||||
Request( |
|
||||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], |
|
||||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], |
|
||||||
bus=0, |
|
||||||
), |
|
||||||
], |
|
||||||
) |
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,15 +0,0 @@ |
|||||||
from collections.abc import Callable |
|
||||||
from typing import NamedTuple, Protocol |
|
||||||
|
|
||||||
|
|
||||||
class CanData(NamedTuple): |
|
||||||
address: int |
|
||||||
dat: bytes |
|
||||||
src: int |
|
||||||
|
|
||||||
|
|
||||||
CanSendCallable = Callable[[list[CanData]], None] |
|
||||||
|
|
||||||
|
|
||||||
class CanRecvCallable(Protocol): |
|
||||||
def __call__(self, wait_for_one: bool = False) -> list[list[CanData]]: ... |
|
||||||
@ -1,178 +0,0 @@ |
|||||||
import os |
|
||||||
import time |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import carlog, gen_empty_fingerprint |
|
||||||
from openpilot.selfdrive.car.can_definitions import CanRecvCallable, CanSendCallable |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars |
|
||||||
from openpilot.selfdrive.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car |
|
||||||
from openpilot.selfdrive.car.interfaces import get_interface_attr |
|
||||||
from openpilot.selfdrive.car.mock.values import CAR as MOCK |
|
||||||
from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN |
|
||||||
|
|
||||||
FRAME_FINGERPRINT = 100 # 1s |
|
||||||
|
|
||||||
|
|
||||||
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 |
|
||||||
CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState |
|
||||||
CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController |
|
||||||
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(can_recv: CanRecvCallable) -> tuple[str | None, 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: |
|
||||||
# can_recv(wait_for_one=True) may return zero or multiple packets, so we increment frame for each one we receive |
|
||||||
can_packets = can_recv(wait_for_one=True) |
|
||||||
for can_packet in can_packets: |
|
||||||
for can in can_packet: |
|
||||||
# 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(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int, |
|
||||||
cached_params: CarParams | None) -> tuple[str | None, dict, str, list[CarParams.CarFw], CarParams.FingerprintSource, bool]: |
|
||||||
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() |
|
||||||
|
|
||||||
start_time = time.monotonic() |
|
||||||
if not skip_fw_query: |
|
||||||
if cached_params is not None and cached_params.carName != "mock" and len(cached_params.carFw) > 0 and \ |
|
||||||
cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache: |
|
||||||
carlog.warning("Using cached CarParams") |
|
||||||
vin_rx_addr, vin_rx_bus, vin = -1, -1, cached_params.carVin |
|
||||||
car_fw = list(cached_params.carFw) |
|
||||||
cached = True |
|
||||||
else: |
|
||||||
carlog.warning("Getting VIN & FW versions") |
|
||||||
# enable OBD multiplexing for VIN query |
|
||||||
# NOTE: this takes ~0.1s and is relied on to allow sendcan subscriber to connect in time |
|
||||||
set_obd_multiplexing(True) |
|
||||||
# VIN query only reliably works through OBDII |
|
||||||
vin_rx_addr, vin_rx_bus, vin = get_vin(can_recv, can_send, (0, 1)) |
|
||||||
ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing, num_pandas=num_pandas) |
|
||||||
car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas) |
|
||||||
cached = False |
|
||||||
|
|
||||||
exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin) |
|
||||||
else: |
|
||||||
vin_rx_addr, vin_rx_bus, vin = -1, -1, VIN_UNKNOWN |
|
||||||
exact_fw_match, fw_candidates, car_fw = True, set(), [] |
|
||||||
cached = False |
|
||||||
|
|
||||||
if not is_valid_vin(vin): |
|
||||||
carlog.error({"event": "Malformed VIN", "vin": vin}) |
|
||||||
vin = VIN_UNKNOWN |
|
||||||
carlog.warning("VIN %s", vin) |
|
||||||
|
|
||||||
# disable OBD multiplexing for CAN fingerprinting and potential ECU knockouts |
|
||||||
set_obd_multiplexing(False) |
|
||||||
|
|
||||||
fw_query_time = time.monotonic() - start_time |
|
||||||
|
|
||||||
# CAN fingerprint |
|
||||||
# drain CAN socket so we get the latest messages |
|
||||||
can_recv() |
|
||||||
car_fingerprint, finger = can_fingerprint(can_recv) |
|
||||||
|
|
||||||
exact_match = True |
|
||||||
source = CarParams.FingerprintSource.can |
|
||||||
|
|
||||||
# If FW query returns exactly 1 candidate, use it |
|
||||||
if len(fw_candidates) == 1: |
|
||||||
car_fingerprint = list(fw_candidates)[0] |
|
||||||
source = CarParams.FingerprintSource.fw |
|
||||||
exact_match = exact_fw_match |
|
||||||
|
|
||||||
if fixed_fingerprint: |
|
||||||
car_fingerprint = fixed_fingerprint |
|
||||||
source = CarParams.FingerprintSource.fixed |
|
||||||
|
|
||||||
carlog.error({"event": "fingerprinted", "car_fingerprint": str(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, |
|
||||||
"vin_rx_bus": vin_rx_bus, "fingerprints": repr(finger), "fw_query_time": fw_query_time}) |
|
||||||
|
|
||||||
return car_fingerprint, finger, vin, car_fw, source, exact_match |
|
||||||
|
|
||||||
|
|
||||||
def get_car_interface(CP: CarParams): |
|
||||||
CarInterface, CarController, CarState = interfaces[CP.carFingerprint] |
|
||||||
return CarInterface(CP, CarController, CarState) |
|
||||||
|
|
||||||
|
|
||||||
def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, experimental_long_allowed: bool, |
|
||||||
num_pandas: int = 1, cached_params: CarParams | None = None): |
|
||||||
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(can_recv, can_send, set_obd_multiplexing, num_pandas, cached_params) |
|
||||||
|
|
||||||
if candidate is None: |
|
||||||
carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)}) |
|
||||||
candidate = "MOCK" |
|
||||||
|
|
||||||
CarInterface, _, _ = interfaces[candidate] |
|
||||||
CP: CarParams = 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 get_car_interface(CP) |
|
||||||
|
|
||||||
|
|
||||||
def get_demo_car_params(): |
|
||||||
platform = MOCK.MOCK |
|
||||||
CarInterface, _, _ = interfaces[platform] |
|
||||||
CP = CarInterface.get_non_essential_params(platform) |
|
||||||
return CP |
|
||||||
@ -1,84 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import DT_CTRL, apply_meas_steer_torque_limits |
|
||||||
from openpilot.selfdrive.car.chrysler import chryslercan |
|
||||||
from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
|
|
||||||
|
|
||||||
class CarController(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.apply_steer_last = 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 = copy.copy(CC.actuators) |
|
||||||
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX |
|
||||||
new_actuators.steerOutputCan = self.apply_steer_last |
|
||||||
|
|
||||||
return new_actuators, can_sends |
|
||||||
@ -1,159 +0,0 @@ |
|||||||
from opendbc.can.parser import CANParser |
|
||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
|
|
||||||
|
|
||||||
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"] |
|
||||||
|
|
||||||
self.distance_button = 0 |
|
||||||
|
|
||||||
def update(self, cp, cp_cam, *_) -> structs.CarState: |
|
||||||
|
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
prev_distance_button = self.distance_button |
|
||||||
self.distance_button = cp.vl["CRUISE_BUTTONS"]["ACC_Distance_Dec"] |
|
||||||
|
|
||||||
# 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"] |
|
||||||
|
|
||||||
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) |
|
||||||
|
|
||||||
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) |
|
||||||
@ -1,71 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.chrysler.values import RAM_CARS |
|
||||||
|
|
||||||
GearShifter = structs.CarState.GearShifter |
|
||||||
VisualAlert = structs.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) |
|
||||||
@ -1,716 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.chrysler.values import CAR |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
FW_VERSIONS = { |
|
||||||
CAR.CHRYSLER_PACIFICA_2018: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68227902AF', |
|
||||||
b'68227902AG', |
|
||||||
b'68227902AH', |
|
||||||
b'68227905AG', |
|
||||||
b'68360252AC', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68211617AF', |
|
||||||
b'68211617AG', |
|
||||||
b'68358974AC', |
|
||||||
b'68405937AA', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x747, None): [ |
|
||||||
b'68222747AG', |
|
||||||
b'68330876AA', |
|
||||||
b'68330876AB', |
|
||||||
b'68352227AA', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'04672758AA', |
|
||||||
b'04672758AB', |
|
||||||
b'68226356AF', |
|
||||||
b'68226356AH', |
|
||||||
b'68226356AI', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68288891AE', |
|
||||||
b'68378884AA', |
|
||||||
b'68525338AA', |
|
||||||
b'68525338AB', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'68267018AO ', |
|
||||||
b'68267020AJ ', |
|
||||||
b'68303534AG ', |
|
||||||
b'68303534AJ ', |
|
||||||
b'68340762AD ', |
|
||||||
b'68340764AD ', |
|
||||||
b'68352652AE ', |
|
||||||
b'68352654AE ', |
|
||||||
b'68366851AH ', |
|
||||||
b'68366853AE ', |
|
||||||
b'68366853AG ', |
|
||||||
b'68372861AF ', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x7e1, None): [ |
|
||||||
b'68277370AJ', |
|
||||||
b'68277370AM', |
|
||||||
b'68277372AD', |
|
||||||
b'68277372AE', |
|
||||||
b'68277372AN', |
|
||||||
b'68277374AA', |
|
||||||
b'68277374AB', |
|
||||||
b'68277374AD', |
|
||||||
b'68277374AN', |
|
||||||
b'68367471AC', |
|
||||||
b'68367471AD', |
|
||||||
b'68380571AB', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.CHRYSLER_PACIFICA_2020: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68405327AC', |
|
||||||
b'68436233AB', |
|
||||||
b'68436233AC', |
|
||||||
b'68436234AB', |
|
||||||
b'68436250AE', |
|
||||||
b'68529067AA', |
|
||||||
b'68594993AB', |
|
||||||
b'68594994AB', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68405565AB', |
|
||||||
b'68405565AC', |
|
||||||
b'68444299AC', |
|
||||||
b'68480707AC', |
|
||||||
b'68480708AC', |
|
||||||
b'68526663AB', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x747, None): [ |
|
||||||
b'68397394AA', |
|
||||||
b'68433480AB', |
|
||||||
b'68453575AF', |
|
||||||
b'68577676AA', |
|
||||||
b'68593395AA', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'04672758AA', |
|
||||||
b'04672758AB', |
|
||||||
b'68417813AF', |
|
||||||
b'68540436AA', |
|
||||||
b'68540436AC', |
|
||||||
b'68540436AD', |
|
||||||
b'68598670AB', |
|
||||||
b'68598670AC', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68416742AA', |
|
||||||
b'68460393AA', |
|
||||||
b'68460393AB', |
|
||||||
b'68494461AB', |
|
||||||
b'68494461AC', |
|
||||||
b'68524936AA', |
|
||||||
b'68524936AB', |
|
||||||
b'68525338AB', |
|
||||||
b'68594337AB', |
|
||||||
b'68594340AB', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'68413871AD ', |
|
||||||
b'68413871AE ', |
|
||||||
b'68413871AH ', |
|
||||||
b'68413871AI ', |
|
||||||
b'68413873AH ', |
|
||||||
b'68413873AI ', |
|
||||||
b'68443120AE ', |
|
||||||
b'68443123AC ', |
|
||||||
b'68443125AC ', |
|
||||||
b'68496647AI ', |
|
||||||
b'68496647AJ ', |
|
||||||
b'68496650AH ', |
|
||||||
b'68496650AI ', |
|
||||||
b'68496652AH ', |
|
||||||
b'68526752AD ', |
|
||||||
b'68526752AE ', |
|
||||||
b'68526754AE ', |
|
||||||
b'68536264AE ', |
|
||||||
b'68700304AB ', |
|
||||||
b'68700306AB ', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x7e1, None): [ |
|
||||||
b'68414271AC', |
|
||||||
b'68414271AD', |
|
||||||
b'68414275AC', |
|
||||||
b'68414275AD', |
|
||||||
b'68443154AB', |
|
||||||
b'68443155AC', |
|
||||||
b'68443158AB', |
|
||||||
b'68501050AD', |
|
||||||
b'68501051AD', |
|
||||||
b'68501055AD', |
|
||||||
b'68527221AB', |
|
||||||
b'68527223AB', |
|
||||||
b'68586231AD', |
|
||||||
b'68586233AD', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.CHRYSLER_PACIFICA_2018_HYBRID: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68239262AH', |
|
||||||
b'68239262AI', |
|
||||||
b'68239262AJ', |
|
||||||
b'68239263AH', |
|
||||||
b'68239263AJ', |
|
||||||
b'68358439AE', |
|
||||||
b'68358439AG', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68238840AH', |
|
||||||
b'68358990AC', |
|
||||||
b'68405939AA', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'04672758AA', |
|
||||||
b'68226356AI', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68288309AC', |
|
||||||
b'68288309AD', |
|
||||||
b'68525339AA', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'68277480AV ', |
|
||||||
b'68277480AX ', |
|
||||||
b'68277480AZ ', |
|
||||||
b'68366580AI ', |
|
||||||
b'68366580AK ', |
|
||||||
b'68366580AM ', |
|
||||||
], |
|
||||||
(Ecu.hybrid, 0x7e2, None): [ |
|
||||||
b'05190175BF', |
|
||||||
b'05190175BH', |
|
||||||
b'05190226AI', |
|
||||||
b'05190226AK', |
|
||||||
b'05190226AM', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.CHRYSLER_PACIFICA_2019_HYBRID: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68405292AC', |
|
||||||
b'68434956AC', |
|
||||||
b'68434956AD', |
|
||||||
b'68434960AE', |
|
||||||
b'68434960AF', |
|
||||||
b'68529064AB', |
|
||||||
b'68594990AB', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68405567AB', |
|
||||||
b'68405567AC', |
|
||||||
b'68453076AD', |
|
||||||
b'68480710AC', |
|
||||||
b'68526665AB', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'04672758AB', |
|
||||||
b'68417813AF', |
|
||||||
b'68540436AA', |
|
||||||
b'68540436AB', |
|
||||||
b'68540436AC', |
|
||||||
b'68540436AD', |
|
||||||
b'68598670AB', |
|
||||||
b'68598670AC', |
|
||||||
b'68645752AA', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68416741AA', |
|
||||||
b'68460392AA', |
|
||||||
b'68525339AA', |
|
||||||
b'68525339AB', |
|
||||||
b'68594341AB', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'68416680AE ', |
|
||||||
b'68416680AF ', |
|
||||||
b'68416680AG ', |
|
||||||
b'68444228AC ', |
|
||||||
b'68444228AD ', |
|
||||||
b'68444228AE ', |
|
||||||
b'68444228AF ', |
|
||||||
b'68499122AD ', |
|
||||||
b'68499122AE ', |
|
||||||
b'68499122AF ', |
|
||||||
b'68526772AD ', |
|
||||||
b'68526772AH ', |
|
||||||
b'68599493AC ', |
|
||||||
b'68657433AA ', |
|
||||||
], |
|
||||||
(Ecu.hybrid, 0x7e2, None): [ |
|
||||||
b'05185116AF', |
|
||||||
b'05185116AJ', |
|
||||||
b'05185116AK', |
|
||||||
b'05190240AP', |
|
||||||
b'05190240AQ', |
|
||||||
b'05190240AR', |
|
||||||
b'05190265AG', |
|
||||||
b'05190265AH', |
|
||||||
b'05190289AE', |
|
||||||
b'68540977AH', |
|
||||||
b'68540977AK', |
|
||||||
b'68597647AE', |
|
||||||
b'68632416AB', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.JEEP_GRAND_CHEROKEE: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68243549AG', |
|
||||||
b'68302211AC', |
|
||||||
b'68302212AD', |
|
||||||
b'68302223AC', |
|
||||||
b'68302246AC', |
|
||||||
b'68331511AC', |
|
||||||
b'68331574AC', |
|
||||||
b'68331687AC', |
|
||||||
b'68331690AC', |
|
||||||
b'68340272AD', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68309533AA', |
|
||||||
b'68316742AB', |
|
||||||
b'68355363AB', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x747, None): [ |
|
||||||
b'68252642AG', |
|
||||||
b'68306178AD', |
|
||||||
b'68336275AB', |
|
||||||
b'68336276AB', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'04672627AB', |
|
||||||
b'68251506AF', |
|
||||||
b'68332015AB', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68276201AG', |
|
||||||
b'68321644AB', |
|
||||||
b'68321644AC', |
|
||||||
b'68321646AC', |
|
||||||
b'68321648AC', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'05035920AE ', |
|
||||||
b'68252272AG ', |
|
||||||
b'68284455AI ', |
|
||||||
b'68284456AI ', |
|
||||||
b'68284477AF ', |
|
||||||
b'68325564AH ', |
|
||||||
b'68325564AI ', |
|
||||||
b'68325565AH ', |
|
||||||
b'68325565AI ', |
|
||||||
b'68325618AD ', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x7e1, None): [ |
|
||||||
b'05035517AH', |
|
||||||
b'68253222AF', |
|
||||||
b'68311218AC', |
|
||||||
b'68311223AF', |
|
||||||
b'68311223AG', |
|
||||||
b'68361911AE', |
|
||||||
b'68361911AF', |
|
||||||
b'68361911AH', |
|
||||||
b'68361916AD', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.JEEP_GRAND_CHEROKEE_2019: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68402703AB', |
|
||||||
b'68402704AB', |
|
||||||
b'68402708AB', |
|
||||||
b'68402971AD', |
|
||||||
b'68454144AD', |
|
||||||
b'68454145AB', |
|
||||||
b'68454152AB', |
|
||||||
b'68454156AB', |
|
||||||
b'68516650AB', |
|
||||||
b'68516651AB', |
|
||||||
b'68516669AB', |
|
||||||
b'68516671AB', |
|
||||||
b'68516683AB', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68355363AB', |
|
||||||
b'68355364AB', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x747, None): [ |
|
||||||
b'68408639AC', |
|
||||||
b'68408639AD', |
|
||||||
b'68499978AB', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'04672788AA', |
|
||||||
b'68456722AC', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68417279AA', |
|
||||||
b'68417280AA', |
|
||||||
b'68417281AA', |
|
||||||
b'68453431AA', |
|
||||||
b'68453433AA', |
|
||||||
b'68453435AA', |
|
||||||
b'68499171AA', |
|
||||||
b'68499171AB', |
|
||||||
b'68501183AA', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'05035674AB ', |
|
||||||
b'68412635AG ', |
|
||||||
b'68412660AD ', |
|
||||||
b'68422860AB', |
|
||||||
b'68449435AE ', |
|
||||||
b'68496223AA ', |
|
||||||
b'68504959AD ', |
|
||||||
b'68504959AE ', |
|
||||||
b'68504960AD ', |
|
||||||
b'68504993AC ', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x7e1, None): [ |
|
||||||
b'05035707AA', |
|
||||||
b'68419672AC', |
|
||||||
b'68419678AB', |
|
||||||
b'68423905AB', |
|
||||||
b'68449258AC', |
|
||||||
b'68495807AA', |
|
||||||
b'68495807AB', |
|
||||||
b'68503641AC', |
|
||||||
b'68503664AC', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.RAM_1500_5TH_GEN: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68294051AG', |
|
||||||
b'68294051AI', |
|
||||||
b'68294052AG', |
|
||||||
b'68294052AH', |
|
||||||
b'68294059AI', |
|
||||||
b'68294063AG', |
|
||||||
b'68294063AH', |
|
||||||
b'68294063AI', |
|
||||||
b'68434846AC', |
|
||||||
b'68434847AC', |
|
||||||
b'68434849AC', |
|
||||||
b'68434856AC', |
|
||||||
b'68434858AC', |
|
||||||
b'68434859AC', |
|
||||||
b'68434860AC', |
|
||||||
b'68453471AD', |
|
||||||
b'68453483AC', |
|
||||||
b'68453483AD', |
|
||||||
b'68453487AD', |
|
||||||
b'68453491AC', |
|
||||||
b'68453491AD', |
|
||||||
b'68453499AD', |
|
||||||
b'68453503AC', |
|
||||||
b'68453503AD', |
|
||||||
b'68453505AC', |
|
||||||
b'68453505AD', |
|
||||||
b'68453511AC', |
|
||||||
b'68453513AC', |
|
||||||
b'68453513AD', |
|
||||||
b'68453514AD', |
|
||||||
b'68505633AB', |
|
||||||
b'68510277AG', |
|
||||||
b'68510277AH', |
|
||||||
b'68510280AG', |
|
||||||
b'68510282AG', |
|
||||||
b'68510282AH', |
|
||||||
b'68510283AG', |
|
||||||
b'68527346AE', |
|
||||||
b'68527361AD', |
|
||||||
b'68527375AD', |
|
||||||
b'68527381AD', |
|
||||||
b'68527381AE', |
|
||||||
b'68527382AE', |
|
||||||
b'68527383AD', |
|
||||||
b'68527383AE', |
|
||||||
b'68527387AE', |
|
||||||
b'68527397AD', |
|
||||||
b'68527403AC', |
|
||||||
b'68527403AD', |
|
||||||
b'68546047AF', |
|
||||||
b'68631938AA', |
|
||||||
b'68631939AA', |
|
||||||
b'68631940AA', |
|
||||||
b'68631940AB', |
|
||||||
b'68631942AA', |
|
||||||
b'68631943AB', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68428609AB', |
|
||||||
b'68441329AB', |
|
||||||
b'68473844AB', |
|
||||||
b'68490898AA', |
|
||||||
b'68500728AA', |
|
||||||
b'68615033AA', |
|
||||||
b'68615034AA', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x747, None): [ |
|
||||||
b'68292406AG', |
|
||||||
b'68292406AH', |
|
||||||
b'68432418AB', |
|
||||||
b'68432418AC', |
|
||||||
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'68312176AF', |
|
||||||
b'68312176AG', |
|
||||||
b'68440789AC', |
|
||||||
b'68466110AA', |
|
||||||
b'68466110AB', |
|
||||||
b'68466113AA', |
|
||||||
b'68469901AA', |
|
||||||
b'68469907AA', |
|
||||||
b'68522583AA', |
|
||||||
b'68522583AB', |
|
||||||
b'68522584AA', |
|
||||||
b'68522585AB', |
|
||||||
b'68552788AA', |
|
||||||
b'68552789AA', |
|
||||||
b'68552790AA', |
|
||||||
b'68552791AB', |
|
||||||
b'68552794AA', |
|
||||||
b'68552794AD', |
|
||||||
b'68585106AB', |
|
||||||
b'68585107AB', |
|
||||||
b'68585108AB', |
|
||||||
b'68585109AB', |
|
||||||
b'68585112AB', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'05035699AG ', |
|
||||||
b'05035841AC ', |
|
||||||
b'05035841AD ', |
|
||||||
b'05036026AB ', |
|
||||||
b'05036065AE ', |
|
||||||
b'05036066AE ', |
|
||||||
b'05036067AE ', |
|
||||||
b'05036193AA ', |
|
||||||
b'05149368AA ', |
|
||||||
b'05149374AA ', |
|
||||||
b'05149591AD ', |
|
||||||
b'05149591AE ', |
|
||||||
b'05149592AE ', |
|
||||||
b'05149599AE ', |
|
||||||
b'05149600AD ', |
|
||||||
b'05149600AE ', |
|
||||||
b'05149605AE ', |
|
||||||
b'05149846AA ', |
|
||||||
b'05149848AA ', |
|
||||||
b'05149848AC ', |
|
||||||
b'05190341AD', |
|
||||||
b'68378695AJ ', |
|
||||||
b'68378696AJ ', |
|
||||||
b'68378696AK ', |
|
||||||
b'68378701AI ', |
|
||||||
b'68378702AI ', |
|
||||||
b'68378710AL ', |
|
||||||
b'68378742AI ', |
|
||||||
b'68378742AK ', |
|
||||||
b'68378743AM ', |
|
||||||
b'68378748AL ', |
|
||||||
b'68378758AM ', |
|
||||||
b'68448163AJ', |
|
||||||
b'68448163AK', |
|
||||||
b'68448163AL', |
|
||||||
b'68448165AG', |
|
||||||
b'68448165AK', |
|
||||||
b'68455111AC ', |
|
||||||
b'68455119AC ', |
|
||||||
b'68455145AC ', |
|
||||||
b'68455145AE ', |
|
||||||
b'68455146AC ', |
|
||||||
b'68460927AA ', |
|
||||||
b'68467915AC ', |
|
||||||
b'68467916AC ', |
|
||||||
b'68467936AC ', |
|
||||||
b'68500630AD', |
|
||||||
b'68500630AE', |
|
||||||
b'68500631AE', |
|
||||||
b'68502719AC ', |
|
||||||
b'68502722AC ', |
|
||||||
b'68502733AC ', |
|
||||||
b'68502734AF ', |
|
||||||
b'68502740AF ', |
|
||||||
b'68502741AF ', |
|
||||||
b'68502742AC ', |
|
||||||
b'68502742AF ', |
|
||||||
b'68539650AD', |
|
||||||
b'68539650AF', |
|
||||||
b'68539651AD', |
|
||||||
b'68586101AA ', |
|
||||||
b'68586102AA ', |
|
||||||
b'68586105AB ', |
|
||||||
b'68629919AC ', |
|
||||||
b'68629922AC ', |
|
||||||
b'68629925AC ', |
|
||||||
b'68629926AC ', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x7e1, None): [ |
|
||||||
b'05035706AD', |
|
||||||
b'05035842AB', |
|
||||||
b'05036069AA', |
|
||||||
b'05036181AA', |
|
||||||
b'05149536AC', |
|
||||||
b'05149537AC', |
|
||||||
b'05149543AC', |
|
||||||
b'68360078AL', |
|
||||||
b'68360080AL', |
|
||||||
b'68360080AM', |
|
||||||
b'68360081AM', |
|
||||||
b'68360085AJ', |
|
||||||
b'68360085AL', |
|
||||||
b'68360086AH', |
|
||||||
b'68360086AK', |
|
||||||
b'68360086AN', |
|
||||||
b'68384328AD', |
|
||||||
b'68384332AD', |
|
||||||
b'68445531AC', |
|
||||||
b'68445533AB', |
|
||||||
b'68445536AB', |
|
||||||
b'68445537AB', |
|
||||||
b'68466081AB', |
|
||||||
b'68466087AB', |
|
||||||
b'68484466AC', |
|
||||||
b'68484467AC', |
|
||||||
b'68484471AC', |
|
||||||
b'68502994AD', |
|
||||||
b'68502996AD', |
|
||||||
b'68520867AE', |
|
||||||
b'68520867AF', |
|
||||||
b'68520870AC', |
|
||||||
b'68520871AC', |
|
||||||
b'68528325AE', |
|
||||||
b'68540431AB', |
|
||||||
b'68540433AB', |
|
||||||
b'68551676AA', |
|
||||||
b'68629935AB', |
|
||||||
b'68629936AC', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.RAM_HD_5TH_GEN: { |
|
||||||
(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', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.DODGE_DURANGO: { |
|
||||||
(Ecu.combinationMeter, 0x742, None): [ |
|
||||||
b'68454261AD', |
|
||||||
b'68471535AE', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x744, None): [ |
|
||||||
b'68355362AB', |
|
||||||
b'68492238AD', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x747, None): [ |
|
||||||
b'68408639AD', |
|
||||||
b'68499978AB', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x753, None): [ |
|
||||||
b'68440581AE', |
|
||||||
b'68456722AC', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x75a, None): [ |
|
||||||
b'68453435AA', |
|
||||||
b'68498477AA', |
|
||||||
], |
|
||||||
(Ecu.engine, 0x7e0, None): [ |
|
||||||
b'05035786AE ', |
|
||||||
b'68449476AE ', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x7e1, None): [ |
|
||||||
b'05035826AC', |
|
||||||
b'68449265AC', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
@ -1,74 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from panda import Panda |
|
||||||
from openpilot.selfdrive.car import get_safety_config, structs |
|
||||||
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: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
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(structs.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 |
|
||||||
|
|
||||||
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.CHRYSLER_PACIFICA_2019_HYBRID, CAR.CHRYSLER_PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019, CAR.DODGE_DURANGO) |
|
||||||
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.CHRYSLER_PACIFICA_2018, CAR.CHRYSLER_PACIFICA_2018_HYBRID, CAR.CHRYSLER_PACIFICA_2019_HYBRID, |
|
||||||
CAR.CHRYSLER_PACIFICA_2020, CAR.DODGE_DURANGO): |
|
||||||
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.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_5TH_GEN: |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
ret.wheelbase = 3.88 |
|
||||||
# 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_5TH_GEN: |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
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. |
|
||||||
# TODO: Durango 2020 may be able to steer to zero once above 38 kph |
|
||||||
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 |
|
||||||
@ -1,85 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
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.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 = structs.RadarData() |
|
||||||
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] = structs.RadarData.RadarPoint() |
|
||||||
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 |
|
||||||
@ -1,152 +0,0 @@ |
|||||||
from enum import IntFlag |
|
||||||
from dataclasses import dataclass, field |
|
||||||
|
|
||||||
from panda.python import uds |
|
||||||
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
|
|
||||||
class ChryslerFlags(IntFlag): |
|
||||||
# Detected flags |
|
||||||
HIGHER_MIN_STEERING_SPEED = 1 |
|
||||||
|
|
||||||
@dataclass |
|
||||||
class ChryslerCarDocs(CarDocs): |
|
||||||
package: str = "Adaptive Cruise Control (ACC)" |
|
||||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca])) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class ChryslerPlatformConfig(PlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion')) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True) |
|
||||||
class ChryslerCarSpecs(CarSpecs): |
|
||||||
minSteerSpeed: float = 3.8 # m/s |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
# Chrysler |
|
||||||
CHRYSLER_PACIFICA_2018_HYBRID = ChryslerPlatformConfig( |
|
||||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2017-18")], |
|
||||||
ChryslerCarSpecs(mass=2242., wheelbase=3.089, steerRatio=16.2), |
|
||||||
) |
|
||||||
CHRYSLER_PACIFICA_2019_HYBRID = ChryslerPlatformConfig( |
|
||||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2019-24")], |
|
||||||
CHRYSLER_PACIFICA_2018_HYBRID.specs, |
|
||||||
) |
|
||||||
CHRYSLER_PACIFICA_2018 = ChryslerPlatformConfig( |
|
||||||
[ChryslerCarDocs("Chrysler Pacifica 2017-18")], |
|
||||||
CHRYSLER_PACIFICA_2018_HYBRID.specs, |
|
||||||
) |
|
||||||
CHRYSLER_PACIFICA_2020 = ChryslerPlatformConfig( |
|
||||||
[ |
|
||||||
ChryslerCarDocs("Chrysler Pacifica 2019-20"), |
|
||||||
ChryslerCarDocs("Chrysler Pacifica 2021-23", package="All"), |
|
||||||
], |
|
||||||
CHRYSLER_PACIFICA_2018_HYBRID.specs, |
|
||||||
) |
|
||||||
|
|
||||||
# Dodge |
|
||||||
DODGE_DURANGO = ChryslerPlatformConfig( |
|
||||||
[ChryslerCarDocs("Dodge Durango 2020-21")], |
|
||||||
CHRYSLER_PACIFICA_2018_HYBRID.specs, |
|
||||||
) |
|
||||||
|
|
||||||
# Jeep |
|
||||||
JEEP_GRAND_CHEROKEE = ChryslerPlatformConfig( # includes 2017 Trailhawk |
|
||||||
[ChryslerCarDocs("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk")], |
|
||||||
ChryslerCarSpecs(mass=1778., wheelbase=2.71, steerRatio=16.7), |
|
||||||
) |
|
||||||
|
|
||||||
JEEP_GRAND_CHEROKEE_2019 = ChryslerPlatformConfig( # includes 2020 Trailhawk |
|
||||||
[ChryslerCarDocs("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4")], |
|
||||||
JEEP_GRAND_CHEROKEE.specs, |
|
||||||
) |
|
||||||
|
|
||||||
# Ram |
|
||||||
RAM_1500_5TH_GEN = ChryslerPlatformConfig( |
|
||||||
[ChryslerCarDocs("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram]))], |
|
||||||
ChryslerCarSpecs(mass=2493., wheelbase=3.88, steerRatio=16.3, minSteerSpeed=14.5), |
|
||||||
dbc_dict('chrysler_ram_dt_generated', None), |
|
||||||
) |
|
||||||
RAM_HD_5TH_GEN = ChryslerPlatformConfig( |
|
||||||
[ |
|
||||||
ChryslerCarDocs("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])), |
|
||||||
ChryslerCarDocs("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])), |
|
||||||
], |
|
||||||
ChryslerCarSpecs(mass=3405., wheelbase=3.785, steerRatio=15.61, minSteerSpeed=16.), |
|
||||||
dbc_dict('chrysler_ram_hd_generated', None), |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
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_5TH_GEN, } |
|
||||||
RAM_HD = {CAR.RAM_HD_5TH_GEN, } |
|
||||||
RAM_CARS = RAM_DT | RAM_HD |
|
||||||
|
|
||||||
|
|
||||||
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.abs, 0x7e4, None), # alt address for abs on hybrids, NOTE: not on all hybrid platforms |
|
||||||
], |
|
||||||
) |
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,4 +0,0 @@ |
|||||||
import os |
|
||||||
|
|
||||||
|
|
||||||
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) |
|
||||||
@ -1,19 +0,0 @@ |
|||||||
import numpy as np |
|
||||||
|
|
||||||
class Conversions: |
|
||||||
# Speed |
|
||||||
MPH_TO_KPH = 1.609344 |
|
||||||
KPH_TO_MPH = 1. / MPH_TO_KPH |
|
||||||
MS_TO_KPH = 3.6 |
|
||||||
KPH_TO_MS = 1. / MS_TO_KPH |
|
||||||
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH |
|
||||||
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS |
|
||||||
MS_TO_KNOTS = 1.9438 |
|
||||||
KNOTS_TO_MS = 1. / MS_TO_KNOTS |
|
||||||
|
|
||||||
# Angle |
|
||||||
DEG_TO_RAD = np.pi / 180. |
|
||||||
RAD_TO_DEG = 1. / DEG_TO_RAD |
|
||||||
|
|
||||||
# Mass |
|
||||||
LB_TO_KG = 0.453592 |
|
||||||
@ -1,18 +0,0 @@ |
|||||||
class FirstOrderFilter: |
|
||||||
# first order filter |
|
||||||
def __init__(self, x0, rc, dt, initialized=True): |
|
||||||
self.x = x0 |
|
||||||
self.dt = dt |
|
||||||
self.update_alpha(rc) |
|
||||||
self.initialized = initialized |
|
||||||
|
|
||||||
def update_alpha(self, rc): |
|
||||||
self.alpha = self.dt / (rc + self.dt) |
|
||||||
|
|
||||||
def update(self, x): |
|
||||||
if self.initialized: |
|
||||||
self.x = (1. - self.alpha) * self.x + self.alpha * x |
|
||||||
else: |
|
||||||
self.initialized = True |
|
||||||
self.x = x |
|
||||||
return self.x |
|
||||||
@ -1,22 +0,0 @@ |
|||||||
def clip(x, lo, hi): |
|
||||||
return max(lo, min(hi, x)) |
|
||||||
|
|
||||||
|
|
||||||
def interp(x, xp, fp): |
|
||||||
N = len(xp) |
|
||||||
|
|
||||||
def get_interp(xv): |
|
||||||
hi = 0 |
|
||||||
while hi < N and xv > xp[hi]: |
|
||||||
hi += 1 |
|
||||||
low = hi - 1 |
|
||||||
return fp[-1] if hi == N and xv > xp[low] else ( |
|
||||||
fp[0] if hi == 0 else |
|
||||||
(xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) |
|
||||||
|
|
||||||
return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x) |
|
||||||
|
|
||||||
|
|
||||||
def mean(x): |
|
||||||
return sum(x) / len(x) |
|
||||||
|
|
||||||
@ -1,54 +0,0 @@ |
|||||||
import numpy as np |
|
||||||
|
|
||||||
|
|
||||||
def get_kalman_gain(dt, A, C, Q, R, iterations=100): |
|
||||||
P = np.zeros_like(Q) |
|
||||||
for _ in range(iterations): |
|
||||||
P = A.dot(P).dot(A.T) + dt * Q |
|
||||||
S = C.dot(P).dot(C.T) + R |
|
||||||
K = P.dot(C.T).dot(np.linalg.inv(S)) |
|
||||||
P = (np.eye(len(P)) - K.dot(C)).dot(P) |
|
||||||
return K |
|
||||||
|
|
||||||
|
|
||||||
class KF1D: |
|
||||||
# this EKF assumes constant covariance matrix, so calculations are much simpler |
|
||||||
# the Kalman gain also needs to be precomputed using the control module |
|
||||||
|
|
||||||
def __init__(self, x0, A, C, K): |
|
||||||
self.x0_0 = x0[0][0] |
|
||||||
self.x1_0 = x0[1][0] |
|
||||||
self.A0_0 = A[0][0] |
|
||||||
self.A0_1 = A[0][1] |
|
||||||
self.A1_0 = A[1][0] |
|
||||||
self.A1_1 = A[1][1] |
|
||||||
self.C0_0 = C[0] |
|
||||||
self.C0_1 = C[1] |
|
||||||
self.K0_0 = K[0][0] |
|
||||||
self.K1_0 = K[1][0] |
|
||||||
|
|
||||||
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 |
|
||||||
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 |
|
||||||
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 |
|
||||||
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 |
|
||||||
|
|
||||||
# K matrix needs to be pre-computed as follow: |
|
||||||
# import control |
|
||||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) |
|
||||||
# self.K = np.transpose(K) |
|
||||||
|
|
||||||
def update(self, meas): |
|
||||||
#self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) |
|
||||||
x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas |
|
||||||
x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas |
|
||||||
self.x0_0 = x0_0 |
|
||||||
self.x1_0 = x1_0 |
|
||||||
return [self.x0_0, self.x1_0] |
|
||||||
|
|
||||||
@property |
|
||||||
def x(self): |
|
||||||
return [[self.x0_0], [self.x1_0]] |
|
||||||
|
|
||||||
def set_x(self, x): |
|
||||||
self.x0_0 = x[0][0] |
|
||||||
self.x1_0 = x[1][0] |
|
||||||
@ -1,36 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import carlog |
|
||||||
from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery |
|
||||||
|
|
||||||
EXT_DIAG_REQUEST = b'\x10\x03' |
|
||||||
EXT_DIAG_RESPONSE = b'\x50\x03' |
|
||||||
|
|
||||||
COM_CONT_RESPONSE = b'' |
|
||||||
|
|
||||||
|
|
||||||
def disable_ecu(can_recv, can_send, 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!""" |
|
||||||
carlog.warning(f"ecu disable {hex(addr), sub_addr} ...") |
|
||||||
|
|
||||||
for i in range(retry): |
|
||||||
try: |
|
||||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug) |
|
||||||
|
|
||||||
for _, _ in query.get_data(timeout).items(): |
|
||||||
carlog.warning("communication control disable tx/rx ...") |
|
||||||
|
|
||||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], debug=debug) |
|
||||||
query.get_data(0) |
|
||||||
|
|
||||||
carlog.warning("ecu disabled") |
|
||||||
return True |
|
||||||
|
|
||||||
except Exception: |
|
||||||
carlog.exception("ecu disable exception") |
|
||||||
|
|
||||||
carlog.error(f"ecu disable retry ({i + 1}) ...") |
|
||||||
carlog.error("ecu disable failed") |
|
||||||
return False |
|
||||||
@ -1,59 +1,21 @@ |
|||||||
from collections import defaultdict |
#!/usr/bin/env python3 |
||||||
import jinja2 |
import argparse |
||||||
from enum import Enum |
import os |
||||||
from natsort import natsorted |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import gen_empty_fingerprint |
from openpilot.common.basedir import BASEDIR |
||||||
from openpilot.selfdrive.car.structs import CarParams |
from opendbc.car.docs import get_all_car_docs, generate_cars_md |
||||||
from openpilot.selfdrive.car.docs_definitions import CarDocs, Column, CommonFootnote, PartType |
|
||||||
from openpilot.selfdrive.car.car_helpers import interfaces, get_interface_attr |
|
||||||
from openpilot.selfdrive.car.values import PLATFORMS |
|
||||||
|
|
||||||
|
CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md") |
||||||
|
CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") |
||||||
|
|
||||||
def get_all_footnotes() -> dict[Enum, int]: |
if __name__ == "__main__": |
||||||
all_footnotes = list(CommonFootnote) |
parser = argparse.ArgumentParser(description="Auto generates supported cars documentation", |
||||||
for footnotes in get_interface_attr("Footnote", ignore_none=True).values(): |
formatter_class=argparse.ArgumentDefaultsHelpFormatter) |
||||||
all_footnotes.extend(footnotes) |
|
||||||
return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)} |
|
||||||
|
|
||||||
|
parser.add_argument("--template", default=CARS_MD_TEMPLATE, help="Override default template filename") |
||||||
|
parser.add_argument("--out", default=CARS_MD_OUT, help="Override default generated filename") |
||||||
|
args = parser.parse_args() |
||||||
|
|
||||||
def get_all_car_docs() -> list[CarDocs]: |
with open(args.out, 'w') as f: |
||||||
all_car_docs: list[CarDocs] = [] |
f.write(generate_cars_md(get_all_car_docs(), args.template)) |
||||||
footnotes = get_all_footnotes() |
print(f"Generated and written to {args.out}") |
||||||
for model, platform in PLATFORMS.items(): |
|
||||||
car_docs = platform.config.car_docs |
|
||||||
# If available, uses experimental longitudinal limits for the docs |
|
||||||
CP = interfaces[model][0].get_params(platform, fingerprint=gen_empty_fingerprint(), |
|
||||||
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)], experimental_long=True, docs=True) |
|
||||||
|
|
||||||
if CP.dashcamOnly or not len(car_docs): |
|
||||||
continue |
|
||||||
|
|
||||||
# A platform can include multiple car models |
|
||||||
for _car_docs in car_docs: |
|
||||||
if not hasattr(_car_docs, "row"): |
|
||||||
_car_docs.init_make(CP) |
|
||||||
_car_docs.init(CP, footnotes) |
|
||||||
all_car_docs.append(_car_docs) |
|
||||||
|
|
||||||
# Sort cars by make and model + year |
|
||||||
sorted_cars: list[CarDocs] = natsorted(all_car_docs, key=lambda car: car.name.lower()) |
|
||||||
return sorted_cars |
|
||||||
|
|
||||||
|
|
||||||
def group_by_make(all_car_docs: list[CarDocs]) -> dict[str, list[CarDocs]]: |
|
||||||
sorted_car_docs = defaultdict(list) |
|
||||||
for car_docs in all_car_docs: |
|
||||||
sorted_car_docs[car_docs.make].append(car_docs) |
|
||||||
return dict(sorted_car_docs) |
|
||||||
|
|
||||||
|
|
||||||
def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str) -> str: |
|
||||||
with open(template_fn) as f: |
|
||||||
template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True) |
|
||||||
|
|
||||||
footnotes = [fn.value.text for fn in get_all_footnotes()] |
|
||||||
cars_md: str = template.render(all_car_docs=all_car_docs, PartType=PartType, |
|
||||||
group_by_make=group_by_make, footnotes=footnotes, |
|
||||||
Column=Column) |
|
||||||
return cars_md |
|
||||||
|
|||||||
@ -1,368 +0,0 @@ |
|||||||
import re |
|
||||||
from collections import namedtuple |
|
||||||
import copy |
|
||||||
from dataclasses import dataclass, field |
|
||||||
from enum import Enum |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
|
|
||||||
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_a = BaseCarHarness("VW A connector") |
|
||||||
vw_j533 = BaseCarHarness("VW 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", parts=[Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft, Cable.long_obdc_cable, |
|
||||||
Cable.usbc_coupler]) |
|
||||||
|
|
||||||
|
|
||||||
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: list[EnumBase] = None, remove: 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 CarDocs: |
|
||||||
# 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: str | None = None |
|
||||||
|
|
||||||
video_link: str | None = None |
|
||||||
footnotes: list[Enum] = field(default_factory=list) |
|
||||||
min_steer_speed: float | None = None |
|
||||||
min_enable_speed: float | None = None |
|
||||||
auto_resume: bool | None = 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: 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.5, f"{CP.carFingerprint}: Minimum steer speed set in both CarDocs 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 and self.min_enable_speed <= 0 |
|
||||||
|
|
||||||
# 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, 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: CarParams): |
|
||||||
"""CarDocs 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 and not 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: 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 |
|
||||||
@ -1,56 +0,0 @@ |
|||||||
import time |
|
||||||
|
|
||||||
from panda.python.uds import SERVICE_TYPE |
|
||||||
from openpilot.selfdrive.car import make_tester_present_msg, carlog |
|
||||||
from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType |
|
||||||
|
|
||||||
|
|
||||||
def _is_tester_present_response(msg: CanData, subaddr: 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(can_recv: CanRecvCallable, can_send: CanSendCallable, 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(can_recv, can_send, queries, responses, timeout=timeout, debug=debug) |
|
||||||
|
|
||||||
|
|
||||||
def get_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, 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] |
|
||||||
|
|
||||||
can_recv() |
|
||||||
can_send(msgs) |
|
||||||
start_time = time.monotonic() |
|
||||||
while time.monotonic() - start_time < timeout: |
|
||||||
can_packets = can_recv(wait_for_one=True) |
|
||||||
for packet in can_packets: |
|
||||||
for msg in packet: |
|
||||||
if not len(msg.dat): |
|
||||||
carlog.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: |
|
||||||
carlog.exception("ECU addr scan exception") |
|
||||||
return ecu_responses |
|
||||||
@ -1,344 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.interfaces import get_interface_attr |
|
||||||
from openpilot.selfdrive.car.body.values import CAR as BODY |
|
||||||
from openpilot.selfdrive.car.chrysler.values import CAR as CHRYSLER |
|
||||||
from openpilot.selfdrive.car.ford.values import CAR as FORD |
|
||||||
from openpilot.selfdrive.car.gm.values import CAR as GM |
|
||||||
from openpilot.selfdrive.car.honda.values import CAR as HONDA |
|
||||||
from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI |
|
||||||
from openpilot.selfdrive.car.mazda.values import CAR as MAZDA |
|
||||||
from openpilot.selfdrive.car.mock.values import CAR as MOCK |
|
||||||
from openpilot.selfdrive.car.nissan.values import CAR as NISSAN |
|
||||||
from openpilot.selfdrive.car.subaru.values import CAR as SUBARU |
|
||||||
from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA |
|
||||||
from openpilot.selfdrive.car.volkswagen.values import CAR as VW |
|
||||||
|
|
||||||
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()) |
|
||||||
|
|
||||||
|
|
||||||
# A dict that maps old platform strings to their latest representations |
|
||||||
MIGRATION = { |
|
||||||
"ACURA ILX 2016 ACURAWATCH PLUS": HONDA.ACURA_ILX, |
|
||||||
"ACURA RDX 2018 ACURAWATCH PLUS": HONDA.ACURA_RDX, |
|
||||||
"ACURA RDX 2020 TECH": HONDA.ACURA_RDX_3G, |
|
||||||
"AUDI A3": VW.AUDI_A3_MK3, |
|
||||||
"HONDA ACCORD 2018 HYBRID TOURING": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA ACCORD 1.5T 2018": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA ACCORD 2018 LX 1.5T": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA ACCORD 2018 SPORT 2T": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA ACCORD 2T 2018": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA ACCORD HYBRID 2018": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA CIVIC 2016 TOURING": HONDA.HONDA_CIVIC, |
|
||||||
"HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019": HONDA.HONDA_CIVIC_BOSCH, |
|
||||||
"HONDA CIVIC SEDAN 1.6 DIESEL": HONDA.HONDA_CIVIC_BOSCH_DIESEL, |
|
||||||
"HONDA CR-V 2016 EXECUTIVE": HONDA.HONDA_CRV_EU, |
|
||||||
"HONDA CR-V 2016 TOURING": HONDA.HONDA_CRV, |
|
||||||
"HONDA CR-V 2017 EX": HONDA.HONDA_CRV_5G, |
|
||||||
"HONDA CR-V 2019 HYBRID": HONDA.HONDA_CRV_HYBRID, |
|
||||||
"HONDA FIT 2018 EX": HONDA.HONDA_FIT, |
|
||||||
"HONDA HRV 2019 TOURING": HONDA.HONDA_HRV, |
|
||||||
"HONDA INSIGHT 2019 TOURING": HONDA.HONDA_INSIGHT, |
|
||||||
"HONDA ODYSSEY 2018 EX-L": HONDA.HONDA_ODYSSEY, |
|
||||||
"HONDA ODYSSEY 2019 EXCLUSIVE CHN": HONDA.HONDA_ODYSSEY_CHN, |
|
||||||
"HONDA PILOT 2017 TOURING": HONDA.HONDA_PILOT, |
|
||||||
"HONDA PILOT 2019 ELITE": HONDA.HONDA_PILOT, |
|
||||||
"HONDA PILOT 2019": HONDA.HONDA_PILOT, |
|
||||||
"HONDA PASSPORT 2021": HONDA.HONDA_PILOT, |
|
||||||
"HONDA RIDGELINE 2017 BLACK EDITION": HONDA.HONDA_RIDGELINE, |
|
||||||
"HYUNDAI ELANTRA LIMITED ULTIMATE 2017": HYUNDAI.HYUNDAI_ELANTRA, |
|
||||||
"HYUNDAI SANTA FE LIMITED 2019": HYUNDAI.HYUNDAI_SANTA_FE, |
|
||||||
"HYUNDAI TUCSON DIESEL 2019": HYUNDAI.HYUNDAI_TUCSON, |
|
||||||
"KIA OPTIMA 2016": HYUNDAI.KIA_OPTIMA_G4, |
|
||||||
"KIA OPTIMA 2019": HYUNDAI.KIA_OPTIMA_G4_FL, |
|
||||||
"KIA OPTIMA SX 2019 & 2016": HYUNDAI.KIA_OPTIMA_G4_FL, |
|
||||||
"LEXUS CT 200H 2018": TOYOTA.LEXUS_CTH, |
|
||||||
"LEXUS ES 300H 2018": TOYOTA.LEXUS_ES, |
|
||||||
"LEXUS ES 300H 2019": TOYOTA.LEXUS_ES_TSS2, |
|
||||||
"LEXUS IS300 2018": TOYOTA.LEXUS_IS, |
|
||||||
"LEXUS NX300 2018": TOYOTA.LEXUS_NX, |
|
||||||
"LEXUS NX300H 2018": TOYOTA.LEXUS_NX, |
|
||||||
"LEXUS RX 350 2016": TOYOTA.LEXUS_RX, |
|
||||||
"LEXUS RX350 2020": TOYOTA.LEXUS_RX_TSS2, |
|
||||||
"LEXUS RX450 HYBRID 2020": TOYOTA.LEXUS_RX_TSS2, |
|
||||||
"TOYOTA SIENNA XLE 2018": TOYOTA.TOYOTA_SIENNA, |
|
||||||
"TOYOTA C-HR HYBRID 2018": TOYOTA.TOYOTA_CHR, |
|
||||||
"TOYOTA COROLLA HYBRID TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2, |
|
||||||
"TOYOTA RAV4 HYBRID 2019": TOYOTA.TOYOTA_RAV4_TSS2, |
|
||||||
"LEXUS ES HYBRID 2019": TOYOTA.LEXUS_ES_TSS2, |
|
||||||
"LEXUS NX HYBRID 2018": TOYOTA.LEXUS_NX, |
|
||||||
"LEXUS NX HYBRID 2020": TOYOTA.LEXUS_NX_TSS2, |
|
||||||
"LEXUS RX HYBRID 2020": TOYOTA.LEXUS_RX_TSS2, |
|
||||||
"TOYOTA ALPHARD HYBRID 2021": TOYOTA.TOYOTA_ALPHARD_TSS2, |
|
||||||
"TOYOTA AVALON HYBRID 2019": TOYOTA.TOYOTA_AVALON_2019, |
|
||||||
"TOYOTA AVALON HYBRID 2022": TOYOTA.TOYOTA_AVALON_TSS2, |
|
||||||
"TOYOTA CAMRY HYBRID 2018": TOYOTA.TOYOTA_CAMRY, |
|
||||||
"TOYOTA CAMRY HYBRID 2021": TOYOTA.TOYOTA_CAMRY_TSS2, |
|
||||||
"TOYOTA C-HR HYBRID 2022": TOYOTA.TOYOTA_CHR_TSS2, |
|
||||||
"TOYOTA HIGHLANDER HYBRID 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2, |
|
||||||
"TOYOTA RAV4 HYBRID 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022, |
|
||||||
"TOYOTA RAV4 HYBRID 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023, |
|
||||||
"TOYOTA HIGHLANDER HYBRID 2018": TOYOTA.TOYOTA_HIGHLANDER, |
|
||||||
"LEXUS ES HYBRID 2018": TOYOTA.LEXUS_ES, |
|
||||||
"LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX, |
|
||||||
"HYUNDAI TUCSON HYBRID 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN, |
|
||||||
"KIA SPORTAGE HYBRID 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, |
|
||||||
"KIA SORENTO PLUG-IN HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, |
|
||||||
"CADILLAC ESCALADE ESV PLATINUM 2019": GM.CADILLAC_ESCALADE_ESV_2019, |
|
||||||
|
|
||||||
# Removal of platform_str, see https://github.com/commaai/openpilot/pull/31868/ |
|
||||||
"COMMA BODY": BODY.COMMA_BODY, |
|
||||||
"CHRYSLER PACIFICA HYBRID 2017": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID, |
|
||||||
"CHRYSLER_PACIFICA_2017_HYBRID": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID, |
|
||||||
"CHRYSLER PACIFICA HYBRID 2018": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID, |
|
||||||
"CHRYSLER PACIFICA HYBRID 2019": CHRYSLER.CHRYSLER_PACIFICA_2019_HYBRID, |
|
||||||
"CHRYSLER PACIFICA 2018": CHRYSLER.CHRYSLER_PACIFICA_2018, |
|
||||||
"CHRYSLER PACIFICA 2020": CHRYSLER.CHRYSLER_PACIFICA_2020, |
|
||||||
"DODGE DURANGO 2021": CHRYSLER.DODGE_DURANGO, |
|
||||||
"JEEP GRAND CHEROKEE V6 2018": CHRYSLER.JEEP_GRAND_CHEROKEE, |
|
||||||
"JEEP GRAND CHEROKEE 2019": CHRYSLER.JEEP_GRAND_CHEROKEE_2019, |
|
||||||
"RAM 1500 5TH GEN": CHRYSLER.RAM_1500_5TH_GEN, |
|
||||||
"RAM HD 5TH GEN": CHRYSLER.RAM_HD_5TH_GEN, |
|
||||||
"FORD BRONCO SPORT 1ST GEN": FORD.FORD_BRONCO_SPORT_MK1, |
|
||||||
"FORD ESCAPE 4TH GEN": FORD.FORD_ESCAPE_MK4, |
|
||||||
"FORD EXPLORER 6TH GEN": FORD.FORD_EXPLORER_MK6, |
|
||||||
"FORD F-150 14TH GEN": FORD.FORD_F_150_MK14, |
|
||||||
"FORD F-150 LIGHTNING 1ST GEN": FORD.FORD_F_150_LIGHTNING_MK1, |
|
||||||
"FORD FOCUS 4TH GEN": FORD.FORD_FOCUS_MK4, |
|
||||||
"FORD MAVERICK 1ST GEN": FORD.FORD_MAVERICK_MK1, |
|
||||||
"FORD MUSTANG MACH-E 1ST GEN": FORD.FORD_MUSTANG_MACH_E_MK1, |
|
||||||
"HOLDEN ASTRA RS-V BK 2017": GM.HOLDEN_ASTRA, |
|
||||||
"CHEVROLET VOLT PREMIER 2017": GM.CHEVROLET_VOLT, |
|
||||||
"CADILLAC ATS Premium Performance 2018": GM.CADILLAC_ATS, |
|
||||||
"CHEVROLET MALIBU PREMIER 2017": GM.CHEVROLET_MALIBU, |
|
||||||
"GMC ACADIA DENALI 2018": GM.GMC_ACADIA, |
|
||||||
"BUICK LACROSSE 2017": GM.BUICK_LACROSSE, |
|
||||||
"BUICK REGAL ESSENCE 2018": GM.BUICK_REGAL, |
|
||||||
"CADILLAC ESCALADE 2017": GM.CADILLAC_ESCALADE, |
|
||||||
"CADILLAC ESCALADE ESV 2016": GM.CADILLAC_ESCALADE_ESV, |
|
||||||
"CADILLAC ESCALADE ESV 2019": GM.CADILLAC_ESCALADE_ESV_2019, |
|
||||||
"CHEVROLET BOLT EUV 2022": GM.CHEVROLET_BOLT_EUV, |
|
||||||
"CHEVROLET SILVERADO 1500 2020": GM.CHEVROLET_SILVERADO, |
|
||||||
"CHEVROLET EQUINOX 2019": GM.CHEVROLET_EQUINOX, |
|
||||||
"CHEVROLET TRAILBLAZER 2021": GM.CHEVROLET_TRAILBLAZER, |
|
||||||
"HONDA ACCORD 2018": HONDA.HONDA_ACCORD, |
|
||||||
"HONDA CIVIC (BOSCH) 2019": HONDA.HONDA_CIVIC_BOSCH, |
|
||||||
"HONDA CIVIC SEDAN 1.6 DIESEL 2019": HONDA.HONDA_CIVIC_BOSCH_DIESEL, |
|
||||||
"HONDA CIVIC 2022": HONDA.HONDA_CIVIC_2022, |
|
||||||
"HONDA CR-V 2017": HONDA.HONDA_CRV_5G, |
|
||||||
"HONDA CR-V HYBRID 2019": HONDA.HONDA_CRV_HYBRID, |
|
||||||
"HONDA HR-V 2023": HONDA.HONDA_HRV_3G, |
|
||||||
"ACURA RDX 2020": HONDA.ACURA_RDX_3G, |
|
||||||
"HONDA INSIGHT 2019": HONDA.HONDA_INSIGHT, |
|
||||||
"HONDA E 2020": HONDA.HONDA_E, |
|
||||||
"ACURA ILX 2016": HONDA.ACURA_ILX, |
|
||||||
"HONDA CR-V 2016": HONDA.HONDA_CRV, |
|
||||||
"HONDA CR-V EU 2016": HONDA.HONDA_CRV_EU, |
|
||||||
"HONDA FIT 2018": HONDA.HONDA_FIT, |
|
||||||
"HONDA FREED 2020": HONDA.HONDA_FREED, |
|
||||||
"HONDA HRV 2019": HONDA.HONDA_HRV, |
|
||||||
"HONDA ODYSSEY 2018": HONDA.HONDA_ODYSSEY, |
|
||||||
"HONDA ODYSSEY CHN 2019": HONDA.HONDA_ODYSSEY_CHN, |
|
||||||
"ACURA RDX 2018": HONDA.ACURA_RDX, |
|
||||||
"HONDA PILOT 2017": HONDA.HONDA_PILOT, |
|
||||||
"HONDA RIDGELINE 2017": HONDA.HONDA_RIDGELINE, |
|
||||||
"HONDA CIVIC 2016": HONDA.HONDA_CIVIC, |
|
||||||
"HYUNDAI AZERA 6TH GEN": HYUNDAI.HYUNDAI_AZERA_6TH_GEN, |
|
||||||
"HYUNDAI AZERA HYBRID 6TH GEN": HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN, |
|
||||||
"HYUNDAI ELANTRA 2017": HYUNDAI.HYUNDAI_ELANTRA, |
|
||||||
"HYUNDAI I30 N LINE 2019 & GT 2018 DCT": HYUNDAI.HYUNDAI_ELANTRA_GT_I30, |
|
||||||
"HYUNDAI ELANTRA 2021": HYUNDAI.HYUNDAI_ELANTRA_2021, |
|
||||||
"HYUNDAI ELANTRA HYBRID 2021": HYUNDAI.HYUNDAI_ELANTRA_HEV_2021, |
|
||||||
"HYUNDAI GENESIS 2015-2016": HYUNDAI.HYUNDAI_GENESIS, |
|
||||||
"HYUNDAI IONIQ HYBRID 2017-2019": HYUNDAI.HYUNDAI_IONIQ, |
|
||||||
"HYUNDAI IONIQ HYBRID 2020-2022": HYUNDAI.HYUNDAI_IONIQ_HEV_2022, |
|
||||||
"HYUNDAI IONIQ ELECTRIC LIMITED 2019": HYUNDAI.HYUNDAI_IONIQ_EV_LTD, |
|
||||||
"HYUNDAI IONIQ ELECTRIC 2020": HYUNDAI.HYUNDAI_IONIQ_EV_2020, |
|
||||||
"HYUNDAI IONIQ PLUG-IN HYBRID 2019": HYUNDAI.HYUNDAI_IONIQ_PHEV_2019, |
|
||||||
"HYUNDAI IONIQ PHEV 2020": HYUNDAI.HYUNDAI_IONIQ_PHEV, |
|
||||||
"HYUNDAI KONA 2020": HYUNDAI.HYUNDAI_KONA, |
|
||||||
"HYUNDAI KONA ELECTRIC 2019": HYUNDAI.HYUNDAI_KONA_EV, |
|
||||||
"HYUNDAI KONA ELECTRIC 2022": HYUNDAI.HYUNDAI_KONA_EV_2022, |
|
||||||
"HYUNDAI KONA ELECTRIC 2ND GEN": HYUNDAI.HYUNDAI_KONA_EV_2ND_GEN, |
|
||||||
"HYUNDAI KONA HYBRID 2020": HYUNDAI.HYUNDAI_KONA_HEV, |
|
||||||
"HYUNDAI SANTA FE 2019": HYUNDAI.HYUNDAI_SANTA_FE, |
|
||||||
"HYUNDAI SANTA FE 2022": HYUNDAI.HYUNDAI_SANTA_FE_2022, |
|
||||||
"HYUNDAI SANTA FE HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_HEV_2022, |
|
||||||
"HYUNDAI SANTA FE PlUG-IN HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022, |
|
||||||
"HYUNDAI SONATA 2020": HYUNDAI.HYUNDAI_SONATA, |
|
||||||
"HYUNDAI SONATA 2019": HYUNDAI.HYUNDAI_SONATA_LF, |
|
||||||
"HYUNDAI STARIA 4TH GEN": HYUNDAI.HYUNDAI_STARIA_4TH_GEN, |
|
||||||
"HYUNDAI TUCSON 2019": HYUNDAI.HYUNDAI_TUCSON, |
|
||||||
"HYUNDAI PALISADE 2020": HYUNDAI.HYUNDAI_PALISADE, |
|
||||||
"HYUNDAI VELOSTER 2019": HYUNDAI.HYUNDAI_VELOSTER, |
|
||||||
"HYUNDAI SONATA HYBRID 2021": HYUNDAI.HYUNDAI_SONATA_HYBRID, |
|
||||||
"HYUNDAI IONIQ 5 2022": HYUNDAI.HYUNDAI_IONIQ_5, |
|
||||||
"HYUNDAI IONIQ 6 2023": HYUNDAI.HYUNDAI_IONIQ_6, |
|
||||||
"HYUNDAI TUCSON 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN, |
|
||||||
"HYUNDAI SANTA CRUZ 1ST GEN": HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, |
|
||||||
"HYUNDAI CUSTIN 1ST GEN": HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN, |
|
||||||
"KIA FORTE E 2018 & GT 2021": HYUNDAI.KIA_FORTE, |
|
||||||
"KIA K5 2021": HYUNDAI.KIA_K5_2021, |
|
||||||
"KIA K5 HYBRID 2020": HYUNDAI.KIA_K5_HEV_2020, |
|
||||||
"KIA K8 HYBRID 1ST GEN": HYUNDAI.KIA_K8_HEV_1ST_GEN, |
|
||||||
"KIA NIRO EV 2020": HYUNDAI.KIA_NIRO_EV, |
|
||||||
"KIA NIRO EV 2ND GEN": HYUNDAI.KIA_NIRO_EV_2ND_GEN, |
|
||||||
"KIA NIRO HYBRID 2019": HYUNDAI.KIA_NIRO_PHEV, |
|
||||||
"KIA NIRO PLUG-IN HYBRID 2022": HYUNDAI.KIA_NIRO_PHEV_2022, |
|
||||||
"KIA NIRO HYBRID 2021": HYUNDAI.KIA_NIRO_HEV_2021, |
|
||||||
"KIA NIRO HYBRID 2ND GEN": HYUNDAI.KIA_NIRO_HEV_2ND_GEN, |
|
||||||
"KIA OPTIMA 4TH GEN": HYUNDAI.KIA_OPTIMA_G4, |
|
||||||
"KIA OPTIMA 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_G4_FL, |
|
||||||
"KIA OPTIMA HYBRID 2017 & SPORTS 2019": HYUNDAI.KIA_OPTIMA_H, |
|
||||||
"KIA OPTIMA HYBRID 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_H_G4_FL, |
|
||||||
"KIA SELTOS 2021": HYUNDAI.KIA_SELTOS, |
|
||||||
"KIA SPORTAGE 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, |
|
||||||
"KIA SORENTO GT LINE 2018": HYUNDAI.KIA_SORENTO, |
|
||||||
"KIA SORENTO 4TH GEN": HYUNDAI.KIA_SORENTO_4TH_GEN, |
|
||||||
"KIA SORENTO HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, |
|
||||||
"KIA STINGER GT2 2018": HYUNDAI.KIA_STINGER, |
|
||||||
"KIA STINGER 2022": HYUNDAI.KIA_STINGER_2022, |
|
||||||
"KIA CEED INTRO ED 2019": HYUNDAI.KIA_CEED, |
|
||||||
"KIA EV6 2022": HYUNDAI.KIA_EV6, |
|
||||||
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN, |
|
||||||
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN, |
|
||||||
"GENESIS G70 2018": HYUNDAI.GENESIS_G70, |
|
||||||
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020, |
|
||||||
"GENESIS GV70 1ST GEN": HYUNDAI.GENESIS_GV70_1ST_GEN, |
|
||||||
"GENESIS G80 2017": HYUNDAI.GENESIS_G80, |
|
||||||
"GENESIS G90 2017": HYUNDAI.GENESIS_G90, |
|
||||||
"GENESIS GV80 2023": HYUNDAI.GENESIS_GV80, |
|
||||||
"MAZDA CX-5": MAZDA.MAZDA_CX5, |
|
||||||
"MAZDA CX-9": MAZDA.MAZDA_CX9, |
|
||||||
"MAZDA 3": MAZDA.MAZDA_3, |
|
||||||
"MAZDA 6": MAZDA.MAZDA_6, |
|
||||||
"MAZDA CX-9 2021": MAZDA.MAZDA_CX9_2021, |
|
||||||
"MAZDA CX-5 2022": MAZDA.MAZDA_CX5_2022, |
|
||||||
"NISSAN X-TRAIL 2017": NISSAN.NISSAN_XTRAIL, |
|
||||||
"NISSAN LEAF 2018": NISSAN.NISSAN_LEAF, |
|
||||||
"NISSAN LEAF 2018 Instrument Cluster": NISSAN.NISSAN_LEAF_IC, |
|
||||||
"NISSAN ROGUE 2019": NISSAN.NISSAN_ROGUE, |
|
||||||
"NISSAN ALTIMA 2020": NISSAN.NISSAN_ALTIMA, |
|
||||||
"SUBARU ASCENT LIMITED 2019": SUBARU.SUBARU_ASCENT, |
|
||||||
"SUBARU OUTBACK 6TH GEN": SUBARU.SUBARU_OUTBACK, |
|
||||||
"SUBARU LEGACY 7TH GEN": SUBARU.SUBARU_LEGACY, |
|
||||||
"SUBARU IMPREZA LIMITED 2019": SUBARU.SUBARU_IMPREZA, |
|
||||||
"SUBARU IMPREZA SPORT 2020": SUBARU.SUBARU_IMPREZA_2020, |
|
||||||
"SUBARU CROSSTREK HYBRID 2020": SUBARU.SUBARU_CROSSTREK_HYBRID, |
|
||||||
"SUBARU FORESTER 2019": SUBARU.SUBARU_FORESTER, |
|
||||||
"SUBARU FORESTER HYBRID 2020": SUBARU.SUBARU_FORESTER_HYBRID, |
|
||||||
"SUBARU FORESTER 2017 - 2018": SUBARU.SUBARU_FORESTER_PREGLOBAL, |
|
||||||
"SUBARU LEGACY 2015 - 2018": SUBARU.SUBARU_LEGACY_PREGLOBAL, |
|
||||||
"SUBARU OUTBACK 2015 - 2017": SUBARU.SUBARU_OUTBACK_PREGLOBAL, |
|
||||||
"SUBARU OUTBACK 2018 - 2019": SUBARU.SUBARU_OUTBACK_PREGLOBAL_2018, |
|
||||||
"SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022, |
|
||||||
"SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023, |
|
||||||
"SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023, |
|
||||||
"TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2, |
|
||||||
"TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON, |
|
||||||
"TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019, |
|
||||||
"TOYOTA AVALON 2022": TOYOTA.TOYOTA_AVALON_TSS2, |
|
||||||
"TOYOTA CAMRY 2018": TOYOTA.TOYOTA_CAMRY, |
|
||||||
"TOYOTA CAMRY 2021": TOYOTA.TOYOTA_CAMRY_TSS2, |
|
||||||
"TOYOTA C-HR 2018": TOYOTA.TOYOTA_CHR, |
|
||||||
"TOYOTA C-HR 2021": TOYOTA.TOYOTA_CHR_TSS2, |
|
||||||
"TOYOTA COROLLA 2017": TOYOTA.TOYOTA_COROLLA, |
|
||||||
"TOYOTA COROLLA TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2, |
|
||||||
"TOYOTA HIGHLANDER 2017": TOYOTA.TOYOTA_HIGHLANDER, |
|
||||||
"TOYOTA HIGHLANDER 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2, |
|
||||||
"TOYOTA PRIUS 2017": TOYOTA.TOYOTA_PRIUS, |
|
||||||
"TOYOTA PRIUS v 2017": TOYOTA.TOYOTA_PRIUS_V, |
|
||||||
"TOYOTA PRIUS TSS2 2021": TOYOTA.TOYOTA_PRIUS_TSS2, |
|
||||||
"TOYOTA RAV4 2017": TOYOTA.TOYOTA_RAV4, |
|
||||||
"TOYOTA RAV4 HYBRID 2017": TOYOTA.TOYOTA_RAV4H, |
|
||||||
"TOYOTA RAV4 2019": TOYOTA.TOYOTA_RAV4_TSS2, |
|
||||||
"TOYOTA RAV4 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022, |
|
||||||
"TOYOTA RAV4 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023, |
|
||||||
"TOYOTA MIRAI 2021": TOYOTA.TOYOTA_MIRAI, |
|
||||||
"TOYOTA SIENNA 2018": TOYOTA.TOYOTA_SIENNA, |
|
||||||
"LEXUS CT HYBRID 2018": TOYOTA.LEXUS_CTH, |
|
||||||
"LEXUS ES 2018": TOYOTA.LEXUS_ES, |
|
||||||
"LEXUS ES 2019": TOYOTA.LEXUS_ES_TSS2, |
|
||||||
"LEXUS IS 2018": TOYOTA.LEXUS_IS, |
|
||||||
"LEXUS IS 2023": TOYOTA.LEXUS_IS_TSS2, |
|
||||||
"LEXUS NX 2018": TOYOTA.LEXUS_NX, |
|
||||||
"LEXUS NX 2020": TOYOTA.LEXUS_NX_TSS2, |
|
||||||
"LEXUS LC 2024": TOYOTA.LEXUS_LC_TSS2, |
|
||||||
"LEXUS RC 2020": TOYOTA.LEXUS_RC, |
|
||||||
"LEXUS RX 2016": TOYOTA.LEXUS_RX, |
|
||||||
"LEXUS RX 2020": TOYOTA.LEXUS_RX_TSS2, |
|
||||||
"LEXUS GS F 2016": TOYOTA.LEXUS_GS_F, |
|
||||||
"VOLKSWAGEN ARTEON 1ST GEN": VW.VOLKSWAGEN_ARTEON_MK1, |
|
||||||
"VOLKSWAGEN ATLAS 1ST GEN": VW.VOLKSWAGEN_ATLAS_MK1, |
|
||||||
"VOLKSWAGEN CADDY 3RD GEN": VW.VOLKSWAGEN_CADDY_MK3, |
|
||||||
"VOLKSWAGEN CRAFTER 2ND GEN": VW.VOLKSWAGEN_CRAFTER_MK2, |
|
||||||
"VOLKSWAGEN GOLF 7TH GEN": VW.VOLKSWAGEN_GOLF_MK7, |
|
||||||
"VOLKSWAGEN JETTA 7TH GEN": VW.VOLKSWAGEN_JETTA_MK7, |
|
||||||
"VOLKSWAGEN PASSAT 8TH GEN": VW.VOLKSWAGEN_PASSAT_MK8, |
|
||||||
"VOLKSWAGEN PASSAT NMS": VW.VOLKSWAGEN_PASSAT_NMS, |
|
||||||
"VOLKSWAGEN POLO 6TH GEN": VW.VOLKSWAGEN_POLO_MK6, |
|
||||||
"VOLKSWAGEN SHARAN 2ND GEN": VW.VOLKSWAGEN_SHARAN_MK2, |
|
||||||
"VOLKSWAGEN TAOS 1ST GEN": VW.VOLKSWAGEN_TAOS_MK1, |
|
||||||
"VOLKSWAGEN T-CROSS 1ST GEN": VW.VOLKSWAGEN_TCROSS_MK1, |
|
||||||
"VOLKSWAGEN TIGUAN 2ND GEN": VW.VOLKSWAGEN_TIGUAN_MK2, |
|
||||||
"VOLKSWAGEN TOURAN 2ND GEN": VW.VOLKSWAGEN_TOURAN_MK2, |
|
||||||
"VOLKSWAGEN TRANSPORTER T6.1": VW.VOLKSWAGEN_TRANSPORTER_T61, |
|
||||||
"VOLKSWAGEN T-ROC 1ST GEN": VW.VOLKSWAGEN_TROC_MK1, |
|
||||||
"AUDI A3 3RD GEN": VW.AUDI_A3_MK3, |
|
||||||
"AUDI Q2 1ST GEN": VW.AUDI_Q2_MK1, |
|
||||||
"AUDI Q3 2ND GEN": VW.AUDI_Q3_MK2, |
|
||||||
"SEAT ATECA 1ST GEN": VW.SEAT_ATECA_MK1, |
|
||||||
"SEAT LEON 3RD GEN": VW.SEAT_ATECA_MK1, |
|
||||||
"SEAT_LEON_MK3": VW.SEAT_ATECA_MK1, |
|
||||||
"SKODA FABIA 4TH GEN": VW.SKODA_FABIA_MK4, |
|
||||||
"SKODA KAMIQ 1ST GEN": VW.SKODA_KAMIQ_MK1, |
|
||||||
"SKODA KAROQ 1ST GEN": VW.SKODA_KAROQ_MK1, |
|
||||||
"SKODA KODIAQ 1ST GEN": VW.SKODA_KODIAQ_MK1, |
|
||||||
"SKODA OCTAVIA 3RD GEN": VW.SKODA_OCTAVIA_MK3, |
|
||||||
"SKODA SCALA 1ST GEN": VW.SKODA_KAMIQ_MK1, |
|
||||||
"SKODA_SCALA_MK1": VW.SKODA_KAMIQ_MK1, |
|
||||||
"SKODA SUPERB 3RD GEN": VW.SKODA_SUPERB_MK3, |
|
||||||
|
|
||||||
"mock": MOCK.MOCK, |
|
||||||
} |
|
||||||
@ -1,118 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import apply_std_steer_angle_limits, structs |
|
||||||
from openpilot.selfdrive.car.ford import fordcan |
|
||||||
from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase, V_CRUISE_MAX |
|
||||||
|
|
||||||
LongCtrlState = structs.CarControl.Actuators.LongControlState |
|
||||||
VisualAlert = structs.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(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.packer = CANPacker(dbc_name) |
|
||||||
self.CAN = fordcan.CanBus(CP) |
|
||||||
|
|
||||||
self.apply_curvature_last = 0 |
|
||||||
self.main_on_last = False |
|
||||||
self.lkas_enabled_last = False |
|
||||||
self.steer_alert_last = False |
|
||||||
self.lead_distance_bars_last = None |
|
||||||
|
|
||||||
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.flags & FordFlags.CANFD: |
|
||||||
# TODO: extended mode |
|
||||||
mode = 1 if CC.latActive else 0 |
|
||||||
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 |
|
||||||
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 |
|
||||||
# TODO: look into using the actuators packet to send the desired speed |
|
||||||
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=V_CRUISE_MAX)) |
|
||||||
|
|
||||||
### 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 hud_control.leadDistanceBars != self.lead_distance_bars_last: |
|
||||||
send_ui = True |
|
||||||
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 |
|
||||||
self.lead_distance_bars_last = hud_control.leadDistanceBars |
|
||||||
|
|
||||||
new_actuators = copy.copy(actuators) |
|
||||||
new_actuators.curvature = self.apply_curvature_last |
|
||||||
|
|
||||||
self.frame += 1 |
|
||||||
return new_actuators, can_sends |
|
||||||
@ -1,174 +0,0 @@ |
|||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.ford.fordcan import CanBus |
|
||||||
from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
GearShifter = structs.CarState.GearShifter |
|
||||||
TransmissionType = structs.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["PowertrainData_10"]["TrnRng_D_Rq"] |
|
||||||
|
|
||||||
self.distance_button = 0 |
|
||||||
|
|
||||||
def update(self, cp, cp_cam, *_) -> structs.CarState: |
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
# 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 |
|
||||||
ret.vehicleSensorsInvalid = 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 = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0 # 0 is default mode |
|
||||||
|
|
||||||
if self.CP.flags & FordFlags.CANFD: |
|
||||||
# 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 |
|
||||||
is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False |
|
||||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else 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["PowertrainData_10"]["TrnRng_D_Rq"]) |
|
||||||
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"]) |
|
||||||
prev_distance_button = self.distance_button |
|
||||||
self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"] |
|
||||||
|
|
||||||
# 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.flags & FordFlags.CANFD 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"] |
|
||||||
|
|
||||||
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) |
|
||||||
|
|
||||||
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.flags & FordFlags.CANFD: |
|
||||||
messages += [ |
|
||||||
("Lane_Assist_Data3_FD1", 33), |
|
||||||
] |
|
||||||
else: |
|
||||||
messages += [ |
|
||||||
("INSTRUMENT_PANEL", 1), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.transmissionType == TransmissionType.automatic: |
|
||||||
messages += [ |
|
||||||
("PowertrainData_10", 10), |
|
||||||
] |
|
||||||
elif CP.transmissionType == TransmissionType.manual: |
|
||||||
messages += [ |
|
||||||
("Engine_Clutch_Data", 33), |
|
||||||
("BCM_Lamp_Stat_FD1", 1), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.enableBsm and not (CP.flags & FordFlags.CANFD): |
|
||||||
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.flags & FordFlags.CANFD: |
|
||||||
messages += [ |
|
||||||
("Side_Detect_L_Stat", 5), |
|
||||||
("Side_Detect_R_Stat", 5), |
|
||||||
] |
|
||||||
|
|
||||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).camera) |
|
||||||
@ -1,166 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.ford.values import CAR |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
FW_VERSIONS = { |
|
||||||
CAR.FORD_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', |
|
||||||
b'LX6C-14D003-AL\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', |
|
||||||
b'LX6C-2D053-RF\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', |
|
||||||
b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_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-AK\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', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_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-BJ\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-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_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'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_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', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_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', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_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', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_MAVERICK_MK1: { |
|
||||||
(Ecu.eps, 0x730, None): [ |
|
||||||
b'NZ6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x760, None): [ |
|
||||||
b'NZ6C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
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', |
|
||||||
b'PZ6C-2D053-EF\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', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.FORD_RANGER_MK2: { |
|
||||||
(Ecu.eps, 0x730, None): [ |
|
||||||
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.abs, 0x760, None): [ |
|
||||||
b'PB3C-2D053-ZD\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', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
@ -1,339 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import CanBusBase, structs |
|
||||||
|
|
||||||
HUDControl = structs.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" maneuvers, 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" maneuvers: 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)[1] |
|
||||||
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": -5.0, # 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": hud_control.leadDistanceBars, # Time gap |
|
||||||
}) |
|
||||||
|
|
||||||
# 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) |
|
||||||
@ -1,65 +0,0 @@ |
|||||||
from panda import Panda |
|
||||||
from openpilot.selfdrive.car import get_safety_config, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.ford.fordcan import CanBus |
|
||||||
from openpilot.selfdrive.car.ford.values import Ecu, FordFlags |
|
||||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase |
|
||||||
|
|
||||||
TransmissionType = structs.CarParams.TransmissionType |
|
||||||
|
|
||||||
|
|
||||||
class CarInterface(CarInterfaceBase): |
|
||||||
@staticmethod |
|
||||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
ret.carName = "ford" |
|
||||||
ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD) |
|
||||||
|
|
||||||
ret.radarUnavailable = True |
|
||||||
ret.steerControlType = structs.CarParams.SteerControlType.angle |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
ret.steerLimitTimer = 1.0 |
|
||||||
|
|
||||||
CAN = CanBus(fingerprint=fingerprint) |
|
||||||
cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)] |
|
||||||
if CAN.main >= 4: |
|
||||||
cfgs.insert(0, get_safety_config(structs.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 ret.flags & FordFlags.CANFD: |
|
||||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD |
|
||||||
else: |
|
||||||
# Lock out if the car does not have needed lateral and longitudinal control APIs. |
|
||||||
# Note that we also check CAN for adaptive cruise, but no known signal for LCA exists |
|
||||||
pscm_config = next((fw for fw in car_fw if fw.ecu == Ecu.eps and b'\x22\xDE\x01' in fw.request), None) |
|
||||||
if pscm_config: |
|
||||||
if len(pscm_config.fwVersion) != 24: |
|
||||||
ret.dashcamOnly = True |
|
||||||
else: |
|
||||||
config_tja = pscm_config.fwVersion[7] # Traffic Jam Assist |
|
||||||
config_lca = pscm_config.fwVersion[8] # Lane Centering Assist |
|
||||||
if config_tja != 0xFF or config_lca != 0xFF: |
|
||||||
ret.dashcamOnly = True |
|
||||||
|
|
||||||
# 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 |
|
||||||
@ -1,143 +0,0 @@ |
|||||||
from math import cos, sin |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.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 = structs.RadarData() |
|
||||||
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] = structs.RadarData.RadarPoint() |
|
||||||
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] = structs.RadarData.RadarPoint() |
|
||||||
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] |
|
||||||
@ -1,28 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from collections import defaultdict |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.ford.values import get_platform_codes |
|
||||||
from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
cars_for_code: defaultdict = defaultdict(lambda: defaultdict(set)) |
|
||||||
|
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
print(car_model) |
|
||||||
for ecu in sorted(ecus): |
|
||||||
platform_codes = get_platform_codes(ecus[ecu]) |
|
||||||
for code in platform_codes: |
|
||||||
cars_for_code[ecu][code].add(car_model) |
|
||||||
|
|
||||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):') |
|
||||||
print(f' Codes: {sorted(platform_codes)}') |
|
||||||
print() |
|
||||||
|
|
||||||
print('\nCar models vs. platform codes:') |
|
||||||
for ecu, codes in cars_for_code.items(): |
|
||||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):') |
|
||||||
for code, cars in codes.items(): |
|
||||||
print(f' {code!r}: {sorted(map(str, cars))}') |
|
||||||
@ -1,142 +0,0 @@ |
|||||||
import random |
|
||||||
from collections.abc import Iterable |
|
||||||
|
|
||||||
from hypothesis import settings, given, strategies as st |
|
||||||
from parameterized import parameterized |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.fw_versions import build_fw_dict |
|
||||||
from openpilot.selfdrive.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes |
|
||||||
from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
|
|
||||||
ECU_ADDRESSES = { |
|
||||||
Ecu.eps: 0x730, # Power Steering Control Module (PSCM) |
|
||||||
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS) |
|
||||||
Ecu.fwdRadar: 0x764, # Cruise Control Module (CCM) |
|
||||||
Ecu.fwdCamera: 0x706, # Image Processing Module A (IPMA) |
|
||||||
Ecu.engine: 0x7E0, # Powertrain Control Module (PCM) |
|
||||||
Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM) |
|
||||||
Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM) |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
ECU_PART_NUMBER = { |
|
||||||
Ecu.eps: [ |
|
||||||
b"14D003", |
|
||||||
], |
|
||||||
Ecu.abs: [ |
|
||||||
b"2D053", |
|
||||||
], |
|
||||||
Ecu.fwdRadar: [ |
|
||||||
b"14D049", |
|
||||||
], |
|
||||||
Ecu.fwdCamera: [ |
|
||||||
b"14F397", # Ford Q3 |
|
||||||
b"14H102", # Ford Q4 |
|
||||||
], |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
class TestFordFW: |
|
||||||
def test_fw_query_config(self): |
|
||||||
for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus: |
|
||||||
assert ecu in ECU_ADDRESSES, "Unknown ECU" |
|
||||||
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch" |
|
||||||
assert subaddr is None, "Unexpected ECU subaddress" |
|
||||||
|
|
||||||
@parameterized.expand(FW_VERSIONS.items()) |
|
||||||
def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[Ecu, int, int | None], Iterable[bytes]]): |
|
||||||
for (ecu, addr, subaddr), fws in fw_versions.items(): |
|
||||||
assert ecu in ECU_PART_NUMBER, "Unexpected ECU" |
|
||||||
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch" |
|
||||||
assert subaddr is None, "Unexpected ECU subaddress" |
|
||||||
|
|
||||||
for fw in fws: |
|
||||||
assert len(fw) == 24, "Expected ECU response to be 24 bytes" |
|
||||||
|
|
||||||
match = FW_PATTERN.match(fw) |
|
||||||
assert match is not None, f"Unable to parse FW: {fw!r}" |
|
||||||
if match: |
|
||||||
part_number = match.group("part_number") |
|
||||||
assert part_number in ECU_PART_NUMBER[ecu], f"Unexpected part number for {fw!r}" |
|
||||||
|
|
||||||
codes = get_platform_codes([fw]) |
|
||||||
assert 1 == len(codes), f"Unable to parse FW: {fw!r}" |
|
||||||
|
|
||||||
@settings(max_examples=100) |
|
||||||
@given(data=st.data()) |
|
||||||
def test_platform_codes_fuzzy_fw(self, data): |
|
||||||
"""Ensure function doesn't raise an exception""" |
|
||||||
fw_strategy = st.lists(st.binary()) |
|
||||||
fws = data.draw(fw_strategy) |
|
||||||
get_platform_codes(fws) |
|
||||||
|
|
||||||
def test_platform_codes_spot_check(self): |
|
||||||
# Asserts basic platform code parsing behavior for a few cases |
|
||||||
results = get_platform_codes([ |
|
||||||
b"JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
b"NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
b"PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
b"LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
]) |
|
||||||
assert results == {(b"X6A", b"J"), (b"Z6T", b"N"), (b"J6T", b"P"), (b"B5A", b"L")} |
|
||||||
|
|
||||||
def test_fuzzy_match(self): |
|
||||||
for platform, fw_by_addr in FW_VERSIONS.items(): |
|
||||||
# Ensure there's no overlaps in platform codes |
|
||||||
for _ in range(20): |
|
||||||
car_fw = [] |
|
||||||
for ecu, fw_versions in fw_by_addr.items(): |
|
||||||
ecu_name, addr, sub_addr = ecu |
|
||||||
fw = random.choice(fw_versions) |
|
||||||
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr, |
|
||||||
subAddress=0 if sub_addr is None else sub_addr)) |
|
||||||
|
|
||||||
CP = CarParams(carFw=car_fw) |
|
||||||
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS) |
|
||||||
assert matches == {platform} |
|
||||||
|
|
||||||
def test_match_fw_fuzzy(self): |
|
||||||
offline_fw = { |
|
||||||
(Ecu.eps, 0x730, None): [ |
|
||||||
b"L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
b"L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
], |
|
||||||
(Ecu.abs, 0x760, None): [ |
|
||||||
b"L1MC-2D053-BA\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", |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x764, None): [ |
|
||||||
b"LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
b"LB5T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
], |
|
||||||
# We consider all model year hints for ECU, even with different platform codes |
|
||||||
(Ecu.fwdCamera, 0x706, None): [ |
|
||||||
b"LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
b"NC5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", |
|
||||||
], |
|
||||||
} |
|
||||||
expected_fingerprint = CAR.FORD_EXPLORER_MK6 |
|
||||||
|
|
||||||
# ensure that we fuzzy match on all non-exact FW with changed revisions |
|
||||||
live_fw = { |
|
||||||
(0x730, None): {b"L1MC-14D003-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, |
|
||||||
(0x760, None): {b"L1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, |
|
||||||
(0x764, None): {b"LB5T-14D049-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, |
|
||||||
(0x706, None): {b"LB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, |
|
||||||
} |
|
||||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw}) |
|
||||||
assert candidates == {expected_fingerprint} |
|
||||||
|
|
||||||
# model year hint in between the range should match |
|
||||||
live_fw[(0x706, None)] = {b"MB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"} |
|
||||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw,}) |
|
||||||
assert candidates == {expected_fingerprint} |
|
||||||
|
|
||||||
# unseen model year hint should not match |
|
||||||
live_fw[(0x760, None)] = {b"M1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"} |
|
||||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw}) |
|
||||||
assert len(candidates) == 0, "Should not match new model year hint" |
|
||||||
@ -1,279 +0,0 @@ |
|||||||
import copy |
|
||||||
import re |
|
||||||
from dataclasses import dataclass, field, replace |
|
||||||
from enum import Enum, IntFlag |
|
||||||
|
|
||||||
import panda.python.uds as uds |
|
||||||
from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, dbc_dict, DbcDict, PlatformConfig, Platforms |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \ |
|
||||||
Device |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16 |
|
||||||
|
|
||||||
Ecu = 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 FordFlags(IntFlag): |
|
||||||
# Static flags |
|
||||||
CANFD = 1 |
|
||||||
|
|
||||||
|
|
||||||
class RADAR: |
|
||||||
DELPHI_ESR = 'ford_fusion_2018_adas' |
|
||||||
DELPHI_MRR = 'FORD_CADS' |
|
||||||
|
|
||||||
|
|
||||||
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 FordCarDocs(CarDocs): |
|
||||||
package: str = "Co-Pilot360 Assist+" |
|
||||||
hybrid: bool = False |
|
||||||
plug_in_hybrid: bool = False |
|
||||||
|
|
||||||
def init_make(self, CP: CarParams): |
|
||||||
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3 |
|
||||||
if CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MAVERICK_MK1, CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1): |
|
||||||
self.car_parts = CarParts([Device.threex_angled_mount, harness]) |
|
||||||
else: |
|
||||||
self.car_parts = CarParts([Device.threex, harness]) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class FordPlatformConfig(PlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR)) |
|
||||||
|
|
||||||
def init(self): |
|
||||||
for car_docs in list(self.car_docs): |
|
||||||
if car_docs.hybrid: |
|
||||||
name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}" |
|
||||||
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name)) |
|
||||||
if car_docs.plug_in_hybrid: |
|
||||||
name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}" |
|
||||||
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name)) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class FordCANFDPlatformConfig(FordPlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None)) |
|
||||||
|
|
||||||
def init(self): |
|
||||||
super().init() |
|
||||||
self.flags |= FordFlags.CANFD |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
FORD_BRONCO_SPORT_MK1 = FordPlatformConfig( |
|
||||||
[FordCarDocs("Ford Bronco Sport 2021-23")], |
|
||||||
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7), |
|
||||||
) |
|
||||||
FORD_ESCAPE_MK4 = FordPlatformConfig( |
|
||||||
[ |
|
||||||
FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True), |
|
||||||
FordCarDocs("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True), |
|
||||||
], |
|
||||||
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7), |
|
||||||
) |
|
||||||
FORD_EXPLORER_MK6 = FordPlatformConfig( |
|
||||||
[ |
|
||||||
FordCarDocs("Ford Explorer 2020-23", hybrid=True), # Hybrid: Limited and Platinum only |
|
||||||
FordCarDocs("Lincoln Aviator 2020-23", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only |
|
||||||
], |
|
||||||
CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8), |
|
||||||
) |
|
||||||
FORD_F_150_MK14 = FordCANFDPlatformConfig( |
|
||||||
[FordCarDocs("Ford F-150 2022-23", "Co-Pilot360 Active 2.0", hybrid=True)], |
|
||||||
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0), |
|
||||||
) |
|
||||||
FORD_F_150_LIGHTNING_MK1 = FordCANFDPlatformConfig( |
|
||||||
[FordCarDocs("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0")], |
|
||||||
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9), |
|
||||||
) |
|
||||||
FORD_FOCUS_MK4 = FordPlatformConfig( |
|
||||||
[FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only |
|
||||||
CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0), |
|
||||||
) |
|
||||||
FORD_MAVERICK_MK1 = FordPlatformConfig( |
|
||||||
[ |
|
||||||
FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True), |
|
||||||
FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True), |
|
||||||
], |
|
||||||
CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0), |
|
||||||
) |
|
||||||
FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig( |
|
||||||
[FordCarDocs("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0")], |
|
||||||
CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio |
|
||||||
) |
|
||||||
FORD_RANGER_MK2 = FordCANFDPlatformConfig( |
|
||||||
[FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering")], |
|
||||||
CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0), |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
# FW response contains a combined software and part number |
|
||||||
# A-Z except no I, O or W |
|
||||||
# e.g. NZ6A-14C204-AAA |
|
||||||
# 1222-333333-444 |
|
||||||
# 1 = Model year hint (approximates model year/generation) |
|
||||||
# 2 = Platform hint |
|
||||||
# 3 = Part number |
|
||||||
# 4 = Software version |
|
||||||
FW_ALPHABET = b'A-HJ-NP-VX-Z' |
|
||||||
FW_PATTERN = re.compile(b'^(?P<model_year_hint>[' + FW_ALPHABET + b'])' + |
|
||||||
b'(?P<platform_hint>[0-9' + FW_ALPHABET + b']{3})-' + |
|
||||||
b'(?P<part_number>[0-9' + FW_ALPHABET + b']{5,6})-' + |
|
||||||
b'(?P<software_revision>[' + FW_ALPHABET + b']{2,})\x00*$') |
|
||||||
|
|
||||||
|
|
||||||
def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes, bytes]]: |
|
||||||
codes = set() |
|
||||||
for fw in fw_versions: |
|
||||||
match = FW_PATTERN.match(fw) |
|
||||||
if match is not None: |
|
||||||
codes.add((match.group('platform_hint'), match.group('model_year_hint'))) |
|
||||||
|
|
||||||
return codes |
|
||||||
|
|
||||||
|
|
||||||
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]: |
|
||||||
candidates: set[str] = set() |
|
||||||
|
|
||||||
for candidate, fws in offline_fw_versions.items(): |
|
||||||
# Keep track of ECUs which pass all checks (platform hint, within model year hint 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 & model year hints |
|
||||||
codes = get_platform_codes(expected_versions) |
|
||||||
expected_platform_codes = {code for code, _ in codes} |
|
||||||
expected_model_year_hints = {model_year_hint for _, model_year_hint in codes} |
|
||||||
|
|
||||||
# Found platform codes & model year hints |
|
||||||
codes = get_platform_codes(live_fw_versions.get(addr, set())) |
|
||||||
found_platform_codes = {code for code, _ in codes} |
|
||||||
found_model_year_hints = {model_year_hint for _, model_year_hint in codes} |
|
||||||
|
|
||||||
# Check platform code matches for any found versions |
|
||||||
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes): |
|
||||||
break |
|
||||||
|
|
||||||
# Check any model year hint within range in the database. Note that some models have more than one |
|
||||||
# platform code per ECU which we don't consider as separate ranges |
|
||||||
if not any(min(expected_model_year_hints) <= found_model_year_hint <= max(expected_model_year_hints) for |
|
||||||
found_model_year_hint in found_model_year_hints): |
|
||||||
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 |
|
||||||
|
|
||||||
|
|
||||||
# All of these ECUs must be present and are expected to have platform codes we can match |
|
||||||
PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps) |
|
||||||
|
|
||||||
DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00 |
|
||||||
|
|
||||||
ASBUILT_BLOCKS: list[tuple[int, list]] = [ |
|
||||||
(1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]), |
|
||||||
(2, [Ecu.abs, Ecu.debug, Ecu.eps]), |
|
||||||
(3, [Ecu.abs, Ecu.debug, Ecu.eps]), |
|
||||||
(4, [Ecu.debug, Ecu.fwdCamera]), |
|
||||||
(5, [Ecu.debug]), |
|
||||||
(6, [Ecu.debug]), |
|
||||||
(7, [Ecu.debug]), |
|
||||||
(8, [Ecu.debug]), |
|
||||||
(9, [Ecu.debug]), |
|
||||||
(16, [Ecu.debug, Ecu.fwdCamera]), |
|
||||||
(18, [Ecu.fwdCamera]), |
|
||||||
(20, [Ecu.fwdCamera]), |
|
||||||
(21, [Ecu.fwdCamera]), |
|
||||||
] |
|
||||||
|
|
||||||
|
|
||||||
def ford_asbuilt_block_request(block_id: int): |
|
||||||
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1) |
|
||||||
|
|
||||||
|
|
||||||
def ford_asbuilt_block_response(block_id: int): |
|
||||||
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1) |
|
||||||
|
|
||||||
|
|
||||||
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 |
|
||||||
Request( |
|
||||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], |
|
||||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], |
|
||||||
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], |
|
||||||
logging=True, |
|
||||||
), |
|
||||||
Request( |
|
||||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], |
|
||||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], |
|
||||||
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], |
|
||||||
bus=0, |
|
||||||
auxiliary=True, |
|
||||||
), |
|
||||||
*[Request( |
|
||||||
[StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)], |
|
||||||
[StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)], |
|
||||||
whitelist_ecus=ecus, |
|
||||||
bus=0, |
|
||||||
logging=True, |
|
||||||
) for block_id, ecus in ASBUILT_BLOCKS], |
|
||||||
], |
|
||||||
extra_ecus=[ |
|
||||||
(Ecu.engine, 0x7e0, None), # Powertrain Control Module |
|
||||||
# Note: We are unlikely to get a response from behind the gateway |
|
||||||
(Ecu.shiftByWire, 0x732, None), # Gear Shift Module |
|
||||||
(Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module |
|
||||||
], |
|
||||||
# Custom fuzzy fingerprinting function using platform and model year hints |
|
||||||
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, |
|
||||||
) |
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,119 +0,0 @@ |
|||||||
import copy |
|
||||||
from dataclasses import dataclass, field |
|
||||||
import struct |
|
||||||
from collections.abc import Callable |
|
||||||
|
|
||||||
import panda.python.uds as uds |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
|
|
||||||
AddrType = tuple[int, int | None] |
|
||||||
EcuAddrBusType = tuple[int, int | None, int] |
|
||||||
EcuAddrSubAddr = tuple[CarParams.Ecu, int, int | None] |
|
||||||
|
|
||||||
LiveFwVersions = dict[AddrType, set[bytes]] |
|
||||||
OfflineFwVersions = dict[str, dict[EcuAddrSubAddr, list[bytes]]] |
|
||||||
|
|
||||||
# A global list of addresses we will only ever consider for VIN responses |
|
||||||
# engine, hybrid controller, Ford abs, Hyundai CAN FD cluster, 29-bit engine, PGM-FI |
|
||||||
# TODO: move these to each brand's FW query config |
|
||||||
STANDARD_VIN_ADDRS = [0x7e0, 0x7e2, 0x760, 0x7c6, 0x18da10f1, 0x18da0ef1] |
|
||||||
|
|
||||||
|
|
||||||
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) |
|
||||||
|
|
||||||
SUPPLIER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ |
|
||||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) |
|
||||||
SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ |
|
||||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_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) |
|
||||||
|
|
||||||
GM_VIN_REQUEST = b'\x1a\x90' |
|
||||||
GM_VIN_RESPONSE = b'\x5a\x90' |
|
||||||
|
|
||||||
KWP_VIN_REQUEST = b'\x21\x81' |
|
||||||
KWP_VIN_RESPONSE = b'\x61\x81' |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class Request: |
|
||||||
request: list[bytes] |
|
||||||
response: list[bytes] |
|
||||||
whitelist_ecus: list[CarParams.Ecu] = 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 |
|
||||||
# pandad 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[CarParams.Ecu, list[str]] = field(default_factory=dict) |
|
||||||
# Ecus added for data collection, not to be fingerprinted on |
|
||||||
extra_ecus: list[tuple[CarParams.Ecu, int, int | None]] = field(default_factory=list) |
|
||||||
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions and VIN, |
|
||||||
# returns set of candidates. Only will match if one candidate is returned |
|
||||||
match_fw_to_car_fuzzy: Callable[[LiveFwVersions, str, OfflineFwVersions], set[str]] | None = 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) |
|
||||||
|
|
||||||
def get_all_ecus(self, offline_fw_versions: OfflineFwVersions, |
|
||||||
include_extra_ecus: bool = True) -> set[EcuAddrSubAddr]: |
|
||||||
# Add ecus in database + extra ecus |
|
||||||
brand_ecus = {ecu for ecus in offline_fw_versions.values() for ecu in ecus} |
|
||||||
|
|
||||||
if include_extra_ecus: |
|
||||||
brand_ecus |= set(self.extra_ecus) |
|
||||||
|
|
||||||
return brand_ecus |
|
||||||
@ -1,327 +0,0 @@ |
|||||||
from collections import defaultdict |
|
||||||
from collections.abc import Callable, Iterator |
|
||||||
from typing import Protocol, TypeVar |
|
||||||
|
|
||||||
from tqdm import tqdm |
|
||||||
|
|
||||||
import panda.python.uds as uds |
|
||||||
from openpilot.selfdrive.car import carlog |
|
||||||
from openpilot.selfdrive.car.can_definitions import CanRecvCallable, CanSendCallable |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs |
|
||||||
from openpilot.selfdrive.car.fingerprints import FW_VERSIONS |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions |
|
||||||
from openpilot.selfdrive.car.interfaces import get_interface_attr |
|
||||||
from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery |
|
||||||
|
|
||||||
Ecu = 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: dict[str, FwQueryConfig] = 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] |
|
||||||
|
|
||||||
T = TypeVar('T') |
|
||||||
ObdCallback = Callable[[bool], None] |
|
||||||
|
|
||||||
|
|
||||||
def chunks(l: list[T], n: int = 128) -> Iterator[list[T]]: |
|
||||||
for i in range(0, len(l), n): |
|
||||||
yield l[i:i + n] |
|
||||||
|
|
||||||
|
|
||||||
def is_brand(brand: str, filter_brand: str | None) -> 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[CarParams.CarFw], filter_brand: 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) |
|
||||||
|
|
||||||
|
|
||||||
class MatchFwToCar(Protocol): |
|
||||||
def __call__(self, live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True) -> set[str]: |
|
||||||
... |
|
||||||
|
|
||||||
|
|
||||||
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True, exclude: str = None) -> set[str]: |
|
||||||
"""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() |
|
||||||
match: str | None = 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 match is None: |
|
||||||
match = candidates[0] |
|
||||||
# We uniquely matched two different cars. No fuzzy match possible |
|
||||||
elif match != 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 match and len(matched_ecus) >= 2: |
|
||||||
if log: |
|
||||||
carlog.error(f"Fingerprinted {match} using fuzzy match. {len(matched_ecus)} matching ECUs") |
|
||||||
return {match} |
|
||||||
else: |
|
||||||
return set() |
|
||||||
|
|
||||||
|
|
||||||
def match_fw_to_car_exact(live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True, extra_fw_versions: dict = None) -> 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.""" |
|
||||||
if extra_fw_versions is None: |
|
||||||
extra_fw_versions = {} |
|
||||||
|
|
||||||
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(): |
|
||||||
expected_versions = expected_versions + extra_fw_versions.get(candidate, {}).get(ecu, []) |
|
||||||
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 |
|
||||||
# FIXME: this logic can be improved to be more specific, should require one of the two 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: list[CarParams.CarFw], vin: str, allow_exact: bool = True, |
|
||||||
allow_fuzzy: bool = True, log: bool = True) -> tuple[bool, set[str]]: |
|
||||||
# Try exact matching first |
|
||||||
exact_matches: list[tuple[bool, MatchFwToCar]] = [] |
|
||||||
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[str] = 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, vin, VERSIONS[brand]) |
|
||||||
|
|
||||||
if len(matches): |
|
||||||
return exact_match, matches |
|
||||||
|
|
||||||
return True, set() |
|
||||||
|
|
||||||
|
|
||||||
def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]: |
|
||||||
# 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[EcuAddrBusType] = set() |
|
||||||
|
|
||||||
for brand, config, r in REQUESTS: |
|
||||||
# Skip query if no panda available |
|
||||||
if r.bus > num_pandas * 4 - 1: |
|
||||||
continue |
|
||||||
|
|
||||||
for ecu_type, addr, sub_addr in config.get_all_ecus(VERSIONS[brand]): |
|
||||||
# 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(obd_multiplexing) |
|
||||||
for query in queries[obd_multiplexing]: |
|
||||||
ecu_responses.update(get_ecu_addrs(can_recv, can_send, set(query), responses, timeout=0.1)) |
|
||||||
return ecu_responses |
|
||||||
|
|
||||||
|
|
||||||
def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, set[AddrType]]: |
|
||||||
"""Returns dictionary of brands and matches with ECUs in their FW versions""" |
|
||||||
|
|
||||||
brand_addrs = {brand: {(addr, subaddr) for _, addr, subaddr in config.get_all_ecus(VERSIONS[brand])} for |
|
||||||
brand, config in FW_QUERY_CONFIGS.items()} |
|
||||||
brand_matches: dict[str, set[AddrType]] = {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 get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, vin: str, |
|
||||||
ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, debug: bool = False, |
|
||||||
progress: bool = False) -> list[CarParams.CarFw]: |
|
||||||
"""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(can_recv, can_send, set_obd_multiplexing, 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, vin, log=False) |
|
||||||
if len(matches) == 1: |
|
||||||
break |
|
||||||
|
|
||||||
return all_car_fw |
|
||||||
|
|
||||||
|
|
||||||
def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str = None, |
|
||||||
extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, debug: bool = False, |
|
||||||
progress: bool = False) -> list[CarParams.CarFw]: |
|
||||||
versions = VERSIONS.copy() |
|
||||||
|
|
||||||
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_type, addr, sub_addr in config.get_all_ecus(brand_versions): |
|
||||||
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_group in tqdm(addrs, disable=not progress): # split by subaddr, if any |
|
||||||
for addr_chunk in chunks(addr_group): |
|
||||||
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(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(can_send, can_recv, 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 = CarParams.CarFw() |
|
||||||
|
|
||||||
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: |
|
||||||
carlog.exception("FW query exception") |
|
||||||
|
|
||||||
return car_fw |
|
||||||
@ -1,163 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, structs |
|
||||||
from openpilot.selfdrive.car.gm import gmcan |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import interp |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
|
|
||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert |
|
||||||
NetworkLocation = structs.CarParams.NetworkLocation |
|
||||||
LongCtrlState = structs.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(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.start_time = 0. |
|
||||||
self.apply_steer_last = 0 |
|
||||||
self.apply_gas = 0 |
|
||||||
self.apply_brake = 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, 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 = copy.copy(actuators) |
|
||||||
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 |
|
||||||
@ -1,193 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import mean |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, CruiseButtons, STEER_THRESHOLD |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
TransmissionType = structs.CarParams.TransmissionType |
|
||||||
NetworkLocation = structs.CarParams.NetworkLocation |
|
||||||
|
|
||||||
STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS |
|
||||||
|
|
||||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, |
|
||||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} |
|
||||||
|
|
||||||
|
|
||||||
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 |
|
||||||
|
|
||||||
self.distance_button = 0 |
|
||||||
|
|
||||||
def update(self, pt_cp, cam_cp, _, __, loopback_cp) -> structs.CarState: |
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
prev_cruise_buttons = self.cruise_buttons |
|
||||||
prev_distance_button = self.distance_button |
|
||||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] |
|
||||||
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] |
|
||||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] |
|
||||||
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) |
|
||||||
# This is to avoid a fault where you engage while still moving backwards after shifting to D. |
|
||||||
# An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2) |
|
||||||
self.moving_backward = (pt_cp.vl["EBCMWheelSpdRear"]["RLWheelDir"] == 2) or (pt_cp.vl["EBCMWheelSpdRear"]["RRWheelDir"] == 2) |
|
||||||
|
|
||||||
# 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) |
|
||||||
|
|
||||||
if self.CP.enableBsm: |
|
||||||
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 |
|
||||||
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 |
|
||||||
|
|
||||||
# Don't add event if transitioning from INIT, unless it's to an actual button |
|
||||||
if self.cruise_buttons != CruiseButtons.UNPRESS or prev_cruise_buttons != CruiseButtons.INIT: |
|
||||||
ret.buttonEvents = [ |
|
||||||
*create_button_events(self.cruise_buttons, prev_cruise_buttons, BUTTONS_DICT, |
|
||||||
unpressed_btn=CruiseButtons.UNPRESS), |
|
||||||
*create_button_events(self.distance_button, prev_distance_button, |
|
||||||
{1: ButtonType.gapAdjustCruise}) |
|
||||||
] |
|
||||||
|
|
||||||
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), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.enableBsm: |
|
||||||
messages.append(("BCMBlindSpotMonitor", 10)) |
|
||||||
|
|
||||||
# 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) |
|
||||||
@ -1,63 +0,0 @@ |
|||||||
# ruff: noqa: E501 |
|
||||||
from openpilot.selfdrive.car.gm.values import CAR |
|
||||||
|
|
||||||
# Trailblazer also matches as a SILVERADO, TODO: split with fw versions |
|
||||||
# FIXME: There are Equinox users with different message lengths, specifically 304 and 320 |
|
||||||
|
|
||||||
|
|
||||||
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.CHEVROLET_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.CHEVROLET_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.GMC_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.CADILLAC_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.CADILLAC_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.CADILLAC_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.CHEVROLET_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.CHEVROLET_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.CHEVROLET_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 |
|
||||||
}, |
|
||||||
{ |
|
||||||
190: 6, 201: 8, 211: 2, 717: 5, 241: 6, 451: 8, 298: 8, 452: 8, 453: 6, 479: 3, 485: 8, 249: 8, 500: 6, 587: 8, 1611: 8, 289: 8, 481: 7, 193: 8, 197: 8, 209: 7, 455: 7, 489: 8, 309: 8, 413: 8, 501: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 311: 8, 510: 8, 528: 5, 532: 6, 715: 8, 560: 8, 562: 8, 707: 8, 789: 5, 869: 4, 880: 6, 761: 7, 840: 5, 842: 5, 844: 8, 313: 8, 381: 8, 386: 8, 810: 8, 322: 7, 384: 4, 800: 6, 1033: 7, 1034: 7, 1296: 4, 753: 5, 388: 8, 288: 5, 497: 8, 463: 3, 304: 3, 977: 8, 1001: 8, 1280: 4, 320: 4, 352: 5, 563: 5, 565: 5, 1221: 5, 1011: 6, 1017: 8, 1020: 8, 1249: 8, 1300: 8, 328: 1, 1217: 8, 1233: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1930: 7, 1271: 8 |
|
||||||
}], |
|
||||||
} |
|
||||||
|
|
||||||
FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = { |
|
||||||
} |
|
||||||
@ -1,173 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.can_definitions import CanData |
|
||||||
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 [CanData(0x409, dat, bus), CanData(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)[1] |
|
||||||
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.CHEVROLET_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, hud_control, fcw): |
|
||||||
target_speed = min(target_speed_kph, 255) |
|
||||||
|
|
||||||
values = { |
|
||||||
"ACCAlwaysOne": 1, |
|
||||||
"ACCResumeButton": 0, |
|
||||||
"ACCSpeedSetpoint": target_speed, |
|
||||||
"ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive" |
|
||||||
"ACCCmdActive": enabled, |
|
||||||
"ACCAlwaysOne2": 1, |
|
||||||
"ACCLeadCar": hud_control.leadVisible, |
|
||||||
"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 CanData(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 CanData(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 CanData(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 CanData(0x104c006c, dat, bus) |
|
||||||
@ -1,194 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import os |
|
||||||
from math import fabs, exp |
|
||||||
from panda import Panda |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import get_safety_config, get_friction, structs |
|
||||||
from openpilot.selfdrive.car.common.basedir import BASEDIR |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG |
|
||||||
from openpilot.selfdrive.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus |
|
||||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel |
|
||||||
|
|
||||||
TransmissionType = structs.CarParams.TransmissionType |
|
||||||
NetworkLocation = structs.CarParams.NetworkLocation |
|
||||||
|
|
||||||
NON_LINEAR_TORQUE_PARAMS = { |
|
||||||
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], |
|
||||||
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772], |
|
||||||
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122] |
|
||||||
} |
|
||||||
|
|
||||||
NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'torque_data/neural_ff_weights.json') |
|
||||||
|
|
||||||
|
|
||||||
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.CHEVROLET_VOLT: |
|
||||||
return self.get_steer_feedforward_volt |
|
||||||
else: |
|
||||||
return CarInterfaceBase.get_steer_feedforward_default |
|
||||||
|
|
||||||
def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning, |
|
||||||
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool, gravity_adjusted: bool) -> float: |
|
||||||
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) |
|
||||||
|
|
||||||
def sig(val): |
|
||||||
# https://timvieira.github.io/blog/post/2014/02/11/exp-normalize-trick |
|
||||||
if val >= 0: |
|
||||||
return 1 / (1 + exp(-val)) - 0.5 |
|
||||||
else: |
|
||||||
z = exp(val) |
|
||||||
return z / (1 + z) - 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) # type: ignore[call-overload] |
|
||||||
assert non_linear_torque_params, "The params are not defined" |
|
||||||
a, b, c, _ = non_linear_torque_params |
|
||||||
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) |
|
||||||
return float(steer_torque) + friction |
|
||||||
|
|
||||||
def torque_from_lateral_accel_neural(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning, |
|
||||||
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool, gravity_adjusted: bool) -> float: |
|
||||||
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) |
|
||||||
inputs = list(latcontrol_inputs) |
|
||||||
if gravity_adjusted: |
|
||||||
inputs[0] += inputs[1] |
|
||||||
return float(self.neural_ff_model.predict(inputs)) + friction |
|
||||||
|
|
||||||
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: |
|
||||||
if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_EUV: |
|
||||||
self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint) |
|
||||||
return self.torque_from_lateral_accel_neural |
|
||||||
elif 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: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
ret.carName = "gm" |
|
||||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.gm)] |
|
||||||
ret.autoResumeSng = False |
|
||||||
ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] |
|
||||||
|
|
||||||
if candidate in EV_CAR: |
|
||||||
ret.transmissionType = TransmissionType.direct |
|
||||||
else: |
|
||||||
ret.transmissionType = TransmissionType.automatic |
|
||||||
|
|
||||||
ret.longitudinalTuning.kiBP = [5., 35.] |
|
||||||
|
|
||||||
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.kiV = [2.0, 1.5] |
|
||||||
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.kiV = [2.4, 1.5] |
|
||||||
|
|
||||||
# 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.CHEVROLET_MALIBU, CAR.BUICK_REGAL} 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.steerLimitTimer = 0.4 |
|
||||||
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz |
|
||||||
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking |
|
||||||
|
|
||||||
if candidate == CAR.CHEVROLET_VOLT: |
|
||||||
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.GMC_ACADIA: |
|
||||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
elif candidate == CAR.BUICK_LACROSSE: |
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
elif candidate == CAR.CADILLAC_ESCALADE: |
|
||||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm |
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
elif candidate in (CAR.CADILLAC_ESCALADE_ESV, CAR.CADILLAC_ESCALADE_ESV_2019): |
|
||||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm |
|
||||||
|
|
||||||
if candidate == CAR.CADILLAC_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.CHEVROLET_BOLT_EUV: |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
elif candidate == CAR.CHEVROLET_SILVERADO: |
|
||||||
# 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.CHEVROLET_EQUINOX: |
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
elif candidate == CAR.CHEVROLET_TRAILBLAZER: |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
return ret |
|
||||||
@ -1,101 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import math |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
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 = structs.RadarData() |
|
||||||
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] = structs.RadarData.RadarPoint() |
|
||||||
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 |
|
||||||
@ -1,20 +0,0 @@ |
|||||||
from parameterized import parameterized |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.gm.fingerprints import FINGERPRINTS |
|
||||||
from openpilot.selfdrive.car.gm.values import CAMERA_ACC_CAR, GM_RX_OFFSET |
|
||||||
|
|
||||||
CAMERA_DIAGNOSTIC_ADDRESS = 0x24b |
|
||||||
|
|
||||||
|
|
||||||
class TestGMFingerprint: |
|
||||||
@parameterized.expand(FINGERPRINTS.items()) |
|
||||||
def test_can_fingerprints(self, car_model, fingerprints): |
|
||||||
assert len(fingerprints) > 0 |
|
||||||
|
|
||||||
assert all(len(finger) for finger in fingerprints) |
|
||||||
|
|
||||||
# The camera can sometimes be communicating on startup |
|
||||||
if car_model in CAMERA_ACC_CAR: |
|
||||||
for finger in fingerprints: |
|
||||||
for required_addr in (CAMERA_DIAGNOSTIC_ADDRESS, CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET): |
|
||||||
assert finger.get(required_addr) == 8, required_addr |
|
||||||
@ -1,234 +0,0 @@ |
|||||||
from dataclasses import dataclass, field |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import dbc_dict, PlatformConfig, DbcDict, Platforms, CarSpecs |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries |
|
||||||
|
|
||||||
Ecu = 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.] |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class GMCarDocs(CarDocs): |
|
||||||
package: str = "Adaptive Cruise Control (ACC)" |
|
||||||
|
|
||||||
def init_make(self, CP: CarParams): |
|
||||||
if CP.networkLocation == CarParams.NetworkLocation.fwdCamera: |
|
||||||
self.car_parts = CarParts.common([CarHarness.gm]) |
|
||||||
else: |
|
||||||
self.car_parts = CarParts.common([CarHarness.obd_ii]) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True, kw_only=True) |
|
||||||
class GMCarSpecs(CarSpecs): |
|
||||||
tireStiffnessFactor: float = 0.444 # not optimized yet |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class GMPlatformConfig(PlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class GMASCMPlatformConfig(GMPlatformConfig): |
|
||||||
def init(self): |
|
||||||
# ASCM is supported, but due to a janky install and hardware configuration, we are not showing in the car docs |
|
||||||
self.car_docs = [] |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
HOLDEN_ASTRA = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Holden Astra 2017")], |
|
||||||
GMCarSpecs(mass=1363, wheelbase=2.662, steerRatio=15.7, centerToFrontRatio=0.4), |
|
||||||
) |
|
||||||
CHEVROLET_VOLT = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ")], |
|
||||||
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469), |
|
||||||
) |
|
||||||
CADILLAC_ATS = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Cadillac ATS Premium Performance 2018")], |
|
||||||
GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3), |
|
||||||
) |
|
||||||
CHEVROLET_MALIBU = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Chevrolet Malibu Premier 2017")], |
|
||||||
GMCarSpecs(mass=1496, wheelbase=2.83, steerRatio=15.8, centerToFrontRatio=0.4), |
|
||||||
) |
|
||||||
GMC_ACADIA = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo")], |
|
||||||
GMCarSpecs(mass=1975, wheelbase=2.86, steerRatio=14.4, centerToFrontRatio=0.4), |
|
||||||
) |
|
||||||
BUICK_LACROSSE = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Buick LaCrosse 2017-19", "Driver Confidence Package 2")], |
|
||||||
GMCarSpecs(mass=1712, wheelbase=2.91, steerRatio=15.8, centerToFrontRatio=0.4), |
|
||||||
) |
|
||||||
BUICK_REGAL = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Buick Regal Essence 2018")], |
|
||||||
GMCarSpecs(mass=1714, wheelbase=2.83, steerRatio=14.4, centerToFrontRatio=0.4), |
|
||||||
) |
|
||||||
CADILLAC_ESCALADE = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Cadillac Escalade 2017", "Driver Assist Package")], |
|
||||||
GMCarSpecs(mass=2564, wheelbase=2.95, steerRatio=17.3), |
|
||||||
) |
|
||||||
CADILLAC_ESCALADE_ESV = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS")], |
|
||||||
GMCarSpecs(mass=2739, wheelbase=3.302, steerRatio=17.3, tireStiffnessFactor=1.0), |
|
||||||
) |
|
||||||
CADILLAC_ESCALADE_ESV_2019 = GMASCMPlatformConfig( |
|
||||||
[GMCarDocs("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS")], |
|
||||||
CADILLAC_ESCALADE_ESV.specs, |
|
||||||
) |
|
||||||
CHEVROLET_BOLT_EUV = GMPlatformConfig( |
|
||||||
[ |
|
||||||
GMCarDocs("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"), |
|
||||||
GMCarDocs("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), |
|
||||||
], |
|
||||||
GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), |
|
||||||
) |
|
||||||
CHEVROLET_SILVERADO = GMPlatformConfig( |
|
||||||
[ |
|
||||||
GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"), |
|
||||||
GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), |
|
||||||
], |
|
||||||
GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0), |
|
||||||
) |
|
||||||
CHEVROLET_EQUINOX = GMPlatformConfig( |
|
||||||
[GMCarDocs("Chevrolet Equinox 2019-22")], |
|
||||||
GMCarSpecs(mass=1588, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4), |
|
||||||
) |
|
||||||
CHEVROLET_TRAILBLAZER = GMPlatformConfig( |
|
||||||
[GMCarDocs("Chevrolet Trailblazer 2021-22")], |
|
||||||
GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
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 |
|
||||||
|
|
||||||
|
|
||||||
# In a Data Module, an identifier is a string used to recognize an object, |
|
||||||
# either by itself or together with the identifiers of parent objects. |
|
||||||
# Each returns a 4 byte hex representation of the decimal part number. `b"\x02\x8c\xf0'"` -> 42790951 |
|
||||||
GM_BOOT_SOFTWARE_PART_NUMER_REQUEST = b'\x1a\xc0' # likely does not contain anything useful |
|
||||||
GM_SOFTWARE_MODULE_1_REQUEST = b'\x1a\xc1' |
|
||||||
GM_SOFTWARE_MODULE_2_REQUEST = b'\x1a\xc2' |
|
||||||
GM_SOFTWARE_MODULE_3_REQUEST = b'\x1a\xc3' |
|
||||||
|
|
||||||
# Part number of XML data file that is used to configure ECU |
|
||||||
GM_XML_DATA_FILE_PART_NUMBER = b'\x1a\x9c' |
|
||||||
GM_XML_CONFIG_COMPAT_ID = b'\x1a\x9b' # used to know if XML file is compatible with the ECU software/hardware |
|
||||||
|
|
||||||
# This DID is for identifying the part number that reflects the mix of hardware, |
|
||||||
# software, and calibrations in the ECU when it first arrives at the vehicle assembly plant. |
|
||||||
# If there's an Alpha Code, it's associated with this part number and stored in the DID $DB. |
|
||||||
GM_END_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcb' |
|
||||||
GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdb' |
|
||||||
GM_BASE_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcc' |
|
||||||
GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdc' |
|
||||||
GM_FW_RESPONSE = b'\x5a' |
|
||||||
|
|
||||||
GM_FW_REQUESTS = [ |
|
||||||
GM_BOOT_SOFTWARE_PART_NUMER_REQUEST, |
|
||||||
GM_SOFTWARE_MODULE_1_REQUEST, |
|
||||||
GM_SOFTWARE_MODULE_2_REQUEST, |
|
||||||
GM_SOFTWARE_MODULE_3_REQUEST, |
|
||||||
GM_XML_DATA_FILE_PART_NUMBER, |
|
||||||
GM_XML_CONFIG_COMPAT_ID, |
|
||||||
GM_END_MODEL_PART_NUMBER_REQUEST, |
|
||||||
GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST, |
|
||||||
GM_BASE_MODEL_PART_NUMBER_REQUEST, |
|
||||||
GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST, |
|
||||||
] |
|
||||||
|
|
||||||
GM_RX_OFFSET = 0x400 |
|
||||||
|
|
||||||
FW_QUERY_CONFIG = FwQueryConfig( |
|
||||||
requests=[request for req in GM_FW_REQUESTS for request in [ |
|
||||||
Request( |
|
||||||
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, req], |
|
||||||
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, GM_FW_RESPONSE + bytes([req[-1]])], |
|
||||||
rx_offset=GM_RX_OFFSET, |
|
||||||
bus=0, |
|
||||||
logging=True, |
|
||||||
), |
|
||||||
]], |
|
||||||
extra_ecus=[(Ecu.fwdCamera, 0x24b, None)], |
|
||||||
) |
|
||||||
|
|
||||||
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV} |
|
||||||
|
|
||||||
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness) |
|
||||||
CAMERA_ACC_CAR = {CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_EQUINOX, CAR.CHEVROLET_TRAILBLAZER} |
|
||||||
|
|
||||||
STEER_THRESHOLD = 1.0 |
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,254 +0,0 @@ |
|||||||
import copy |
|
||||||
from collections import namedtuple |
|
||||||
|
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import DT_CTRL, rate_limit, make_tester_present_msg, structs |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip, interp |
|
||||||
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.car.interfaces import CarControllerBase |
|
||||||
|
|
||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert |
|
||||||
LongCtrlState = structs.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] |
|
||||||
elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw): |
|
||||||
steer_required = VISUAL_HUD[hud_alert] |
|
||||||
else: |
|
||||||
acc_alert = VISUAL_HUD[hud_alert] |
|
||||||
|
|
||||||
return fcw_display, steer_required, acc_alert |
|
||||||
|
|
||||||
|
|
||||||
HUDData = namedtuple("HUDData", |
|
||||||
["pcm_accel", "v_cruise", "lead_visible", |
|
||||||
"lanes_visible", "fcw", "acc_alert", "steer_required", "lead_distance_bars"]) |
|
||||||
|
|
||||||
|
|
||||||
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(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.packer = CANPacker(dbc_name) |
|
||||||
self.params = CarControllerParams(CP) |
|
||||||
self.CAN = hondacan.CanBus(CP) |
|
||||||
|
|
||||||
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(make_tester_present_msg(0x18DAB0F1, 1, suppress_response=True)) |
|
||||||
|
|
||||||
# Send steering command. |
|
||||||
can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, 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 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.CAN, 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, self.CAN, CruiseButtons.CANCEL, self.CP.carFingerprint)) |
|
||||||
elif CC.cruiseControl.resume: |
|
||||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, 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, self.CAN, 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, self.CAN, 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 |
|
||||||
|
|
||||||
# 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, hud_control.leadDistanceBars) |
|
||||||
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CAN, 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 |
|
||||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX |
|
||||||
|
|
||||||
new_actuators = copy.copy(actuators) |
|
||||||
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 |
|
||||||
@ -1,307 +0,0 @@ |
|||||||
from collections import defaultdict |
|
||||||
|
|
||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import interp |
|
||||||
from openpilot.selfdrive.car.honda.hondacan import CanBus, get_cruise_speed_conversion |
|
||||||
from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \ |
|
||||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \ |
|
||||||
HondaFlags, CruiseButtons, CruiseSettings |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
|
|
||||||
TransmissionType = structs.CarParams.TransmissionType |
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
|
|
||||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, |
|
||||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} |
|
||||||
SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.altButton1} |
|
||||||
|
|
||||||
|
|
||||||
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.HONDA_ODYSSEY_CHN: |
|
||||||
messages += [ |
|
||||||
("SCM_FEEDBACK", 25), |
|
||||||
("SCM_BUTTONS", 50), |
|
||||||
] |
|
||||||
else: |
|
||||||
messages += [ |
|
||||||
("SCM_FEEDBACK", 10), |
|
||||||
("SCM_BUTTONS", 25), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.carFingerprint in (CAR.HONDA_CRV_HYBRID, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E): |
|
||||||
messages.append((gearbox_msg, 50)) |
|
||||||
else: |
|
||||||
messages.append((gearbox_msg, 100)) |
|
||||||
|
|
||||||
if CP.flags & HondaFlags.BOSCH_ALT_BRAKE: |
|
||||||
messages.append(("BRAKE_MODULE", 50)) |
|
||||||
|
|
||||||
if CP.carFingerprint in (HONDA_BOSCH | {CAR.HONDA_CIVIC, CAR.HONDA_ODYSSEY, CAR.HONDA_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.HONDA_ODYSSEY_CHN: |
|
||||||
messages.append(("CRUISE_PARAMS", 10)) |
|
||||||
else: |
|
||||||
messages.append(("CRUISE_PARAMS", 50)) |
|
||||||
|
|
||||||
# TODO: clean this up |
|
||||||
if CP.carFingerprint in (CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CRV_HYBRID, CAR.HONDA_INSIGHT, |
|
||||||
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.HONDA_CIVIC_2022, CAR.HONDA_HRV_3G): |
|
||||||
pass |
|
||||||
elif CP.carFingerprint in (CAR.HONDA_ODYSSEY_CHN, CAR.HONDA_FREED, CAR.HONDA_HRV): |
|
||||||
pass |
|
||||||
else: |
|
||||||
messages.append(("DOORS_STATUS", 3)) |
|
||||||
|
|
||||||
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.HONDA_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, __) -> structs.CarState: |
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
# 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 |
|
||||||
prev_cruise_buttons = self.cruise_buttons |
|
||||||
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.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CRV_HYBRID, CAR.HONDA_INSIGHT, |
|
||||||
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.HONDA_CIVIC_2022, CAR.HONDA_HRV_3G): |
|
||||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) |
|
||||||
elif self.CP.carFingerprint in (CAR.HONDA_ODYSSEY_CHN, CAR.HONDA_FREED, CAR.HONDA_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.HONDA_CIVIC, CAR.HONDA_ODYSSEY, CAR.HONDA_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)) |
|
||||||
|
|
||||||
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) # type: ignore[call-overload] |
|
||||||
|
|
||||||
if self.CP.carFingerprint in HONDA_BOSCH: |
|
||||||
# The PCM always manages its own cruise control state, but doesn't publish it |
|
||||||
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: |
|
||||||
ret.cruiseState.nonAdaptive = cp_cam.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0 |
|
||||||
|
|
||||||
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.flags & HondaFlags.BOSCH_ALT_BRAKE: |
|
||||||
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.HONDA_PILOT, CAR.HONDA_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 |
|
||||||
|
|
||||||
ret.buttonEvents = [ |
|
||||||
*create_button_events(self.cruise_buttons, prev_cruise_buttons, BUTTONS_DICT), |
|
||||||
*create_button_events(self.cruise_setting, prev_cruise_setting, SETTINGS_BUTTONS_DICT), |
|
||||||
] |
|
||||||
|
|
||||||
return ret |
|
||||||
|
|
||||||
def get_can_parser(self, CP): |
|
||||||
messages = get_can_messages(CP, self.gearbox_msg) |
|
||||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).pt) |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def get_cam_can_parser(CP): |
|
||||||
messages = [ |
|
||||||
("STEERING_CONTROL", 100), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS: |
|
||||||
messages += [ |
|
||||||
("ACC_HUD", 10), |
|
||||||
("LKAS_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, CanBus(CP).camera) |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def get_body_can_parser(CP): |
|
||||||
if CP.enableBsm: |
|
||||||
messages = [ |
|
||||||
("BSM_STATUS_LEFT", 3), |
|
||||||
("BSM_STATUS_RIGHT", 3), |
|
||||||
] |
|
||||||
bus_body = CanBus(CP).radar # 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 |
|
||||||
@ -1,899 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.honda.values import CAR |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
# Modified FW can be identified by the second dash being replaced by a comma |
|
||||||
# For example: `b'39990-TVA,A150\x00\x00'` |
|
||||||
# |
|
||||||
# TODO: vsa is "essential" for fpv2 but doesn't appear on some CAR.FREED models |
|
||||||
|
|
||||||
|
|
||||||
FW_VERSIONS = { |
|
||||||
CAR.HONDA_ACCORD: { |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TVC-A910\x00\x00', |
|
||||||
b'54008-TWA-A910\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-6A7-A220\x00\x00', |
|
||||||
b'28101-6A7-A230\x00\x00', |
|
||||||
b'28101-6A7-A320\x00\x00', |
|
||||||
b'28101-6A7-A330\x00\x00', |
|
||||||
b'28101-6A7-A410\x00\x00', |
|
||||||
b'28101-6A7-A510\x00\x00', |
|
||||||
b'28101-6A7-A610\x00\x00', |
|
||||||
b'28101-6A7-A710\x00\x00', |
|
||||||
b'28101-6A9-H140\x00\x00', |
|
||||||
b'28101-6A9-H420\x00\x00', |
|
||||||
b'28102-6B8-A560\x00\x00', |
|
||||||
b'28102-6B8-A570\x00\x00', |
|
||||||
b'28102-6B8-A700\x00\x00', |
|
||||||
b'28102-6B8-A800\x00\x00', |
|
||||||
b'28102-6B8-C560\x00\x00', |
|
||||||
b'28102-6B8-C570\x00\x00', |
|
||||||
b'28102-6B8-M520\x00\x00', |
|
||||||
b'28102-6B8-R700\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [ |
|
||||||
b'46114-TVA-A050\x00\x00', |
|
||||||
b'46114-TVA-A060\x00\x00', |
|
||||||
b'46114-TVA-A080\x00\x00', |
|
||||||
b'46114-TVA-A120\x00\x00', |
|
||||||
b'46114-TVA-A320\x00\x00', |
|
||||||
b'46114-TVE-H550\x00\x00', |
|
||||||
b'46114-TVE-H560\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TVA-B040\x00\x00', |
|
||||||
b'57114-TVA-B050\x00\x00', |
|
||||||
b'57114-TVA-B060\x00\x00', |
|
||||||
b'57114-TVA-B530\x00\x00', |
|
||||||
b'57114-TVA-C040\x00\x00', |
|
||||||
b'57114-TVA-C050\x00\x00', |
|
||||||
b'57114-TVA-C060\x00\x00', |
|
||||||
b'57114-TVA-C530\x00\x00', |
|
||||||
b'57114-TVA-E520\x00\x00', |
|
||||||
b'57114-TVE-H250\x00\x00', |
|
||||||
b'57114-TWA-A040\x00\x00', |
|
||||||
b'57114-TWA-A050\x00\x00', |
|
||||||
b'57114-TWA-A530\x00\x00', |
|
||||||
b'57114-TWA-B520\x00\x00', |
|
||||||
b'57114-TWA-C510\x00\x00', |
|
||||||
b'57114-TWB-H030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TBX-H120\x00\x00', |
|
||||||
b'39990-TVA,A150\x00\x00', |
|
||||||
b'39990-TVA-A140\x00\x00', |
|
||||||
b'39990-TVA-A150\x00\x00', |
|
||||||
b'39990-TVA-A160\x00\x00', |
|
||||||
b'39990-TVA-A340\x00\x00', |
|
||||||
b'39990-TVA-X030\x00\x00', |
|
||||||
b'39990-TVA-X040\x00\x00', |
|
||||||
b'39990-TVE-H130\x00\x00', |
|
||||||
b'39990-TWB-H120\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TBX-H230\x00\x00', |
|
||||||
b'77959-TVA-A460\x00\x00', |
|
||||||
b'77959-TVA-F330\x00\x00', |
|
||||||
b'77959-TVA-H230\x00\x00', |
|
||||||
b'77959-TVA-L420\x00\x00', |
|
||||||
b'77959-TVA-X330\x00\x00', |
|
||||||
b'77959-TWA-A440\x00\x00', |
|
||||||
b'77959-TWA-L420\x00\x00', |
|
||||||
b'77959-TWB-H220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.hud, 0x18da61f1, None): [ |
|
||||||
b'78209-TVA-A010\x00\x00', |
|
||||||
b'78209-TVA-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TBX-H140\x00\x00', |
|
||||||
b'36802-TVA-A150\x00\x00', |
|
||||||
b'36802-TVA-A160\x00\x00', |
|
||||||
b'36802-TVA-A170\x00\x00', |
|
||||||
b'36802-TVA-A180\x00\x00', |
|
||||||
b'36802-TVA-A330\x00\x00', |
|
||||||
b'36802-TVC-A330\x00\x00', |
|
||||||
b'36802-TVE-H070\x00\x00', |
|
||||||
b'36802-TWA-A070\x00\x00', |
|
||||||
b'36802-TWA-A080\x00\x00', |
|
||||||
b'36802-TWA-A210\x00\x00', |
|
||||||
b'36802-TWA-A330\x00\x00', |
|
||||||
b'36802-TWB-H060\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TBX-H130\x00\x00', |
|
||||||
b'36161-TVA-A060\x00\x00', |
|
||||||
b'36161-TVA-A330\x00\x00', |
|
||||||
b'36161-TVC-A330\x00\x00', |
|
||||||
b'36161-TVE-H050\x00\x00', |
|
||||||
b'36161-TWA-A070\x00\x00', |
|
||||||
b'36161-TWA-A330\x00\x00', |
|
||||||
b'36161-TWB-H040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TVA-A010\x00\x00', |
|
||||||
b'38897-TVA-A020\x00\x00', |
|
||||||
b'38897-TVA-A230\x00\x00', |
|
||||||
b'38897-TVA-A240\x00\x00', |
|
||||||
b'38897-TWA-A120\x00\x00', |
|
||||||
b'38897-TWD-J020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CIVIC: { |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-5CG-A040\x00\x00', |
|
||||||
b'28101-5CG-A050\x00\x00', |
|
||||||
b'28101-5CG-A070\x00\x00', |
|
||||||
b'28101-5CG-A080\x00\x00', |
|
||||||
b'28101-5CG-A320\x00\x00', |
|
||||||
b'28101-5CG-A810\x00\x00', |
|
||||||
b'28101-5CG-A820\x00\x00', |
|
||||||
b'28101-5DJ-A040\x00\x00', |
|
||||||
b'28101-5DJ-A060\x00\x00', |
|
||||||
b'28101-5DJ-A510\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TBA-A540\x00\x00', |
|
||||||
b'57114-TBA-A550\x00\x00', |
|
||||||
b'57114-TBA-A560\x00\x00', |
|
||||||
b'57114-TBA-A570\x00\x00', |
|
||||||
b'57114-TEA-Q220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TBA,A030\x00\x00', |
|
||||||
b'39990-TBA-A030\x00\x00', |
|
||||||
b'39990-TBG-A030\x00\x00', |
|
||||||
b'39990-TEA-T020\x00\x00', |
|
||||||
b'39990-TEG-A010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TBA-A030\x00\x00', |
|
||||||
b'77959-TBA-A040\x00\x00', |
|
||||||
b'77959-TBG-A020\x00\x00', |
|
||||||
b'77959-TBG-A030\x00\x00', |
|
||||||
b'77959-TEA-Q820\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-TBA-A020\x00\x00', |
|
||||||
b'36161-TBA-A030\x00\x00', |
|
||||||
b'36161-TBA-A040\x00\x00', |
|
||||||
b'36161-TBC-A020\x00\x00', |
|
||||||
b'36161-TBC-A030\x00\x00', |
|
||||||
b'36161-TED-Q320\x00\x00', |
|
||||||
b'36161-TEG-A010\x00\x00', |
|
||||||
b'36161-TEG-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TBA-A010\x00\x00', |
|
||||||
b'38897-TBA-A020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CIVIC_BOSCH: { |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-5CG-A920\x00\x00', |
|
||||||
b'28101-5CG-AB10\x00\x00', |
|
||||||
b'28101-5CG-C110\x00\x00', |
|
||||||
b'28101-5CG-C220\x00\x00', |
|
||||||
b'28101-5CG-C320\x00\x00', |
|
||||||
b'28101-5CG-G020\x00\x00', |
|
||||||
b'28101-5CG-L020\x00\x00', |
|
||||||
b'28101-5CK-A130\x00\x00', |
|
||||||
b'28101-5CK-A140\x00\x00', |
|
||||||
b'28101-5CK-A150\x00\x00', |
|
||||||
b'28101-5CK-C130\x00\x00', |
|
||||||
b'28101-5CK-C140\x00\x00', |
|
||||||
b'28101-5CK-C150\x00\x00', |
|
||||||
b'28101-5CK-G210\x00\x00', |
|
||||||
b'28101-5CK-J710\x00\x00', |
|
||||||
b'28101-5CK-Q610\x00\x00', |
|
||||||
b'28101-5DJ-A610\x00\x00', |
|
||||||
b'28101-5DJ-A710\x00\x00', |
|
||||||
b'28101-5DV-E330\x00\x00', |
|
||||||
b'28101-5DV-E610\x00\x00', |
|
||||||
b'28101-5DV-E820\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TBG-A330\x00\x00', |
|
||||||
b'57114-TBG-A340\x00\x00', |
|
||||||
b'57114-TBG-A350\x00\x00', |
|
||||||
b'57114-TGG-A340\x00\x00', |
|
||||||
b'57114-TGG-C320\x00\x00', |
|
||||||
b'57114-TGG-G320\x00\x00', |
|
||||||
b'57114-TGG-L320\x00\x00', |
|
||||||
b'57114-TGG-L330\x00\x00', |
|
||||||
b'57114-TGH-L130\x00\x00', |
|
||||||
b'57114-TGK-T320\x00\x00', |
|
||||||
b'57114-TGL-G330\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TBA-C020\x00\x00', |
|
||||||
b'39990-TBA-C120\x00\x00', |
|
||||||
b'39990-TEA-T820\x00\x00', |
|
||||||
b'39990-TEZ-T020\x00\x00', |
|
||||||
b'39990-TGG,A020\x00\x00', |
|
||||||
b'39990-TGG,A120\x00\x00', |
|
||||||
b'39990-TGG-A020\x00\x00', |
|
||||||
b'39990-TGG-A120\x00\x00', |
|
||||||
b'39990-TGG-J510\x00\x00', |
|
||||||
b'39990-TGH-J530\x00\x00', |
|
||||||
b'39990-TGL-E130\x00\x00', |
|
||||||
b'39990-TGN-E120\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TBA-A060\x00\x00', |
|
||||||
b'77959-TBG-A050\x00\x00', |
|
||||||
b'77959-TEA-G020\x00\x00', |
|
||||||
b'77959-TGG-A020\x00\x00', |
|
||||||
b'77959-TGG-A030\x00\x00', |
|
||||||
b'77959-TGG-E010\x00\x00', |
|
||||||
b'77959-TGG-G010\x00\x00', |
|
||||||
b'77959-TGG-G110\x00\x00', |
|
||||||
b'77959-TGG-J320\x00\x00', |
|
||||||
b'77959-TGG-Z820\x00\x00', |
|
||||||
b'77959-TGH-J110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TBA-A150\x00\x00', |
|
||||||
b'36802-TBA-A160\x00\x00', |
|
||||||
b'36802-TFJ-G060\x00\x00', |
|
||||||
b'36802-TGG-A050\x00\x00', |
|
||||||
b'36802-TGG-A060\x00\x00', |
|
||||||
b'36802-TGG-A070\x00\x00', |
|
||||||
b'36802-TGG-A130\x00\x00', |
|
||||||
b'36802-TGG-G040\x00\x00', |
|
||||||
b'36802-TGG-G130\x00\x00', |
|
||||||
b'36802-TGH-A140\x00\x00', |
|
||||||
b'36802-TGK-Q120\x00\x00', |
|
||||||
b'36802-TGL-G040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TBA-A130\x00\x00', |
|
||||||
b'36161-TBA-A140\x00\x00', |
|
||||||
b'36161-TFJ-G070\x00\x00', |
|
||||||
b'36161-TGG-A060\x00\x00', |
|
||||||
b'36161-TGG-A080\x00\x00', |
|
||||||
b'36161-TGG-A120\x00\x00', |
|
||||||
b'36161-TGG-G050\x00\x00', |
|
||||||
b'36161-TGG-G070\x00\x00', |
|
||||||
b'36161-TGG-G130\x00\x00', |
|
||||||
b'36161-TGG-G140\x00\x00', |
|
||||||
b'36161-TGH-A140\x00\x00', |
|
||||||
b'36161-TGK-Q120\x00\x00', |
|
||||||
b'36161-TGL-G050\x00\x00', |
|
||||||
b'36161-TGL-G070\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TBA-A020\x00\x00', |
|
||||||
b'38897-TBA-A110\x00\x00', |
|
||||||
b'38897-TGH-A010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [ |
|
||||||
b'39494-TGL-G030\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CIVIC_BOSCH_DIESEL: { |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-59Y-G220\x00\x00', |
|
||||||
b'28101-59Y-G620\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TGN-E320\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TFK-G020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TFK-G210\x00\x00', |
|
||||||
b'77959-TGN-G220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TFK-G130\x00\x00', |
|
||||||
b'36802-TGN-G130\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TGN-E010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TFK-G130\x00\x00', |
|
||||||
b'36161-TGN-G130\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TBA-A020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CRV: { |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-T1W-A230\x00\x00', |
|
||||||
b'57114-T1W-A240\x00\x00', |
|
||||||
b'57114-TFF-A940\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T0A-A230\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T1W-A830\x00\x00', |
|
||||||
b'36161-T1W-C830\x00\x00', |
|
||||||
b'36161-T1X-A830\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CRV_5G: { |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-5RG-A020\x00\x00', |
|
||||||
b'28101-5RG-A030\x00\x00', |
|
||||||
b'28101-5RG-A040\x00\x00', |
|
||||||
b'28101-5RG-A120\x00\x00', |
|
||||||
b'28101-5RG-A220\x00\x00', |
|
||||||
b'28101-5RH-A020\x00\x00', |
|
||||||
b'28101-5RH-A030\x00\x00', |
|
||||||
b'28101-5RH-A040\x00\x00', |
|
||||||
b'28101-5RH-A120\x00\x00', |
|
||||||
b'28101-5RH-A220\x00\x00', |
|
||||||
b'28101-5RL-Q010\x00\x00', |
|
||||||
b'28101-5RM-F010\x00\x00', |
|
||||||
b'28101-5RM-K010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TLA-A040\x00\x00', |
|
||||||
b'57114-TLA-A050\x00\x00', |
|
||||||
b'57114-TLA-A060\x00\x00', |
|
||||||
b'57114-TLB-A830\x00\x00', |
|
||||||
b'57114-TMC-Z040\x00\x00', |
|
||||||
b'57114-TMC-Z050\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TLA,A040\x00\x00', |
|
||||||
b'39990-TLA-A040\x00\x00', |
|
||||||
b'39990-TLA-A110\x00\x00', |
|
||||||
b'39990-TLA-A220\x00\x00', |
|
||||||
b'39990-TME-T030\x00\x00', |
|
||||||
b'39990-TME-T120\x00\x00', |
|
||||||
b'39990-TMT-T010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [ |
|
||||||
b'46114-TLA-A040\x00\x00', |
|
||||||
b'46114-TLA-A050\x00\x00', |
|
||||||
b'46114-TLA-A930\x00\x00', |
|
||||||
b'46114-TMC-U020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TLA-A010\x00\x00', |
|
||||||
b'38897-TLA-A110\x00\x00', |
|
||||||
b'38897-TNY-G010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TLA-A040\x00\x00', |
|
||||||
b'36802-TLA-A050\x00\x00', |
|
||||||
b'36802-TLA-A060\x00\x00', |
|
||||||
b'36802-TLA-A070\x00\x00', |
|
||||||
b'36802-TMC-Q040\x00\x00', |
|
||||||
b'36802-TMC-Q070\x00\x00', |
|
||||||
b'36802-TNY-A030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TLA-A060\x00\x00', |
|
||||||
b'36161-TLA-A070\x00\x00', |
|
||||||
b'36161-TLA-A080\x00\x00', |
|
||||||
b'36161-TMC-Q020\x00\x00', |
|
||||||
b'36161-TMC-Q030\x00\x00', |
|
||||||
b'36161-TMC-Q040\x00\x00', |
|
||||||
b'36161-TNY-A020\x00\x00', |
|
||||||
b'36161-TNY-A030\x00\x00', |
|
||||||
b'36161-TNY-A040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TLA-A240\x00\x00', |
|
||||||
b'77959-TLA-A250\x00\x00', |
|
||||||
b'77959-TLA-A320\x00\x00', |
|
||||||
b'77959-TLA-A410\x00\x00', |
|
||||||
b'77959-TLA-A420\x00\x00', |
|
||||||
b'77959-TLA-Q040\x00\x00', |
|
||||||
b'77959-TLA-Z040\x00\x00', |
|
||||||
b'77959-TMM-F040\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CRV_EU: { |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-T1V-G920\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T1V-G520\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-T1V-G010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-5LH-E120\x00\x00', |
|
||||||
b'28103-5LH-E100\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T1G-G940\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CRV_HYBRID: { |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TMB-H030\x00\x00', |
|
||||||
b'57114-TPA-G020\x00\x00', |
|
||||||
b'57114-TPG-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TMA-H020\x00\x00', |
|
||||||
b'39990-TPA-G030\x00\x00', |
|
||||||
b'39990-TPG-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TMA-H110\x00\x00', |
|
||||||
b'38897-TPG-A110\x00\x00', |
|
||||||
b'38897-TPG-A210\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TMB-H510\x00\x00', |
|
||||||
b'54008-TMB-H610\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TMB-H040\x00\x00', |
|
||||||
b'36161-TPA-E050\x00\x00', |
|
||||||
b'36161-TPG-A030\x00\x00', |
|
||||||
b'36161-TPG-A040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TMB-H040\x00\x00', |
|
||||||
b'36802-TPA-E040\x00\x00', |
|
||||||
b'36802-TPG-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TLA-C320\x00\x00', |
|
||||||
b'77959-TLA-C410\x00\x00', |
|
||||||
b'77959-TLA-C420\x00\x00', |
|
||||||
b'77959-TLA-G220\x00\x00', |
|
||||||
b'77959-TLA-H240\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_FIT: { |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-T5R-L020\x00\x00', |
|
||||||
b'57114-T5R-L220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-T5R-C020\x00\x00', |
|
||||||
b'39990-T5R-C030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-T5A-J010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T5R-A040\x00\x00', |
|
||||||
b'36161-T5R-A240\x00\x00', |
|
||||||
b'36161-T5R-A520\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T5R-A230\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_FREED: { |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TDK-J010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TDK-J050\x00\x00', |
|
||||||
b'39990-TDK-N020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TDK-J120\x00\x00', |
|
||||||
b'57114-TDK-J330\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-TDK-J070\x00\x00', |
|
||||||
b'36161-TDK-J080\x00\x00', |
|
||||||
b'36161-TDK-J530\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_ODYSSEY: { |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-THR-A010\x00\x00', |
|
||||||
b'38897-THR-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-THR-A020\x00\x00', |
|
||||||
b'39990-THR-A030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-THR-A010\x00\x00', |
|
||||||
b'77959-THR-A110\x00\x00', |
|
||||||
b'77959-THR-X010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-THR-A020\x00\x00', |
|
||||||
b'36161-THR-A030\x00\x00', |
|
||||||
b'36161-THR-A110\x00\x00', |
|
||||||
b'36161-THR-A720\x00\x00', |
|
||||||
b'36161-THR-A730\x00\x00', |
|
||||||
b'36161-THR-A810\x00\x00', |
|
||||||
b'36161-THR-A910\x00\x00', |
|
||||||
b'36161-THR-C010\x00\x00', |
|
||||||
b'36161-THR-D110\x00\x00', |
|
||||||
b'36161-THR-K020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-5NZ-A110\x00\x00', |
|
||||||
b'28101-5NZ-A310\x00\x00', |
|
||||||
b'28101-5NZ-C310\x00\x00', |
|
||||||
b'28102-5MX-A001\x00\x00', |
|
||||||
b'28102-5MX-A600\x00\x00', |
|
||||||
b'28102-5MX-A610\x00\x00', |
|
||||||
b'28102-5MX-A700\x00\x00', |
|
||||||
b'28102-5MX-A710\x00\x00', |
|
||||||
b'28102-5MX-A900\x00\x00', |
|
||||||
b'28102-5MX-A910\x00\x00', |
|
||||||
b'28102-5MX-C001\x00\x00', |
|
||||||
b'28102-5MX-C610\x00\x00', |
|
||||||
b'28102-5MX-C910\x00\x00', |
|
||||||
b'28102-5MX-D001\x00\x00', |
|
||||||
b'28102-5MX-D710\x00\x00', |
|
||||||
b'28102-5MX-K610\x00\x00', |
|
||||||
b'28103-5NZ-A100\x00\x00', |
|
||||||
b'28103-5NZ-A300\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-THR-A040\x00\x00', |
|
||||||
b'57114-THR-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-THR-A020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_ODYSSEY_CHN: { |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-T6D-H220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-T6A-J010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T6A-P040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T6A-P110\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_PILOT: { |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TG7-A520\x00\x00', |
|
||||||
b'54008-TG7-A530\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-5EY-A040\x00\x00', |
|
||||||
b'28101-5EY-A050\x00\x00', |
|
||||||
b'28101-5EY-A100\x00\x00', |
|
||||||
b'28101-5EY-A430\x00\x00', |
|
||||||
b'28101-5EY-A500\x00\x00', |
|
||||||
b'28101-5EZ-A050\x00\x00', |
|
||||||
b'28101-5EZ-A060\x00\x00', |
|
||||||
b'28101-5EZ-A100\x00\x00', |
|
||||||
b'28101-5EZ-A210\x00\x00', |
|
||||||
b'28101-5EZ-A330\x00\x00', |
|
||||||
b'28101-5EZ-A430\x00\x00', |
|
||||||
b'28101-5EZ-A500\x00\x00', |
|
||||||
b'28101-5EZ-A600\x00\x00', |
|
||||||
b'28101-5EZ-A700\x00\x00', |
|
||||||
b'28103-5EY-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TG7-A030\x00\x00', |
|
||||||
b'38897-TG7-A040\x00\x00', |
|
||||||
b'38897-TG7-A110\x00\x00', |
|
||||||
b'38897-TG7-A210\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TG7-A030\x00\x00', |
|
||||||
b'39990-TG7-A040\x00\x00', |
|
||||||
b'39990-TG7-A060\x00\x00', |
|
||||||
b'39990-TG7-A070\x00\x00', |
|
||||||
b'39990-TGS-A230\x00\x00', |
|
||||||
b'39990-TGS-A320\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-TG7-A310\x00\x00', |
|
||||||
b'36161-TG7-A520\x00\x00', |
|
||||||
b'36161-TG7-A630\x00\x00', |
|
||||||
b'36161-TG7-A720\x00\x00', |
|
||||||
b'36161-TG7-A820\x00\x00', |
|
||||||
b'36161-TG7-A930\x00\x00', |
|
||||||
b'36161-TG7-C520\x00\x00', |
|
||||||
b'36161-TG7-D520\x00\x00', |
|
||||||
b'36161-TG7-D630\x00\x00', |
|
||||||
b'36161-TG7-Y630\x00\x00', |
|
||||||
b'36161-TG8-A410\x00\x00', |
|
||||||
b'36161-TG8-A520\x00\x00', |
|
||||||
b'36161-TG8-A630\x00\x00', |
|
||||||
b'36161-TG8-A720\x00\x00', |
|
||||||
b'36161-TG8-A830\x00\x00', |
|
||||||
b'36161-TGS-A030\x00\x00', |
|
||||||
b'36161-TGS-A130\x00\x00', |
|
||||||
b'36161-TGS-A220\x00\x00', |
|
||||||
b'36161-TGS-A320\x00\x00', |
|
||||||
b'36161-TGT-A030\x00\x00', |
|
||||||
b'36161-TGT-A130\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TG7-A020\x00\x00', |
|
||||||
b'77959-TG7-A110\x00\x00', |
|
||||||
b'77959-TG7-A210\x00\x00', |
|
||||||
b'77959-TG7-Y210\x00\x00', |
|
||||||
b'77959-TGS-A010\x00\x00', |
|
||||||
b'77959-TGS-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TG7-A130\x00\x00', |
|
||||||
b'57114-TG7-A140\x00\x00', |
|
||||||
b'57114-TG7-A230\x00\x00', |
|
||||||
b'57114-TG7-A240\x00\x00', |
|
||||||
b'57114-TG7-A630\x00\x00', |
|
||||||
b'57114-TG7-A730\x00\x00', |
|
||||||
b'57114-TG8-A140\x00\x00', |
|
||||||
b'57114-TG8-A230\x00\x00', |
|
||||||
b'57114-TG8-A240\x00\x00', |
|
||||||
b'57114-TG8-A630\x00\x00', |
|
||||||
b'57114-TG8-A730\x00\x00', |
|
||||||
b'57114-TGS-A530\x00\x00', |
|
||||||
b'57114-TGT-A530\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.ACURA_RDX: { |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TX4-A220\x00\x00', |
|
||||||
b'57114-TX5-A220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-TX4-A030\x00\x00', |
|
||||||
b'36161-TX5-A030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TX4-B010\x00\x00', |
|
||||||
b'77959-TX4-C010\x00\x00', |
|
||||||
b'77959-TX4-C020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.ACURA_RDX_3G: { |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TJB-A030\x00\x00', |
|
||||||
b'57114-TJB-A040\x00\x00', |
|
||||||
b'57114-TJB-A120\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TJB-A040\x00\x00', |
|
||||||
b'36802-TJB-A050\x00\x00', |
|
||||||
b'36802-TJB-A540\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TJB-A040\x00\x00', |
|
||||||
b'36161-TJB-A530\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TJB-A520\x00\x00', |
|
||||||
b'54008-TJB-A530\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28102-5YK-A610\x00\x00', |
|
||||||
b'28102-5YK-A620\x00\x00', |
|
||||||
b'28102-5YK-A630\x00\x00', |
|
||||||
b'28102-5YK-A700\x00\x00', |
|
||||||
b'28102-5YK-A711\x00\x00', |
|
||||||
b'28102-5YK-A800\x00\x00', |
|
||||||
b'28102-5YL-A620\x00\x00', |
|
||||||
b'28102-5YL-A700\x00\x00', |
|
||||||
b'28102-5YL-A711\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TJB-A040\x00\x00', |
|
||||||
b'77959-TJB-A120\x00\x00', |
|
||||||
b'77959-TJB-A210\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [ |
|
||||||
b'46114-TJB-A040\x00\x00', |
|
||||||
b'46114-TJB-A050\x00\x00', |
|
||||||
b'46114-TJB-A060\x00\x00', |
|
||||||
b'46114-TJB-A120\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TJB-A040\x00\x00', |
|
||||||
b'38897-TJB-A110\x00\x00', |
|
||||||
b'38897-TJB-A120\x00\x00', |
|
||||||
b'38897-TJB-A220\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TJB-A030\x00\x00', |
|
||||||
b'39990-TJB-A040\x00\x00', |
|
||||||
b'39990-TJB-A070\x00\x00', |
|
||||||
b'39990-TJB-A130\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_RIDGELINE: { |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-T6Z-A020\x00\x00', |
|
||||||
b'39990-T6Z-A030\x00\x00', |
|
||||||
b'39990-T6Z-A050\x00\x00', |
|
||||||
b'39990-T6Z-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T6Z-A020\x00\x00', |
|
||||||
b'36161-T6Z-A310\x00\x00', |
|
||||||
b'36161-T6Z-A420\x00\x00', |
|
||||||
b'36161-T6Z-A520\x00\x00', |
|
||||||
b'36161-T6Z-A620\x00\x00', |
|
||||||
b'36161-T6Z-A720\x00\x00', |
|
||||||
b'36161-TJZ-A120\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-T6Z-A010\x00\x00', |
|
||||||
b'38897-T6Z-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T6Z-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-T6Z-A120\x00\x00', |
|
||||||
b'57114-T6Z-A130\x00\x00', |
|
||||||
b'57114-T6Z-A520\x00\x00', |
|
||||||
b'57114-T6Z-A610\x00\x00', |
|
||||||
b'57114-TJZ-A520\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_INSIGHT: { |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TXM-A040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TXM-A070\x00\x00', |
|
||||||
b'36802-TXM-A080\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TXM-A050\x00\x00', |
|
||||||
b'36161-TXM-A060\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TXM-A230\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TXM-A030\x00\x00', |
|
||||||
b'57114-TXM-A040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TWA-A910\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TXM-A020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_HRV: { |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-T7A-A010\x00\x00', |
|
||||||
b'38897-T7A-A110\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-THX-A020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T7A-A040\x00\x00', |
|
||||||
b'36161-T7A-A140\x00\x00', |
|
||||||
b'36161-T7A-A240\x00\x00', |
|
||||||
b'36161-T7A-C440\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T7A-A230\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_HRV_3G: { |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-3M0-G110\x00\x00', |
|
||||||
b'39990-3W0-A030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-3M0-M110\x00\x00', |
|
||||||
b'38897-3W1-A010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-3M0-K840\x00\x00', |
|
||||||
b'77959-3V0-A820\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'8S102-3M6-P030\x00\x00', |
|
||||||
b'8S102-3W0-A060\x00\x00', |
|
||||||
b'8S102-3W0-AB10\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-3M6-M010\x00\x00', |
|
||||||
b'57114-3W0-A040\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-6EH-A010\x00\x00', |
|
||||||
b'28101-6JC-M310\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [ |
|
||||||
b'46114-3W0-A020\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.ACURA_ILX: { |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TX6-A010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-TV9-A140\x00\x00', |
|
||||||
b'36161-TX6-A030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TX6-A230\x00\x00', |
|
||||||
b'77959-TX6-C210\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_E: { |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-TYF-N030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-TYF-E140\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.shiftByWire, 0x18da0bf1, None): [ |
|
||||||
b'54008-TYF-E010\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-TYF-G430\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36802-TYF-E030\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x18dab5f1, None): [ |
|
||||||
b'36161-TYF-E020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-TYF-E030\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.HONDA_CIVIC_2022: { |
|
||||||
(Ecu.eps, 0x18da30f1, None): [ |
|
||||||
b'39990-T24-T120\x00\x00', |
|
||||||
b'39990-T39-A130\x00\x00', |
|
||||||
b'39990-T43-J020\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18daeff1, None): [ |
|
||||||
b'38897-T20-A020\x00\x00', |
|
||||||
b'38897-T20-A210\x00\x00', |
|
||||||
b'38897-T20-A310\x00\x00', |
|
||||||
b'38897-T20-A510\x00\x00', |
|
||||||
b'38897-T21-A010\x00\x00', |
|
||||||
b'38897-T24-Z120\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.srs, 0x18da53f1, None): [ |
|
||||||
b'77959-T20-A970\x00\x00', |
|
||||||
b'77959-T20-A980\x00\x00', |
|
||||||
b'77959-T20-M820\x00\x00', |
|
||||||
b'77959-T47-A940\x00\x00', |
|
||||||
b'77959-T47-A950\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.fwdRadar, 0x18dab0f1, None): [ |
|
||||||
b'36161-T20-A060\x00\x00', |
|
||||||
b'36161-T20-A070\x00\x00', |
|
||||||
b'36161-T20-A080\x00\x00', |
|
||||||
b'36161-T24-T070\x00\x00', |
|
||||||
b'36161-T47-A050\x00\x00', |
|
||||||
b'36161-T47-A070\x00\x00', |
|
||||||
b'8S102-T20-AA10\x00\x00', |
|
||||||
b'8S102-T47-AA10\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.vsa, 0x18da28f1, None): [ |
|
||||||
b'57114-T20-AB40\x00\x00', |
|
||||||
b'57114-T24-TB30\x00\x00', |
|
||||||
b'57114-T43-JB30\x00\x00', |
|
||||||
], |
|
||||||
(Ecu.transmission, 0x18da1ef1, None): [ |
|
||||||
b'28101-65D-A020\x00\x00', |
|
||||||
b'28101-65D-A120\x00\x00', |
|
||||||
b'28101-65H-A020\x00\x00', |
|
||||||
b'28101-65H-A120\x00\x00', |
|
||||||
b'28101-65J-N010\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
@ -1,211 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import CanBusBase |
|
||||||
from openpilot.selfdrive.car.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 |
|
||||||
|
|
||||||
|
|
||||||
class CanBus(CanBusBase): |
|
||||||
def __init__(self, CP=None, fingerprint=None) -> None: |
|
||||||
# use fingerprint if specified |
|
||||||
super().__init__(CP if fingerprint is None else None, fingerprint) |
|
||||||
|
|
||||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS): |
|
||||||
self._pt, self._radar = self.offset + 1, self.offset |
|
||||||
else: |
|
||||||
self._pt, self._radar = self.offset, self.offset + 1 |
|
||||||
|
|
||||||
@property |
|
||||||
def pt(self) -> int: |
|
||||||
return self._pt |
|
||||||
|
|
||||||
@property |
|
||||||
def radar(self) -> int: |
|
||||||
return self._radar |
|
||||||
|
|
||||||
@property |
|
||||||
def camera(self) -> int: |
|
||||||
return self.offset + 2 |
|
||||||
|
|
||||||
|
|
||||||
def get_lkas_cmd_bus(CAN, 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 CAN.pt |
|
||||||
# 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, CAN, 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, |
|
||||||
} |
|
||||||
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values) |
|
||||||
|
|
||||||
|
|
||||||
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, car_fingerprint): |
|
||||||
commands = [] |
|
||||||
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", CAN.pt, acc_control_on_values)) |
|
||||||
|
|
||||||
commands.append(packer.make_can_msg("ACC_CONTROL", CAN.pt, acc_control_values)) |
|
||||||
return commands |
|
||||||
|
|
||||||
|
|
||||||
def create_steering_control(packer, CAN, 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(CAN, car_fingerprint, radar_disabled) |
|
||||||
return packer.make_can_msg("STEERING_CONTROL", bus, values) |
|
||||||
|
|
||||||
|
|
||||||
def create_bosch_supplemental_1(packer, CAN, car_fingerprint): |
|
||||||
# non-active params |
|
||||||
values = { |
|
||||||
"SET_ME_X04": 0x04, |
|
||||||
"SET_ME_X80": 0x80, |
|
||||||
"SET_ME_X10": 0x10, |
|
||||||
} |
|
||||||
bus = get_lkas_cmd_bus(CAN, car_fingerprint) |
|
||||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) |
|
||||||
|
|
||||||
|
|
||||||
def create_ui_commands(packer, CAN, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): |
|
||||||
commands = [] |
|
||||||
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl |
|
||||||
bus_lkas = get_lkas_cmd_bus(CAN, CP.carFingerprint, radar_disabled) |
|
||||||
|
|
||||||
if CP.openpilotLongitudinalControl: |
|
||||||
acc_hud_values = { |
|
||||||
'CRUISE_SPEED': hud.v_cruise, |
|
||||||
'ENABLE_MINI_CAR': 1 if enabled else 0, |
|
||||||
# only moves the lead car without ACC_ON |
|
||||||
'HUD_DISTANCE': (hud.lead_distance_bars + 1) % 4, # wraps to 0 at 4 bars |
|
||||||
'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: |
|
||||||
# Shows the distance bars, TODO: stock camera shows updates temporarily while disabled |
|
||||||
acc_hud_values['ACC_ON'] = int(enabled) |
|
||||||
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", CAN.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', CAN.pt, radar_hud_values)) |
|
||||||
|
|
||||||
if CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH: |
|
||||||
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", CAN.pt, {})) |
|
||||||
|
|
||||||
return commands |
|
||||||
|
|
||||||
|
|
||||||
def spam_buttons_command(packer, CAN, button_val, car_fingerprint): |
|
||||||
values = { |
|
||||||
'CRUISE_BUTTONS': button_val, |
|
||||||
'CRUISE_SETTING': 0, |
|
||||||
} |
|
||||||
# send buttons to camera on radarless cars |
|
||||||
bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt |
|
||||||
return packer.make_can_msg("SCM_BUTTONS", bus, values) |
|
||||||
@ -1,215 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from panda import Panda |
|
||||||
from openpilot.selfdrive.car import get_safety_config, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import interp |
|
||||||
from openpilot.selfdrive.car.honda.hondacan import CanBus |
|
||||||
from openpilot.selfdrive.car.honda.values import CarControllerParams, HondaFlags, CAR, HONDA_BOSCH, \ |
|
||||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS |
|
||||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase |
|
||||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu |
|
||||||
|
|
||||||
TransmissionType = structs.CarParams.TransmissionType |
|
||||||
|
|
||||||
|
|
||||||
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 |
|
||||||
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: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
ret.carName = "honda" |
|
||||||
|
|
||||||
CAN = CanBus(ret, fingerprint) |
|
||||||
|
|
||||||
if candidate in HONDA_BOSCH: |
|
||||||
ret.safetyConfigs = [get_safety_config(structs.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(structs.CarParams.SafetyModel.hondaNidec)] |
|
||||||
ret.openpilotLongitudinalControl = True |
|
||||||
|
|
||||||
ret.pcmCruise = True |
|
||||||
|
|
||||||
if candidate == CAR.HONDA_CRV_5G: |
|
||||||
ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar] |
|
||||||
|
|
||||||
# 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 ICE 1.5T CVT has different gearbox message |
|
||||||
if candidate == CAR.HONDA_ACCORD and 0x191 in fingerprint[CAN.pt]: |
|
||||||
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.longitudinalActuatorDelay = 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.kiBP = [0., 5., 35.] |
|
||||||
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5] |
|
||||||
|
|
||||||
eps_modified = False |
|
||||||
for fw in car_fw: |
|
||||||
if fw.ecu == "eps" and b"," in fw.fwVersion: |
|
||||||
eps_modified = True |
|
||||||
|
|
||||||
if candidate == CAR.HONDA_CIVIC: |
|
||||||
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.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CIVIC_2022): |
|
||||||
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 == CAR.HONDA_ACCORD: |
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end |
|
||||||
|
|
||||||
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.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # 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.HONDA_CRV, CAR.HONDA_CRV_EU): |
|
||||||
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]] |
|
||||||
ret.wheelSpeedFactor = 1.025 |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_CRV_5G: |
|
||||||
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.wheelSpeedFactor = 1.025 |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_CRV_HYBRID: |
|
||||||
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.6], [0.18]] |
|
||||||
ret.wheelSpeedFactor = 1.025 |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_FIT: |
|
||||||
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.2], [0.05]] |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_FREED: |
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] |
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] |
|
||||||
|
|
||||||
elif candidate in (CAR.HONDA_HRV, CAR.HONDA_HRV_3G): |
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] |
|
||||||
if candidate == CAR.HONDA_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.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.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] |
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] |
|
||||||
|
|
||||||
elif candidate in (CAR.HONDA_ODYSSEY, CAR.HONDA_ODYSSEY_CHN): |
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] |
|
||||||
if candidate == CAR.HONDA_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.HONDA_PILOT: |
|
||||||
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.38], [0.11]] |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_RIDGELINE: |
|
||||||
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.38], [0.11]] |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_INSIGHT: |
|
||||||
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.6], [0.18]] |
|
||||||
|
|
||||||
elif candidate == CAR.HONDA_E: |
|
||||||
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.6], [0.18]] # TODO: can probably use some tuning |
|
||||||
|
|
||||||
else: |
|
||||||
raise ValueError(f"unsupported car {candidate}") |
|
||||||
|
|
||||||
# These cars use alternate user brake msg (0x1BE) |
|
||||||
# TODO: Only detect feature for Accord/Accord Hybrid, not all Bosch DBCs have BRAKE_MODULE |
|
||||||
if 0x1BE in fingerprint[CAN.pt] and candidate in (CAR.HONDA_ACCORD, CAR.HONDA_HRV_3G): |
|
||||||
ret.flags |= HondaFlags.BOSCH_ALT_BRAKE.value |
|
||||||
|
|
||||||
if ret.flags & HondaFlags.BOSCH_ALT_BRAKE: |
|
||||||
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 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.HONDA_CIVIC}) |
|
||||||
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.51 * CV.MPH_TO_MS |
|
||||||
|
|
||||||
ret.steerActuatorDelay = 0.1 |
|
||||||
ret.steerLimitTimer = 0.8 |
|
||||||
|
|
||||||
return ret |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def init(CP, can_recv, can_send): |
|
||||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl: |
|
||||||
disable_ecu(can_recv, can_send, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') |
|
||||||
@ -1,84 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
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 = structs.RadarData() |
|
||||||
|
|
||||||
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] = structs.RadarData.RadarPoint() |
|
||||||
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 |
|
||||||
@ -1,14 +0,0 @@ |
|||||||
import re |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.honda.fingerprints import FW_VERSIONS |
|
||||||
|
|
||||||
HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$" |
|
||||||
|
|
||||||
|
|
||||||
class TestHondaFingerprint: |
|
||||||
def test_fw_version_format(self): |
|
||||||
# Asserts all FW versions follow an expected format |
|
||||||
for fw_by_ecu in FW_VERSIONS.values(): |
|
||||||
for fws in fw_by_ecu.values(): |
|
||||||
for fw in fws: |
|
||||||
assert re.match(HONDA_FW_VERSION_RE, fw) is not None, fw |
|
||||||
@ -1,330 +0,0 @@ |
|||||||
from dataclasses import dataclass |
|
||||||
from enum import Enum, IntFlag |
|
||||||
|
|
||||||
from panda.python import uds |
|
||||||
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 |
|
||||||
|
|
||||||
Ecu = structs.CarParams.Ecu |
|
||||||
VisualAlert = structs.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): |
|
||||||
# Detected flags |
|
||||||
# Bosch models with alternate set of LKAS_HUD messages |
|
||||||
BOSCH_EXT_HUD = 1 |
|
||||||
BOSCH_ALT_BRAKE = 2 |
|
||||||
|
|
||||||
# Static flags |
|
||||||
BOSCH = 4 |
|
||||||
BOSCH_RADARLESS = 8 |
|
||||||
|
|
||||||
NIDEC = 16 |
|
||||||
NIDEC_ALT_PCM_ACCEL = 32 |
|
||||||
NIDEC_ALT_SCM_MESSAGES = 64 |
|
||||||
|
|
||||||
|
|
||||||
# Car button codes |
|
||||||
class CruiseButtons: |
|
||||||
RES_ACCEL = 4 |
|
||||||
DECEL_SET = 3 |
|
||||||
CANCEL = 2 |
|
||||||
MAIN = 1 |
|
||||||
|
|
||||||
|
|
||||||
class CruiseSettings: |
|
||||||
DISTANCE = 3 |
|
||||||
LKAS = 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 |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class HondaCarDocs(CarDocs): |
|
||||||
package: str = "Honda Sensing" |
|
||||||
|
|
||||||
def init_make(self, CP: structs.CarParams): |
|
||||||
if CP.flags & HondaFlags.BOSCH: |
|
||||||
self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.flags & HondaFlags.BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a]) |
|
||||||
else: |
|
||||||
self.car_parts = CarParts.common([CarHarness.nidec]) |
|
||||||
|
|
||||||
|
|
||||||
class Footnote(Enum): |
|
||||||
CIVIC_DIESEL = CarFootnote( |
|
||||||
"2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.", |
|
||||||
Column.FSR_STEERING) |
|
||||||
|
|
||||||
|
|
||||||
class HondaBoschPlatformConfig(PlatformConfig): |
|
||||||
def init(self): |
|
||||||
self.flags |= HondaFlags.BOSCH |
|
||||||
|
|
||||||
|
|
||||||
class HondaNidecPlatformConfig(PlatformConfig): |
|
||||||
def init(self): |
|
||||||
self.flags |= HondaFlags.NIDEC |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
# Bosch Cars |
|
||||||
HONDA_ACCORD = HondaBoschPlatformConfig( |
|
||||||
[ |
|
||||||
HondaCarDocs("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), |
|
||||||
HondaCarDocs("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), |
|
||||||
HondaCarDocs("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), |
|
||||||
], |
|
||||||
# steerRatio: 11.82 is spec end-to-end |
|
||||||
CarSpecs(mass=3279 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.33, centerToFrontRatio=0.39, tireStiffnessFactor=0.8467), |
|
||||||
dbc_dict('honda_accord_2018_can_generated', None), |
|
||||||
) |
|
||||||
HONDA_CIVIC_BOSCH = HondaBoschPlatformConfig( |
|
||||||
[ |
|
||||||
HondaCarDocs("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), |
|
||||||
HondaCarDocs("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), |
|
||||||
], |
|
||||||
CarSpecs(mass=1326, wheelbase=2.7, steerRatio=15.38, centerToFrontRatio=0.4), # steerRatio: 10.93 is end-to-end spec |
|
||||||
dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), |
|
||||||
) |
|
||||||
HONDA_CIVIC_BOSCH_DIESEL = HondaBoschPlatformConfig( |
|
||||||
[], # don't show in docs |
|
||||||
HONDA_CIVIC_BOSCH.specs, |
|
||||||
dbc_dict('honda_accord_2018_can_generated', None), |
|
||||||
) |
|
||||||
HONDA_CIVIC_2022 = HondaBoschPlatformConfig( |
|
||||||
[ |
|
||||||
HondaCarDocs("Honda Civic 2022-24", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), |
|
||||||
HondaCarDocs("Honda Civic Hatchback 2022-24", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), |
|
||||||
], |
|
||||||
HONDA_CIVIC_BOSCH.specs, |
|
||||||
dbc_dict('honda_civic_ex_2022_can_generated', None), |
|
||||||
flags=HondaFlags.BOSCH_RADARLESS, |
|
||||||
) |
|
||||||
HONDA_CRV_5G = HondaBoschPlatformConfig( |
|
||||||
[HondaCarDocs("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
# steerRatio: 12.3 is spec end-to-end |
|
||||||
CarSpecs(mass=3410 * CV.LB_TO_KG, wheelbase=2.66, steerRatio=16.0, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), |
|
||||||
dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'), |
|
||||||
flags=HondaFlags.BOSCH_ALT_BRAKE, |
|
||||||
) |
|
||||||
HONDA_CRV_HYBRID = HondaBoschPlatformConfig( |
|
||||||
[HondaCarDocs("Honda CR-V Hybrid 2017-21", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
# mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end |
|
||||||
CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), |
|
||||||
dbc_dict('honda_accord_2018_can_generated', None), |
|
||||||
) |
|
||||||
HONDA_HRV_3G = HondaBoschPlatformConfig( |
|
||||||
[HondaCarDocs("Honda HR-V 2023", "All")], |
|
||||||
CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.61, steerRatio=15.2, centerToFrontRatio=0.41, tireStiffnessFactor=0.5), |
|
||||||
dbc_dict('honda_civic_ex_2022_can_generated', None), |
|
||||||
flags=HondaFlags.BOSCH_RADARLESS, |
|
||||||
) |
|
||||||
ACURA_RDX_3G = HondaBoschPlatformConfig( |
|
||||||
[HondaCarDocs("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=4068 * CV.LB_TO_KG, wheelbase=2.75, steerRatio=11.95, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), # as spec |
|
||||||
dbc_dict('acura_rdx_2020_can_generated', None), |
|
||||||
flags=HondaFlags.BOSCH_ALT_BRAKE, |
|
||||||
) |
|
||||||
HONDA_INSIGHT = HondaBoschPlatformConfig( |
|
||||||
[HondaCarDocs("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.0, centerToFrontRatio=0.39, tireStiffnessFactor=0.82), # as spec |
|
||||||
dbc_dict('honda_insight_ex_2019_can_generated', None), |
|
||||||
) |
|
||||||
HONDA_E = HondaBoschPlatformConfig( |
|
||||||
[HondaCarDocs("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=3338.8 * CV.LB_TO_KG, wheelbase=2.5, centerToFrontRatio=0.5, steerRatio=16.71, tireStiffnessFactor=0.82), |
|
||||||
dbc_dict('acura_rdx_2020_can_generated', None), |
|
||||||
) |
|
||||||
|
|
||||||
# Nidec Cars |
|
||||||
ACURA_ILX = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=3095 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=18.61, centerToFrontRatio=0.37, tireStiffnessFactor=0.72), # 15.3 is spec end-to-end |
|
||||||
dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_CRV = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=3572 * CV.LB_TO_KG, wheelbase=2.62, steerRatio=16.89, centerToFrontRatio=0.41, tireStiffnessFactor=0.444), # as spec |
|
||||||
dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_CRV_EU = HondaNidecPlatformConfig( |
|
||||||
[], # Euro version of CRV Touring, don't show in docs |
|
||||||
HONDA_CRV.specs, |
|
||||||
dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_FIT = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=2644 * CV.LB_TO_KG, wheelbase=2.53, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), |
|
||||||
dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_FREED = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=3086. * CV.LB_TO_KG, wheelbase=2.74, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), # mostly copied from FIT |
|
||||||
dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_HRV = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
HONDA_HRV_3G.specs, |
|
||||||
dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_ODYSSEY = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda Odyssey 2018-20")], |
|
||||||
CarSpecs(mass=1900, wheelbase=3.0, steerRatio=14.35, centerToFrontRatio=0.41, tireStiffnessFactor=0.82), |
|
||||||
dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_PCM_ACCEL, |
|
||||||
) |
|
||||||
HONDA_ODYSSEY_CHN = HondaNidecPlatformConfig( |
|
||||||
[], # Chinese version of Odyssey, don't show in docs |
|
||||||
HONDA_ODYSSEY.specs, |
|
||||||
dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
ACURA_RDX = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=3925 * CV.LB_TO_KG, wheelbase=2.68, steerRatio=15.0, centerToFrontRatio=0.38, tireStiffnessFactor=0.444), # as spec |
|
||||||
dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_PILOT = HondaNidecPlatformConfig( |
|
||||||
[ |
|
||||||
HondaCarDocs("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), |
|
||||||
HondaCarDocs("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), |
|
||||||
], |
|
||||||
CarSpecs(mass=4278 * CV.LB_TO_KG, wheelbase=2.86, centerToFrontRatio=0.428, steerRatio=16.0, tireStiffnessFactor=0.444), # as spec |
|
||||||
dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_RIDGELINE = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda Ridgeline 2017-24", min_steer_speed=12. * CV.MPH_TO_MS)], |
|
||||||
CarSpecs(mass=4515 * CV.LB_TO_KG, wheelbase=3.18, centerToFrontRatio=0.41, steerRatio=15.59, tireStiffnessFactor=0.444), # as spec |
|
||||||
dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, |
|
||||||
) |
|
||||||
HONDA_CIVIC = HondaNidecPlatformConfig( |
|
||||||
[HondaCarDocs("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE")], |
|
||||||
CarSpecs(mass=1326, wheelbase=2.70, centerToFrontRatio=0.4, steerRatio=15.38), # 10.93 is end-to-end spec |
|
||||||
dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
HONDA_ALT_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ |
|
||||||
p16(0xF112) |
|
||||||
HONDA_ALT_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 manufacturer-specific identifier for current ECUs |
|
||||||
Request( |
|
||||||
[HONDA_ALT_VERSION_REQUEST], |
|
||||||
[HONDA_ALT_VERSION_RESPONSE], |
|
||||||
bus=1, |
|
||||||
logging=True, |
|
||||||
), |
|
||||||
# Nidec PT bus |
|
||||||
Request( |
|
||||||
[StdQueries.UDS_VERSION_REQUEST], |
|
||||||
[StdQueries.UDS_VERSION_RESPONSE], |
|
||||||
bus=0, |
|
||||||
), |
|
||||||
# Bosch PT bus |
|
||||||
Request( |
|
||||||
[StdQueries.UDS_VERSION_REQUEST], |
|
||||||
[StdQueries.UDS_VERSION_RESPONSE], |
|
||||||
bus=1, |
|
||||||
obd_multiplexing=False, |
|
||||||
), |
|
||||||
], |
|
||||||
# We lose these ECUs without the comma power on these cars. |
|
||||||
# Note that we still attempt to match with them when they are present |
|
||||||
# This is or'd with (ALL_ECUS - ESSENTIAL_ECUS) from fw_versions.py |
|
||||||
non_essential_ecus={ |
|
||||||
Ecu.eps: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_2022, CAR.HONDA_E, CAR.HONDA_HRV_3G], |
|
||||||
Ecu.vsa: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_2022, CAR.HONDA_CRV_5G, CAR.HONDA_CRV_HYBRID, |
|
||||||
CAR.HONDA_E, CAR.HONDA_HRV_3G, CAR.HONDA_INSIGHT], |
|
||||||
}, |
|
||||||
extra_ecus=[ |
|
||||||
(Ecu.combinationMeter, 0x18da60f1, None), |
|
||||||
(Ecu.programmedFuelInjection, 0x18da10f1, None), |
|
||||||
# The only other ECU on PT bus accessible by camera on radarless Civic |
|
||||||
# This is likely a manufacturer-specific sub-address implementation: the camera responds to this and 0x18dab0f1 |
|
||||||
# Unclear what the part number refers to: 8S103 is 'Camera Set Mono', while 36160 is 'Camera Monocular - Honda' |
|
||||||
# TODO: add query back, camera does not support querying both in parallel and 0x18dab0f1 often fails to respond |
|
||||||
# (Ecu.unknown, 0x18DAB3F1, None), |
|
||||||
], |
|
||||||
) |
|
||||||
|
|
||||||
STEER_THRESHOLD = { |
|
||||||
# default is 1200, overrides go here |
|
||||||
CAR.ACURA_RDX: 400, |
|
||||||
CAR.HONDA_CRV_EU: 400, |
|
||||||
} |
|
||||||
|
|
||||||
HONDA_NIDEC_ALT_PCM_ACCEL = CAR.with_flags(HondaFlags.NIDEC_ALT_PCM_ACCEL) |
|
||||||
HONDA_NIDEC_ALT_SCM_MESSAGES = CAR.with_flags(HondaFlags.NIDEC_ALT_SCM_MESSAGES) |
|
||||||
HONDA_BOSCH = CAR.with_flags(HondaFlags.BOSCH) |
|
||||||
HONDA_BOSCH_RADARLESS = CAR.with_flags(HondaFlags.BOSCH_RADARLESS) |
|
||||||
|
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,207 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip |
|
||||||
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican |
|
||||||
from openpilot.selfdrive.car.hyundai.carstate import CarState |
|
||||||
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus |
|
||||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
|
|
||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert |
|
||||||
LongCtrlState = structs.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(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.CAN = CanBus(CP) |
|
||||||
self.params = CarControllerParams(CP) |
|
||||||
self.packer = CANPacker(dbc_name) |
|
||||||
self.angle_limit_counter = 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(make_tester_present_msg(addr, bus, suppress_response=True)) |
|
||||||
|
|
||||||
# for blinkers |
|
||||||
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: |
|
||||||
can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True)) |
|
||||||
|
|
||||||
# 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, hud_control)) |
|
||||||
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.CP, 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, 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 = copy.copy(actuators) |
|
||||||
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: structs.CarControl, CS: 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)) |
|
||||||
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)] * 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 |
|
||||||
@ -1,380 +0,0 @@ |
|||||||
from collections import deque |
|
||||||
import copy |
|
||||||
import math |
|
||||||
|
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
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, Buttons, CarControllerParams |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
|
|
||||||
PREV_BUTTON_SAMPLES = 8 |
|
||||||
CLUSTER_SAMPLE_RATE = 20 # frames |
|
||||||
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS |
|
||||||
|
|
||||||
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, |
|
||||||
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} |
|
||||||
|
|
||||||
|
|
||||||
class CarState(CarStateBase): |
|
||||||
def __init__(self, CP): |
|
||||||
super().__init__(CP) |
|
||||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) |
|
||||||
|
|
||||||
self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) |
|
||||||
self.main_buttons: deque = 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.flags & HyundaiFlags.EV else \ |
|
||||||
"ACCELERATOR_ALT" if CP.flags & HyundaiFlags.HYBRID 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, *_) -> structs.CarState: |
|
||||||
if self.CP.carFingerprint in CANFD_CAR: |
|
||||||
return self.update_canfd(cp, cp_cam) |
|
||||||
|
|
||||||
ret = structs.CarState() |
|
||||||
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.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1 |
|
||||||
ret.espActive = cp.vl["TCS11"]["ABS_ACT"] == 1 |
|
||||||
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED |
|
||||||
|
|
||||||
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): |
|
||||||
if self.CP.flags & HyundaiFlags.HYBRID: |
|
||||||
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.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): |
|
||||||
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 |
|
||||||
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"]) |
|
||||||
|
|
||||||
if self.CP.openpilotLongitudinalControl: |
|
||||||
ret.buttonEvents = create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT) |
|
||||||
|
|
||||||
return ret |
|
||||||
|
|
||||||
def update_canfd(self, cp, cp_cam) -> structs.CarState: |
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
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.flags & (HyundaiFlags.EV | HyundaiFlags.HYBRID): |
|
||||||
offset = 255. if self.CP.flags & HyundaiFlags.EV 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.HYUNDAI_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.flags & HyundaiFlags.EV: |
|
||||||
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1 |
|
||||||
|
|
||||||
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"]) |
|
||||||
|
|
||||||
if self.CP.openpilotLongitudinalControl: |
|
||||||
ret.buttonEvents = create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT) |
|
||||||
|
|
||||||
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), |
|
||||||
("TCS11", 100), |
|
||||||
("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.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): |
|
||||||
messages.append(("E_EMS11", 50)) |
|
||||||
else: |
|
||||||
messages += [ |
|
||||||
("EMS12", 100), |
|
||||||
("EMS16", 100), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): |
|
||||||
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.flags & HyundaiFlags.EV: |
|
||||||
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) |
|
||||||
File diff suppressed because it is too large
Load Diff
@ -1,213 +0,0 @@ |
|||||||
import crcmod |
|
||||||
from openpilot.selfdrive.car.hyundai.values import CAR, HyundaiFlags |
|
||||||
|
|
||||||
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) |
|
||||||
|
|
||||||
def create_lkas11(packer, frame, CP, 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 CP.carFingerprint in (CAR.HYUNDAI_SONATA, CAR.HYUNDAI_PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.HYUNDAI_SANTA_FE, |
|
||||||
CAR.HYUNDAI_IONIQ_EV_2020, CAR.HYUNDAI_IONIQ_PHEV, CAR.KIA_SELTOS, CAR.HYUNDAI_ELANTRA_2021, CAR.GENESIS_G70_2020, |
|
||||||
CAR.HYUNDAI_ELANTRA_HEV_2021, CAR.HYUNDAI_SONATA_HYBRID, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022, |
|
||||||
CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_IONIQ_HEV_2022, CAR.HYUNDAI_SANTA_FE_HEV_2022, |
|
||||||
CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, |
|
||||||
CAR.HYUNDAI_AZERA_6TH_GEN, CAR.HYUNDAI_AZERA_HEV_6TH_GEN, CAR.HYUNDAI_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 CP.carFingerprint 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 CP.carFingerprint == 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)[1] |
|
||||||
|
|
||||||
if CP.flags & HyundaiFlags.CHECKSUM_CRC8: |
|
||||||
# CRC Checksum as seen on 2019 Hyundai Santa Fe |
|
||||||
dat = dat[:6] + dat[7:8] |
|
||||||
checksum = hyundai_checksum(dat) |
|
||||||
elif CP.flags & HyundaiFlags.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, CP): |
|
||||||
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 CP.flags & HyundaiFlags.CAMERA_SCC 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, hud_control, set_speed, stopping, long_override, use_fca): |
|
||||||
commands = [] |
|
||||||
|
|
||||||
scc11_values = { |
|
||||||
"MainMode_ACC": 1, |
|
||||||
"TauGapSet": hud_control.leadDistanceBars, |
|
||||||
"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)[1] |
|
||||||
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 hud_control.leadVisible 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)[1] |
|
||||||
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) |
|
||||||
@ -1,223 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import CanBusBase |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip |
|
||||||
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, hud_control): |
|
||||||
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": hud_control.leadDistanceBars, |
|
||||||
} |
|
||||||
|
|
||||||
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 |
|
||||||
@ -1,147 +0,0 @@ |
|||||||
from panda import Panda |
|
||||||
from openpilot.selfdrive.car import get_safety_config, structs |
|
||||||
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.interfaces import CarInterfaceBase |
|
||||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu |
|
||||||
|
|
||||||
Ecu = structs.CarParams.Ecu |
|
||||||
|
|
||||||
ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) |
|
||||||
|
|
||||||
|
|
||||||
class CarInterface(CarInterfaceBase): |
|
||||||
@staticmethod |
|
||||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
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 if car is hybrid |
|
||||||
if 0x105 in fingerprint[CAN.ECAN]: |
|
||||||
ret.flags |= HyundaiFlags.HYBRID.value |
|
||||||
elif candidate in EV_CAR: |
|
||||||
ret.flags |= HyundaiFlags.EV.value |
|
||||||
|
|
||||||
# detect HDA2 with ADAS Driving ECU |
|
||||||
if hda2: |
|
||||||
ret.flags |= HyundaiFlags.CANFD_HDA2.value |
|
||||||
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: |
|
||||||
# TODO: detect EV and hybrid |
|
||||||
if candidate in HYBRID_CAR: |
|
||||||
ret.flags |= HyundaiFlags.HYBRID.value |
|
||||||
elif candidate in EV_CAR: |
|
||||||
ret.flags |= HyundaiFlags.EV.value |
|
||||||
|
|
||||||
# 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 == CAR.KIA_OPTIMA_G4_FL: |
|
||||||
ret.steerActuatorDelay = 0.2 |
|
||||||
|
|
||||||
# *** longitudinal control *** |
|
||||||
if candidate in CANFD_CAR: |
|
||||||
ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR) |
|
||||||
else: |
|
||||||
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.longitudinalActuatorDelay = 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 *** |
|
||||||
if candidate in CANFD_CAR: |
|
||||||
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiCanfd), ] |
|
||||||
if CAN.ECAN >= 4: |
|
||||||
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput)) |
|
||||||
ret.safetyConfigs = cfgs |
|
||||||
|
|
||||||
if ret.flags & HyundaiFlags.CANFD_HDA2: |
|
||||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 |
|
||||||
if 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: |
|
||||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC |
|
||||||
else: |
|
||||||
if candidate in LEGACY_SAFETY_MODE_CAR: |
|
||||||
# these cars require a special panda safety mode due to missing counters and checksums in the messages |
|
||||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiLegacy)] |
|
||||||
else: |
|
||||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundai, 0)] |
|
||||||
|
|
||||||
if candidate in CAMERA_SCC_CAR: |
|
||||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC |
|
||||||
|
|
||||||
if ret.openpilotLongitudinalControl: |
|
||||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG |
|
||||||
if ret.flags & HyundaiFlags.HYBRID: |
|
||||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS |
|
||||||
elif ret.flags & HyundaiFlags.EV: |
|
||||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS |
|
||||||
|
|
||||||
if candidate in (CAR.HYUNDAI_KONA, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_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, can_recv, can_send): |
|
||||||
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(can_recv, can_send, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') |
|
||||||
|
|
||||||
# for blinkers |
|
||||||
if CP.flags & HyundaiFlags.ENABLE_BLINKERS: |
|
||||||
disable_ecu(can_recv, can_send, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') |
|
||||||
@ -1,79 +0,0 @@ |
|||||||
import math |
|
||||||
|
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
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 = structs.RadarData() |
|
||||||
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] = structs.RadarData.RadarPoint() |
|
||||||
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 |
|
||||||
@ -1,21 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.hyundai.values import PLATFORM_CODE_ECUS, get_platform_codes |
|
||||||
from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
print() |
|
||||||
print(car_model) |
|
||||||
for ecu in sorted(ecus): |
|
||||||
if ecu[0] not in PLATFORM_CODE_ECUS: |
|
||||||
continue |
|
||||||
|
|
||||||
platform_codes = get_platform_codes(ecus[ecu]) |
|
||||||
codes = {code for code, _ in platform_codes} |
|
||||||
dates = {date for _, date in platform_codes if date is not None} |
|
||||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):') |
|
||||||
print(f' Codes: {codes}') |
|
||||||
print(f' Dates: {dates}') |
|
||||||
@ -1,235 +0,0 @@ |
|||||||
from hypothesis import settings, given, strategies as st |
|
||||||
|
|
||||||
import pytest |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import gen_empty_fingerprint |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.fw_versions import build_fw_dict |
|
||||||
from openpilot.selfdrive.car.hyundai.interface import CarInterface |
|
||||||
from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR |
|
||||||
from openpilot.selfdrive.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \ |
|
||||||
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \ |
|
||||||
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ |
|
||||||
HyundaiFlags, get_platform_codes |
|
||||||
from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
# Some platforms have date codes in a different format we don't yet parse (or are missing). |
|
||||||
# For now, assert list of expected missing date cars |
|
||||||
NO_DATES_PLATFORMS = { |
|
||||||
# CAN FD |
|
||||||
CAR.KIA_SPORTAGE_5TH_GEN, |
|
||||||
CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN, |
|
||||||
CAR.HYUNDAI_TUCSON_4TH_GEN, |
|
||||||
# CAN |
|
||||||
CAR.HYUNDAI_ELANTRA, |
|
||||||
CAR.HYUNDAI_ELANTRA_GT_I30, |
|
||||||
CAR.KIA_CEED, |
|
||||||
CAR.KIA_FORTE, |
|
||||||
CAR.KIA_OPTIMA_G4, |
|
||||||
CAR.KIA_OPTIMA_G4_FL, |
|
||||||
CAR.KIA_SORENTO, |
|
||||||
CAR.HYUNDAI_KONA, |
|
||||||
CAR.HYUNDAI_KONA_EV, |
|
||||||
CAR.HYUNDAI_KONA_EV_2022, |
|
||||||
CAR.HYUNDAI_KONA_HEV, |
|
||||||
CAR.HYUNDAI_SONATA_LF, |
|
||||||
CAR.HYUNDAI_VELOSTER, |
|
||||||
} |
|
||||||
|
|
||||||
CANFD_EXPECTED_ECUS = {Ecu.fwdCamera, Ecu.fwdRadar} |
|
||||||
|
|
||||||
|
|
||||||
class TestHyundaiFingerprint: |
|
||||||
def test_feature_detection(self): |
|
||||||
# HDA2 |
|
||||||
for has_adas in (True, False): |
|
||||||
car_fw = [CarParams.CarFw(ecu=Ecu.adas if has_adas else Ecu.fwdCamera)] |
|
||||||
CP = CarInterface.get_params(CAR.KIA_EV6, gen_empty_fingerprint(), car_fw, False, False) |
|
||||||
assert bool(CP.flags & HyundaiFlags.CANFD_HDA2) == has_adas |
|
||||||
|
|
||||||
# radar available |
|
||||||
for radar in (True, False): |
|
||||||
fingerprint = gen_empty_fingerprint() |
|
||||||
if radar: |
|
||||||
fingerprint[1][RADAR_START_ADDR] = 8 |
|
||||||
CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, car_fw, False, False) |
|
||||||
assert CP.radarUnavailable != radar |
|
||||||
|
|
||||||
def test_can_features(self): |
|
||||||
# Test no EV/HEV in any gear lists (should all use ELECT_GEAR) |
|
||||||
assert set.union(*CAN_GEARS.values()) & (HYBRID_CAR | EV_CAR) == set() |
|
||||||
|
|
||||||
# Test CAN FD car not in CAN feature lists |
|
||||||
can_specific_feature_list = set.union(*CAN_GEARS.values(), *CHECKSUM.values(), LEGACY_SAFETY_MODE_CAR, UNSUPPORTED_LONGITUDINAL_CAR, CAMERA_SCC_CAR) |
|
||||||
for car_model in CANFD_CAR: |
|
||||||
assert car_model not in can_specific_feature_list, "CAN FD car unexpectedly found in a CAN feature list" |
|
||||||
|
|
||||||
def test_hybrid_ev_sets(self): |
|
||||||
assert HYBRID_CAR & EV_CAR == set(), "Shared cars between hybrid and EV" |
|
||||||
assert CANFD_CAR & HYBRID_CAR == set(), "Hard coding CAN FD cars as hybrid is no longer supported" |
|
||||||
|
|
||||||
def test_canfd_ecu_whitelist(self): |
|
||||||
# Asserts only expected Ecus can exist in database for CAN-FD cars |
|
||||||
for car_model in CANFD_CAR: |
|
||||||
ecus = {fw[0] for fw in FW_VERSIONS[car_model].keys()} |
|
||||||
ecus_not_in_whitelist = ecus - CANFD_EXPECTED_ECUS |
|
||||||
ecu_strings = ", ".join([f"Ecu.{ecu}" for ecu in ecus_not_in_whitelist]) |
|
||||||
assert len(ecus_not_in_whitelist) == 0, \ |
|
||||||
f"{car_model}: Car model has unexpected ECUs: {ecu_strings}" |
|
||||||
|
|
||||||
def test_blacklisted_parts(self, subtests): |
|
||||||
# Asserts no ECUs known to be shared across platforms exist in the database. |
|
||||||
# Tucson having Santa Cruz camera and EPS for example |
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
with subtests.test(car_model=car_model.value): |
|
||||||
if car_model == CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN: |
|
||||||
pytest.skip("Skip checking Santa Cruz for its parts") |
|
||||||
|
|
||||||
for code, _ in get_platform_codes(ecus[(Ecu.fwdCamera, 0x7c4, None)]): |
|
||||||
if b"-" not in code: |
|
||||||
continue |
|
||||||
part = code.split(b"-")[1] |
|
||||||
assert not part.startswith(b'CW'), "Car has bad part number" |
|
||||||
|
|
||||||
def test_correct_ecu_response_database(self, subtests): |
|
||||||
""" |
|
||||||
Assert standard responses for certain ECUs, since they can |
|
||||||
respond to multiple queries with different data |
|
||||||
""" |
|
||||||
expected_fw_prefix = HYUNDAI_VERSION_REQUEST_LONG[1:] |
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
with subtests.test(car_model=car_model.value): |
|
||||||
for ecu, fws in ecus.items(): |
|
||||||
assert all(fw.startswith(expected_fw_prefix) for fw in fws), \ |
|
||||||
f"FW from unexpected request in database: {(ecu, fws)}" |
|
||||||
|
|
||||||
@settings(max_examples=100) |
|
||||||
@given(data=st.data()) |
|
||||||
def test_platform_codes_fuzzy_fw(self, data): |
|
||||||
"""Ensure function doesn't raise an exception""" |
|
||||||
fw_strategy = st.lists(st.binary()) |
|
||||||
fws = data.draw(fw_strategy) |
|
||||||
get_platform_codes(fws) |
|
||||||
|
|
||||||
def test_expected_platform_codes(self, subtests): |
|
||||||
# Ensures we don't accidentally add multiple platform codes for a car unless it is intentional |
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
with subtests.test(car_model=car_model.value): |
|
||||||
for ecu, fws in ecus.items(): |
|
||||||
if ecu[0] not in PLATFORM_CODE_ECUS: |
|
||||||
continue |
|
||||||
|
|
||||||
# Third and fourth character are usually EV/hybrid identifiers |
|
||||||
codes = {code.split(b"-")[0][:2] for code, _ in get_platform_codes(fws)} |
|
||||||
if car_model == CAR.HYUNDAI_PALISADE: |
|
||||||
assert codes == {b"LX", b"ON"}, f"Car has unexpected platform codes: {car_model} {codes}" |
|
||||||
elif car_model == CAR.HYUNDAI_KONA_EV and ecu[0] == Ecu.fwdCamera: |
|
||||||
assert codes == {b"OE", b"OS"}, f"Car has unexpected platform codes: {car_model} {codes}" |
|
||||||
else: |
|
||||||
assert len(codes) == 1, f"Car has multiple platform codes: {car_model} {codes}" |
|
||||||
|
|
||||||
# Tests for platform codes, part numbers, and FW dates which Hyundai will use to fuzzy |
|
||||||
# fingerprint in the absence of full FW matches: |
|
||||||
def test_platform_code_ecus_available(self, subtests): |
|
||||||
# TODO: add queries for these non-CAN FD cars to get EPS |
|
||||||
no_eps_platforms = CANFD_CAR | {CAR.KIA_SORENTO, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, |
|
||||||
CAR.KIA_OPTIMA_H_G4_FL, CAR.HYUNDAI_SONATA_LF, CAR.HYUNDAI_TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA} |
|
||||||
|
|
||||||
# Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms |
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
with subtests.test(car_model=car_model.value): |
|
||||||
for platform_code_ecu in PLATFORM_CODE_ECUS: |
|
||||||
if platform_code_ecu in (Ecu.fwdRadar, Ecu.eps) and car_model == CAR.HYUNDAI_GENESIS: |
|
||||||
continue |
|
||||||
if platform_code_ecu == Ecu.eps and car_model in no_eps_platforms: |
|
||||||
continue |
|
||||||
assert platform_code_ecu in [e[0] for e in ecus] |
|
||||||
|
|
||||||
def test_fw_format(self, subtests): |
|
||||||
# Asserts: |
|
||||||
# - every supported ECU FW version returns one platform code |
|
||||||
# - every supported ECU FW version has a part number |
|
||||||
# - expected parsing of ECU FW dates |
|
||||||
|
|
||||||
for car_model, ecus in FW_VERSIONS.items(): |
|
||||||
with subtests.test(car_model=car_model.value): |
|
||||||
for ecu, fws in ecus.items(): |
|
||||||
if ecu[0] not in PLATFORM_CODE_ECUS: |
|
||||||
continue |
|
||||||
|
|
||||||
codes = set() |
|
||||||
for fw in fws: |
|
||||||
result = get_platform_codes([fw]) |
|
||||||
assert 1 == len(result), f"Unable to parse FW: {fw}" |
|
||||||
codes |= result |
|
||||||
|
|
||||||
if ecu[0] not in DATE_FW_ECUS or car_model in NO_DATES_PLATFORMS: |
|
||||||
assert all(date is None for _, date in codes) |
|
||||||
else: |
|
||||||
assert all(date is not None for _, date in codes) |
|
||||||
|
|
||||||
if car_model == CAR.HYUNDAI_GENESIS: |
|
||||||
pytest.skip("No part numbers for car model") |
|
||||||
|
|
||||||
# Hyundai places the ECU part number in their FW versions, assert all parsable |
|
||||||
# Some examples of valid formats: b"56310-L0010", b"56310L0010", b"56310/M6300" |
|
||||||
assert all(b"-" in code for code, _ in codes), \ |
|
||||||
f"FW does not have part number: {fw}" |
|
||||||
|
|
||||||
def test_platform_codes_spot_check(self): |
|
||||||
# Asserts basic platform code parsing behavior for a few cases |
|
||||||
results = get_platform_codes([b"\xf1\x00DH LKAS 1.1 -150210"]) |
|
||||||
assert results == {(b"DH", b"150210")} |
|
||||||
|
|
||||||
# Some cameras and all radars do not have dates |
|
||||||
results = get_platform_codes([b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 "]) |
|
||||||
assert results == {(b"AEhe-G2000", None)} |
|
||||||
|
|
||||||
results = get_platform_codes([b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 "]) |
|
||||||
assert results == {(b"CV1-CV000", None)} |
|
||||||
|
|
||||||
results = get_platform_codes([ |
|
||||||
b"\xf1\x00DH LKAS 1.1 -150210", |
|
||||||
b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ", |
|
||||||
b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ", |
|
||||||
]) |
|
||||||
assert results == {(b"DH", b"150210"), (b"AEhe-G2000", None), (b"CV1-CV000", None)} |
|
||||||
|
|
||||||
results = get_platform_codes([ |
|
||||||
b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.07 99211-S8100 220222", |
|
||||||
b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 211103", |
|
||||||
b"\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 190405", |
|
||||||
b"\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 190720", |
|
||||||
]) |
|
||||||
assert results == {(b"LX2-S8100", b"220222"), (b"LX2-S8100", b"211103"), |
|
||||||
(b"ON-S9100", b"190405"), (b"ON-S9100", b"190720")} |
|
||||||
|
|
||||||
def test_fuzzy_excluded_platforms(self): |
|
||||||
# Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared. |
|
||||||
# This list can be shrunk as we combine platforms and detect features |
|
||||||
excluded_platforms = { |
|
||||||
CAR.GENESIS_G70, # shared platform code, part number, and date |
|
||||||
CAR.GENESIS_G70_2020, |
|
||||||
} |
|
||||||
excluded_platforms |= CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST # shared platform codes |
|
||||||
excluded_platforms |= NO_DATES_PLATFORMS # date codes are required to match |
|
||||||
|
|
||||||
platforms_with_shared_codes = set() |
|
||||||
for platform, fw_by_addr in FW_VERSIONS.items(): |
|
||||||
car_fw = [] |
|
||||||
for ecu, fw_versions in fw_by_addr.items(): |
|
||||||
ecu_name, addr, sub_addr = ecu |
|
||||||
for fw in fw_versions: |
|
||||||
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr, |
|
||||||
subAddress=0 if sub_addr is None else sub_addr)) |
|
||||||
|
|
||||||
CP = CarParams(carFw=car_fw) |
|
||||||
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS) |
|
||||||
if len(matches) == 1: |
|
||||||
assert list(matches)[0] == platform |
|
||||||
else: |
|
||||||
platforms_with_shared_codes.add(platform) |
|
||||||
|
|
||||||
assert platforms_with_shared_codes == excluded_platforms |
|
||||||
@ -1,752 +0,0 @@ |
|||||||
import re |
|
||||||
from dataclasses import dataclass, field |
|
||||||
from enum import Enum, IntFlag |
|
||||||
|
|
||||||
from panda.python import uds |
|
||||||
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 |
|
||||||
|
|
||||||
Ecu = 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.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ, |
|
||||||
CAR.HYUNDAI_IONIQ_EV_LTD, CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.HYUNDAI_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): |
|
||||||
# Dynamic Flags |
|
||||||
CANFD_HDA2 = 1 |
|
||||||
CANFD_ALT_BUTTONS = 2 |
|
||||||
CANFD_ALT_GEARS = 2 ** 2 |
|
||||||
CANFD_CAMERA_SCC = 2 ** 3 |
|
||||||
|
|
||||||
ALT_LIMITS = 2 ** 4 |
|
||||||
ENABLE_BLINKERS = 2 ** 5 |
|
||||||
CANFD_ALT_GEARS_2 = 2 ** 6 |
|
||||||
SEND_LFA = 2 ** 7 |
|
||||||
USE_FCA = 2 ** 8 |
|
||||||
CANFD_HDA2_ALT_STEERING = 2 ** 9 |
|
||||||
|
|
||||||
# these cars use a different gas signal |
|
||||||
HYBRID = 2 ** 10 |
|
||||||
EV = 2 ** 11 |
|
||||||
|
|
||||||
# Static flags |
|
||||||
|
|
||||||
# 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 |
|
||||||
MANDO_RADAR = 2 ** 12 |
|
||||||
CANFD = 2 ** 13 |
|
||||||
|
|
||||||
# The radar does SCC on these cars when HDA I, rather than the camera |
|
||||||
RADAR_SCC = 2 ** 14 |
|
||||||
CAMERA_SCC = 2 ** 15 |
|
||||||
CHECKSUM_CRC8 = 2 ** 16 |
|
||||||
CHECKSUM_6B = 2 ** 17 |
|
||||||
|
|
||||||
# these cars require a special panda safety mode due to missing counters and checksums in the messages |
|
||||||
LEGACY = 2 ** 18 |
|
||||||
|
|
||||||
# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc. |
|
||||||
UNSUPPORTED_LONGITUDINAL = 2 ** 19 |
|
||||||
|
|
||||||
CANFD_NO_RADAR_DISABLE = 2 ** 20 |
|
||||||
|
|
||||||
CLUSTER_GEARS = 2 ** 21 |
|
||||||
TCU_GEARS = 2 ** 22 |
|
||||||
|
|
||||||
MIN_STEER_32_MPH = 2 ** 23 |
|
||||||
|
|
||||||
|
|
||||||
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 HyundaiCarDocs(CarDocs): |
|
||||||
package: str = "Smart Cruise Control (SCC)" |
|
||||||
|
|
||||||
def init_make(self, CP: CarParams): |
|
||||||
if CP.flags & HyundaiFlags.CANFD: |
|
||||||
self.footnotes.insert(0, Footnote.CANFD) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class HyundaiPlatformConfig(PlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict("hyundai_kia_generic", None)) |
|
||||||
|
|
||||||
def init(self): |
|
||||||
if self.flags & HyundaiFlags.MANDO_RADAR: |
|
||||||
self.dbc_dict = dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated') |
|
||||||
|
|
||||||
if self.flags & HyundaiFlags.MIN_STEER_32_MPH: |
|
||||||
self.specs = self.specs.override(minSteerSpeed=32 * CV.MPH_TO_MS) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class HyundaiCanFDPlatformConfig(PlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict("hyundai_canfd", None)) |
|
||||||
|
|
||||||
def init(self): |
|
||||||
self.flags |= HyundaiFlags.CANFD |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
# Hyundai |
|
||||||
HYUNDAI_AZERA_6TH_GEN = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
CarSpecs(mass=1600, wheelbase=2.885, steerRatio=14.5), |
|
||||||
) |
|
||||||
HYUNDAI_AZERA_HEV_6TH_GEN = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), |
|
||||||
HyundaiCarDocs("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), |
|
||||||
], |
|
||||||
CarSpecs(mass=1675, wheelbase=2.885, steerRatio=14.5), |
|
||||||
flags=HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_ELANTRA = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
# TODO: 2017-18 could be Hyundai G |
|
||||||
HyundaiCarDocs("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), |
|
||||||
HyundaiCarDocs("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), |
|
||||||
], |
|
||||||
# steerRatio: 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535, stiffnessFactor settled on 1.0081302973865127 |
|
||||||
CarSpecs(mass=1275, wheelbase=2.7, steerRatio=15.4, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
HYUNDAI_ELANTRA_GT_I30 = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), |
|
||||||
HyundaiCarDocs("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), |
|
||||||
], |
|
||||||
HYUNDAI_ELANTRA.specs, |
|
||||||
flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
HYUNDAI_ELANTRA_2021 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
CarSpecs(mass=2800 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
HYUNDAI_ELANTRA_HEV_2021 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
CarSpecs(mass=3017 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_GENESIS = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
# TODO: check 2015 packages |
|
||||||
HyundaiCarDocs("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), |
|
||||||
HyundaiCarDocs("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), |
|
||||||
], |
|
||||||
CarSpecs(mass=2060, wheelbase=3.01, steerRatio=16.5, minSteerSpeed=60 * CV.KPH_TO_MS), |
|
||||||
flags=HyundaiFlags.CHECKSUM_6B | HyundaiFlags.LEGACY, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c]))], |
|
||||||
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_HEV_2022 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h]))], # TODO: confirm 2020-21 harness, |
|
||||||
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_EV_LTD = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c]))], |
|
||||||
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV | HyundaiFlags.LEGACY | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_EV_2020 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], |
|
||||||
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.EV, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_PHEV_2019 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c]))], |
|
||||||
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_PHEV = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], |
|
||||||
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_KONA = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Kona 2020", min_enable_speed=6 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b]))], |
|
||||||
CarSpecs(mass=1275, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.CLUSTER_GEARS, |
|
||||||
) |
|
||||||
HYUNDAI_KONA_EV = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g]))], |
|
||||||
CarSpecs(mass=1685, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.EV, |
|
||||||
) |
|
||||||
HYUNDAI_KONA_EV_2022 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o]))], |
|
||||||
CarSpecs(mass=1743, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.CAMERA_SCC | HyundaiFlags.EV, |
|
||||||
) |
|
||||||
HYUNDAI_KONA_EV_2ND_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("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]))], |
|
||||||
CarSpecs(mass=1740, wheelbase=2.66, steerRatio=13.6, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, |
|
||||||
) |
|
||||||
HYUNDAI_KONA_HEV = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i]))], # TODO: check packages, |
|
||||||
CarSpecs(mass=1425, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_SANTA_FE = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_d]))], |
|
||||||
CarSpecs(mass=3982 * CV.LB_TO_KG, wheelbase=2.766, steerRatio=16.55, tireStiffnessFactor=0.82), |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
HYUNDAI_SANTA_FE_2022 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_l]))], |
|
||||||
HYUNDAI_SANTA_FE.specs, |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
HYUNDAI_SANTA_FE_HEV_2022 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))], |
|
||||||
HYUNDAI_SANTA_FE.specs, |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_SANTA_FE_PHEV_2022 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))], |
|
||||||
HYUNDAI_SANTA_FE.specs, |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_SONATA = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
CarSpecs(mass=1513, wheelbase=2.84, steerRatio=13.27 * 1.15, tireStiffnessFactor=0.65), # 15% higher at the center seems reasonable |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
HYUNDAI_SONATA_LF = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e]))], |
|
||||||
CarSpecs(mass=1536, wheelbase=2.804, steerRatio=13.27 * 1.15), # 15% higher at the center seems reasonable |
|
||||||
|
|
||||||
flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, |
|
||||||
) |
|
||||||
HYUNDAI_STARIA_4TH_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
CarSpecs(mass=2205, wheelbase=3.273, 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 |
|
||||||
) |
|
||||||
HYUNDAI_TUCSON = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])), |
|
||||||
HyundaiCarDocs("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])), |
|
||||||
], |
|
||||||
CarSpecs(mass=3520 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=16.1, tireStiffnessFactor=0.385), |
|
||||||
flags=HyundaiFlags.TCU_GEARS, |
|
||||||
) |
|
||||||
HYUNDAI_PALISADE = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])), |
|
||||||
HyundaiCarDocs("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), |
|
||||||
], |
|
||||||
CarSpecs(mass=1999, wheelbase=2.9, steerRatio=15.6 * 1.15, tireStiffnessFactor=0.63), |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
HYUNDAI_VELOSTER = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e]))], |
|
||||||
CarSpecs(mass=2917 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75 * 1.15, tireStiffnessFactor=0.5), |
|
||||||
flags=HyundaiFlags.LEGACY | HyundaiFlags.TCU_GEARS, |
|
||||||
) |
|
||||||
HYUNDAI_SONATA_HYBRID = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
HYUNDAI_SONATA.specs, |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_5 = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Hyundai Ioniq 5 (Non-US only) 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_q])), |
|
||||||
HyundaiCarDocs("Hyundai Ioniq 5 (without HDA II) 2022-24", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])), |
|
||||||
HyundaiCarDocs("Hyundai Ioniq 5 (with HDA II) 2022-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), |
|
||||||
], |
|
||||||
CarSpecs(mass=1948, wheelbase=2.97, steerRatio=14.26, tireStiffnessFactor=0.65), |
|
||||||
flags=HyundaiFlags.EV, |
|
||||||
) |
|
||||||
HYUNDAI_IONIQ_6 = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Ioniq 6 (with HDA II) 2023-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))], |
|
||||||
HYUNDAI_IONIQ_5.specs, |
|
||||||
flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, |
|
||||||
) |
|
||||||
HYUNDAI_TUCSON_4TH_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])), |
|
||||||
HyundaiCarDocs("Hyundai Tucson 2023-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), |
|
||||||
HyundaiCarDocs("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), |
|
||||||
HyundaiCarDocs("Hyundai Tucson Plug-in Hybrid 2024", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), |
|
||||||
], |
|
||||||
CarSpecs(mass=1630, wheelbase=2.756, steerRatio=13.7, tireStiffnessFactor=0.385), |
|
||||||
) |
|
||||||
HYUNDAI_SANTA_CRUZ_1ST_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Santa Cruz 2022-24", car_parts=CarParts.common([CarHarness.hyundai_n]))], |
|
||||||
# weight from Limited trim - the only supported trim, steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf |
|
||||||
CarSpecs(mass=1870, wheelbase=3, steerRatio=14.2), |
|
||||||
) |
|
||||||
HYUNDAI_CUSTIN_1ST_GEN = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
CarSpecs(mass=1690, wheelbase=3.055, steerRatio=17), # mass: from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0, steerRatio: from learner |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
|
|
||||||
# Kia |
|
||||||
KIA_FORTE = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Forte 2019-21", min_enable_speed=6 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), |
|
||||||
HyundaiCarDocs("Kia Forte 2022-23", car_parts=CarParts.common([CarHarness.hyundai_e])), |
|
||||||
], |
|
||||||
CarSpecs(mass=2878 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5) |
|
||||||
) |
|
||||||
KIA_K5_2021 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
CarSpecs(mass=3381 * CV.LB_TO_KG, wheelbase=2.85, steerRatio=13.27, tireStiffnessFactor=0.5), # 2021 Kia K5 Steering Ratio (all trims) |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
KIA_K5_HEV_2020 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
KIA_K5_2021.specs, |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
KIA_K8_HEV_1ST_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q]))], |
|
||||||
# mass: https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid, steerRatio: guesstimate from K5 platform |
|
||||||
CarSpecs(mass=1630, wheelbase=2.895, steerRatio=13.27) |
|
||||||
) |
|
||||||
KIA_NIRO_EV = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), |
|
||||||
HyundaiCarDocs("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])), |
|
||||||
HyundaiCarDocs("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])), |
|
||||||
HyundaiCarDocs("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), |
|
||||||
], |
|
||||||
CarSpecs(mass=3543 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=13.6, tireStiffnessFactor=0.385), # average of all the cars |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV, |
|
||||||
) |
|
||||||
KIA_NIRO_EV_2ND_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
KIA_NIRO_EV.specs, |
|
||||||
flags=HyundaiFlags.EV, |
|
||||||
) |
|
||||||
KIA_NIRO_PHEV = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Niro Hybrid 2018", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), |
|
||||||
HyundaiCarDocs("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), |
|
||||||
HyundaiCarDocs("Kia Niro Plug-in Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_d])), |
|
||||||
], |
|
||||||
KIA_NIRO_EV.specs, |
|
||||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.HYBRID | HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
KIA_NIRO_PHEV_2022 = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Niro Plug-in Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), |
|
||||||
HyundaiCarDocs("Kia Niro Plug-in Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), |
|
||||||
], |
|
||||||
KIA_NIRO_EV.specs, |
|
||||||
flags=HyundaiFlags.HYBRID | HyundaiFlags.MANDO_RADAR, |
|
||||||
) |
|
||||||
KIA_NIRO_HEV_2021 = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), |
|
||||||
HyundaiCarDocs("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), |
|
||||||
], |
|
||||||
KIA_NIRO_EV.specs, |
|
||||||
flags=HyundaiFlags.HYBRID, |
|
||||||
) |
|
||||||
KIA_NIRO_HEV_2ND_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
KIA_NIRO_EV.specs, |
|
||||||
) |
|
||||||
KIA_OPTIMA_G4 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Optima 2017", "Advanced Smart Cruise Control", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_b]))], # TODO: may support 2016, 2018 |
|
||||||
CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), |
|
||||||
flags=HyundaiFlags.LEGACY | HyundaiFlags.TCU_GEARS | HyundaiFlags.MIN_STEER_32_MPH, |
|
||||||
) |
|
||||||
KIA_OPTIMA_G4_FL = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g]))], |
|
||||||
CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), |
|
||||||
flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, |
|
||||||
) |
|
||||||
# TODO: may support adjacent years. may have a non-zero minimum steering speed |
|
||||||
KIA_OPTIMA_H = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c]))], |
|
||||||
CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), |
|
||||||
flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, |
|
||||||
) |
|
||||||
KIA_OPTIMA_H_G4_FL = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h]))], |
|
||||||
CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), |
|
||||||
flags=HyundaiFlags.HYBRID | HyundaiFlags.UNSUPPORTED_LONGITUDINAL, |
|
||||||
) |
|
||||||
KIA_SELTOS = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a]))], |
|
||||||
CarSpecs(mass=1337, wheelbase=2.63, steerRatio=14.56), |
|
||||||
flags=HyundaiFlags.CHECKSUM_CRC8, |
|
||||||
) |
|
||||||
KIA_SPORTAGE_5TH_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Sportage 2023-24", car_parts=CarParts.common([CarHarness.hyundai_n])), |
|
||||||
HyundaiCarDocs("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), |
|
||||||
], |
|
||||||
# weight from SX and above trims, average of FWD and AWD version, steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications |
|
||||||
CarSpecs(mass=1725, wheelbase=2.756, steerRatio=13.6), |
|
||||||
) |
|
||||||
KIA_SORENTO = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_e])), |
|
||||||
HyundaiCarDocs("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])), |
|
||||||
], |
|
||||||
CarSpecs(mass=1985, wheelbase=2.78, steerRatio=14.4 * 1.1), # 10% higher at the center seems reasonable |
|
||||||
flags=HyundaiFlags.CHECKSUM_6B | HyundaiFlags.UNSUPPORTED_LONGITUDINAL, |
|
||||||
) |
|
||||||
KIA_SORENTO_4TH_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
CarSpecs(mass=3957 * CV.LB_TO_KG, wheelbase=2.81, steerRatio=13.5), # average of the platforms |
|
||||||
flags=HyundaiFlags.RADAR_SCC, |
|
||||||
) |
|
||||||
KIA_SORENTO_HEV_4TH_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), |
|
||||||
HyundaiCarDocs("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), |
|
||||||
], |
|
||||||
CarSpecs(mass=4395 * CV.LB_TO_KG, wheelbase=2.81, steerRatio=13.5), # average of the platforms |
|
||||||
flags=HyundaiFlags.RADAR_SCC, |
|
||||||
) |
|
||||||
KIA_STINGER = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", |
|
||||||
car_parts=CarParts.common([CarHarness.hyundai_c]))], |
|
||||||
CarSpecs(mass=1825, wheelbase=2.78, steerRatio=14.4 * 1.15) # 15% higher at the center seems reasonable |
|
||||||
) |
|
||||||
KIA_STINGER_2022 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], |
|
||||||
KIA_STINGER.specs, |
|
||||||
) |
|
||||||
KIA_CEED = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e]))], |
|
||||||
CarSpecs(mass=1450, wheelbase=2.65, steerRatio=13.75, tireStiffnessFactor=0.5), |
|
||||||
flags=HyundaiFlags.LEGACY, |
|
||||||
) |
|
||||||
KIA_EV6 = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia EV6 (Non-US only) 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_p])), |
|
||||||
HyundaiCarDocs("Kia EV6 (without HDA II) 2022-24", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])), |
|
||||||
HyundaiCarDocs("Kia EV6 (with HDA II) 2022-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])) |
|
||||||
], |
|
||||||
CarSpecs(mass=2055, wheelbase=2.9, steerRatio=16, tireStiffnessFactor=0.65), |
|
||||||
flags=HyundaiFlags.EV, |
|
||||||
) |
|
||||||
KIA_CARNIVAL_4TH_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])), |
|
||||||
HyundaiCarDocs("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k])) |
|
||||||
], |
|
||||||
CarSpecs(mass=2087, wheelbase=3.09, steerRatio=14.23), |
|
||||||
flags=HyundaiFlags.RADAR_SCC, |
|
||||||
) |
|
||||||
|
|
||||||
# Genesis |
|
||||||
GENESIS_GV60_EV_1ST_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
HyundaiCarDocs("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), |
|
||||||
HyundaiCarDocs("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), |
|
||||||
], |
|
||||||
CarSpecs(mass=2205, wheelbase=2.9, steerRatio=12.6), # steerRatio: https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1. |
|
||||||
flags=HyundaiFlags.EV, |
|
||||||
) |
|
||||||
GENESIS_G70 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Genesis G70 2018", "All", car_parts=CarParts.common([CarHarness.hyundai_f]))], |
|
||||||
CarSpecs(mass=1640, wheelbase=2.84, steerRatio=13.56), |
|
||||||
flags=HyundaiFlags.LEGACY, |
|
||||||
) |
|
||||||
GENESIS_G70_2020 = HyundaiPlatformConfig( |
|
||||||
[ |
|
||||||
# TODO: 2021 MY harness is unknown |
|
||||||
HyundaiCarDocs("Genesis G70 2019-21", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), |
|
||||||
# TODO: From 3.3T Sport Advanced 2022 & Prestige 2023 Trim, 2.0T is unknown |
|
||||||
HyundaiCarDocs("Genesis G70 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), |
|
||||||
], |
|
||||||
GENESIS_G70.specs, |
|
||||||
flags=HyundaiFlags.MANDO_RADAR, |
|
||||||
) |
|
||||||
GENESIS_GV70_1ST_GEN = HyundaiCanFDPlatformConfig( |
|
||||||
[ |
|
||||||
# TODO: Hyundai P is likely the correct harness for HDA II for 2.5T (unsupported due to missing ADAS ECU, is that the radar?) |
|
||||||
HyundaiCarDocs("Genesis GV70 (2.5T Trim, without HDA II) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), |
|
||||||
HyundaiCarDocs("Genesis GV70 (3.5T Trim, without HDA II) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), |
|
||||||
], |
|
||||||
CarSpecs(mass=1950, wheelbase=2.87, steerRatio=14.6), |
|
||||||
flags=HyundaiFlags.RADAR_SCC, |
|
||||||
) |
|
||||||
GENESIS_G80 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], |
|
||||||
CarSpecs(mass=2060, wheelbase=3.01, steerRatio=16.5), |
|
||||||
flags=HyundaiFlags.LEGACY, |
|
||||||
) |
|
||||||
GENESIS_G90 = HyundaiPlatformConfig( |
|
||||||
[HyundaiCarDocs("Genesis G90 2017-20", "All", car_parts=CarParts.common([CarHarness.hyundai_c]))], |
|
||||||
CarSpecs(mass=2200, wheelbase=3.15, steerRatio=12.069), |
|
||||||
) |
|
||||||
GENESIS_GV80 = HyundaiCanFDPlatformConfig( |
|
||||||
[HyundaiCarDocs("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m]))], |
|
||||||
CarSpecs(mass=2258, wheelbase=2.95, steerRatio=14.14), |
|
||||||
flags=HyundaiFlags.RADAR_SCC, |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
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, bytes | None]]: |
|
||||||
# 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, vin, 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. |
|
||||||
fuzzy_platform_blacklist = {str(c) for c in (CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST)} |
|
||||||
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_ECU_MANUFACTURING_DATE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ |
|
||||||
p16(uds.DATA_IDENTIFIER_TYPE.ECU_MANUFACTURING_DATE) |
|
||||||
|
|
||||||
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])') |
|
||||||
|
|
||||||
# We've seen both ICE and hybrid for these platforms, and they have hybrid descriptors (e.g. MQ4 vs MQ4H) |
|
||||||
CANFD_FUZZY_WHITELIST = {CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_K8_HEV_1ST_GEN, |
|
||||||
# TODO: the hybrid variant is not out yet |
|
||||||
CAR.KIA_CARNIVAL_4TH_GEN} |
|
||||||
|
|
||||||
# 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] |
|
||||||
|
|
||||||
# Note: an ECU on CAN FD cars may sometimes send 0x30080aaaaaaaaaaa (flow control continue) while we |
|
||||||
# are attempting to query ECUs. This currently does not seem to affect fingerprinting from the camera |
|
||||||
FW_QUERY_CONFIG = FwQueryConfig( |
|
||||||
requests=[ |
|
||||||
# TODO: add back whitelists |
|
||||||
# CAN queries (OBD-II port) |
|
||||||
Request( |
|
||||||
[HYUNDAI_VERSION_REQUEST_LONG], |
|
||||||
[HYUNDAI_VERSION_RESPONSE], |
|
||||||
), |
|
||||||
|
|
||||||
# CAN & CAN-FD queries (from camera) |
|
||||||
Request( |
|
||||||
[HYUNDAI_VERSION_REQUEST_LONG], |
|
||||||
[HYUNDAI_VERSION_RESPONSE], |
|
||||||
bus=0, |
|
||||||
auxiliary=True, |
|
||||||
), |
|
||||||
Request( |
|
||||||
[HYUNDAI_VERSION_REQUEST_LONG], |
|
||||||
[HYUNDAI_VERSION_RESPONSE], |
|
||||||
bus=1, |
|
||||||
auxiliary=True, |
|
||||||
obd_multiplexing=False, |
|
||||||
), |
|
||||||
|
|
||||||
# CAN & CAN FD query to understand the three digit date code |
|
||||||
# HDA2 cars usually use 6 digit date codes, so skip bus 1 |
|
||||||
Request( |
|
||||||
[HYUNDAI_ECU_MANUFACTURING_DATE], |
|
||||||
[HYUNDAI_VERSION_RESPONSE], |
|
||||||
bus=0, |
|
||||||
auxiliary=True, |
|
||||||
logging=True, |
|
||||||
), |
|
||||||
|
|
||||||
# CAN-FD alt request logging queries for hvac and parkingAdas |
|
||||||
Request( |
|
||||||
[HYUNDAI_VERSION_REQUEST_ALT], |
|
||||||
[HYUNDAI_VERSION_RESPONSE], |
|
||||||
bus=0, |
|
||||||
auxiliary=True, |
|
||||||
logging=True, |
|
||||||
), |
|
||||||
Request( |
|
||||||
[HYUNDAI_VERSION_REQUEST_ALT], |
|
||||||
[HYUNDAI_VERSION_RESPONSE], |
|
||||||
bus=1, |
|
||||||
auxiliary=True, |
|
||||||
logging=True, |
|
||||||
obd_multiplexing=False, |
|
||||||
), |
|
||||||
], |
|
||||||
# We lose these ECUs without the comma power on these cars. |
|
||||||
# Note that we still attempt to match with them when they are present |
|
||||||
non_essential_ecus={ |
|
||||||
Ecu.abs: [CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_ELANTRA_2021, |
|
||||||
CAR.HYUNDAI_SANTA_FE, CAR.HYUNDAI_KONA_EV_2022, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.KIA_SORENTO, |
|
||||||
CAR.KIA_CEED, CAR.KIA_SELTOS], |
|
||||||
}, |
|
||||||
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), |
|
||||||
(Ecu.combinationMeter, 0x7c6, None), # CAN FD Instrument cluster |
|
||||||
], |
|
||||||
# Custom fuzzy fingerprinting function using platform codes, part numbers + FW dates: |
|
||||||
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, |
|
||||||
) |
|
||||||
|
|
||||||
CHECKSUM = { |
|
||||||
"crc8": CAR.with_flags(HyundaiFlags.CHECKSUM_CRC8), |
|
||||||
"6B": CAR.with_flags(HyundaiFlags.CHECKSUM_6B), |
|
||||||
} |
|
||||||
|
|
||||||
CAN_GEARS = { |
|
||||||
# which message has the gear. hybrid and EV use ELECT_GEAR |
|
||||||
"use_cluster_gears": CAR.with_flags(HyundaiFlags.CLUSTER_GEARS), |
|
||||||
"use_tcu_gears": CAR.with_flags(HyundaiFlags.TCU_GEARS), |
|
||||||
} |
|
||||||
|
|
||||||
CANFD_CAR = CAR.with_flags(HyundaiFlags.CANFD) |
|
||||||
CANFD_RADAR_SCC_CAR = CAR.with_flags(HyundaiFlags.RADAR_SCC) |
|
||||||
|
|
||||||
# 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.with_flags(HyundaiFlags.CANFD_NO_RADAR_DISABLE) |
|
||||||
|
|
||||||
# The camera does SCC on these cars, rather than the radar |
|
||||||
CAMERA_SCC_CAR = CAR.with_flags(HyundaiFlags.CAMERA_SCC) |
|
||||||
|
|
||||||
HYBRID_CAR = CAR.with_flags(HyundaiFlags.HYBRID) |
|
||||||
|
|
||||||
EV_CAR = CAR.with_flags(HyundaiFlags.EV) |
|
||||||
|
|
||||||
LEGACY_SAFETY_MODE_CAR = CAR.with_flags(HyundaiFlags.LEGACY) |
|
||||||
|
|
||||||
UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL) |
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,451 +0,0 @@ |
|||||||
import json |
|
||||||
import os |
|
||||||
import numpy as np |
|
||||||
import tomllib |
|
||||||
from abc import abstractmethod, ABC |
|
||||||
from enum import StrEnum |
|
||||||
from typing import Any, NamedTuple |
|
||||||
from collections.abc import Callable |
|
||||||
from functools import cache |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_friction, STD_CARGO_KG |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable |
|
||||||
from openpilot.selfdrive.car.common.basedir import BASEDIR |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.common.simple_kalman import KF1D, get_kalman_gain |
|
||||||
from openpilot.selfdrive.car.common.numpy_fast import clip |
|
||||||
from openpilot.selfdrive.car.values import PLATFORMS |
|
||||||
|
|
||||||
GearShifter = structs.CarState.GearShifter |
|
||||||
|
|
||||||
V_CRUISE_MAX = 145 |
|
||||||
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, 'torque_data/params.toml') |
|
||||||
TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'torque_data/override.toml') |
|
||||||
TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'torque_data/substitute.toml') |
|
||||||
|
|
||||||
GEAR_SHIFTER_MAP: dict[str, 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, |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
class LatControlInputs(NamedTuple): |
|
||||||
lateral_acceleration: float |
|
||||||
roll_compensation: float |
|
||||||
vego: float |
|
||||||
aego: float |
|
||||||
|
|
||||||
|
|
||||||
TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, float, float, bool, bool], float] |
|
||||||
|
|
||||||
|
|
||||||
@cache |
|
||||||
def get_torque_params(): |
|
||||||
with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f: |
|
||||||
sub = tomllib.load(f) |
|
||||||
with open(TORQUE_PARAMS_PATH, 'rb') as f: |
|
||||||
params = tomllib.load(f) |
|
||||||
with open(TORQUE_OVERRIDE_PATH, 'rb') as f: |
|
||||||
override = tomllib.load(f) |
|
||||||
|
|
||||||
torque_params = {} |
|
||||||
for candidate in (sub.keys() | params.keys() | override.keys()) - {'legend'}: |
|
||||||
if sum([candidate in x for x in [sub, params, override]]) > 1: |
|
||||||
raise RuntimeError(f'{candidate} is defined twice in torque config') |
|
||||||
|
|
||||||
sub_candidate = sub.get(candidate, candidate) |
|
||||||
|
|
||||||
if sub_candidate in override: |
|
||||||
out = override[sub_candidate] |
|
||||||
elif sub_candidate in params: |
|
||||||
out = params[sub_candidate] |
|
||||||
else: |
|
||||||
raise NotImplementedError(f"Did not find torque params for {sub_candidate}") |
|
||||||
|
|
||||||
torque_params[sub_candidate] = {key: out[i] for i, key in enumerate(params['legend'])} |
|
||||||
if candidate in sub: |
|
||||||
torque_params[candidate] = torque_params[sub_candidate] |
|
||||||
|
|
||||||
return torque_params |
|
||||||
|
|
||||||
# generic car and radar interfaces |
|
||||||
|
|
||||||
class CarInterfaceBase(ABC): |
|
||||||
def __init__(self, CP: structs.CarParams, CarController, CarState): |
|
||||||
self.CP = CP |
|
||||||
|
|
||||||
self.frame = 0 |
|
||||||
self.v_ego_cluster_seen = False |
|
||||||
|
|
||||||
self.CS: CarStateBase = 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) |
|
||||||
|
|
||||||
dbc_name = "" if self.cp is None else self.cp.dbc_name |
|
||||||
self.CC: CarControllerBase = CarController(dbc_name, CP) |
|
||||||
|
|
||||||
def apply(self, c: structs.CarControl, now_nanos: int) -> tuple[structs.CarControl.Actuators, list[CanData]]: |
|
||||||
return self.CC.update(c, self.CS, now_nanos) |
|
||||||
|
|
||||||
@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) -> structs.CarParams: |
|
||||||
""" |
|
||||||
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[structs.CarParams.CarFw], |
|
||||||
experimental_long: bool, docs: bool) -> structs.CarParams: |
|
||||||
ret = CarInterfaceBase.get_std_params(candidate) |
|
||||||
|
|
||||||
platform = PLATFORMS[candidate] |
|
||||||
ret.mass = platform.config.specs.mass |
|
||||||
ret.wheelbase = platform.config.specs.wheelbase |
|
||||||
ret.steerRatio = platform.config.specs.steerRatio |
|
||||||
ret.centerToFront = ret.wheelbase * platform.config.specs.centerToFrontRatio |
|
||||||
ret.minEnableSpeed = platform.config.specs.minEnableSpeed |
|
||||||
ret.minSteerSpeed = platform.config.specs.minSteerSpeed |
|
||||||
ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor |
|
||||||
ret.flags |= int(platform.config.flags) |
|
||||||
|
|
||||||
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: structs.CarParams, candidate, fingerprint: dict[int, dict[int, int]], |
|
||||||
car_fw: list[structs.CarParams.CarFw], experimental_long: bool, docs: bool) -> structs.CarParams: |
|
||||||
raise NotImplementedError |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def init(CP: structs.CarParams, can_recv: CanRecvCallable, can_send: CanSendCallable): |
|
||||||
pass |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def get_steer_feedforward_default(desired_angle, v_ego): |
|
||||||
# Proportional to realigning tire momentum: lateral acceleration. |
|
||||||
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, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning, |
|
||||||
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool, gravity_adjusted: 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 (latcontrol_inputs.lateral_acceleration / 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: str) -> structs.CarParams: |
|
||||||
ret = structs.CarParams() |
|
||||||
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 = structs.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.kf = 1. |
|
||||||
ret.longitudinalTuning.kpBP = [0.] |
|
||||||
ret.longitudinalTuning.kpV = [0.] |
|
||||||
ret.longitudinalTuning.kiBP = [0.] |
|
||||||
ret.longitudinalTuning.kiV = [0.] |
|
||||||
# TODO estimate car specific lag, use .15s for now |
|
||||||
ret.longitudinalActuatorDelay = 0.15 |
|
||||||
ret.steerLimitTimer = 1.0 |
|
||||||
return ret |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def configure_torque_tune(candidate: str, tune: structs.CarParams.LateralTuning, steering_angle_deadzone_deg: float = 0.0, use_steering_angle: bool = 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 |
|
||||||
|
|
||||||
def _update(self) -> structs.CarState: |
|
||||||
return self.CS.update(*self.can_parsers) |
|
||||||
|
|
||||||
def update(self, can_packets: list[tuple[int, list[CanData]]]) -> structs.CarState: |
|
||||||
# parse can |
|
||||||
for cp in self.can_parsers: |
|
||||||
if cp is not None: |
|
||||||
cp.update_strings(can_packets) |
|
||||||
|
|
||||||
# get CarState |
|
||||||
ret = self._update() |
|
||||||
|
|
||||||
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 |
|
||||||
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 |
|
||||||
|
|
||||||
# save for next iteration |
|
||||||
self.CS.out = ret |
|
||||||
|
|
||||||
return ret |
|
||||||
|
|
||||||
|
|
||||||
class RadarInterfaceBase(ABC): |
|
||||||
def __init__(self, CP: structs.CarParams): |
|
||||||
self.CP = CP |
|
||||||
self.rcp = None |
|
||||||
self.pts: dict[int, structs.RadarData.RadarPoint] = {} |
|
||||||
self.delay = 0 |
|
||||||
self.radar_ts = CP.radarTimeStep |
|
||||||
self.frame = 0 |
|
||||||
|
|
||||||
def update(self, can_strings) -> structs.RadarData | None: |
|
||||||
self.frame += 1 |
|
||||||
if (self.frame % int(100 * self.radar_ts)) == 0: |
|
||||||
return structs.RadarData() |
|
||||||
return None |
|
||||||
|
|
||||||
|
|
||||||
class CarStateBase(ABC): |
|
||||||
def __init__(self, CP: structs.CarParams): |
|
||||||
self.CP = CP |
|
||||||
self.car_fingerprint = CP.carFingerprint |
|
||||||
self.out = structs.CarState() |
|
||||||
|
|
||||||
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) |
|
||||||
|
|
||||||
@abstractmethod |
|
||||||
def update(self, cp, cp_cam, cp_adas, cp_body, cp_loopback) -> structs.CarState: |
|
||||||
pass |
|
||||||
|
|
||||||
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 = structs.CarState.WheelSpeeds() |
|
||||||
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: str | None) -> GearShifter: |
|
||||||
if gear is None: |
|
||||||
return GearShifter.unknown |
|
||||||
return GEAR_SHIFTER_MAP.get(gear.upper(), GearShifter.unknown) |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def get_can_parser(CP): |
|
||||||
return None |
|
||||||
|
|
||||||
@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 |
|
||||||
|
|
||||||
|
|
||||||
class CarControllerBase(ABC): |
|
||||||
def __init__(self, dbc_name: str, CP: structs.CarParams): |
|
||||||
self.CP = CP |
|
||||||
self.frame = 0 |
|
||||||
|
|
||||||
@abstractmethod |
|
||||||
def update(self, CC: structs.CarControl, CS: CarStateBase, now_nanos: int) -> tuple[structs.CarControl.Actuators, list[CanData]]: |
|
||||||
pass |
|
||||||
|
|
||||||
|
|
||||||
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)]): |
|
||||||
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 |
|
||||||
|
|
||||||
|
|
||||||
class NanoFFModel: |
|
||||||
def __init__(self, weights_loc: str, platform: str): |
|
||||||
self.weights_loc = weights_loc |
|
||||||
self.platform = platform |
|
||||||
self.load_weights(platform) |
|
||||||
|
|
||||||
def load_weights(self, platform: str): |
|
||||||
with open(self.weights_loc) as fob: |
|
||||||
self.weights = {k: np.array(v) for k, v in json.load(fob)[platform].items()} |
|
||||||
|
|
||||||
def relu(self, x: np.ndarray): |
|
||||||
return np.maximum(0.0, x) |
|
||||||
|
|
||||||
def forward(self, x: np.ndarray): |
|
||||||
assert x.ndim == 1 |
|
||||||
x = (x - self.weights['input_norm_mat'][:, 0]) / (self.weights['input_norm_mat'][:, 1] - self.weights['input_norm_mat'][:, 0]) |
|
||||||
x = self.relu(np.dot(x, self.weights['w_1']) + self.weights['b_1']) |
|
||||||
x = self.relu(np.dot(x, self.weights['w_2']) + self.weights['b_2']) |
|
||||||
x = self.relu(np.dot(x, self.weights['w_3']) + self.weights['b_3']) |
|
||||||
x = np.dot(x, self.weights['w_4']) + self.weights['b_4'] |
|
||||||
return x |
|
||||||
|
|
||||||
def predict(self, x: list[float], do_sample: bool = False): |
|
||||||
x = self.forward(np.array(x)) |
|
||||||
if do_sample: |
|
||||||
pred = np.random.laplace(x[0], np.exp(x[1]) / self.weights['temperature']) |
|
||||||
else: |
|
||||||
pred = x[0] |
|
||||||
pred = pred * (self.weights['output_norm_mat'][1] - self.weights['output_norm_mat'][0]) + self.weights['output_norm_mat'][0] |
|
||||||
return pred |
|
||||||
@ -1,172 +0,0 @@ |
|||||||
import time |
|
||||||
from collections import defaultdict |
|
||||||
from functools import partial |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import carlog |
|
||||||
from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import AddrType |
|
||||||
from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr |
|
||||||
|
|
||||||
|
|
||||||
class IsoTpParallelQuery: |
|
||||||
def __init__(self, can_send: CanSendCallable, can_recv: CanRecvCallable, bus: int, addrs: list[int] | list[AddrType], |
|
||||||
request: list[bytes], response: list[bytes], response_offset: int = 0x8, |
|
||||||
functional_addrs: list[int] = None, debug: bool = False, response_pending_timeout: float = 10) -> None: |
|
||||||
self.can_send = can_send |
|
||||||
self.can_recv = can_recv |
|
||||||
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: dict[int, list[CanData]] = defaultdict(list) |
|
||||||
|
|
||||||
def rx(self) -> None: |
|
||||||
"""Drain can socket and sort messages into buffers based on address""" |
|
||||||
can_packets = self.can_recv(wait_for_one=True) |
|
||||||
|
|
||||||
for packet in can_packets: |
|
||||||
for msg in packet: |
|
||||||
if msg.src == self.bus and msg.address in self.msg_addrs.values(): |
|
||||||
self.msg_buffer[msg.address].append(CanData(msg.address, msg.dat, msg.src)) |
|
||||||
|
|
||||||
def _can_tx(self, tx_addr: int, dat: bytes, bus: int): |
|
||||||
"""Helper function to send single message""" |
|
||||||
msg = CanData(tx_addr, dat, bus) |
|
||||||
self.can_send([msg]) |
|
||||||
|
|
||||||
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[1][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) -> None: |
|
||||||
self.can_recv() |
|
||||||
self.msg_buffer = defaultdict(list) |
|
||||||
|
|
||||||
def _create_isotp_msg(self, tx_addr: int, sub_addr: int | None, rx_addr: int): |
|
||||||
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: float, total_timeout: float = 60.) -> dict[AddrType, bytes]: |
|
||||||
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: |
|
||||||
carlog.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: |
|
||||||
carlog.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 |
|
||||||
carlog.error(f"iso-tp query response pending: {tx_addr}") |
|
||||||
else: |
|
||||||
request_done[tx_addr] = True |
|
||||||
carlog.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: |
|
||||||
carlog.error(f"iso-tp query timeout after receiving partial response: {tx_addr}") |
|
||||||
elif tx_addr in addrs_responded: |
|
||||||
carlog.error(f"iso-tp query timeout while receiving response: {tx_addr}") |
|
||||||
# TODO: handle functional addresses |
|
||||||
# else: |
|
||||||
# carlog.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: |
|
||||||
carlog.error("iso-tp query timeout while receiving data") |
|
||||||
break |
|
||||||
|
|
||||||
return results |
|
||||||
@ -1,65 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, structs |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
from openpilot.selfdrive.car.mazda import mazdacan |
|
||||||
from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons |
|
||||||
|
|
||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert |
|
||||||
|
|
||||||
|
|
||||||
class CarController(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.apply_steer_last = 0 |
|
||||||
self.packer = CANPacker(dbc_name) |
|
||||||
self.brake_counter = 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, 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, 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, |
|
||||||
self.frame, apply_steer, CS.cam_lkas)) |
|
||||||
|
|
||||||
new_actuators = copy.copy(CC.actuators) |
|
||||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX |
|
||||||
new_actuators.steerOutputCan = apply_steer |
|
||||||
|
|
||||||
self.frame += 1 |
|
||||||
return new_actuators, can_sends |
|
||||||
@ -1,158 +0,0 @@ |
|||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
|
|
||||||
|
|
||||||
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 |
|
||||||
|
|
||||||
self.distance_button = 0 |
|
||||||
|
|
||||||
def update(self, cp, cp_cam, *_) -> structs.CarState: |
|
||||||
|
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
prev_distance_button = self.distance_button |
|
||||||
self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] |
|
||||||
|
|
||||||
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 |
|
||||||
|
|
||||||
# TODO: add button types for inc and dec |
|
||||||
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) |
|
||||||
|
|
||||||
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.flags & MazdaFlags.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.flags & MazdaFlags.GEN1: |
|
||||||
messages += [ |
|
||||||
# sig_address, frequency |
|
||||||
("CAM_LANEINFO", 2), |
|
||||||
("CAM_LKAS", 16), |
|
||||||
] |
|
||||||
|
|
||||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) |
|
||||||
@ -1,266 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.mazda.values import CAR |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
FW_VERSIONS = { |
|
||||||
CAR.MAZDA_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'PW67-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
b'PX2D-188K2-G\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-B\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'PW66-21PS1-B\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'PYJ3-21PS1-H\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.MAZDA_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.MAZDA_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.MAZDA_3: { |
|
||||||
(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.MAZDA_6: { |
|
||||||
(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.MAZDA_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', |
|
||||||
b'PXM7-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', |
|
||||||
b'GSH7-67XK2-U\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', |
|
||||||
b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
@ -1,29 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from openpilot.selfdrive.car import get_safety_config, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS |
|
||||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class CarInterface(CarInterfaceBase): |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
ret.carName = "mazda" |
|
||||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.mazda)] |
|
||||||
ret.radarUnavailable = True |
|
||||||
|
|
||||||
ret.dashcamOnly = candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021) |
|
||||||
|
|
||||||
ret.steerActuatorDelay = 0.1 |
|
||||||
ret.steerLimitTimer = 0.8 |
|
||||||
|
|
||||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
|
||||||
|
|
||||||
if candidate not in (CAR.MAZDA_CX5_2022,): |
|
||||||
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS |
|
||||||
|
|
||||||
ret.centerToFront = ret.wheelbase * 0.41 |
|
||||||
|
|
||||||
return ret |
|
||||||
@ -1,128 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags |
|
||||||
|
|
||||||
|
|
||||||
def create_steering_control(packer, CP, 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 CP.flags & MazdaFlags.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, CP, counter, button): |
|
||||||
|
|
||||||
can = int(button == Buttons.CANCEL) |
|
||||||
res = int(button == Buttons.RESUME) |
|
||||||
|
|
||||||
if CP.flags & MazdaFlags.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) |
|
||||||
@ -1,5 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase |
|
||||||
|
|
||||||
class RadarInterface(RadarInterfaceBase): |
|
||||||
pass |
|
||||||
@ -1,104 +0,0 @@ |
|||||||
from dataclasses import dataclass, field |
|
||||||
from enum import IntFlag |
|
||||||
|
|
||||||
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts |
|
||||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries |
|
||||||
|
|
||||||
Ecu = 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 |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class MazdaCarDocs(CarDocs): |
|
||||||
package: str = "All" |
|
||||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.mazda])) |
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True, kw_only=True) |
|
||||||
class MazdaCarSpecs(CarSpecs): |
|
||||||
tireStiffnessFactor: float = 0.7 # not optimized yet |
|
||||||
|
|
||||||
|
|
||||||
class MazdaFlags(IntFlag): |
|
||||||
# Static flags |
|
||||||
# Gen 1 hardware: same CAN messages and same camera |
|
||||||
GEN1 = 1 |
|
||||||
|
|
||||||
|
|
||||||
@dataclass |
|
||||||
class MazdaPlatformConfig(PlatformConfig): |
|
||||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('mazda_2017', None)) |
|
||||||
flags: int = MazdaFlags.GEN1 |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
MAZDA_CX5 = MazdaPlatformConfig( |
|
||||||
[MazdaCarDocs("Mazda CX-5 2017-21")], |
|
||||||
MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5) |
|
||||||
) |
|
||||||
MAZDA_CX9 = MazdaPlatformConfig( |
|
||||||
[MazdaCarDocs("Mazda CX-9 2016-20")], |
|
||||||
MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6) |
|
||||||
) |
|
||||||
MAZDA_3 = MazdaPlatformConfig( |
|
||||||
[MazdaCarDocs("Mazda 3 2017-18")], |
|
||||||
MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0) |
|
||||||
) |
|
||||||
MAZDA_6 = MazdaPlatformConfig( |
|
||||||
[MazdaCarDocs("Mazda 6 2017-20")], |
|
||||||
MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5) |
|
||||||
) |
|
||||||
MAZDA_CX9_2021 = MazdaPlatformConfig( |
|
||||||
[MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")], |
|
||||||
MAZDA_CX9.specs |
|
||||||
) |
|
||||||
MAZDA_CX5_2022 = MazdaPlatformConfig( |
|
||||||
[MazdaCarDocs("Mazda CX-5 2022-24")], |
|
||||||
MAZDA_CX5.specs, |
|
||||||
) |
|
||||||
|
|
||||||
|
|
||||||
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=[ |
|
||||||
# TODO: check data to ensure ABS does not skip ISO-TP frames on bus 0 |
|
||||||
Request( |
|
||||||
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], |
|
||||||
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], |
|
||||||
bus=0, |
|
||||||
), |
|
||||||
], |
|
||||||
) |
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
@ -1,6 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
|
|
||||||
|
|
||||||
class CarController(CarControllerBase): |
|
||||||
def update(self, CC, CS, now_nanos): |
|
||||||
return CC.actuators, [] |
|
||||||
@ -1,7 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
|
|
||||||
|
|
||||||
class CarState(CarStateBase): |
|
||||||
def update(self, *_) -> structs.CarState: |
|
||||||
return structs.CarState() |
|
||||||
@ -1,17 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from openpilot.selfdrive.car import structs |
|
||||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase |
|
||||||
|
|
||||||
|
|
||||||
# mocked car interface for dashcam mode |
|
||||||
class CarInterface(CarInterfaceBase): |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams: |
|
||||||
ret.carName = "mock" |
|
||||||
ret.mass = 1700. |
|
||||||
ret.wheelbase = 2.70 |
|
||||||
ret.centerToFront = ret.wheelbase * 0.5 |
|
||||||
ret.steerRatio = 13. |
|
||||||
ret.dashcamOnly = True |
|
||||||
return ret |
|
||||||
@ -1,4 +0,0 @@ |
|||||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase |
|
||||||
|
|
||||||
class RadarInterface(RadarInterfaceBase): |
|
||||||
pass |
|
||||||
@ -1,9 +0,0 @@ |
|||||||
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms |
|
||||||
|
|
||||||
|
|
||||||
class CAR(Platforms): |
|
||||||
MOCK = PlatformConfig( |
|
||||||
[], |
|
||||||
CarSpecs(mass=1700, wheelbase=2.7, steerRatio=13), |
|
||||||
{} |
|
||||||
) |
|
||||||
@ -1,81 +0,0 @@ |
|||||||
import copy |
|
||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import apply_std_steer_angle_limits, structs |
|
||||||
from openpilot.selfdrive.car.interfaces import CarControllerBase |
|
||||||
from openpilot.selfdrive.car.nissan import nissancan |
|
||||||
from openpilot.selfdrive.car.nissan.values import CAR, CarControllerParams |
|
||||||
|
|
||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert |
|
||||||
|
|
||||||
|
|
||||||
class CarController(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.car_fingerprint = CP.carFingerprint |
|
||||||
|
|
||||||
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.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_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.NISSAN_LEAF, CAR.NISSAN_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.NISSAN_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 = copy.copy(actuators) |
|
||||||
new_actuators.steeringAngleDeg = apply_angle |
|
||||||
|
|
||||||
self.frame += 1 |
|
||||||
return new_actuators, can_sends |
|
||||||
@ -1,201 +0,0 @@ |
|||||||
import copy |
|
||||||
from collections import deque |
|
||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from openpilot.selfdrive.car import create_button_events, structs |
|
||||||
from openpilot.selfdrive.car.common.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
from openpilot.selfdrive.car.nissan.values import CAR, DBC, CarControllerParams |
|
||||||
|
|
||||||
ButtonType = structs.CarState.ButtonEvent.Type |
|
||||||
|
|
||||||
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"] |
|
||||||
|
|
||||||
self.distance_button = 0 |
|
||||||
|
|
||||||
def update(self, cp, cp_cam, cp_adas, *_) -> structs.CarState: |
|
||||||
ret = structs.CarState() |
|
||||||
|
|
||||||
prev_distance_button = self.distance_button |
|
||||||
self.distance_button = cp.vl["CRUISE_THROTTLE"]["FOLLOW_DISTANCE_BUTTON"] |
|
||||||
|
|
||||||
if self.CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA): |
|
||||||
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] |
|
||||||
elif self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): |
|
||||||
ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] |
|
||||||
|
|
||||||
ret.gasPressed = bool(ret.gas > 3) |
|
||||||
|
|
||||||
if self.CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA): |
|
||||||
ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) |
|
||||||
elif self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_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.NISSAN_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.NISSAN_ROGUE, CAR.NISSAN_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.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): |
|
||||||
if self.CP.carFingerprint == CAR.NISSAN_LEAF: |
|
||||||
ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 |
|
||||||
elif self.CP.carFingerprint == CAR.NISSAN_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.NISSAN_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.NISSAN_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.NISSAN_LEAF, CAR.NISSAN_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.NISSAN_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.NISSAN_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.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): |
|
||||||
self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) |
|
||||||
|
|
||||||
if self.CP.carFingerprint != CAR.NISSAN_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"]) |
|
||||||
|
|
||||||
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) |
|
||||||
|
|
||||||
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.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA): |
|
||||||
messages += [ |
|
||||||
("GAS_PEDAL", 100), |
|
||||||
("CRUISE_THROTTLE", 50), |
|
||||||
("HUD", 25), |
|
||||||
] |
|
||||||
|
|
||||||
elif CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): |
|
||||||
messages += [ |
|
||||||
("BRAKE_PEDAL", 100), |
|
||||||
("CRUISE_THROTTLE", 50), |
|
||||||
("CANCEL_MSG", 50), |
|
||||||
("HUD_SETTINGS", 25), |
|
||||||
("SEATBELT", 10), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.carFingerprint == CAR.NISSAN_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.NISSAN_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.NISSAN_ROGUE, CAR.NISSAN_XTRAIL): |
|
||||||
messages.append(("PRO_PILOT", 100)) |
|
||||||
elif CP.carFingerprint == CAR.NISSAN_ALTIMA: |
|
||||||
messages.append(("STEER_TORQUE_SENSOR", 100)) |
|
||||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) |
|
||||||
|
|
||||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1) |
|
||||||
@ -1,119 +0,0 @@ |
|||||||
# ruff: noqa: E501 |
|
||||||
from openpilot.selfdrive.car.structs import CarParams |
|
||||||
from openpilot.selfdrive.car.nissan.values import CAR |
|
||||||
|
|
||||||
Ecu = CarParams.Ecu |
|
||||||
|
|
||||||
FINGERPRINTS = { |
|
||||||
CAR.NISSAN_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.NISSAN_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.NISSAN_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.NISSAN_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.NISSAN_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.NISSAN_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.NISSAN_LEAF: { |
|
||||||
(Ecu.abs, 0x740, None): [ |
|
||||||
b'476605SA1C', |
|
||||||
b'476605SA7D', |
|
||||||
b'476605SC2D', |
|
||||||
b'476606WK7B', |
|
||||||
b'476606WK9B', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x742, None): [ |
|
||||||
b'5SA2A\x99A\x05\x02N123F\x15b\x00\x00\x00\x00\x00\x00\x00\x80', |
|
||||||
b'5SA2A\xb7A\x05\x02N123F\x15\xa2\x00\x00\x00\x00\x00\x00\x00\x80', |
|
||||||
b'5SN2A\xb7A\x05\x02N123F\x15\xa2\x00\x00\x00\x00\x00\x00\x00\x80', |
|
||||||
b'5SN2A\xb7A\x05\x02N126F\x15\xb2\x00\x00\x00\x00\x00\x00\x00\x80', |
|
||||||
], |
|
||||||
(Ecu.fwdCamera, 0x707, None): [ |
|
||||||
b'5SA0ADB\x04\x18\x00\x00\x00\x00\x00_*6\x04\x94a\x00\x00\x00\x80', |
|
||||||
b'5SA2ADB\x04\x18\x00\x00\x00\x00\x00_*6\x04\x94a\x00\x00\x00\x80', |
|
||||||
b'6WK2ADB\x04\x18\x00\x00\x00\x00\x00R;1\x18\x99\x10\x00\x00\x00\x80', |
|
||||||
b'6WK2BDB\x04\x18\x00\x00\x00\x00\x00R;1\x18\x99\x10\x00\x00\x00\x80', |
|
||||||
b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80', |
|
||||||
], |
|
||||||
(Ecu.gateway, 0x18dad0f1, None): [ |
|
||||||
b'284U25SA3C', |
|
||||||
b'284U25SP0C', |
|
||||||
b'284U25SP1C', |
|
||||||
b'284U26WK0A', |
|
||||||
b'284U26WK0C', |
|
||||||
], |
|
||||||
}, |
|
||||||
CAR.NISSAN_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.NISSAN_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', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue