clearpilot: initial commit of full source
This commit is contained in:
73
selfdrive/car/CARS_template.md
Executable file
73
selfdrive/car/CARS_template.md
Executable file
@@ -0,0 +1,73 @@
|
||||
{% set footnote_tag = '[<sup>{}</sup>](#footnotes)' %}
|
||||
{% set star_icon = '[](##)' %}
|
||||
{% set video_icon = '<a href="{}" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>' %}
|
||||
{# Force hardware column wider by using a blank image with max width. #}
|
||||
{% set width_tag = '<a href="##"><img width=2000></a>%s<br> ' %}
|
||||
{% set hardware_col_name = 'Hardware Needed' %}
|
||||
{% set wide_hardware_col_name = width_tag|format(hardware_col_name) -%}
|
||||
|
||||
<!--- AUTOGENERATED FROM selfdrive/car/CARS_template.md, DO NOT EDIT. --->
|
||||
|
||||
# Supported Cars
|
||||
|
||||
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
|
||||
|
||||
# {{all_car_docs | length}} Supported Cars
|
||||
|
||||
|{{Column | map(attribute='value') | join('|') | replace(hardware_col_name, wide_hardware_col_name)}}|
|
||||
|---|---|---|{% for _ in range((Column | length) - 3) %}{{':---:|'}}{% endfor +%}
|
||||
{% for car_docs in all_car_docs %}
|
||||
|{% for column in Column %}{{car_docs.get_column(column, star_icon, video_icon, footnote_tag)}}|{% endfor %}
|
||||
|
||||
{% endfor %}
|
||||
|
||||
### Footnotes
|
||||
{% for footnote in footnotes %}
|
||||
<sup>{{loop.index}}</sup>{{footnote}} <br />
|
||||
{% endfor %}
|
||||
|
||||
## Community Maintained Cars
|
||||
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
|
||||
# Don't see your car here?
|
||||
|
||||
**openpilot can support many more cars than it currently does.** There are a few reasons your car may not be supported.
|
||||
If your car doesn't fit into any of the incompatibility criteria here, then there's a good chance it can be supported! We're adding support for new cars all the time. **We don't have a roadmap for car support**, and in fact, most car support comes from users like you!
|
||||
|
||||
### Which cars are able to be supported?
|
||||
|
||||
openpilot uses the existing steering, gas, and brake interfaces in your car. If your car lacks any one of these interfaces, openpilot will not be able to control the car. If your car has [ACC](https://en.wikipedia.org/wiki/Adaptive_cruise_control) and any form of [LKAS](https://en.wikipedia.org/wiki/Automated_Lane_Keeping_Systems)/[LCA](https://en.wikipedia.org/wiki/Lane_centering), then it almost certainly has these interfaces. These features generally started shipping on cars around 2016. Note that manufacturers will often make their own [marketing terms](https://en.wikipedia.org/wiki/Adaptive_cruise_control#Vehicle_models_supporting_adaptive_cruise_control) for these features, such as Hyundai's "Smart Cruise Control" branding of Adaptive Cruise Control.
|
||||
|
||||
If your car has the following packages or features, then it's a good candidate for support.
|
||||
|
||||
| Make | Required Package/Features |
|
||||
| ---- | ------------------------- |
|
||||
| Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. |
|
||||
| Ford | Any car with Lane Centering will likely work. |
|
||||
| Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. |
|
||||
| Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. |
|
||||
| Nissan | Any car with ProPILOT will likely work. |
|
||||
| Toyota & Lexus | Any car that has Toyota/Lexus Safety Sense with "Lane Departure Alert with Steering Assist (LDA w/SA)" and/or "Lane Tracing Assist (LTA)" will work. Note that LDA without Steering Assist will not work. These features come standard on most newer models. |
|
||||
| Hyundai, Kia, & Genesis | Any car with Smart Cruise Control (SCC) and Lane Following Assist (LFA) or Lane Keeping Assist (LKAS) will work. LKAS/LFA comes standard on most newer models. Any form of SCC will work, such as NSCC. |
|
||||
| Chrysler, Jeep, & Ram | Any car with LaneSense and Adaptive Cruise Control will likely work. These come standard on many newer models. |
|
||||
|
||||
### FlexRay
|
||||
|
||||
All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay.
|
||||
|
||||
### Toyota Security
|
||||
|
||||
openpilot does not yet support these Toyota models due to a new message authentication method.
|
||||
[Vote](https://comma.ai/shop#toyota-security) if you'd like to see openpilot support on these models.
|
||||
|
||||
* Toyota RAV4 Prime 2021+
|
||||
* Toyota Sienna 2021+
|
||||
* Toyota Venza 2021+
|
||||
* Toyota Sequoia 2023+
|
||||
* Toyota Tundra 2022+
|
||||
* Toyota Highlander 2024+
|
||||
* Toyota Corolla Cross 2022+ (only US model)
|
||||
* Lexus NX 2022+
|
||||
* Toyota bZ4x 2023+
|
||||
* Subaru Solterra 2023+
|
||||
|
||||
19
selfdrive/car/README.md
Executable file
19
selfdrive/car/README.md
Executable file
@@ -0,0 +1,19 @@
|
||||
## Car port structure
|
||||
|
||||
### interface.py
|
||||
Generic interface to send and receive messages from CAN (controlsd uses this to communicate with car)
|
||||
|
||||
### fingerprints.py
|
||||
Fingerprints for matching to a specific car
|
||||
|
||||
### carcontroller.py
|
||||
Builds CAN messages to send to car
|
||||
|
||||
##### carstate.py
|
||||
Reads CAN from car and builds openpilot CarState message
|
||||
|
||||
##### values.py
|
||||
Limits for actuation, general constants for cars, and supported car documentation
|
||||
|
||||
##### radar_interface.py
|
||||
Interface for parsing radar points from the car
|
||||
317
selfdrive/car/__init__.py
Executable file
317
selfdrive/car/__init__.py
Executable file
@@ -0,0 +1,317 @@
|
||||
# functions common among cars
|
||||
from collections import defaultdict, namedtuple
|
||||
from dataclasses import dataclass
|
||||
from enum import IntFlag, ReprEnum
|
||||
from dataclasses import replace
|
||||
|
||||
import capnp
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.utils import Freezable
|
||||
from openpilot.selfdrive.car.docs_definitions import CarDocs
|
||||
|
||||
|
||||
# kg of standard extra cargo to count for drive, gas, etc...
|
||||
STD_CARGO_KG = 136.
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])
|
||||
|
||||
|
||||
def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
|
||||
if val > val_steady + hyst_gap:
|
||||
val_steady = val - hyst_gap
|
||||
elif val < val_steady - hyst_gap:
|
||||
val_steady = val + hyst_gap
|
||||
return val_steady
|
||||
|
||||
|
||||
def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, capnp.lib.capnp._EnumModule],
|
||||
unpressed_btn: int = 0) -> list[capnp.lib.capnp._DynamicStructBuilder]:
|
||||
events: list[capnp.lib.capnp._DynamicStructBuilder] = []
|
||||
|
||||
if cur_btn == prev_btn:
|
||||
return events
|
||||
|
||||
# Add events for button presses, multiple when a button switches without going to unpressed
|
||||
for pressed, btn in ((False, prev_btn), (True, cur_btn)):
|
||||
if btn != unpressed_btn:
|
||||
events.append(car.CarState.ButtonEvent(pressed=pressed,
|
||||
type=buttons_dict.get(btn, ButtonType.unknown)))
|
||||
return events
|
||||
|
||||
|
||||
def gen_empty_fingerprint():
|
||||
return {i: {} for i in range(8)}
|
||||
|
||||
|
||||
# these params were derived for the Civic and used to calculate params for other cars
|
||||
class VehicleDynamicsParams:
|
||||
MASS = 1326. + STD_CARGO_KG
|
||||
WHEELBASE = 2.70
|
||||
CENTER_TO_FRONT = WHEELBASE * 0.4
|
||||
CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT
|
||||
ROTATIONAL_INERTIA = 2500
|
||||
TIRE_STIFFNESS_FRONT = 192150
|
||||
TIRE_STIFFNESS_REAR = 202500
|
||||
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
def scale_rot_inertia(mass, wheelbase):
|
||||
return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
|
||||
|
||||
|
||||
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
|
||||
# mass and CG position, so all cars will have approximately similar dyn behaviors
|
||||
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
|
||||
center_to_rear = wheelbase - center_to_front
|
||||
tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
|
||||
(center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
|
||||
|
||||
tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
|
||||
(center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
|
||||
|
||||
return tire_stiffness_front, tire_stiffness_rear
|
||||
|
||||
|
||||
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 crc8_pedal(data):
|
||||
crc = 0xFF # standard init value
|
||||
poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1
|
||||
size = len(data)
|
||||
for i in range(size - 1, -1, -1):
|
||||
crc ^= data[i]
|
||||
for _ in range(8):
|
||||
if ((crc & 0x80) != 0):
|
||||
crc = ((crc << 1) ^ poly) & 0xFF
|
||||
else:
|
||||
crc <<= 1
|
||||
return crc
|
||||
|
||||
|
||||
def create_gas_interceptor_command(packer, gas_amount, idx):
|
||||
# Common gas pedal msg generator
|
||||
enable = gas_amount > 0.001
|
||||
|
||||
values = {
|
||||
"ENABLE": enable,
|
||||
"COUNTER_PEDAL": idx & 0xF,
|
||||
}
|
||||
|
||||
if enable:
|
||||
values["GAS_COMMAND"] = gas_amount * 255.
|
||||
values["GAS_COMMAND2"] = gas_amount * 255.
|
||||
|
||||
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
|
||||
|
||||
checksum = crc8_pedal(dat[:-1])
|
||||
values["CHECKSUM_PEDAL"] = checksum
|
||||
|
||||
return packer.make_can_msg("GAS_COMMAND", 0, values)
|
||||
|
||||
|
||||
def make_can_msg(addr, dat, bus):
|
||||
return [addr, 0, dat, bus]
|
||||
|
||||
|
||||
def get_safety_config(safety_model, safety_param = None):
|
||||
ret = car.CarParams.SafetyConfig.new_message()
|
||||
ret.safetyModel = safety_model
|
||||
if safety_param is not None:
|
||||
ret.safetyParam = safety_param
|
||||
return ret
|
||||
|
||||
|
||||
class CanBusBase:
|
||||
offset: int
|
||||
|
||||
def __init__(self, CP, fingerprint: 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)
|
||||
|
||||
|
||||
@dataclass(order=True)
|
||||
class PlatformConfig(Freezable):
|
||||
platform_str: str
|
||||
car_docs: list[CarDocs]
|
||||
specs: CarSpecs
|
||||
|
||||
dbc_dict: DbcDict
|
||||
|
||||
flags: int = 0
|
||||
|
||||
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()
|
||||
self.freeze()
|
||||
|
||||
|
||||
class Platforms(str, ReprEnum):
|
||||
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
|
||||
|
||||
@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}
|
||||
|
||||
@classmethod
|
||||
def without_flags(cls, flags: IntFlag) -> set['Platforms']:
|
||||
return {p for p in cls if not (p.config.flags & flags)}
|
||||
|
||||
@classmethod
|
||||
def print_debug(cls, flags):
|
||||
platforms_with_flag = defaultdict(list)
|
||||
for flag in flags:
|
||||
for platform in cls:
|
||||
if platform.config.flags & flag:
|
||||
assert flag.name is not None
|
||||
platforms_with_flag[flag.name].append(platform)
|
||||
|
||||
for flag, platforms in platforms_with_flag.items():
|
||||
print(f"{flag:32s}: {', '.join(p.name for p in platforms)}")
|
||||
0
selfdrive/car/body/__init__.py
Executable file
0
selfdrive/car/body/__init__.py
Executable file
7
selfdrive/car/body/bodycan.py
Executable file
7
selfdrive/car/body/bodycan.py
Executable file
@@ -0,0 +1,7 @@
|
||||
def create_control(packer, torque_l, torque_r):
|
||||
values = {
|
||||
"TORQUE_L": torque_l,
|
||||
"TORQUE_R": torque_r,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("TORQUE_CMD", 0, values)
|
||||
84
selfdrive/car/body/carcontroller.py
Executable file
84
selfdrive/car/body/carcontroller.py
Executable file
@@ -0,0 +1,84 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car.body import bodycan
|
||||
from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.pid import PIDController
|
||||
|
||||
|
||||
MAX_TORQUE = 500
|
||||
MAX_TORQUE_RATE = 50
|
||||
MAX_ANGLE_ERROR = np.radians(7)
|
||||
MAX_POS_INTEGRATOR = 0.2 # meters
|
||||
MAX_TURN_INTEGRATOR = 0.1 # meters
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.frame = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
# PIDs
|
||||
self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
|
||||
self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
|
||||
|
||||
self.torque_r_filtered = 0.
|
||||
self.torque_l_filtered = 0.
|
||||
|
||||
@staticmethod
|
||||
def deadband_filter(torque, deadband):
|
||||
if torque > 0:
|
||||
torque += deadband
|
||||
else:
|
||||
torque -= deadband
|
||||
return torque
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
|
||||
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 = CC.actuators.copy()
|
||||
new_actuators.accel = torque_l
|
||||
new_actuators.steer = torque_r
|
||||
new_actuators.steerOutputCan = torque_r
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
40
selfdrive/car/body/carstate.py
Executable file
40
selfdrive/car/body/carstate.py
Executable file
@@ -0,0 +1,40 @@
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.car.body.values import DBC
|
||||
|
||||
STARTUP_TICKS = 100
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def update(self, cp, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
|
||||
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
|
||||
|
||||
ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor
|
||||
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = False
|
||||
|
||||
ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'],
|
||||
cp.vl['VAR_VALUES']['FAULT']])
|
||||
|
||||
ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1
|
||||
ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100
|
||||
|
||||
# irrelevant for non-car
|
||||
ret.gearShifter = car.CarState.GearShifter.drive
|
||||
ret.cruiseState.enabled = True
|
||||
ret.cruiseState.available = True
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
("MOTORS_DATA", 100),
|
||||
("VAR_VALUES", 10),
|
||||
("BODY_DATA", 1),
|
||||
]
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
|
||||
28
selfdrive/car/body/fingerprints.py
Executable file
28
selfdrive/car/body/fingerprints.py
Executable file
@@ -0,0 +1,28 @@
|
||||
# ruff: noqa: E501
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.body.values import CAR
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
# debug ecu fw version is the git hash of the firmware
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.BODY: [{
|
||||
513: 8, 516: 8, 514: 3, 515: 4
|
||||
}],
|
||||
}
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.BODY: {
|
||||
(Ecu.engine, 0x720, None): [
|
||||
b'0.0.01',
|
||||
b'0.3.00a',
|
||||
b'02/27/2022',
|
||||
],
|
||||
(Ecu.debug, 0x721, None): [
|
||||
b'166bd860',
|
||||
b'dc780f85',
|
||||
],
|
||||
},
|
||||
}
|
||||
42
selfdrive/car/body/interface.py
Executable file
42
selfdrive/car/body/interface.py
Executable file
@@ -0,0 +1,42 @@
|
||||
import math
|
||||
from cereal import car
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car import get_safety_config
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.notCar = True
|
||||
ret.carName = "body"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)]
|
||||
|
||||
ret.minSteerSpeed = -math.inf
|
||||
ret.maxLateralAccel = math.inf # TODO: set to a reasonable value
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerActuatorDelay = 0.
|
||||
|
||||
ret.wheelSpeedFactor = SPEED_FROM_RPM
|
||||
|
||||
ret.radarUnavailable = True
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
|
||||
return ret
|
||||
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, frogpilot_variables)
|
||||
|
||||
# wait for everything to init first
|
||||
if self.frame > int(5. / DT_CTRL):
|
||||
# body always wants to enable
|
||||
ret.init('events', 1)
|
||||
ret.events[0].name = car.CarEvent.EventName.pcmEnable
|
||||
ret.events[0].enable = True
|
||||
self.frame += 1
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
4
selfdrive/car/body/radar_interface.py
Executable file
4
selfdrive/car/body/radar_interface.py
Executable file
@@ -0,0 +1,4 @@
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
41
selfdrive/car/body/values.py
Executable file
41
selfdrive/car/body/values.py
Executable file
@@ -0,0 +1,41 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict
|
||||
from openpilot.selfdrive.car.docs_definitions import CarDocs
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
SPEED_FROM_RPM = 0.008587
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
ANGLE_DELTA_BP = [0., 5., 15.]
|
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
BODY = PlatformConfig(
|
||||
"COMMA BODY",
|
||||
[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()
|
||||
251
selfdrive/car/car_helpers.py
Executable file
251
selfdrive/car/car_helpers.py
Executable file
@@ -0,0 +1,251 @@
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Callable
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.system.version import get_short_branch, is_comma_remote, is_tested_branch
|
||||
from openpilot.selfdrive.car.interfaces import get_interface_attr
|
||||
from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
|
||||
from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
|
||||
from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
|
||||
from openpilot.selfdrive.car.mock.values import CAR as MOCK
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
import cereal.messaging as messaging
|
||||
import openpilot.selfdrive.sentry as sentry
|
||||
from openpilot.selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
FRAME_FINGERPRINT = 100 # 1s
|
||||
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
|
||||
def get_startup_event(car_recognized, controller_available, fw_seen):
|
||||
if is_comma_remote() and is_tested_branch():
|
||||
event = EventName.startup
|
||||
else:
|
||||
event = EventName.startupMaster
|
||||
|
||||
if not car_recognized:
|
||||
if fw_seen:
|
||||
event = EventName.startupNoCar
|
||||
else:
|
||||
event = EventName.startupNoFw
|
||||
elif car_recognized and not controller_available:
|
||||
event = EventName.startupNoControl
|
||||
return event
|
||||
|
||||
|
||||
def get_one_can(logcan):
|
||||
while True:
|
||||
can = messaging.recv_one_retry(logcan)
|
||||
if len(can.can) > 0:
|
||||
return can
|
||||
|
||||
|
||||
def load_interfaces(brand_names):
|
||||
ret = {}
|
||||
for brand_name in brand_names:
|
||||
path = f'openpilot.selfdrive.car.{brand_name}'
|
||||
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
|
||||
|
||||
if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'):
|
||||
CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState
|
||||
else:
|
||||
CarState = None
|
||||
|
||||
if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'):
|
||||
CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController
|
||||
else:
|
||||
CarController = None
|
||||
|
||||
for model_name in brand_names[brand_name]:
|
||||
ret[model_name] = (CarInterface, CarController, CarState)
|
||||
return ret
|
||||
|
||||
|
||||
def _get_interface_names() -> dict[str, list[str]]:
|
||||
# returns a dict of brand name and its respective models
|
||||
brand_names = {}
|
||||
for brand_name, brand_models in get_interface_attr("CAR").items():
|
||||
brand_names[brand_name] = [model.value for model in brand_models]
|
||||
|
||||
return brand_names
|
||||
|
||||
|
||||
# imports from directory selfdrive/car/<name>/
|
||||
interface_names = _get_interface_names()
|
||||
interfaces = load_interfaces(interface_names)
|
||||
|
||||
|
||||
def can_fingerprint(next_can: Callable) -> tuple[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:
|
||||
a = next_can()
|
||||
|
||||
for can in a.can:
|
||||
# The fingerprint dict is generated for all buses, this way the car interface
|
||||
# can use it to detect a (valid) multipanda setup and initialize accordingly
|
||||
if can.src < 128:
|
||||
if can.src not in finger:
|
||||
finger[can.src] = {}
|
||||
finger[can.src][can.address] = len(can.dat)
|
||||
|
||||
for b in candidate_cars:
|
||||
# Ignore extended messages and VIN query response.
|
||||
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
|
||||
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
|
||||
|
||||
# if we only have one car choice and the time since we got our first
|
||||
# message has elapsed, exit
|
||||
for b in candidate_cars:
|
||||
if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT:
|
||||
# fingerprint done
|
||||
car_fingerprint = candidate_cars[b][0]
|
||||
|
||||
# bail if no cars left or we've been waiting for more than 2s
|
||||
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200
|
||||
succeeded = car_fingerprint is not None
|
||||
done = failed or succeeded
|
||||
|
||||
frame += 1
|
||||
|
||||
return car_fingerprint, finger
|
||||
|
||||
|
||||
# **** for use live only ****
|
||||
def fingerprint(logcan, sendcan, num_pandas):
|
||||
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
|
||||
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
|
||||
disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False)
|
||||
ecu_rx_addrs = set()
|
||||
params = Params()
|
||||
|
||||
start_time = time.monotonic()
|
||||
if not skip_fw_query:
|
||||
cached_params = params.get("CarParamsCache")
|
||||
if cached_params is not None:
|
||||
with car.CarParams.from_bytes(cached_params) as cached_params:
|
||||
if cached_params.carName == "mock":
|
||||
cached_params = None
|
||||
|
||||
if cached_params is not None and len(cached_params.carFw) > 0 and \
|
||||
cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache:
|
||||
cloudlog.warning("Using cached CarParams")
|
||||
vin_rx_addr, vin_rx_bus, vin = -1, -1, cached_params.carVin
|
||||
car_fw = list(cached_params.carFw)
|
||||
cached = True
|
||||
else:
|
||||
cloudlog.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(params, True)
|
||||
# VIN query only reliably works through OBDII
|
||||
vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1))
|
||||
ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas)
|
||||
car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas)
|
||||
cached = False
|
||||
|
||||
exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
|
||||
else:
|
||||
vin_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):
|
||||
cloudlog.event("Malformed VIN", vin=vin, error=True)
|
||||
vin = VIN_UNKNOWN
|
||||
cloudlog.warning("VIN %s", vin)
|
||||
params.put("CarVin", vin)
|
||||
|
||||
# disable OBD multiplexing for CAN fingerprinting and potential ECU knockouts
|
||||
set_obd_multiplexing(params, False)
|
||||
params.put_bool("FirmwareQueryDone", True)
|
||||
|
||||
fw_query_time = time.monotonic() - start_time
|
||||
|
||||
# CAN fingerprint
|
||||
# drain CAN socket so we get the latest messages
|
||||
messaging.drain_sock_raw(logcan)
|
||||
car_fingerprint, finger = can_fingerprint(lambda: get_one_can(logcan))
|
||||
|
||||
exact_match = True
|
||||
source = car.CarParams.FingerprintSource.can
|
||||
|
||||
# If FW query returns exactly 1 candidate, use it
|
||||
if len(fw_candidates) == 1:
|
||||
car_fingerprint = list(fw_candidates)[0]
|
||||
source = car.CarParams.FingerprintSource.fw
|
||||
exact_match = exact_fw_match
|
||||
|
||||
if fixed_fingerprint:
|
||||
car_fingerprint = fixed_fingerprint
|
||||
source = car.CarParams.FingerprintSource.fixed
|
||||
|
||||
cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
|
||||
fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, vin_rx_bus=vin_rx_bus,
|
||||
fingerprints=repr(finger), fw_query_time=fw_query_time, error=True)
|
||||
|
||||
return car_fingerprint, finger, vin, car_fw, source, exact_match
|
||||
|
||||
|
||||
def get_car_interface(CP):
|
||||
CarInterface, CarController, CarState = interfaces[CP.carFingerprint]
|
||||
return CarInterface(CP, CarController, CarState)
|
||||
|
||||
|
||||
def get_car(params, logcan, sendcan, disable_openpilot_long, experimental_long_allowed, num_pandas=1):
|
||||
car_brand = params.get("CarMake", encoding='utf-8')
|
||||
car_model = params.get("CarModel", encoding='utf-8')
|
||||
|
||||
force_fingerprint = params.get_bool("ForceFingerprint")
|
||||
|
||||
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas)
|
||||
|
||||
if candidate is None or force_fingerprint:
|
||||
if car_model is not None:
|
||||
candidate = car_model
|
||||
else:
|
||||
cloudlog.event("car doesn't match any fingerprints", fingerprints=repr(fingerprints), error=True)
|
||||
candidate = "mock"
|
||||
|
||||
if car_model is None and candidate != "mock":
|
||||
params.put("CarMake", candidate.split(' ')[0].title())
|
||||
params.put("CarModel", candidate)
|
||||
|
||||
if get_short_branch() == "FrogPilot-Development" and not Params("/persist/params").get_bool("FrogsGoMoo"):
|
||||
cloudlog.event("Blocked user from using the 'FrogPilot-Development' branch", fingerprints=repr(fingerprints), error=True)
|
||||
candidate = "mock"
|
||||
fingerprint_log = threading.Thread(target=sentry.capture_fingerprint, args=(params, candidate, True,))
|
||||
fingerprint_log.start()
|
||||
elif not params.get_bool("FingerprintLogged"):
|
||||
fingerprint_log = threading.Thread(target=sentry.capture_fingerprint, args=(params, candidate,))
|
||||
fingerprint_log.start()
|
||||
|
||||
CarInterface, _, _ = interfaces[candidate]
|
||||
CP = CarInterface.get_params(params, candidate, fingerprints, car_fw, disable_openpilot_long, 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), CP
|
||||
|
||||
def write_car_param(platform=MOCK.MOCK):
|
||||
params = Params()
|
||||
CarInterface, _, _ = interfaces[platform]
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
params.put("CarParams", CP.to_bytes())
|
||||
|
||||
def get_demo_car_params():
|
||||
platform = MOCK.MOCK
|
||||
CarInterface, _, _ = interfaces[platform]
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
return CP
|
||||
160
selfdrive/car/card.py
Executable file
160
selfdrive/car/card.py
Executable file
@@ -0,0 +1,160 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
|
||||
import cereal.messaging as messaging
|
||||
|
||||
from cereal import car
|
||||
|
||||
from panda import ALTERNATIVE_EXPERIENCE
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
|
||||
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from openpilot.selfdrive.car.car_helpers import get_car, get_one_can
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
|
||||
# Notes:
|
||||
# This is a generic class for interfacing between controlsd and specific implementations for a car.
|
||||
|
||||
# Variables:
|
||||
# CI - carInterfaceBase
|
||||
# CP - carParams
|
||||
# CS - car state, should be consistent between every type of car
|
||||
|
||||
# Methods:
|
||||
# state_update - calls CI.update, gets a new CS. Read only, this is regenerated every time called.
|
||||
# state_publish - reads CS, CP, and carOutput.actuatorsOutput and sends via self.pm messaging
|
||||
# controls_update - reads CI.apply, gets canbus commands, publishes via self.pm, something else executes canbus commands
|
||||
|
||||
class CarD:
|
||||
CI: CarInterfaceBase
|
||||
CS: car.CarState
|
||||
|
||||
def __init__(self, CI=None):
|
||||
self.can_sock = messaging.sub_sock('can', timeout=20)
|
||||
self.sm = messaging.SubMaster(['pandaStates'])
|
||||
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
|
||||
|
||||
self.can_rcv_timeout_counter = 0 # conseuctive timeout count
|
||||
self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count
|
||||
|
||||
self.CC_prev = car.CarControl.new_message()
|
||||
|
||||
self.last_actuators = None
|
||||
|
||||
self.params = Params()
|
||||
|
||||
if CI is None:
|
||||
# wait for one pandaState and one CAN packet
|
||||
print("Waiting for CAN messages...")
|
||||
get_one_can(self.can_sock)
|
||||
|
||||
num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates)
|
||||
disable_openpilot_long = self.params.get_bool("DisableOpenpilotLongitudinal")
|
||||
experimental_long_allowed = not disable_openpilot_long and self.params.get_bool("ExperimentalLongitudinalEnabled")
|
||||
self.CI, self.CP = get_car(self.params, self.can_sock, self.pm.sock['sendcan'], disable_openpilot_long, experimental_long_allowed, num_pandas)
|
||||
else:
|
||||
self.CI, self.CP = CI, CI.CP
|
||||
|
||||
# set alternative experiences from parameters
|
||||
disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
|
||||
self.CP.alternativeExperience = 0
|
||||
if not disengage_on_accelerator:
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
|
||||
|
||||
always_on_lateral = self.params.get_bool("AlwaysOnLateral")
|
||||
if always_on_lateral:
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
|
||||
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX
|
||||
|
||||
car_recognized = self.CP.carName != 'mock'
|
||||
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
|
||||
|
||||
controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
|
||||
|
||||
self.CP.passive = not car_recognized or not controller_available or self.CP.dashcamOnly
|
||||
if self.CP.passive:
|
||||
safety_config = car.CarParams.SafetyConfig.new_message()
|
||||
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
|
||||
self.CP.safetyConfigs = [safety_config]
|
||||
|
||||
# Write previous route's CarParams
|
||||
prev_cp = self.params.get("CarParamsPersistent")
|
||||
if prev_cp is not None:
|
||||
self.params.put("CarParamsPrevRoute", prev_cp)
|
||||
|
||||
# Write CarParams for controls and radard
|
||||
cp_bytes = self.CP.to_bytes()
|
||||
self.params.put("CarParams", cp_bytes)
|
||||
self.params.put_nonblocking("CarParamsCache", cp_bytes)
|
||||
self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
|
||||
|
||||
def initialize(self):
|
||||
"""Initialize CarInterface, once controls are ready"""
|
||||
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
|
||||
|
||||
def state_update(self, frogpilot_variables):
|
||||
"""carState update loop, driven by can"""
|
||||
|
||||
# Update carState from CAN
|
||||
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
|
||||
self.CS = self.CI.update(self.CC_prev, can_strs, frogpilot_variables)
|
||||
|
||||
self.sm.update(0)
|
||||
|
||||
can_rcv_valid = len(can_strs) > 0
|
||||
|
||||
# Check for CAN timeout
|
||||
if not can_rcv_valid:
|
||||
self.can_rcv_timeout_counter += 1
|
||||
self.can_rcv_cum_timeout_counter += 1
|
||||
else:
|
||||
self.can_rcv_timeout_counter = 0
|
||||
|
||||
self.can_rcv_timeout = self.can_rcv_timeout_counter >= 5
|
||||
|
||||
if can_rcv_valid and REPLAY:
|
||||
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
|
||||
|
||||
self.state_publish()
|
||||
|
||||
return self.CS
|
||||
|
||||
def state_publish(self):
|
||||
"""carState and carParams publish loop"""
|
||||
|
||||
# carState
|
||||
cs_send = messaging.new_message('carState')
|
||||
cs_send.valid = self.CS.canValid
|
||||
cs_send.carState = self.CS
|
||||
self.pm.send('carState', cs_send)
|
||||
|
||||
# carParams - logged every 50 seconds (> 1 per segment)
|
||||
if (self.sm.frame % int(50. / DT_CTRL) == 0):
|
||||
cp_send = messaging.new_message('carParams')
|
||||
cp_send.valid = True
|
||||
cp_send.carParams = self.CP
|
||||
self.pm.send('carParams', cp_send)
|
||||
|
||||
# publish new carOutput
|
||||
co_send = messaging.new_message('carOutput')
|
||||
co_send.valid = True
|
||||
if self.last_actuators is not None:
|
||||
co_send.carOutput.actuatorsOutput = self.last_actuators
|
||||
self.pm.send('carOutput', co_send)
|
||||
|
||||
def controls_update(self, CC: car.CarControl, frogpilot_variables):
|
||||
"""control update loop, driven by carControl"""
|
||||
|
||||
# send car controls over can
|
||||
now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
|
||||
self.last_actuators, can_sends = self.CI.apply(CC, now_nanos, frogpilot_variables)
|
||||
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid))
|
||||
|
||||
self.CC_prev = CC
|
||||
0
selfdrive/car/chrysler/__init__.py
Executable file
0
selfdrive/car/chrysler/__init__.py
Executable file
85
selfdrive/car/chrysler/carcontroller.py
Executable file
85
selfdrive/car/chrysler/carcontroller.py
Executable file
@@ -0,0 +1,85 @@
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car import apply_meas_steer_torque_limits
|
||||
from openpilot.selfdrive.car.chrysler import chryslercan
|
||||
from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.frame = 0
|
||||
|
||||
self.hud_count = 0
|
||||
self.last_lkas_falling_edge = 0
|
||||
self.lkas_control_bit_prev = False
|
||||
self.last_button_frame = 0
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.params = CarControllerParams(CP)
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
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, CC.latActive))
|
||||
self.hud_count += 1
|
||||
|
||||
# steering
|
||||
if self.frame % self.params.STEER_STEP == 0:
|
||||
|
||||
# TODO: can we make this more sane? why is it different for all the cars?
|
||||
lkas_control_bit = self.lkas_control_bit_prev
|
||||
if CS.out.vEgo > self.CP.minSteerSpeed:
|
||||
lkas_control_bit = True
|
||||
elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
|
||||
lkas_control_bit = False
|
||||
elif self.CP.carFingerprint in RAM_CARS:
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
|
||||
lkas_control_bit = False
|
||||
|
||||
# EPS faults if LKAS re-enables too quickly
|
||||
lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200)
|
||||
|
||||
if not lkas_control_bit and self.lkas_control_bit_prev:
|
||||
self.last_lkas_falling_edge = self.frame
|
||||
self.lkas_control_bit_prev = lkas_control_bit
|
||||
|
||||
# steer torque
|
||||
new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
|
||||
apply_steer = apply_meas_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params)
|
||||
if not lkas_active or not lkas_control_bit:
|
||||
apply_steer = 0
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
|
||||
|
||||
self.frame += 1
|
||||
|
||||
new_actuators = CC.actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
|
||||
new_actuators.steerOutputCan = self.apply_steer_last
|
||||
|
||||
return new_actuators, can_sends
|
||||
162
selfdrive/car/chrysler/carstate.py
Executable file
162
selfdrive/car/chrysler/carstate.py
Executable file
@@ -0,0 +1,162 @@
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
self.CP = CP
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
|
||||
self.auto_high_beam = 0
|
||||
self.button_counter = 0
|
||||
self.lkas_car_model = -1
|
||||
|
||||
if CP.carFingerprint in RAM_CARS:
|
||||
self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"]
|
||||
else:
|
||||
self.shifter_values = can_define.dv["GEAR"]["PRNDL"]
|
||||
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
def update(self, cp, cp_cam, frogpilot_variables):
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.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"]
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
if self.CP.carFingerprint in RAM_CARS:
|
||||
self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"]
|
||||
else:
|
||||
self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1
|
||||
|
||||
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)
|
||||
72
selfdrive/car/chrysler/chryslercan.py
Executable file
72
selfdrive/car/chrysler/chryslercan.py
Executable file
@@ -0,0 +1,72 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.chrysler.values import RAM_CARS
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, lat_active):
|
||||
# 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 if lat_active else 0
|
||||
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
|
||||
values['LKAS_DISABLED'] = 0 if lat_active else 1
|
||||
|
||||
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)
|
||||
655
selfdrive/car/chrysler/fingerprints.py
Executable file
655
selfdrive/car/chrysler/fingerprints.py
Executable file
@@ -0,0 +1,655 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.chrysler.values import CAR
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.PACIFICA_2017_HYBRID: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68239262AH',
|
||||
b'68239262AI',
|
||||
b'68239262AJ',
|
||||
b'68239263AH',
|
||||
b'68239263AJ',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68238840AH',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'68226356AI',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68288309AC',
|
||||
b'68288309AD',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'68277480AV ',
|
||||
b'68277480AX ',
|
||||
b'68277480AZ ',
|
||||
],
|
||||
(Ecu.hybrid, 0x7e2, None): [
|
||||
b'05190175BF',
|
||||
b'05190175BH',
|
||||
b'05190226AK',
|
||||
],
|
||||
},
|
||||
CAR.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'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'68303534AJ ',
|
||||
b'68340762AD ',
|
||||
b'68340764AD ',
|
||||
b'68352652AE ',
|
||||
b'68352654AE ',
|
||||
b'68366851AH ',
|
||||
b'68366853AE ',
|
||||
b'68372861AF ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'68277370AJ',
|
||||
b'68277370AM',
|
||||
b'68277372AD',
|
||||
b'68277372AN',
|
||||
b'68277374AA',
|
||||
b'68277374AB',
|
||||
b'68277374AD',
|
||||
b'68277374AN',
|
||||
b'68367471AC',
|
||||
b'68380571AB',
|
||||
],
|
||||
},
|
||||
CAR.PACIFICA_2020: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68405327AC',
|
||||
b'68436233AB',
|
||||
b'68436233AC',
|
||||
b'68436234AB',
|
||||
b'68436250AE',
|
||||
b'68529067AA',
|
||||
b'68594993AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68405565AB',
|
||||
b'68405565AC',
|
||||
b'68444299AC',
|
||||
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',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68416742AA',
|
||||
b'68460393AA',
|
||||
b'68460393AB',
|
||||
b'68494461AB',
|
||||
b'68524936AA',
|
||||
b'68524936AB',
|
||||
b'68525338AB',
|
||||
b'68594340AB',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'68413871AD ',
|
||||
b'68413871AE ',
|
||||
b'68413871AH ',
|
||||
b'68413871AI ',
|
||||
b'68413873AI ',
|
||||
b'68443120AE ',
|
||||
b'68443123AC ',
|
||||
b'68443125AC ',
|
||||
b'68526752AD ',
|
||||
b'68526752AE ',
|
||||
b'68526754AE ',
|
||||
b'68536264AE ',
|
||||
b'68700306AB ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'68414271AC',
|
||||
b'68414271AD',
|
||||
b'68414275AC',
|
||||
b'68443154AB',
|
||||
b'68443155AC',
|
||||
b'68443158AB',
|
||||
b'68501050AD',
|
||||
b'68527221AB',
|
||||
b'68527223AB',
|
||||
b'68586231AD',
|
||||
],
|
||||
},
|
||||
CAR.PACIFICA_2018_HYBRID: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68358439AE',
|
||||
b'68358439AG',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68358990AC',
|
||||
b'68405939AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672758AA',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68288309AD',
|
||||
b'68525339AA',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'68366580AI ',
|
||||
b'68366580AK ',
|
||||
b'68366580AM ',
|
||||
],
|
||||
(Ecu.hybrid, 0x7e2, None): [
|
||||
b'05190226AI',
|
||||
b'05190226AK',
|
||||
b'05190226AM',
|
||||
],
|
||||
},
|
||||
CAR.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',
|
||||
],
|
||||
(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'68444228AD ',
|
||||
b'68444228AE ',
|
||||
b'68444228AF ',
|
||||
b'68499122AD ',
|
||||
b'68499122AE ',
|
||||
b'68499122AF ',
|
||||
b'68526772AD ',
|
||||
b'68526772AH ',
|
||||
b'68599493AC ',
|
||||
],
|
||||
(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',
|
||||
],
|
||||
},
|
||||
CAR.JEEP_GRAND_CHEROKEE: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68302211AC',
|
||||
b'68302212AD',
|
||||
b'68302246AC',
|
||||
b'68331511AC',
|
||||
b'68331574AC',
|
||||
b'68331687AC',
|
||||
b'68340272AD',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68316742AB',
|
||||
b'68355363AB',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68306178AD',
|
||||
b'68336276AB',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672627AB',
|
||||
b'68332015AB',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68321644AB',
|
||||
b'68321644AC',
|
||||
b'68321646AC',
|
||||
b'68321648AC',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05035920AE ',
|
||||
b'68284455AI ',
|
||||
b'68284456AI ',
|
||||
b'68284477AF ',
|
||||
b'68325564AH ',
|
||||
b'68325565AH ',
|
||||
b'68325565AI ',
|
||||
b'68325618AD ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035517AH',
|
||||
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'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: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68294051AG',
|
||||
b'68294051AI',
|
||||
b'68294052AG',
|
||||
b'68294052AH',
|
||||
b'68294063AG',
|
||||
b'68294063AH',
|
||||
b'68294063AI',
|
||||
b'68434846AC',
|
||||
b'68434847AC',
|
||||
b'68434849AC',
|
||||
b'68434856AC',
|
||||
b'68434858AC',
|
||||
b'68434859AC',
|
||||
b'68434860AC',
|
||||
b'68453483AC',
|
||||
b'68453483AD',
|
||||
b'68453487AD',
|
||||
b'68453491AC',
|
||||
b'68453499AD',
|
||||
b'68453503AC',
|
||||
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'68527381AE',
|
||||
b'68527382AE',
|
||||
b'68527383AD',
|
||||
b'68527387AE',
|
||||
b'68527403AC',
|
||||
b'68527403AD',
|
||||
b'68546047AF',
|
||||
b'68631938AA',
|
||||
b'68631942AA',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68428609AB',
|
||||
b'68441329AB',
|
||||
b'68473844AB',
|
||||
b'68490898AA',
|
||||
b'68500728AA',
|
||||
b'68615033AA',
|
||||
b'68615034AA',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68292406AH',
|
||||
b'68432418AB',
|
||||
b'68432418AD',
|
||||
b'68436004AD',
|
||||
b'68436004AE',
|
||||
b'68438454AC',
|
||||
b'68438454AD',
|
||||
b'68438456AE',
|
||||
b'68438456AF',
|
||||
b'68535469AB',
|
||||
b'68535470AC',
|
||||
b'68548900AB',
|
||||
b'68586307AB',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672892AB',
|
||||
b'04672932AB',
|
||||
b'04672932AC',
|
||||
b'22DTRHD_AA',
|
||||
b'68320950AH',
|
||||
b'68320950AI',
|
||||
b'68320950AJ',
|
||||
b'68320950AL',
|
||||
b'68320950AM',
|
||||
b'68454268AB',
|
||||
b'68475160AE',
|
||||
b'68475160AF',
|
||||
b'68475160AG',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'21590101AA',
|
||||
b'21590101AB',
|
||||
b'68273275AF',
|
||||
b'68273275AG',
|
||||
b'68273275AH',
|
||||
b'68312176AE',
|
||||
b'68312176AG',
|
||||
b'68440789AC',
|
||||
b'68466110AA',
|
||||
b'68466110AB',
|
||||
b'68469901AA',
|
||||
b'68469907AA',
|
||||
b'68522583AA',
|
||||
b'68522583AB',
|
||||
b'68522584AA',
|
||||
b'68522585AB',
|
||||
b'68552788AA',
|
||||
b'68552789AA',
|
||||
b'68552790AA',
|
||||
b'68552791AB',
|
||||
b'68552794AA',
|
||||
b'68585106AB',
|
||||
b'68585107AB',
|
||||
b'68585108AB',
|
||||
b'68585109AB',
|
||||
b'68585112AB',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05035699AG ',
|
||||
b'05035841AC ',
|
||||
b'05036026AB ',
|
||||
b'05036065AE ',
|
||||
b'05036066AE ',
|
||||
b'05149368AA ',
|
||||
b'05149591AD ',
|
||||
b'05149591AE ',
|
||||
b'05149592AE ',
|
||||
b'05149599AE ',
|
||||
b'05149600AD ',
|
||||
b'05149605AE ',
|
||||
b'05149846AA ',
|
||||
b'05149848AA ',
|
||||
b'05149848AC ',
|
||||
b'05190341AD',
|
||||
b'68378695AJ ',
|
||||
b'68378696AJ ',
|
||||
b'68378701AI ',
|
||||
b'68378702AI ',
|
||||
b'68378710AL ',
|
||||
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'68467915AC ',
|
||||
b'68467916AC ',
|
||||
b'68467936AC ',
|
||||
b'68500630AD',
|
||||
b'68500630AE',
|
||||
b'68502719AC ',
|
||||
b'68502722AC ',
|
||||
b'68502733AC ',
|
||||
b'68502734AF ',
|
||||
b'68502740AF ',
|
||||
b'68502741AF ',
|
||||
b'68502742AC ',
|
||||
b'68539650AD',
|
||||
b'68539650AF',
|
||||
b'68539651AD',
|
||||
b'68586101AA ',
|
||||
b'68586105AB ',
|
||||
b'68629922AC ',
|
||||
b'68629926AC ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035706AD',
|
||||
b'05035842AB',
|
||||
b'05036069AA',
|
||||
b'05149536AC',
|
||||
b'05149537AC',
|
||||
b'05149543AC',
|
||||
b'68360078AL',
|
||||
b'68360080AL',
|
||||
b'68360080AM',
|
||||
b'68360081AM',
|
||||
b'68360085AJ',
|
||||
b'68360085AL',
|
||||
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'68520867AE',
|
||||
b'68520867AF',
|
||||
b'68520870AC',
|
||||
b'68540431AB',
|
||||
b'68540433AB',
|
||||
b'68629936AC',
|
||||
],
|
||||
},
|
||||
CAR.RAM_HD: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68361606AH',
|
||||
b'68437735AC',
|
||||
b'68492693AD',
|
||||
b'68525485AB',
|
||||
b'68525487AB',
|
||||
b'68525498AB',
|
||||
b'68528791AF',
|
||||
b'68628474AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68399794AC',
|
||||
b'68428503AA',
|
||||
b'68428505AA',
|
||||
b'68428507AA',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68334977AH',
|
||||
b'68455481AC',
|
||||
b'68504022AA',
|
||||
b'68504022AB',
|
||||
b'68504022AC',
|
||||
b'68530686AB',
|
||||
b'68530686AC',
|
||||
b'68544596AC',
|
||||
b'68641704AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672895AB',
|
||||
b'04672934AB',
|
||||
b'56029827AG',
|
||||
b'56029827AH',
|
||||
b'68462657AE',
|
||||
b'68484694AD',
|
||||
b'68484694AE',
|
||||
b'68615489AB',
|
||||
],
|
||||
(Ecu.eps, 0x761, None): [
|
||||
b'68421036AC',
|
||||
b'68507906AB',
|
||||
b'68534023AC',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'52370131AF',
|
||||
b'52370231AF',
|
||||
b'52370231AG',
|
||||
b'52370491AA',
|
||||
b'52370931CT',
|
||||
b'52401032AE',
|
||||
b'52421132AF',
|
||||
b'52421332AF',
|
||||
b'68527616AD ',
|
||||
b'M2370131MB',
|
||||
b'M2421132MB',
|
||||
],
|
||||
},
|
||||
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',
|
||||
],
|
||||
},
|
||||
}
|
||||
103
selfdrive/car/chrysler/interface.py
Executable file
103
selfdrive/car/chrysler/interface.py
Executable file
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car, custom
|
||||
from panda import Panda
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "chrysler"
|
||||
ret.dashcamOnly = candidate in RAM_HD
|
||||
|
||||
# radar parsing needs some work, see https://github.com/commaai/openpilot/issues/26842
|
||||
ret.radarUnavailable = True # DBC[candidate]['radar'] is None
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
# safety config
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler)]
|
||||
if candidate in RAM_HD:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_HD
|
||||
elif candidate in RAM_DT:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT
|
||||
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
if candidate not in RAM_CARS:
|
||||
# Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed.
|
||||
new_eps_platform = candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019, 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.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.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:
|
||||
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:
|
||||
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
|
||||
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
# events
|
||||
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
|
||||
|
||||
# Low speed steer alert hysteresis logic
|
||||
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5):
|
||||
self.low_speed_alert = True
|
||||
elif ret.vEgo > (self.CP.minSteerSpeed + 1.):
|
||||
self.low_speed_alert = False
|
||||
if self.low_speed_alert:
|
||||
events.add(car.CarEvent.EventName.belowSteerSpeed)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
86
selfdrive/car/chrysler/radar_interface.py
Executable file
86
selfdrive/car/chrysler/radar_interface.py
Executable file
@@ -0,0 +1,86 @@
|
||||
#!/usr/bin/env python3
|
||||
from opendbc.can.parser import CANParser
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
from openpilot.selfdrive.car.chrysler.values import DBC
|
||||
|
||||
RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724
|
||||
RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages
|
||||
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
|
||||
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
|
||||
|
||||
def _create_radar_can_parser(car_fingerprint):
|
||||
dbc = DBC[car_fingerprint]['radar']
|
||||
if dbc is None:
|
||||
return None
|
||||
|
||||
msg_n = len(RADAR_MSGS_C)
|
||||
# list of [(signal name, message name or number), (...)]
|
||||
# [('RADAR_STATE', 1024),
|
||||
# ('LONG_DIST', 1072),
|
||||
# ('LONG_DIST', 1073),
|
||||
# ('LONG_DIST', 1074),
|
||||
# ('LONG_DIST', 1075),
|
||||
|
||||
messages = list(zip(RADAR_MSGS_C +
|
||||
RADAR_MSGS_D,
|
||||
[20] * msg_n + # 20Hz (0.05s)
|
||||
[20] * msg_n, strict=True)) # 20Hz (0.05s)
|
||||
|
||||
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
|
||||
|
||||
def _address_to_track(address):
|
||||
if address in RADAR_MSGS_C:
|
||||
return (address - RADAR_MSGS_C[0]) // 2
|
||||
if address in RADAR_MSGS_D:
|
||||
return (address - RADAR_MSGS_D[0]) // 2
|
||||
raise ValueError("radar received unexpected address %d" % address)
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
self.CP = CP
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint)
|
||||
self.updated_messages = set()
|
||||
self.trigger_msg = LAST_MSG
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None or self.CP.radarUnavailable:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
ret = car.RadarData.new_message()
|
||||
errors = []
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
ret.errors = errors
|
||||
|
||||
for ii in self.updated_messages: # ii should be the message ID as a number
|
||||
cpt = self.rcp.vl[ii]
|
||||
trackId = _address_to_track(ii)
|
||||
|
||||
if trackId not in self.pts:
|
||||
self.pts[trackId] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[trackId].trackId = trackId
|
||||
self.pts[trackId].aRel = float('nan')
|
||||
self.pts[trackId].yvRel = float('nan')
|
||||
self.pts[trackId].measured = True
|
||||
|
||||
if 'LONG_DIST' in cpt: # c_* message
|
||||
self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car
|
||||
# our lat_dist is positive to the right in car's frame.
|
||||
# TODO what does yRel want?
|
||||
self.pts[trackId].yRel = cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
else: # d_* message
|
||||
self.pts[trackId].vRel = cpt['REL_SPEED']
|
||||
|
||||
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
|
||||
ret.points = [x for x in self.pts.values() if x.dRel != 0]
|
||||
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
166
selfdrive/car/chrysler/values.py
Executable file
166
selfdrive/car/chrysler/values.py
Executable file
@@ -0,0 +1,166 @@
|
||||
from enum import IntFlag
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from cereal import car
|
||||
from panda.python import uds
|
||||
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict
|
||||
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
|
||||
|
||||
Ecu = car.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
|
||||
PACIFICA_2017_HYBRID = ChryslerPlatformConfig(
|
||||
"CHRYSLER PACIFICA HYBRID 2017",
|
||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2017")],
|
||||
ChryslerCarSpecs(mass=2242., wheelbase=3.089, steerRatio=16.2),
|
||||
)
|
||||
PACIFICA_2018_HYBRID = ChryslerPlatformConfig(
|
||||
"CHRYSLER PACIFICA HYBRID 2018",
|
||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2018")],
|
||||
PACIFICA_2017_HYBRID.specs,
|
||||
)
|
||||
PACIFICA_2019_HYBRID = ChryslerPlatformConfig(
|
||||
"CHRYSLER PACIFICA HYBRID 2019",
|
||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2019-23")],
|
||||
PACIFICA_2017_HYBRID.specs,
|
||||
)
|
||||
PACIFICA_2018 = ChryslerPlatformConfig(
|
||||
"CHRYSLER PACIFICA 2018",
|
||||
[ChryslerCarDocs("Chrysler Pacifica 2017-18")],
|
||||
PACIFICA_2017_HYBRID.specs,
|
||||
)
|
||||
PACIFICA_2020 = ChryslerPlatformConfig(
|
||||
"CHRYSLER PACIFICA 2020",
|
||||
[
|
||||
ChryslerCarDocs("Chrysler Pacifica 2019-20"),
|
||||
ChryslerCarDocs("Chrysler Pacifica 2021-23", package="All"),
|
||||
],
|
||||
PACIFICA_2017_HYBRID.specs,
|
||||
)
|
||||
|
||||
# Dodge
|
||||
DODGE_DURANGO = ChryslerPlatformConfig(
|
||||
"DODGE DURANGO 2021",
|
||||
[ChryslerCarDocs("Dodge Durango 2020-21")],
|
||||
PACIFICA_2017_HYBRID.specs,
|
||||
)
|
||||
|
||||
# Jeep
|
||||
JEEP_GRAND_CHEROKEE = ChryslerPlatformConfig( # includes 2017 Trailhawk
|
||||
"JEEP GRAND CHEROKEE V6 2018",
|
||||
[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
|
||||
"JEEP GRAND CHEROKEE 2019",
|
||||
[ChryslerCarDocs("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4")],
|
||||
JEEP_GRAND_CHEROKEE.specs,
|
||||
)
|
||||
|
||||
# Ram
|
||||
RAM_1500 = ChryslerPlatformConfig(
|
||||
"RAM 1500 5TH GEN",
|
||||
[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 = ChryslerPlatformConfig(
|
||||
"RAM HD 5TH GEN",
|
||||
[
|
||||
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, }
|
||||
RAM_HD = {CAR.RAM_HD, }
|
||||
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()
|
||||
49
selfdrive/car/disable_ecu.py
Executable file
49
selfdrive/car/disable_ecu.py
Executable file
@@ -0,0 +1,49 @@
|
||||
#!/usr/bin/env python3
|
||||
from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
EXT_DIAG_REQUEST = b'\x10\x03'
|
||||
EXT_DIAG_RESPONSE = b'\x50\x03'
|
||||
|
||||
COM_CONT_RESPONSE = b''
|
||||
|
||||
|
||||
def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
|
||||
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
|
||||
The ECU will stay silent as long as openpilot keeps sending Tester Present.
|
||||
|
||||
This is used to disable the radar in some cars. Openpilot will emulate the radar.
|
||||
WARNING: THIS DISABLES AEB!"""
|
||||
cloudlog.warning(f"ecu disable {hex(addr), sub_addr} ...")
|
||||
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
cloudlog.warning("communication control disable tx/rx ...")
|
||||
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
|
||||
query.get_data(0)
|
||||
|
||||
cloudlog.warning("ecu disabled")
|
||||
return True
|
||||
|
||||
except Exception:
|
||||
cloudlog.exception("ecu disable exception")
|
||||
|
||||
cloudlog.error(f"ecu disable retry ({i + 1}) ...")
|
||||
cloudlog.error("ecu disable failed")
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
import cereal.messaging as messaging
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
logcan = messaging.sub_sock('can')
|
||||
time.sleep(1)
|
||||
|
||||
# honda bosch radar disable
|
||||
disabled = disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03', timeout=0.5, debug=False)
|
||||
print(f"disabled: {disabled}")
|
||||
80
selfdrive/car/docs.py
Executable file
80
selfdrive/car/docs.py
Executable file
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
from collections import defaultdict
|
||||
import jinja2
|
||||
import os
|
||||
from enum import Enum
|
||||
from natsort import natsorted
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.selfdrive.car import gen_empty_fingerprint
|
||||
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
|
||||
|
||||
|
||||
def get_all_footnotes() -> dict[Enum, int]:
|
||||
all_footnotes = list(CommonFootnote)
|
||||
for footnotes in get_interface_attr("Footnote", ignore_none=True).values():
|
||||
all_footnotes.extend(footnotes)
|
||||
return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)}
|
||||
|
||||
|
||||
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_car_docs() -> list[CarDocs]:
|
||||
all_car_docs: list[CarDocs] = []
|
||||
footnotes = get_all_footnotes()
|
||||
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=[car.CarParams.CarFw(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
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Auto generates supported cars documentation",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
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()
|
||||
|
||||
with open(args.out, 'w') as f:
|
||||
f.write(generate_cars_md(get_all_car_docs(), args.template))
|
||||
print(f"Generated and written to {args.out}")
|
||||
368
selfdrive/car/docs_definitions.py
Executable file
368
selfdrive/car/docs_definitions.py
Executable file
@@ -0,0 +1,368 @@
|
||||
import re
|
||||
from collections import namedtuple
|
||||
import copy
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
|
||||
GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2
|
||||
MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)"
|
||||
|
||||
|
||||
class Column(Enum):
|
||||
MAKE = "Make"
|
||||
MODEL = "Model"
|
||||
PACKAGE = "Supported Package"
|
||||
LONGITUDINAL = "ACC"
|
||||
FSR_LONGITUDINAL = "No ACC accel below"
|
||||
FSR_STEERING = "No ALC below"
|
||||
STEERING_TORQUE = "Steering Torque"
|
||||
AUTO_RESUME = "Resume from stop"
|
||||
HARDWARE = "Hardware Needed"
|
||||
VIDEO = "Video"
|
||||
|
||||
|
||||
class Star(Enum):
|
||||
FULL = "full"
|
||||
HALF = "half"
|
||||
EMPTY = "empty"
|
||||
|
||||
|
||||
# A part + its comprised parts
|
||||
@dataclass
|
||||
class BasePart:
|
||||
name: str
|
||||
parts: list[Enum] = field(default_factory=list)
|
||||
|
||||
def all_parts(self):
|
||||
# Recursively get all parts
|
||||
_parts = 'parts'
|
||||
parts = []
|
||||
parts.extend(getattr(self, _parts))
|
||||
for part in getattr(self, _parts):
|
||||
parts.extend(part.value.all_parts())
|
||||
|
||||
return parts
|
||||
|
||||
|
||||
class EnumBase(Enum):
|
||||
@property
|
||||
def part_type(self):
|
||||
return PartType(self.__class__)
|
||||
|
||||
|
||||
class Mount(EnumBase):
|
||||
mount = BasePart("mount")
|
||||
angled_mount_8_degrees = BasePart("angled mount (8 degrees)")
|
||||
|
||||
|
||||
class Cable(EnumBase):
|
||||
rj45_cable_7ft = BasePart("RJ45 cable (7 ft)")
|
||||
long_obdc_cable = BasePart("long OBD-C cable")
|
||||
usb_a_2_a_cable = BasePart("USB A-A cable")
|
||||
usbc_otg_cable = BasePart("USB C OTG cable")
|
||||
usbc_coupler = BasePart("USB-C coupler")
|
||||
obd_c_cable_1_5ft = BasePart("OBD-C cable (1.5 ft)")
|
||||
right_angle_obd_c_cable_1_5ft = BasePart("right angle OBD-C cable (1.5 ft)")
|
||||
|
||||
|
||||
class Accessory(EnumBase):
|
||||
harness_box = BasePart("harness box")
|
||||
comma_power_v2 = BasePart("comma power v2")
|
||||
|
||||
|
||||
@dataclass
|
||||
class BaseCarHarness(BasePart):
|
||||
parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft])
|
||||
has_connector: bool = True # without are hidden on the harness connector page
|
||||
|
||||
|
||||
class CarHarness(EnumBase):
|
||||
nidec = BaseCarHarness("Honda Nidec connector")
|
||||
bosch_a = BaseCarHarness("Honda Bosch A connector")
|
||||
bosch_b = BaseCarHarness("Honda Bosch B connector")
|
||||
toyota_a = BaseCarHarness("Toyota A connector")
|
||||
toyota_b = BaseCarHarness("Toyota B connector")
|
||||
subaru_a = BaseCarHarness("Subaru A connector")
|
||||
subaru_b = BaseCarHarness("Subaru B connector")
|
||||
subaru_c = BaseCarHarness("Subaru C connector")
|
||||
subaru_d = BaseCarHarness("Subaru D connector")
|
||||
fca = BaseCarHarness("FCA connector")
|
||||
ram = BaseCarHarness("Ram connector")
|
||||
vw = BaseCarHarness("VW connector")
|
||||
j533 = BaseCarHarness("J533 connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
hyundai_a = BaseCarHarness("Hyundai A connector")
|
||||
hyundai_b = BaseCarHarness("Hyundai B connector")
|
||||
hyundai_c = BaseCarHarness("Hyundai C connector")
|
||||
hyundai_d = BaseCarHarness("Hyundai D connector")
|
||||
hyundai_e = BaseCarHarness("Hyundai E connector")
|
||||
hyundai_f = BaseCarHarness("Hyundai F connector")
|
||||
hyundai_g = BaseCarHarness("Hyundai G connector")
|
||||
hyundai_h = BaseCarHarness("Hyundai H connector")
|
||||
hyundai_i = BaseCarHarness("Hyundai I connector")
|
||||
hyundai_j = BaseCarHarness("Hyundai J connector")
|
||||
hyundai_k = BaseCarHarness("Hyundai K connector")
|
||||
hyundai_l = BaseCarHarness("Hyundai L connector")
|
||||
hyundai_m = BaseCarHarness("Hyundai M connector")
|
||||
hyundai_n = BaseCarHarness("Hyundai N connector")
|
||||
hyundai_o = BaseCarHarness("Hyundai O connector")
|
||||
hyundai_p = BaseCarHarness("Hyundai P connector")
|
||||
hyundai_q = BaseCarHarness("Hyundai Q connector")
|
||||
hyundai_r = BaseCarHarness("Hyundai R connector")
|
||||
custom = BaseCarHarness("Developer connector")
|
||||
obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable, Cable.long_obdc_cable], has_connector=False)
|
||||
gm = BaseCarHarness("GM connector", parts=[Accessory.harness_box])
|
||||
nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
mazda = BaseCarHarness("Mazda connector")
|
||||
ford_q3 = BaseCarHarness("Ford Q3 connector")
|
||||
ford_q4 = BaseCarHarness("Ford Q4 connector", 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: car.CarParams, all_footnotes: dict[Enum, int]):
|
||||
self.car_name = CP.carName
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# longitudinal column
|
||||
op_long = "Stock"
|
||||
if CP.experimentalLongitudinalAvailable or CP.enableDsu:
|
||||
op_long = "openpilot available"
|
||||
if CP.enableDsu:
|
||||
self.footnotes.append(CommonFootnote.EXP_LONG_DSU)
|
||||
else:
|
||||
self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL)
|
||||
elif CP.openpilotLongitudinalControl and not CP.enableDsu:
|
||||
op_long = "openpilot"
|
||||
|
||||
# min steer & enable speed columns
|
||||
# TODO: set all the min steer speeds in carParams and remove this
|
||||
if self.min_steer_speed is not None:
|
||||
assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both 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
|
||||
|
||||
# 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: car.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 or CP.experimentalLongitudinalAvailable:
|
||||
sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}."
|
||||
|
||||
return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc)
|
||||
|
||||
else:
|
||||
if CP.carFingerprint == "COMMA BODY":
|
||||
return "The body is a robotics dev kit that can run openpilot. <a href='https://www.commabody.com'>Learn more.</a>"
|
||||
else:
|
||||
raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}")
|
||||
|
||||
def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str:
|
||||
item: 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
|
||||
95
selfdrive/car/ecu_addrs.py
Executable file
95
selfdrive/car/ecu_addrs.py
Executable file
@@ -0,0 +1,95 @@
|
||||
#!/usr/bin/env python3
|
||||
import capnp
|
||||
import time
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from panda.python.uds import SERVICE_TYPE
|
||||
from openpilot.selfdrive.car import make_can_msg
|
||||
from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType
|
||||
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
def make_tester_present_msg(addr, bus, subaddr=None):
|
||||
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
|
||||
if subaddr is not None:
|
||||
dat.insert(0, subaddr)
|
||||
|
||||
dat.extend([0x0] * (8 - len(dat)))
|
||||
return make_can_msg(addr, bytes(dat), bus)
|
||||
|
||||
|
||||
def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: int = None) -> bool:
|
||||
# ISO-TP messages are always padded to 8 bytes
|
||||
# tester present response is always a single frame
|
||||
dat_offset = 1 if subaddr is not None else 0
|
||||
if len(msg.dat) == 8 and 1 <= msg.dat[dat_offset] <= 7:
|
||||
# success response
|
||||
if msg.dat[dat_offset + 1] == (SERVICE_TYPE.TESTER_PRESENT + 0x40):
|
||||
return True
|
||||
# error response
|
||||
if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == SERVICE_TYPE.TESTER_PRESENT:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> set[EcuAddrBusType]:
|
||||
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
|
||||
queries: set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list}
|
||||
responses = queries
|
||||
return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug)
|
||||
|
||||
|
||||
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: set[EcuAddrBusType],
|
||||
responses: set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> set[EcuAddrBusType]:
|
||||
ecu_responses: set[EcuAddrBusType] = set() # set((addr, subaddr, bus),)
|
||||
try:
|
||||
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
|
||||
|
||||
messaging.drain_sock_raw(logcan)
|
||||
sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan'))
|
||||
start_time = time.monotonic()
|
||||
while time.monotonic() - start_time < timeout:
|
||||
can_packets = messaging.drain_sock(logcan, wait_for_one=True)
|
||||
for packet in can_packets:
|
||||
for msg in packet.can:
|
||||
if not len(msg.dat):
|
||||
cloudlog.warning("ECU addr scan: skipping empty remote frame")
|
||||
continue
|
||||
|
||||
subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0]
|
||||
if (msg.address, subaddr, msg.src) in responses and is_tester_present_response(msg, subaddr):
|
||||
if debug:
|
||||
print(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}")
|
||||
if (msg.address, subaddr, msg.src) in ecu_responses:
|
||||
print(f"Duplicate ECU address: {hex(msg.address)}")
|
||||
ecu_responses.add((msg.address, subaddr, msg.src))
|
||||
except Exception:
|
||||
cloudlog.exception("ECU addr scan exception")
|
||||
return ecu_responses
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
|
||||
parser = argparse.ArgumentParser(description='Get addresses of all ECUs')
|
||||
parser.add_argument('--debug', action='store_true')
|
||||
parser.add_argument('--bus', type=int, default=1)
|
||||
parser.add_argument('--timeout', type=float, default=1.0)
|
||||
args = parser.parse_args()
|
||||
|
||||
logcan = messaging.sub_sock('can')
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
time.sleep(1.0)
|
||||
|
||||
print("Getting ECU addresses ...")
|
||||
ecu_addrs = get_all_ecu_addrs(logcan, sendcan, args.bus, args.timeout, debug=args.debug)
|
||||
|
||||
print()
|
||||
print("Found ECUs on addresses:")
|
||||
for addr, subaddr, _ in ecu_addrs:
|
||||
msg = f" 0x{hex(addr)}"
|
||||
if subaddr is not None:
|
||||
msg += f" (sub-address: {hex(subaddr)})"
|
||||
print(msg)
|
||||
120
selfdrive/car/fingerprints.py
Executable file
120
selfdrive/car/fingerprints.py
Executable file
@@ -0,0 +1,120 @@
|
||||
from openpilot.selfdrive.car.interfaces import get_interface_attr
|
||||
from openpilot.selfdrive.car.honda.values import CAR as HONDA
|
||||
from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI
|
||||
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.ACCORD,
|
||||
"HONDA ACCORD 1.5T 2018": HONDA.ACCORD,
|
||||
"HONDA ACCORD 2018 LX 1.5T": HONDA.ACCORD,
|
||||
"HONDA ACCORD 2018 SPORT 2T": HONDA.ACCORD,
|
||||
"HONDA ACCORD 2T 2018": HONDA.ACCORD,
|
||||
"HONDA ACCORD HYBRID 2018": HONDA.ACCORD,
|
||||
"HONDA CIVIC 2016 TOURING": HONDA.CIVIC,
|
||||
"HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019": HONDA.CIVIC_BOSCH,
|
||||
"HONDA CIVIC SEDAN 1.6 DIESEL": HONDA.CIVIC_BOSCH_DIESEL,
|
||||
"HONDA CR-V 2016 EXECUTIVE": HONDA.CRV_EU,
|
||||
"HONDA CR-V 2016 TOURING": HONDA.CRV,
|
||||
"HONDA CR-V 2017 EX": HONDA.CRV_5G,
|
||||
"HONDA CR-V 2019 HYBRID": HONDA.CRV_HYBRID,
|
||||
"HONDA FIT 2018 EX": HONDA.FIT,
|
||||
"HONDA HRV 2019 TOURING": HONDA.HRV,
|
||||
"HONDA INSIGHT 2019 TOURING": HONDA.INSIGHT,
|
||||
"HONDA ODYSSEY 2018 EX-L": HONDA.ODYSSEY,
|
||||
"HONDA ODYSSEY 2019 EXCLUSIVE CHN": HONDA.ODYSSEY_CHN,
|
||||
"HONDA PILOT 2017 TOURING": HONDA.PILOT,
|
||||
"HONDA PILOT 2019 ELITE": HONDA.PILOT,
|
||||
"HONDA PILOT 2019": HONDA.PILOT,
|
||||
"HONDA PASSPORT 2021": HONDA.PILOT,
|
||||
"HONDA RIDGELINE 2017 BLACK EDITION": HONDA.RIDGELINE,
|
||||
"HYUNDAI ELANTRA LIMITED ULTIMATE 2017": HYUNDAI.ELANTRA,
|
||||
"HYUNDAI SANTA FE LIMITED 2019": HYUNDAI.SANTA_FE,
|
||||
"HYUNDAI TUCSON DIESEL 2019": 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.SIENNA,
|
||||
"TOYOTA C-HR HYBRID 2018": TOYOTA.CHR,
|
||||
"TOYOTA COROLLA HYBRID TSS2 2019": TOYOTA.COROLLA_TSS2,
|
||||
"TOYOTA RAV4 HYBRID 2019": 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.ALPHARD_TSS2,
|
||||
"TOYOTA AVALON HYBRID 2019": TOYOTA.AVALON_2019,
|
||||
"TOYOTA AVALON HYBRID 2022": TOYOTA.AVALON_TSS2,
|
||||
"TOYOTA CAMRY HYBRID 2018": TOYOTA.CAMRY,
|
||||
"TOYOTA CAMRY HYBRID 2021": TOYOTA.CAMRY_TSS2,
|
||||
"TOYOTA C-HR HYBRID 2022": TOYOTA.CHR_TSS2,
|
||||
"TOYOTA HIGHLANDER HYBRID 2020": TOYOTA.HIGHLANDER_TSS2,
|
||||
"TOYOTA RAV4 HYBRID 2022": TOYOTA.RAV4_TSS2_2022,
|
||||
"TOYOTA RAV4 HYBRID 2023": TOYOTA.RAV4_TSS2_2023,
|
||||
"TOYOTA HIGHLANDER HYBRID 2018": TOYOTA.HIGHLANDER,
|
||||
"LEXUS ES HYBRID 2018": TOYOTA.LEXUS_ES,
|
||||
"LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX,
|
||||
"HYUNDAI TUCSON HYBRID 4TH GEN": 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,
|
||||
}
|
||||
0
selfdrive/car/ford/__init__.py
Executable file
0
selfdrive/car/ford/__init__.py
Executable file
123
selfdrive/car/ford/carcontroller.py
Executable file
123
selfdrive/car/ford/carcontroller.py
Executable file
@@ -0,0 +1,123 @@
|
||||
from cereal import car
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.selfdrive.car import apply_std_steer_angle_limits
|
||||
from openpilot.selfdrive.car.ford import fordcan
|
||||
from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw):
|
||||
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
|
||||
if v_ego_raw > 9:
|
||||
apply_curvature = clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
|
||||
current_curvature + CarControllerParams.CURVATURE_ERROR)
|
||||
|
||||
# Curvature rate limit after driver torque limit
|
||||
apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams)
|
||||
|
||||
return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.VM = VM
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.CAN = fordcan.CanBus(CP)
|
||||
self.frame = 0
|
||||
|
||||
self.apply_curvature_last = 0
|
||||
self.main_on_last = False
|
||||
self.lkas_enabled_last = False
|
||||
self.steer_alert_last = False
|
||||
self.lead_distance_bars_last = None
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
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
|
||||
if frogpilot_variables.sport_plus:
|
||||
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
|
||||
else:
|
||||
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
|
||||
gas = accel
|
||||
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
|
||||
gas = CarControllerParams.INACTIVE_GAS
|
||||
stopping = CC.actuators.longControlState == LongCtrlState.stopping
|
||||
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=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 = actuators.copy()
|
||||
new_actuators.curvature = self.apply_curvature_last
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
172
selfdrive/car/ford/carstate.py
Executable file
172
selfdrive/car/ford/carstate.py
Executable file
@@ -0,0 +1,172 @@
|
||||
from cereal import car
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car.ford.fordcan import CanBus
|
||||
from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
|
||||
|
||||
self.vehicle_sensors_valid = False
|
||||
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
def update(self, cp, cp_cam, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
|
||||
# The vehicle usually recovers out of this state within a minute of normal driving
|
||||
self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3
|
||||
|
||||
# car speed
|
||||
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
|
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
|
||||
|
||||
# gas pedal
|
||||
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
|
||||
ret.gasPressed = ret.gas > 1e-6
|
||||
|
||||
# brake pedal
|
||||
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
|
||||
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
|
||||
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
|
||||
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
|
||||
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||||
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
|
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
|
||||
ret.espDisabled = 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
|
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
|
||||
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
|
||||
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
|
||||
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
|
||||
ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
|
||||
|
||||
# gear
|
||||
if self.CP.transmissionType == TransmissionType.automatic:
|
||||
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
|
||||
ret.gearShifter = self.parse_gear_shifter(gear)
|
||||
elif self.CP.transmissionType == TransmissionType.manual:
|
||||
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
|
||||
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
|
||||
ret.gearShifter = GearShifter.reverse
|
||||
else:
|
||||
ret.gearShifter = GearShifter.drive
|
||||
|
||||
# safety
|
||||
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
|
||||
ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
|
||||
|
||||
# button presses
|
||||
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
|
||||
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
|
||||
# TODO: block this going to the camera otherwise it will enable stock TJA
|
||||
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
self.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"]
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
|
||||
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),
|
||||
]
|
||||
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
messages += [
|
||||
("Gear_Shift_by_Wire_FD1", 10),
|
||||
]
|
||||
elif CP.transmissionType == TransmissionType.manual:
|
||||
messages += [
|
||||
("Engine_Clutch_Data", 33),
|
||||
("BCM_Lamp_Stat_FD1", 1),
|
||||
]
|
||||
|
||||
if CP.enableBsm and 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)
|
||||
149
selfdrive/car/ford/fingerprints.py
Executable file
149
selfdrive/car/ford/fingerprints.py
Executable file
@@ -0,0 +1,149 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.ford.values import CAR
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.BRONCO_SPORT_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
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.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.EXPLORER_MK6: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LC5T-14F397-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.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.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.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.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.MAVERICK_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
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',
|
||||
],
|
||||
},
|
||||
}
|
||||
340
selfdrive/car/ford/fordcan.py
Executable file
340
selfdrive/car/ford/fordcan.py
Executable file
@@ -0,0 +1,340 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car import CanBusBase
|
||||
|
||||
HUDControl = car.CarControl.HUDControl
|
||||
|
||||
|
||||
class CanBus(CanBusBase):
|
||||
def __init__(self, CP=None, fingerprint=None) -> None:
|
||||
super().__init__(CP, fingerprint)
|
||||
|
||||
@property
|
||||
def main(self) -> int:
|
||||
return self.offset
|
||||
|
||||
@property
|
||||
def radar(self) -> int:
|
||||
return self.offset + 1
|
||||
|
||||
@property
|
||||
def camera(self) -> int:
|
||||
return self.offset + 2
|
||||
|
||||
|
||||
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
|
||||
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
|
||||
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
|
||||
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
|
||||
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
|
||||
|
||||
checksum = mode + counter
|
||||
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
|
||||
checksum += sig_val + (sig_val >> 8)
|
||||
|
||||
return 0xFF - (checksum & 0xFF)
|
||||
|
||||
|
||||
def create_lka_msg(packer, CAN: CanBus):
|
||||
"""
|
||||
Creates an empty CAN message for the Ford LKA Command.
|
||||
|
||||
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
|
||||
|
||||
Frequency is 33Hz.
|
||||
"""
|
||||
|
||||
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
|
||||
|
||||
|
||||
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
|
||||
curvature_rate: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford TJA/LCA Command.
|
||||
|
||||
This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam assist and highway
|
||||
driving. It is not subject to the PSCM lockout.
|
||||
|
||||
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
|
||||
by the following coefficients:
|
||||
c0: lateral offset between the vehicle and the centerline (positive is right)
|
||||
c1: heading angle between the vehicle and the centerline (positive is right)
|
||||
c2: curvature of the centerline (positive is left)
|
||||
c3: rate of change of curvature of the centerline
|
||||
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
|
||||
angle cannot be easily controlled.
|
||||
|
||||
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
|
||||
using tools such as Forscan.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
|
||||
# 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
# Makes no difference with curvature control
|
||||
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
# The stock system always uses comfortable
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
|
||||
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
||||
}
|
||||
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
|
||||
|
||||
|
||||
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
|
||||
curvature_rate: float, counter: int):
|
||||
"""
|
||||
Create a CAN message for the new Ford Lane Centering command.
|
||||
|
||||
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
|
||||
additional signals for a counter and checksum.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
|
||||
# 3=SafeRampOut, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
|
||||
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
|
||||
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
|
||||
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
|
||||
"LatCtlPath_No_Cnt": counter, # [0|15]
|
||||
"LatCtlPath_No_Cs": 0, # [0|255]
|
||||
}
|
||||
|
||||
# calculate checksum
|
||||
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[2]
|
||||
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
|
||||
|
||||
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
|
||||
|
||||
|
||||
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, v_ego_kph: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford ACC Command.
|
||||
|
||||
This command can be used to enable ACC, to set the ACC gas/brake/decel values
|
||||
and to disable ACC.
|
||||
|
||||
Frequency is 50Hz.
|
||||
"""
|
||||
decel = accel < 0 and long_active
|
||||
values = {
|
||||
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
|
||||
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
|
||||
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
|
||||
"AccPrpl_A_Pred": -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)
|
||||
80
selfdrive/car/ford/interface.py
Executable file
80
selfdrive/car/ford/interface.py
Executable file
@@ -0,0 +1,80 @@
|
||||
from cereal import car, custom
|
||||
from panda import Panda
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.ford.fordcan import CanBus
|
||||
from openpilot.selfdrive.car.ford.values import Ecu, FordFlags
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
GearShifter = car.CarState.GearShifter
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "ford"
|
||||
ret.dashcamOnly = False
|
||||
|
||||
ret.radarUnavailable = True
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.2
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
|
||||
CAN = CanBus(fingerprint=fingerprint)
|
||||
cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
if CAN.main >= 4:
|
||||
cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput))
|
||||
ret.safetyConfigs = cfgs
|
||||
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
if experimental_long:
|
||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
if ret.flags & FordFlags.CANFD:
|
||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD
|
||||
|
||||
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
|
||||
found_ecus = [fw.ecu for fw in car_fw]
|
||||
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
|
||||
|
||||
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
|
||||
# TODO: detect bsm in car_fw?
|
||||
ret.enableBsm = 0x3A6 in fingerprint[CAN.main] and 0x3A7 in fingerprint[CAN.main]
|
||||
|
||||
# LCA can steer down to zero
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
return ret
|
||||
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
|
||||
if not self.CS.vehicle_sensors_valid:
|
||||
events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
143
selfdrive/car/ford/radar_interface.py
Executable file
143
selfdrive/car/ford/radar_interface.py
Executable file
@@ -0,0 +1,143 @@
|
||||
from math import cos, sin
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car.ford.fordcan import CanBus
|
||||
from openpilot.selfdrive.car.ford.values import DBC, RADAR
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
|
||||
|
||||
DELPHI_MRR_RADAR_START_ADDR = 0x120
|
||||
DELPHI_MRR_RADAR_MSG_COUNT = 64
|
||||
|
||||
|
||||
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
|
||||
msg_n = len(DELPHI_ESR_RADAR_MSGS)
|
||||
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
|
||||
|
||||
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
|
||||
|
||||
|
||||
def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
|
||||
messages = []
|
||||
|
||||
for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
|
||||
msg = f"MRR_Detection_{i:03d}"
|
||||
messages += [(msg, 20)]
|
||||
|
||||
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
|
||||
self.updated_messages = set()
|
||||
self.track_id = 0
|
||||
self.radar = DBC[CP.carFingerprint]['radar']
|
||||
if self.radar is None or CP.radarUnavailable:
|
||||
self.rcp = None
|
||||
elif self.radar == RADAR.DELPHI_ESR:
|
||||
self.rcp = _create_delphi_esr_radar_can_parser(CP)
|
||||
self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1]
|
||||
self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS}
|
||||
elif self.radar == RADAR.DELPHI_MRR:
|
||||
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
|
||||
self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1
|
||||
else:
|
||||
raise ValueError(f"Unsupported radar: {self.radar}")
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
ret = car.RadarData.new_message()
|
||||
errors = []
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
ret.errors = errors
|
||||
|
||||
if self.radar == RADAR.DELPHI_ESR:
|
||||
self._update_delphi_esr()
|
||||
elif self.radar == RADAR.DELPHI_MRR:
|
||||
self._update_delphi_mrr()
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
|
||||
def _update_delphi_esr(self):
|
||||
for ii in sorted(self.updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.valid_cnt[ii] = 0 # reset counter
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.valid_cnt[ii] += 1
|
||||
else:
|
||||
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
|
||||
#print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
|
||||
|
||||
# radar point only valid if there have been enough valid measurements
|
||||
if self.valid_cnt[ii] > 0:
|
||||
if ii not in self.pts:
|
||||
self.pts[ii] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
|
||||
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['V_Rel']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = True
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
def _update_delphi_mrr(self):
|
||||
for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
|
||||
msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
|
||||
|
||||
# SCAN_INDEX rotates through 0..3 on each message
|
||||
# treat these as separate points
|
||||
scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
|
||||
i = (ii - 1) * 4 + scanIndex
|
||||
|
||||
if i not in self.pts:
|
||||
self.pts[i] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[i].trackId = self.track_id
|
||||
self.pts[i].aRel = float('nan')
|
||||
self.pts[i].yvRel = float('nan')
|
||||
self.track_id += 1
|
||||
|
||||
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
|
||||
|
||||
if valid:
|
||||
azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964]
|
||||
dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984]
|
||||
distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984]
|
||||
dRel = cos(azimuth) * dist # m from front of car
|
||||
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
|
||||
|
||||
# delphi doesn't notify of track switches, so do it manually
|
||||
# TODO: refactor this to radard if more radars behave this way
|
||||
if abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5:
|
||||
self.track_id += 1
|
||||
self.pts[i].trackId = self.track_id
|
||||
|
||||
self.pts[i].dRel = dRel
|
||||
self.pts[i].yRel = yRel
|
||||
self.pts[i].vRel = distRate
|
||||
|
||||
self.pts[i].measured = True
|
||||
|
||||
else:
|
||||
del self.pts[i]
|
||||
0
selfdrive/car/ford/tests/__init__.py
Executable file
0
selfdrive/car/ford/tests/__init__.py
Executable file
82
selfdrive/car/ford/tests/test_ford.py
Executable file
82
selfdrive/car/ford/tests/test_ford.py
Executable file
@@ -0,0 +1,82 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
from parameterized import parameterized
|
||||
from collections.abc import Iterable
|
||||
|
||||
import capnp
|
||||
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.ford.values import FW_QUERY_CONFIG
|
||||
from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = car.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_FW_CORE = {
|
||||
Ecu.eps: [
|
||||
b"14D003",
|
||||
],
|
||||
Ecu.abs: [
|
||||
b"2D053",
|
||||
],
|
||||
Ecu.fwdRadar: [
|
||||
b"14D049",
|
||||
],
|
||||
Ecu.fwdCamera: [
|
||||
b"14F397", # Ford Q3
|
||||
b"14H102", # Ford Q4
|
||||
],
|
||||
Ecu.engine: [
|
||||
b"14C204",
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
class TestFordFW(unittest.TestCase):
|
||||
def test_fw_query_config(self):
|
||||
for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus:
|
||||
self.assertIn(ecu, ECU_ADDRESSES, "Unknown ECU")
|
||||
self.assertEqual(addr, ECU_ADDRESSES[ecu], "ECU address mismatch")
|
||||
self.assertIsNone(subaddr, "Unexpected ECU subaddress")
|
||||
|
||||
@parameterized.expand(FW_VERSIONS.items())
|
||||
def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[capnp.lib.capnp._EnumModule, int, int | None], Iterable[bytes]]):
|
||||
for (ecu, addr, subaddr), fws in fw_versions.items():
|
||||
self.assertIn(ecu, ECU_FW_CORE, "Unexpected ECU")
|
||||
self.assertEqual(addr, ECU_ADDRESSES[ecu], "ECU address mismatch")
|
||||
self.assertIsNone(subaddr, "Unexpected ECU subaddress")
|
||||
|
||||
# Software part number takes the form: PREFIX-CORE-SUFFIX
|
||||
# Prefix changes based on the family of part. It includes the model year
|
||||
# and likely the platform.
|
||||
# Core identifies the type of the item (e.g. 14D003 = PSCM, 14C204 = PCM).
|
||||
# Suffix specifies the version of the part. -AA would be followed by -AB.
|
||||
# Small increments in the suffix are usually compatible.
|
||||
# Details: https://forscan.org/forum/viewtopic.php?p=70008#p70008
|
||||
for fw in fws:
|
||||
self.assertEqual(len(fw), 24, "Expected ECU response to be 24 bytes")
|
||||
|
||||
# TODO: parse with regex, don't need detailed error message
|
||||
fw_parts = fw.rstrip(b'\x00').split(b'-')
|
||||
self.assertEqual(len(fw_parts), 3, "Expected FW to be in format: prefix-core-suffix")
|
||||
|
||||
prefix, core, suffix = fw_parts
|
||||
self.assertEqual(len(prefix), 4, "Expected FW prefix to be 4 characters")
|
||||
self.assertIn(len(core), (5, 6), "Expected FW core to be 5-6 characters")
|
||||
self.assertIn(core, ECU_FW_CORE[ecu], f"Unexpected FW core for {ecu}")
|
||||
self.assertIn(len(suffix), (2, 3), "Expected FW suffix to be 2-3 characters")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
211
selfdrive/car/ford/values.py
Executable file
211
selfdrive/car/ford/values.py
Executable file
@@ -0,0 +1,211 @@
|
||||
import copy
|
||||
from dataclasses import dataclass, field, replace
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
import panda.python.uds as uds
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, dbc_dict, DbcDict, PlatformConfig, Platforms
|
||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
|
||||
Device
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 5 # LateralMotionControl, 20Hz
|
||||
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
|
||||
ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
|
||||
LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
|
||||
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
|
||||
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
|
||||
|
||||
CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1
|
||||
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
|
||||
|
||||
# Curvature rate limits
|
||||
# The curvature signal is limited to 0.003 to 0.009 m^-1/sec by the EPS depending on speed and direction
|
||||
# Limit to ~2 m/s^3 up, ~3 m/s^3 down at 75 mph
|
||||
# Worst case, the low speed limits will allow 4.3 m/s^3 up, 4.9 m/s^3 down at 75 mph
|
||||
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.0002, 0.0001])
|
||||
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.000225, 0.00015])
|
||||
CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
|
||||
|
||||
ACCEL_MAX = 2.0 # m/s^2 max acceleration
|
||||
ACCEL_MAX_PLUS = 4.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: car.CarParams):
|
||||
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
|
||||
if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14, CAR.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):
|
||||
BRONCO_SPORT_MK1 = FordPlatformConfig(
|
||||
"FORD BRONCO SPORT 1ST GEN",
|
||||
[FordCarDocs("Ford Bronco Sport 2021-23")],
|
||||
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
|
||||
)
|
||||
ESCAPE_MK4 = FordPlatformConfig(
|
||||
"FORD ESCAPE 4TH GEN",
|
||||
[
|
||||
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),
|
||||
)
|
||||
EXPLORER_MK6 = FordPlatformConfig(
|
||||
"FORD EXPLORER 6TH GEN",
|
||||
[
|
||||
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),
|
||||
)
|
||||
F_150_MK14 = FordCANFDPlatformConfig(
|
||||
"FORD F-150 14TH GEN",
|
||||
[FordCarDocs("Ford F-150 2022-23", "Co-Pilot360 Active 2.0", hybrid=True)],
|
||||
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
|
||||
)
|
||||
F_150_LIGHTNING_MK1 = FordCANFDPlatformConfig(
|
||||
"FORD F-150 LIGHTNING 1ST GEN",
|
||||
[FordCarDocs("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0")],
|
||||
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
|
||||
)
|
||||
FOCUS_MK4 = FordPlatformConfig(
|
||||
"FORD FOCUS 4TH GEN",
|
||||
[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),
|
||||
)
|
||||
MAVERICK_MK1 = FordPlatformConfig(
|
||||
"FORD MAVERICK 1ST GEN",
|
||||
[
|
||||
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),
|
||||
)
|
||||
MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
|
||||
"FORD MUSTANG MACH-E 1ST GEN",
|
||||
[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
|
||||
)
|
||||
|
||||
|
||||
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
|
||||
],
|
||||
)
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
119
selfdrive/car/fw_query_definitions.py
Executable file
119
selfdrive/car/fw_query_definitions.py
Executable file
@@ -0,0 +1,119 @@
|
||||
#!/usr/bin/env python3
|
||||
import capnp
|
||||
import copy
|
||||
from dataclasses import dataclass, field
|
||||
import struct
|
||||
from collections.abc import Callable
|
||||
|
||||
import panda.python.uds as uds
|
||||
|
||||
AddrType = tuple[int, int | None]
|
||||
EcuAddrBusType = tuple[int, int | None, int]
|
||||
EcuAddrSubAddr = tuple[int, 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[int] = field(default_factory=list)
|
||||
rx_offset: int = 0x8
|
||||
bus: int = 1
|
||||
# Whether this query should be run on the first auxiliary panda (CAN FD cars for example)
|
||||
auxiliary: bool = False
|
||||
# FW responses from these queries will not be used for fingerprinting
|
||||
logging: bool = False
|
||||
# boardd toggles OBD multiplexing on/off as needed
|
||||
obd_multiplexing: bool = True
|
||||
|
||||
|
||||
@dataclass
|
||||
class FwQueryConfig:
|
||||
requests: list[Request]
|
||||
# TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus
|
||||
# Overrides and removes from essential ecus for specific models and ecus (exact matching)
|
||||
non_essential_ecus: dict[capnp.lib.capnp._EnumModule, list[str]] = field(default_factory=dict)
|
||||
# Ecus added for data collection, not to be fingerprinted on
|
||||
extra_ecus: list[tuple[capnp.lib.capnp._EnumModule, int, int | None]] = field(default_factory=list)
|
||||
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions,
|
||||
# returns set of candidates. Only will match if one candidate is returned
|
||||
match_fw_to_car_fuzzy: Callable[[LiveFwVersions, 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
|
||||
396
selfdrive/car/fw_versions.py
Executable file
396
selfdrive/car/fw_versions.py
Executable file
@@ -0,0 +1,396 @@
|
||||
#!/usr/bin/env python3
|
||||
from collections import defaultdict
|
||||
from collections.abc import Iterator
|
||||
from typing import Any, Protocol, TypeVar
|
||||
|
||||
from tqdm import tqdm
|
||||
import capnp
|
||||
|
||||
import panda.python.uds as uds
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
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 = car.CarParams.Ecu
|
||||
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
|
||||
FUZZY_EXCLUDE_ECUS = [Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps, Ecu.debug]
|
||||
|
||||
FW_QUERY_CONFIGS: 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')
|
||||
|
||||
|
||||
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[capnp.lib.capnp._DynamicStructBuilder], 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:
|
||||
cloudlog.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[capnp.lib.capnp._DynamicStructBuilder], 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, VERSIONS[brand])
|
||||
|
||||
if len(matches):
|
||||
return exact_match, matches
|
||||
|
||||
return True, set()
|
||||
|
||||
|
||||
def get_present_ecus(logcan, sendcan, num_pandas: int = 1) -> set[EcuAddrBusType]:
|
||||
params = Params()
|
||||
# queries are split by OBD multiplexing mode
|
||||
queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []}
|
||||
parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []}
|
||||
responses: set[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(params, obd_multiplexing)
|
||||
for query in queries[obd_multiplexing]:
|
||||
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
|
||||
return ecu_responses
|
||||
|
||||
|
||||
def get_brand_ecu_matches(ecu_rx_addrs: 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 set_obd_multiplexing(params: Params, obd_multiplexing: bool):
|
||||
if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing:
|
||||
cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}")
|
||||
params.remove("ObdMultiplexingChanged")
|
||||
params.put_bool("ObdMultiplexingEnabled", obd_multiplexing)
|
||||
params.get_bool("ObdMultiplexingChanged", block=True)
|
||||
cloudlog.warning("OBD multiplexing set successfully")
|
||||
|
||||
|
||||
def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1,
|
||||
debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]:
|
||||
"""Queries for FW versions ordering brands by likelihood, breaks when exact match is found"""
|
||||
|
||||
all_car_fw = []
|
||||
brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
|
||||
|
||||
for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True):
|
||||
# Skip this brand if there are no matching present ECUs
|
||||
if not len(brand_matches[brand]):
|
||||
continue
|
||||
|
||||
car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress)
|
||||
all_car_fw.extend(car_fw)
|
||||
|
||||
# If there is a match using this brand's FW alone, finish querying early
|
||||
_, matches = match_fw_to_car(car_fw, log=False)
|
||||
if len(matches) == 1:
|
||||
break
|
||||
|
||||
return all_car_fw
|
||||
|
||||
|
||||
def get_fw_versions(logcan, sendcan, query_brand: str = None, extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1,
|
||||
debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]:
|
||||
versions = VERSIONS.copy()
|
||||
params = Params()
|
||||
|
||||
if query_brand is not None:
|
||||
versions = {query_brand: versions[query_brand]}
|
||||
|
||||
if extra is not None:
|
||||
versions.update(extra)
|
||||
|
||||
# Extract ECU addresses to query from fingerprints
|
||||
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
|
||||
addrs = []
|
||||
parallel_addrs = []
|
||||
ecu_types = {}
|
||||
|
||||
for brand, brand_versions in versions.items():
|
||||
config = FW_QUERY_CONFIGS[brand]
|
||||
for ecu_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(params, r.obd_multiplexing)
|
||||
|
||||
try:
|
||||
query_addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and
|
||||
(len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)]
|
||||
|
||||
if query_addrs:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, r.bus, query_addrs, r.request, r.response, r.rx_offset, debug=debug)
|
||||
for (tx_addr, sub_addr), version in query.get_data(timeout).items():
|
||||
f = car.CarParams.CarFw.new_message()
|
||||
|
||||
f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown)
|
||||
f.fwVersion = version
|
||||
f.address = tx_addr
|
||||
f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset)
|
||||
f.request = r.request
|
||||
f.brand = brand
|
||||
f.bus = r.bus
|
||||
f.logging = r.logging or (f.ecu, tx_addr, sub_addr) in config.extra_ecus
|
||||
f.obdMultiplexing = r.obd_multiplexing
|
||||
|
||||
if sub_addr is not None:
|
||||
f.subAddress = sub_addr
|
||||
|
||||
car_fw.append(f)
|
||||
except Exception:
|
||||
cloudlog.exception("FW query exception")
|
||||
|
||||
return car_fw
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
import argparse
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.selfdrive.car.vin import get_vin
|
||||
|
||||
parser = argparse.ArgumentParser(description='Get firmware version of ECUs')
|
||||
parser.add_argument('--scan', action='store_true')
|
||||
parser.add_argument('--debug', action='store_true')
|
||||
parser.add_argument('--brand', help='Only query addresses/with requests for this brand')
|
||||
args = parser.parse_args()
|
||||
|
||||
logcan = messaging.sub_sock('can')
|
||||
pandaStates_sock = messaging.sub_sock('pandaStates')
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
# Set up params for boardd
|
||||
params = Params()
|
||||
params.remove("FirmwareQueryDone")
|
||||
params.put_bool("IsOnroad", False)
|
||||
time.sleep(0.2) # thread is 10 Hz
|
||||
params.put_bool("IsOnroad", True)
|
||||
|
||||
extra: Any = None
|
||||
if args.scan:
|
||||
extra = {}
|
||||
# Honda
|
||||
for i in range(256):
|
||||
extra[(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = []
|
||||
extra[(Ecu.unknown, 0x700 + i, None)] = []
|
||||
extra[(Ecu.unknown, 0x750, i)] = []
|
||||
extra = {"any": {"debug": extra}}
|
||||
|
||||
num_pandas = len(messaging.recv_one_retry(pandaStates_sock).pandaStates)
|
||||
|
||||
t = time.time()
|
||||
print("Getting vin...")
|
||||
vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1), retry=10, debug=args.debug)
|
||||
print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}')
|
||||
print(f"Getting VIN took {time.time() - t:.3f} s")
|
||||
print()
|
||||
|
||||
t = time.time()
|
||||
fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True)
|
||||
_, candidates = match_fw_to_car(fw_vers)
|
||||
|
||||
print()
|
||||
print("Found FW versions")
|
||||
print("{")
|
||||
padding = max([len(fw.brand) for fw in fw_vers] or [0])
|
||||
for version in fw_vers:
|
||||
subaddr = None if version.subAddress == 0 else hex(version.subAddress)
|
||||
print(f" Brand: {version.brand:{padding}}, bus: {version.bus} - (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]")
|
||||
print("}")
|
||||
|
||||
print()
|
||||
print("Possible matches:", candidates)
|
||||
print(f"Getting fw took {time.time() - t:.3f} s")
|
||||
0
selfdrive/car/gm/__init__.py
Executable file
0
selfdrive/car/gm/__init__.py
Executable file
257
selfdrive/car/gm/carcontroller.py
Executable file
257
selfdrive/car/gm/carcontroller.py
Executable file
@@ -0,0 +1,257 @@
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.numpy_fast import interp, clip
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command
|
||||
from openpilot.selfdrive.car.gm import gmcan
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
GearShifter = car.CarState.GearShifter
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
|
||||
# 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
|
||||
|
||||
# Constants for pitch compensation
|
||||
PITCH_DEADZONE = 0.01 # [radians] 0.01 ≈ 1% grade
|
||||
BRAKE_PITCH_FACTOR_BP = [5., 10.] # [m/s] smoothly revert to planned accel at low speeds
|
||||
BRAKE_PITCH_FACTOR_V = [0., 1.] # [unitless in [0,1]]; don't touch
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.start_time = 0.
|
||||
self.apply_steer_last = 0
|
||||
self.apply_gas = 0
|
||||
self.apply_brake = 0
|
||||
self.apply_speed = 0
|
||||
self.frame = 0
|
||||
self.last_steer_frame = 0
|
||||
self.last_button_frame = 0
|
||||
self.cancel_counter = 0
|
||||
self.pedal_steady = 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'])
|
||||
|
||||
# FrogPilot variables
|
||||
self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz
|
||||
self.accel_g = 0.0
|
||||
|
||||
@staticmethod
|
||||
def calc_pedal_command(accel: float, long_active: bool) -> float:
|
||||
if not long_active: return 0.
|
||||
|
||||
zero = 0.15625 # 40/256
|
||||
if accel > 0.:
|
||||
# Scales the accel from 0-1 to 0.156-1
|
||||
pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.)
|
||||
else:
|
||||
# if accel is negative, -0.1 -> 0.015625
|
||||
pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen
|
||||
|
||||
return pedal_gas
|
||||
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
actuators = CC.actuators
|
||||
accel = actuators.accel
|
||||
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
|
||||
at_full_stop = CC.longActive and CS.out.standstill
|
||||
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
|
||||
interceptor_gas_cmd = 0
|
||||
|
||||
# Pitch compensated acceleration;
|
||||
# TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay
|
||||
if frogpilot_variables.long_pitch and len(CC.orientationNED) > 1:
|
||||
self.pitch.update(CC.orientationNED[1])
|
||||
self.accel_g = ACCELERATION_DUE_TO_GRAVITY * apply_deadzone(self.pitch.x, PITCH_DEADZONE) # driving uphill is positive pitch
|
||||
accel += self.accel_g
|
||||
|
||||
if not CC.longActive:
|
||||
# ASCM sends max regen when not enabled
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
self.apply_brake = 0
|
||||
elif near_stop and stopping and not CC.cruiseControl.resume:
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE))
|
||||
else:
|
||||
brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V)
|
||||
if self.CP.carFingerprint in EV_CAR and frogpilot_variables.use_ev_tables:
|
||||
self.params.update_ev_gas_brake_threshold(CS.out.vEgo)
|
||||
if frogpilot_variables.sport_plus:
|
||||
self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
|
||||
else:
|
||||
self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(brake_accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
else:
|
||||
if frogpilot_variables.sport_plus:
|
||||
self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
|
||||
else:
|
||||
self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(brake_accel if frogpilot_variables.long_pitch else 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
|
||||
if self.CP.carFingerprint in CC_ONLY_CAR:
|
||||
# gas interceptor only used for full long control on cars without ACC
|
||||
interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive)
|
||||
|
||||
if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill:
|
||||
# "Tap" the accelerator pedal to re-engage ACC
|
||||
interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS
|
||||
self.apply_brake = 0
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
|
||||
idx = (self.frame // 4) % 4
|
||||
|
||||
if self.CP.flags & GMFlags.CC_LONG.value:
|
||||
if CC.longActive and CS.out.vEgo > self.CP.minEnableSpeed:
|
||||
# Using extend instead of append since the message is only sent intermittently
|
||||
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators))
|
||||
if self.CP.enableGasInterceptor:
|
||||
can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx))
|
||||
if self.CP.carFingerprint not in CC_ONLY_CAR:
|
||||
friction_brake_bus = CanBus.CHASSIS
|
||||
# GM Camera exceptions
|
||||
# TODO: can we always check the longControlState?
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera and self.CP.carFingerprint not in CC_ONLY_CAR:
|
||||
at_full_stop = at_full_stop and stopping
|
||||
friction_brake_bus = CanBus.POWERTRAIN
|
||||
|
||||
if self.CP.autoResumeSng:
|
||||
resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume
|
||||
at_full_stop = at_full_stop and not resume
|
||||
|
||||
# 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))
|
||||
else:
|
||||
# to keep accel steady for logs when not sending gas
|
||||
accel += self.accel_g
|
||||
|
||||
# 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)
|
||||
|
||||
# TODO: integrate this with the code block below?
|
||||
if (
|
||||
(self.CP.flags & GMFlags.PEDAL_LONG.value) # Always cancel stock CC when using pedal interceptor
|
||||
or (self.CP.flags & GMFlags.CC_LONG.value and not CC.enabled) # Cancel stock CC if OP is not active
|
||||
) and CS.out.cruiseState.enabled:
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.CANCEL))
|
||||
|
||||
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
|
||||
if self.CP.carFingerprint in SDGM_CAR:
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
else:
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.accel = accel
|
||||
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
|
||||
new_actuators.speed = self.apply_speed
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
271
selfdrive/car/gm/carstate.py
Executable file
271
selfdrive/car/gm/carstate.py
Executable file
@@ -0,0 +1,271 @@
|
||||
import copy
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import mean
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CAMERA_ACC_CAR, CC_ONLY_CAR, SDGM_CAR
|
||||
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
GearShifter = car.CarState.GearShifter
|
||||
STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
|
||||
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
|
||||
self.cluster_min_speed = CV.KPH_TO_MS / 2.
|
||||
|
||||
self.loopback_lka_steering_cmd_updated = False
|
||||
self.loopback_lka_steering_cmd_ts_nanos = 0
|
||||
self.pt_lka_steering_cmd_counter = 0
|
||||
self.cam_lka_steering_cmd_counter = 0
|
||||
self.buttons_counter = 0
|
||||
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
# FrogPilot variables
|
||||
self.single_pedal_mode = False
|
||||
|
||||
def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.prev_distance_button = self.distance_button
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
else:
|
||||
self.cruise_buttons = cam_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"]
|
||||
self.buttons_counter = cam_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 and not self.CP.flags & GMFlags.NO_CAMERA.value:
|
||||
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))
|
||||
|
||||
if self.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value:
|
||||
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
|
||||
else:
|
||||
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
|
||||
self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
|
||||
threshold = 15 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4
|
||||
ret.gasPressed = ret.gas > threshold
|
||||
else:
|
||||
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
|
||||
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
# 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
|
||||
else:
|
||||
# 1 - open, 0 - closed
|
||||
ret.doorOpen = (cam_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
|
||||
cam_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
|
||||
cam_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
|
||||
cam_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
|
||||
|
||||
# 1 - latched
|
||||
ret.seatbeltUnlatched = cam_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
|
||||
ret.leftBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
|
||||
ret.rightBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
|
||||
|
||||
ret.parkingBrake = cam_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 and not self.CP.flags & GMFlags.NO_CAMERA.value:
|
||||
if self.CP.carFingerprint not in CC_ONLY_CAR:
|
||||
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
|
||||
else:
|
||||
ret.stockAeb = False
|
||||
# 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.carFingerprint in CC_ONLY_CAR:
|
||||
ret.accFaulted = False
|
||||
ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
|
||||
ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
|
||||
|
||||
if self.CP.enableBsm:
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
||||
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
||||
else:
|
||||
ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
||||
ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
if self.CP.carFingerprint in SDGM_CAR:
|
||||
self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
|
||||
else:
|
||||
self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
messages = []
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera and not CP.flags & GMFlags.NO_CAMERA.value:
|
||||
messages += [
|
||||
("ASCMLKASteeringCmd", 10),
|
||||
]
|
||||
if CP.carFingerprint in SDGM_CAR:
|
||||
messages += [
|
||||
("BCMTurnSignals", 1),
|
||||
("BCMDoorBeltStatus", 10),
|
||||
("BCMGeneralPlatformStatus", 10),
|
||||
("ASCMSteeringButton", 33),
|
||||
]
|
||||
if CP.enableBsm:
|
||||
messages.append(("BCMBlindSpotMonitor", 10))
|
||||
else:
|
||||
messages += [
|
||||
("AEBCmd", 10),
|
||||
]
|
||||
if CP.carFingerprint not in CC_ONLY_CAR:
|
||||
messages += [
|
||||
("ASCMActiveCruiseControlStatus", 25),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.CAMERA)
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
("PSCMStatus", 10),
|
||||
("ESPStatus", 10),
|
||||
("EBCMWheelSpdFront", 20),
|
||||
("EBCMWheelSpdRear", 20),
|
||||
("EBCMFrictionBrakeStatus", 20),
|
||||
("PSCMSteeringAngle", 100),
|
||||
("ECMAcceleratorPos", 80),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in SDGM_CAR:
|
||||
messages += [
|
||||
("ECMPRDNL2", 40),
|
||||
("AcceleratorPedal2", 40),
|
||||
("ECMEngineStatus", 80),
|
||||
]
|
||||
else:
|
||||
messages += [
|
||||
("ECMPRDNL2", 10),
|
||||
("AcceleratorPedal2", 33),
|
||||
("ECMEngineStatus", 100),
|
||||
("BCMTurnSignals", 1),
|
||||
("BCMDoorBeltStatus", 10),
|
||||
("BCMGeneralPlatformStatus", 10),
|
||||
("ASCMSteeringButton", 33),
|
||||
]
|
||||
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.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value:
|
||||
messages.remove(("ECMAcceleratorPos", 80))
|
||||
messages.append(("EBCMBrakePedalPosition", 100))
|
||||
|
||||
if CP.transmissionType == TransmissionType.direct:
|
||||
messages += [
|
||||
("EBCMRegenPaddle", 50),
|
||||
("EVDriveMode", 0),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in CC_ONLY_CAR:
|
||||
messages += [
|
||||
("ECMCruiseControl", 10),
|
||||
]
|
||||
|
||||
if CP.enableGasInterceptor:
|
||||
messages += [
|
||||
("GAS_SENSOR", 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)
|
||||
191
selfdrive/car/gm/fingerprints.py
Executable file
191
selfdrive/car/gm/fingerprints.py
Executable file
@@ -0,0 +1,191 @@
|
||||
# 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.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
|
||||
},
|
||||
# Volt Premier 2017 w/ flashed firmware, cam harness + pedal
|
||||
{
|
||||
189: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 513: 6, 528: 4, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1922: 7
|
||||
},
|
||||
# jfkoz
|
||||
{
|
||||
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, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 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, 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, 1930: 7, 2017: 8, 2020: 8, 2025: 8, 2028: 8
|
||||
}],
|
||||
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.VOLT_CC: [
|
||||
# FIXME: Need a message to distinguish flashed from non-flashed
|
||||
# Volt Premier w/o acc 2016
|
||||
# {
|
||||
# 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: 8, 308: 4, 309: 8, 311: 8, 313: 8, 320: 8, 328: 1, 352: 5, 368: 8, 381: 6, 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: 4, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 8, 563: 5, 564: 5, 565: 8, 566: 5, 567: 3, 568: 1, 577: 8, 578: 8, 594: 8, 647: 3, 707: 8, 711: 6, 717: 5, 761: 7, 800: 6, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 6, 844: 8, 866: 4, 869: 4, 961: 8, 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, 1601: 8, 1602: 8, 1618: 8, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 1930: 7, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2024: 8, 2025: 8, 2028: 8
|
||||
# },
|
||||
# {
|
||||
# 201: 8, 493: 8, 495: 4, 193: 8, 197: 8, 209: 7, 171: 8, 456: 8, 199: 4, 489: 8, 211: 2, 499: 3, 390: 7, 532: 6, 568: 1, 761: 7, 381: 6, 485: 8, 189: 7, 479: 3, 711: 6, 501: 8, 241: 6, 717: 5, 869: 4, 389: 2, 454: 8, 170: 8, 190: 6, 497: 8, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 500: 6, 508: 8, 528: 4, 647: 3, 1105: 6, 1005: 6, 481: 7, 844: 8, 866: 4, 564: 5, 969: 8, 388: 8, 352: 5, 562: 8, 961: 8, 386: 8, 707: 8, 977: 8, 979: 7, 298: 8, 840: 5, 842: 5, 988: 6, 1001: 8, 560: 8, 546: 7, 558: 8, 309: 8, 995: 7, 311: 8, 566: 5, 567:3, 989: 8, 384: 4, 800: 6, 1033: 7, 1034: 7, 313: 8, 554: 3, 810: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 1223: 3, 1233: 8, 1227: 4, 1417: 8, 1009: 8, 1221: 5, 1275: 3, 1225: 7, 289: 8, 550: 8, 1273: 3, 1928: 7, 1187: 4, 1265: 8, 1927: 7, 1267: 1, 1906: 7, 288: 5, 304: 1, 328: 1, 1912: 7, 320: 3, 1910: 7, 563: 5, 1249: 8, 1930: 7, 1257: 6, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 565: 5, 1280: 4, 1907: 7
|
||||
# },
|
||||
# # Volt Premier w/o ACC 2018 + Pedal
|
||||
# {
|
||||
# 189: 7, 193: 8, 197: 8, 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, 451: 8, 452: 8, 453: 6, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 513: 6, 528: 4, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1922: 7, 1930: 7
|
||||
# }
|
||||
],
|
||||
CAR.BUICK_REGAL: [{
|
||||
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8
|
||||
}],
|
||||
CAR.CADILLAC_ATS: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.MALIBU: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.ACADIA: [{
|
||||
190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7
|
||||
},
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.ESCALADE: [{
|
||||
170: 8, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 4, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
|
||||
}],
|
||||
CAR.ESCALADE_ESV: [{
|
||||
309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
|
||||
}],
|
||||
CAR.ESCALADE_ESV_2019: [{
|
||||
715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7
|
||||
}],
|
||||
CAR.BOLT_EUV: [{
|
||||
189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
}],
|
||||
CAR.BOLT_CC: [
|
||||
# Bolt Premier w/o ACC 2017
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 201: 6, 209: 7, 211: 2, 241: 6, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 311: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 5, 353: 3, 368: 8, 381: 6, 384: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 512: 3, 514: 2, 516: 4, 519: 2, 521: 3, 528: 5, 530: 8, 532: 7, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 8, 566: 6, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 832: 8, 840: 6, 842: 6, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
# Bolt Premier no ACC 2018 + Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 6, 567: 5, 568: 1, 573: 1, 577: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1616: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 2020: 8, 2023: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
# Bolt Premier no ACC 2019 + Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 512: 6, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 2, 569: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1268: 2, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2024: 8
|
||||
},
|
||||
# Bolt Premier no ACC 2020
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 2, 569: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1268: 2, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2024: 8
|
||||
},
|
||||
# Bolt Premier no ACC 2020 2
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 368: 3, 381: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 2, 569: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7
|
||||
},
|
||||
# Bolt Premier no ACC 2020 w pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 368: 3, 381: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 512: 6, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 2, 569: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7
|
||||
},
|
||||
# Bolt EV Premier 2017
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 201: 6, 209: 7, 211: 2, 241: 6, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 311: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 5, 353: 3, 368: 8, 381: 6, 384: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 512: 3, 514: 2, 516: 4, 519: 2, 521: 3, 528: 5, 530: 8, 532: 7, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 8, 566: 6, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 832: 8, 840: 6, 842: 6, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
# Bolt EV Premier 2017 w Pedal
|
||||
{ # pylint: disable=duplicate-key
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 201: 6, 209: 7, 211: 2, 241: 6, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 311: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 5, 353: 3, 368: 8, 381: 6, 384: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 512: 3, 512: 6, 513: 6, 514: 2, 516: 4, 519: 2, 521: 3, 528: 5, 530: 8, 532: 7, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 8, 566: 6, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 832: 8, 840: 6, 842: 6, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 # pylint: disable=duplicate-key # noqa: F601
|
||||
},
|
||||
# Bolt EV Premier 2017 2 w Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 6, 567: 5, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7
|
||||
},
|
||||
# Bolt EV Premier no ACC 2023
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 308: 4, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 398: 8, 407: 7, 417: 8, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 8, 567: 5, 568: 2, 569: 3, 573: 1, 577: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 711: 6, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1010: 8, 1013: 6, 1015: 1, 1017: 8, 1019: 2, 1020: 8, 1037: 5, 1105: 5, 1187: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1601: 8, 1616: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2020: 8, 2023: 8, 2024: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
# Bolt EV Premier no ACC 2021
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 257: 8, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1346: 8, 1347: 8, 1513: 8, 1516: 8, 1601: 8, 1616: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2023: 8, 2024: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
# shermy99's Bolt EV Premier no ACC 2023
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 308: 4, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 398: 8, 407: 7, 417: 8, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513:6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 8, 567: 5, 568: 2, 569: 3, 573: 1, 577: 8, 579: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 711: 6, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1010: 8, 1013: 6, 1015: 1, 1017: 8, 1019: 2, 1020: 8, 1037: 5, 1105: 5, 1187: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1347: 8, 1513: 8, 1516: 8, 1601: 8, 1609: 8, 1613: 8, 1616: 8, 1618: 8, 1649: 8, 1792: 8, 1793: 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, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1913: 7, 1920: 8, 1922: 7, 1924: 8, 1927: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2017: 8, 2018: 8, 2020: 8, 2023: 8, 2025: 8, 2028: 8, 2031: 8
|
||||
}],
|
||||
CAR.SILVERADO: [{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
}],
|
||||
CAR.EQUINOX: [{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
},
|
||||
{
|
||||
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
|
||||
}],
|
||||
CAR.EQUINOX_CC: [
|
||||
# lem's 2020 Equinox, LKAS no ACC
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 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, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 444: 7, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 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, 569: 3, 573: 1, 577: 8, 587: 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, 789: 5, 800: 6, 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, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1273: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1611: 8, 1618: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7, 1930: 7
|
||||
}],
|
||||
# Trailblazer also matches as a Silverado, so comment out to avoid conflicts.
|
||||
# TODO: split with FW versions
|
||||
# CAR.TRAILBLAZER: [
|
||||
# {
|
||||
# 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 707: 8, 715: 8, 717: 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, 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, 1609: 8, 1611: 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, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8
|
||||
# }],
|
||||
CAR.SUBURBAN: [
|
||||
# Chevy Suburban Premier 2019 w Stock ACC no camera
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 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, 510: 8, 528: 5, 532: 6, 534: 2, 562: 8, 563: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8
|
||||
},
|
||||
# Chevy Suburban Premier 2019 w Stock ACC (72 ver)
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 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, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 534: 2, 562: 8, 563: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1355: 8
|
||||
},
|
||||
# Chevy Suburban Premier 2019 w Stock ACC (70 ver)
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 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, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 534: 2, 562: 8, 563: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8
|
||||
}],
|
||||
CAR.SUBURBAN_CC: [
|
||||
# Slav's 2018 Suburban, LKAS no ACC
|
||||
{
|
||||
170: 8, 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, 311: 8, 313: 8, 320: 3, 328: 1, 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, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7
|
||||
}],
|
||||
CAR.YUKON_CC: [
|
||||
# greeninja's 2017 Yukon
|
||||
{
|
||||
193: 8, 197: 8, 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, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 510: 8, 532: 6, 562: 8, 563: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8
|
||||
}],
|
||||
CAR.CT6_CC: [
|
||||
# badgers4life's 2017 CT6
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 4, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 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, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 717: 5, 723: 2, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 961: 8, 969: 8, 977: 8, 979: 7, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 7, 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, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 4, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 8, 1924: 8, 1927: 8, 1928: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
|
||||
}],
|
||||
CAR.TRAILBLAZER_CC: [
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 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, 454: 8, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 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, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 717: 5, 723: 4, 730: 4, 761: 7, 800: 6, 840: 5, 842: 5, 844: 8, 869: 4, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 6, 1017: 8, 1020: 8, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7
|
||||
}],
|
||||
CAR.XT4: [
|
||||
# Cadillac XT4 w/ ACC 2023
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 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, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 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, 719: 5, 761: 7, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 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, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8
|
||||
}],
|
||||
CAR.BABYENCLAVE: [
|
||||
# Buick Baby Enclave w/ ACC 2020-23
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 394: 7, 398: 8, 401: 8, 405: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 450: 4, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 456: 8, 457: 6, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 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, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 872: 1, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1201: 3, 1217: 8, 1218: 3, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1514: 8, 1517: 8, 1601: 8, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1927: 7, 1930: 7, 2018: 8, 2020: 8, 2021: 8, 2028: 8
|
||||
}],
|
||||
CAR.TRAX: [
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 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, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 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, 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, 1930: 7
|
||||
}],
|
||||
}
|
||||
|
||||
FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = {
|
||||
}
|
||||
224
selfdrive/car/gm/gmcan.py
Executable file
224
selfdrive/car/gm/gmcan.py
Executable file
@@ -0,0 +1,224 @@
|
||||
import math
|
||||
|
||||
from cereal import log
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car import make_can_msg
|
||||
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CanBus
|
||||
|
||||
|
||||
def create_buttons(packer, bus, idx, button):
|
||||
values = {
|
||||
"ACCButtons": button,
|
||||
"RollingCounter": idx,
|
||||
"ACCAlwaysOne": 1,
|
||||
"DistanceButton": 0,
|
||||
}
|
||||
|
||||
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
|
||||
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
|
||||
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
|
||||
checksum -= 2 * values["DistanceButton"]
|
||||
|
||||
values["SteeringButtonChecksum"] = checksum
|
||||
return packer.make_can_msg("ASCMSteeringButton", bus, values)
|
||||
|
||||
|
||||
def create_pscm_status(packer, bus, pscm_status):
|
||||
values = {s: pscm_status[s] for s in [
|
||||
"HandsOffSWDetectionMode",
|
||||
"HandsOffSWlDetectionStatus",
|
||||
"LKATorqueDeliveredStatus",
|
||||
"LKADriverAppldTrq",
|
||||
"LKATorqueDelivered",
|
||||
"LKATotalTorqueDelivered",
|
||||
"RollingCounter",
|
||||
"PSCMStatusChecksum",
|
||||
]}
|
||||
checksum_mod = int(1 - values["HandsOffSWlDetectionStatus"]) << 5
|
||||
values["HandsOffSWlDetectionStatus"] = 1
|
||||
values["PSCMStatusChecksum"] += checksum_mod
|
||||
return packer.make_can_msg("PSCMStatus", bus, values)
|
||||
|
||||
|
||||
def create_steering_control(packer, bus, apply_steer, idx, lkas_active):
|
||||
values = {
|
||||
"LKASteeringCmdActive": lkas_active,
|
||||
"LKASteeringCmd": apply_steer,
|
||||
"RollingCounter": idx,
|
||||
"LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_steer & 0x7ff) - idx
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ASCMLKASteeringCmd", bus, values)
|
||||
|
||||
|
||||
def create_adas_keepalive(bus):
|
||||
dat = b"\x00\x00\x00\x00\x00\x00\x00"
|
||||
return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)]
|
||||
|
||||
|
||||
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
|
||||
values = {
|
||||
"GasRegenCmdActive": enabled,
|
||||
"RollingCounter": idx,
|
||||
"GasRegenCmdActiveInv": 1 - enabled,
|
||||
"GasRegenCmd": throttle,
|
||||
"GasRegenFullStopActive": at_full_stop,
|
||||
"GasRegenAlwaysOne": 1,
|
||||
"GasRegenAlwaysOne2": 1,
|
||||
"GasRegenAlwaysOne3": 1,
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
|
||||
values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \
|
||||
(((0xff - dat[2]) & 0xff) << 8) | \
|
||||
((0x100 - dat[3] - idx) & 0xff)
|
||||
|
||||
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
|
||||
|
||||
|
||||
def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP):
|
||||
mode = 0x1
|
||||
|
||||
# TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake
|
||||
if enabled and CP.carFingerprint in (CAR.BOLT_EUV,):
|
||||
mode = 0x9
|
||||
|
||||
if apply_brake > 0:
|
||||
mode = 0xa
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
|
||||
# TODO: this is to have GM bringing the car to complete stop,
|
||||
# but currently it conflicts with OP controls, so turned off. Not set by all cars
|
||||
#elif near_stop:
|
||||
# mode = 0xb
|
||||
|
||||
brake = (0x1000 - apply_brake) & 0xfff
|
||||
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
|
||||
|
||||
values = {
|
||||
"RollingCounter": idx,
|
||||
"FrictionBrakeMode": mode,
|
||||
"FrictionBrakeChecksum": checksum,
|
||||
"FrictionBrakeCmd": -apply_brake
|
||||
}
|
||||
|
||||
return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
|
||||
|
||||
|
||||
def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, 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 make_can_msg(0xa1, bytes(dat), bus)
|
||||
|
||||
|
||||
def create_adas_steering_status(bus, idx):
|
||||
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
|
||||
chksum = 0x60 + sum(dat)
|
||||
dat += [chksum >> 8, chksum & 0xff]
|
||||
return make_can_msg(0x306, bytes(dat), bus)
|
||||
|
||||
|
||||
def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
|
||||
spd = int(speed_ms * 16) & 0xfff
|
||||
accel = 0 & 0xfff
|
||||
# 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L
|
||||
#stick = 0x08
|
||||
near_range_cutoff = 0x27
|
||||
near_range_mode = 1 if spd <= near_range_cutoff else 0
|
||||
far_range_mode = 1 - near_range_mode
|
||||
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0]
|
||||
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4]
|
||||
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff]
|
||||
return make_can_msg(0x308, bytes(dat), bus)
|
||||
|
||||
|
||||
def create_adas_headlights_status(packer, bus):
|
||||
values = {
|
||||
"Always42": 0x42,
|
||||
"Always4": 0x4,
|
||||
}
|
||||
return packer.make_can_msg("ASCMHeadlight", bus, values)
|
||||
|
||||
|
||||
def create_lka_icon_command(bus, active, critical, steer):
|
||||
if active and steer == 1:
|
||||
if critical:
|
||||
dat = b"\x50\xc0\x14"
|
||||
else:
|
||||
dat = b"\x50\x40\x18"
|
||||
elif active:
|
||||
if critical:
|
||||
dat = b"\x40\xc0\x14"
|
||||
else:
|
||||
dat = b"\x40\x40\x18"
|
||||
else:
|
||||
dat = b"\x00\x00\x00"
|
||||
return make_can_msg(0x104c006c, dat, bus)
|
||||
|
||||
|
||||
def create_gm_cc_spam_command(packer, controller, CS, actuators):
|
||||
if controller.params_.get_bool("IsMetric"):
|
||||
_CV = CV.MS_TO_KPH
|
||||
RATE_UP_MAX = 0.04
|
||||
RATE_DOWN_MAX = 0.04
|
||||
else:
|
||||
_CV = CV.MS_TO_MPH
|
||||
RATE_UP_MAX = 0.2
|
||||
RATE_DOWN_MAX = 0.2
|
||||
|
||||
accel = actuators.accel * _CV # m/s/s to mph/s
|
||||
speedSetPoint = int(round(CS.out.cruiseState.speed * _CV))
|
||||
|
||||
cruiseBtn = CruiseButtons.INIT
|
||||
if speedSetPoint == CS.CP.minEnableSpeed and accel < -1:
|
||||
cruiseBtn = CruiseButtons.CANCEL
|
||||
controller.apply_speed = 0
|
||||
rate = 0.04
|
||||
elif accel < 0:
|
||||
cruiseBtn = CruiseButtons.DECEL_SET
|
||||
if speedSetPoint > (CS.out.vEgo * _CV) + 3.0: # If accel is changing directions, bring set speed to current speed as fast as possible
|
||||
rate = RATE_DOWN_MAX
|
||||
else:
|
||||
rate = max(-1 / accel, RATE_DOWN_MAX)
|
||||
controller.apply_speed = speedSetPoint - 1
|
||||
elif accel > 0:
|
||||
cruiseBtn = CruiseButtons.RES_ACCEL
|
||||
if speedSetPoint < (CS.out.vEgo * _CV) - 3.0:
|
||||
rate = RATE_UP_MAX
|
||||
else:
|
||||
rate = max(1 / accel, RATE_UP_MAX)
|
||||
controller.apply_speed = speedSetPoint + 1
|
||||
else:
|
||||
controller.apply_speed = speedSetPoint
|
||||
rate = float('inf')
|
||||
|
||||
# Check rlogs closely - our message shouldn't show up on the pt bus for us
|
||||
# Or bus 2, since we're forwarding... but I think it does
|
||||
if (cruiseBtn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate):
|
||||
controller.last_button_frame = controller.frame
|
||||
idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV
|
||||
return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruiseBtn)]
|
||||
else:
|
||||
return []
|
||||
375
selfdrive/car/gm/interface.py
Executable file
375
selfdrive/car/gm/interface.py
Executable file
@@ -0,0 +1,375 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from cereal import car, custom
|
||||
from math import fabs, exp
|
||||
from panda import Panda
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
|
||||
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SDGM_CAR, SLOW_ACC
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
GearShifter = car.CarState.GearShifter
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
PEDAL_MSG = 0x201
|
||||
CAM_MSG = 0x320 # AEBCmd
|
||||
# TODO: Is this always linked to camera presence?
|
||||
ACCELERATOR_POS_MSG = 0xbe
|
||||
|
||||
NON_LINEAR_TORQUE_PARAMS = {
|
||||
CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
|
||||
CAR.BOLT_CC: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
|
||||
CAR.ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
|
||||
CAR.SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
|
||||
}
|
||||
|
||||
NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/neural_ff_weights.json')
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_variables):
|
||||
if frogpilot_variables.sport_plus:
|
||||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS
|
||||
else:
|
||||
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 in (CAR.VOLT, CAR.VOLT_CC):
|
||||
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: car.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):
|
||||
return 1 / (1 + exp(-val)) - 0.5
|
||||
|
||||
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
|
||||
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
|
||||
# This has big effect on the stability about 0 (noise when going straight)
|
||||
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
|
||||
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
|
||||
assert non_linear_torque_params, "The params are not defined"
|
||||
a, b, c, _ = non_linear_torque_params
|
||||
steer_torque = (sig(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: car.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 in (CAR.BOLT_EUV, CAR.BOLT_CC):
|
||||
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, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
params.put_bool("HideDisableOpenpilotLongitudinal", candidate not in (SDGM_CAR | CAMERA_ACC_CAR))
|
||||
|
||||
ret.carName = "gm"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
|
||||
ret.autoResumeSng = False
|
||||
ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN]
|
||||
if PEDAL_MSG in fingerprint[0]:
|
||||
ret.enableGasInterceptor = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR
|
||||
|
||||
if candidate in EV_CAR:
|
||||
ret.transmissionType = TransmissionType.direct
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.15]
|
||||
|
||||
ret.longitudinalTuning.kpBP = [5., 35.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
|
||||
if candidate in CAMERA_ACC_CAR:
|
||||
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.radarUnavailable = True # no radar
|
||||
ret.pcmCruise = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
|
||||
ret.minEnableSpeed = 5 * CV.KPH_TO_MS
|
||||
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
|
||||
|
||||
# Tuning for experimental long
|
||||
ret.longitudinalTuning.kpV = [2.0, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.72]
|
||||
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
|
||||
if candidate in SLOW_ACC and params.get_bool("GasRegenCmd"):
|
||||
ret.longitudinalTuning.kpV = [1.5, 1.125]
|
||||
ret.stopAccel = -0.25
|
||||
|
||||
if experimental_long:
|
||||
ret.pcmCruise = False
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
|
||||
|
||||
elif candidate in SDGM_CAR:
|
||||
ret.experimentalLongitudinalAvailable = False
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.pcmCruise = True
|
||||
ret.radarUnavailable = True
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by ASCM
|
||||
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
|
||||
|
||||
else: # ASCM, OBD-II harness
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
|
||||
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
|
||||
# supports stop and go, but initial engage must (conservatively) be above 18mph
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
|
||||
|
||||
# Tuning
|
||||
ret.longitudinalTuning.kpV = [2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.36]
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
# Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_ASCM_LONG
|
||||
|
||||
# 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.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
|
||||
|
||||
if candidate in (CAR.VOLT, CAR.VOLT_CC):
|
||||
ret.minEnableSpeed = -1
|
||||
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.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.ESCALADE:
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019):
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
|
||||
if candidate == CAR.ESCALADE_ESV:
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
|
||||
ret.lateralTuning.pid.kf = 0.000045
|
||||
else:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate in (CAR.BOLT_EUV, CAR.BOLT_CC):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
# ACC Bolts use pedal for full longitudinal control, not just sng
|
||||
ret.flags |= GMFlags.PEDAL_LONG.value
|
||||
|
||||
elif candidate == CAR.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 in (CAR.EQUINOX, CAR.EQUINOX_CC):
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate in (CAR.TRAILBLAZER, CAR.TRAILBLAZER_CC):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate in (CAR.SUBURBAN, CAR.SUBURBAN_CC):
|
||||
ret.steerActuatorDelay = 0.075
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.YUKON_CC:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.XT4:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.BABYENCLAVE:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CT6_CC:
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.TRAX:
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
|
||||
ret.minEnableSpeed = -1
|
||||
ret.pcmCruise = False
|
||||
ret.openpilotLongitudinalControl = not disable_openpilot_long
|
||||
ret.stoppingControl = True
|
||||
ret.autoResumeSng = True
|
||||
|
||||
if candidate in CC_ONLY_CAR:
|
||||
ret.flags |= GMFlags.PEDAL_LONG.value
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
|
||||
# Note: Low speed, stop and go not tested. Should be fairly smooth on highway
|
||||
ret.longitudinalTuning.kpBP = [5., 35.]
|
||||
ret.longitudinalTuning.kpV = [0.35, 0.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.0]
|
||||
ret.longitudinalTuning.kiV = [0.1, 0.1]
|
||||
ret.longitudinalTuning.kf = 0.15
|
||||
ret.stoppingDecelRate = 0.8
|
||||
else: # Pedal used for SNG, ACC for longitudinal control otherwise
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
|
||||
ret.startingState = True
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
|
||||
elif candidate in CC_ONLY_CAR:
|
||||
ret.flags |= GMFlags.CC_LONG.value
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CC_LONG
|
||||
ret.radarUnavailable = True
|
||||
ret.experimentalLongitudinalAvailable = False
|
||||
ret.minEnableSpeed = 24 * CV.MPH_TO_MS
|
||||
ret.openpilotLongitudinalControl = not disable_openpilot_long
|
||||
ret.pcmCruise = False
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s
|
||||
ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate)
|
||||
ret.longitudinalActuatorDelayLowerBound = 1. # TODO: measure this
|
||||
ret.longitudinalActuatorDelayUpperBound = 2.
|
||||
|
||||
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
|
||||
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.1]
|
||||
|
||||
if candidate in CC_ONLY_CAR:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
|
||||
|
||||
if ACCELERATOR_POS_MSG not in fingerprint[CanBus.POWERTRAIN]:
|
||||
ret.flags |= GMFlags.NO_ACCELERATOR_POS_MSG.value
|
||||
|
||||
# Exception for flashed cars, or cars whose camera was removed
|
||||
if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and CAM_MSG not in fingerprint[CanBus.CAMERA] and not candidate in SDGM_CAR:
|
||||
ret.flags |= GMFlags.NO_CAMERA.value
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_CAMERA
|
||||
|
||||
if ACCELERATOR_POS_MSG not in fingerprint[CanBus.POWERTRAIN]:
|
||||
ret.flags |= GMFlags.NO_ACCELERATOR_POS_MSG.value
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback, frogpilot_variables)
|
||||
|
||||
# Don't add event if transitioning from INIT, unless it's to an actual button
|
||||
if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT,
|
||||
unpressed_btn=CruiseButtons.UNPRESS),
|
||||
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
|
||||
{1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
# The ECM allows enabling on falling edge of set, but only rising edge of resume
|
||||
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
|
||||
GearShifter.eco, GearShifter.manumatic],
|
||||
pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
|
||||
if not self.CP.pcmCruise:
|
||||
if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
|
||||
events.add(EventName.buttonEnable)
|
||||
|
||||
# Enabling at a standstill with brake is allowed
|
||||
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
|
||||
below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward
|
||||
if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and
|
||||
(self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)):
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if ret.cruiseState.standstill and not self.disable_resumeRequired and not self.CP.autoResumeSng:
|
||||
events.add(EventName.resumeRequired)
|
||||
self.resumeRequired_shown = True
|
||||
|
||||
# Disable the "resumeRequired" event after it's been shown once to not annoy the driver
|
||||
if self.resumeRequired_shown and not ret.cruiseState.standstill:
|
||||
self.disable_resumeRequired = True
|
||||
|
||||
if ret.vEgo < self.CP.minSteerSpeed and not self.disable_belowSteerSpeed:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
self.belowSteerSpeed_shown = True
|
||||
|
||||
# Disable the "belowSteerSpeed" event after it's been shown once to not annoy the driver
|
||||
if self.belowSteerSpeed_shown and ret.vEgo >= self.CP.minSteerSpeed:
|
||||
self.disable_belowSteerSpeed = True
|
||||
|
||||
if (self.CP.flags & GMFlags.CC_LONG.value) and ret.vEgo < self.CP.minEnableSpeed and ret.cruiseState.enabled:
|
||||
events.add(EventName.speedTooLow)
|
||||
|
||||
if (self.CP.flags & GMFlags.PEDAL_LONG.value) and \
|
||||
self.CP.transmissionType == TransmissionType.direct and \
|
||||
not self.CS.single_pedal_mode and \
|
||||
c.longActive:
|
||||
events.add(EventName.pedalInterceptorNoBrake)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
101
selfdrive/car/gm/radar_interface.py
Executable file
101
selfdrive/car/gm/radar_interface.py
Executable file
@@ -0,0 +1,101 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_HEADER_MSG = 1120
|
||||
SLOT_1_MSG = RADAR_HEADER_MSG + 1
|
||||
NUM_SLOTS = 20
|
||||
|
||||
# Actually it's 0x47f, but can parser only reports
|
||||
# messages that are present in DBC
|
||||
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
|
||||
|
||||
|
||||
def create_radar_can_parser(car_fingerprint):
|
||||
# C1A-ARS3-A by Continental
|
||||
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
|
||||
signals = list(zip(['FLRRNumValidTargets',
|
||||
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
|
||||
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
|
||||
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
|
||||
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
|
||||
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
|
||||
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
|
||||
[RADAR_HEADER_MSG] * 7 + radar_targets * 6, strict=True))
|
||||
|
||||
messages = list({(s[1], 14) for s in signals})
|
||||
|
||||
return CANParser(DBC[car_fingerprint]['radar'], messages, CanBus.OBSTACLE)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
|
||||
self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint)
|
||||
|
||||
self.trigger_msg = LAST_RADAR_MSG
|
||||
self.updated_messages = set()
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
ret = car.RadarData.new_message()
|
||||
header = self.rcp.vl[RADAR_HEADER_MSG]
|
||||
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
|
||||
header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \
|
||||
header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt']
|
||||
errors = []
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
if fault:
|
||||
errors.append("fault")
|
||||
ret.errors = errors
|
||||
|
||||
currentTargets = set()
|
||||
num_targets = header['FLRRNumValidTargets']
|
||||
|
||||
# Not all radar messages describe targets,
|
||||
# no need to monitor all of the self.rcp.msgs_upd
|
||||
for ii in self.updated_messages:
|
||||
if ii == RADAR_HEADER_MSG:
|
||||
continue
|
||||
|
||||
if num_targets == 0:
|
||||
break
|
||||
|
||||
cpt = self.rcp.vl[ii]
|
||||
# Zero distance means it's an empty target slot
|
||||
if cpt['TrkRange'] > 0.0:
|
||||
targetId = cpt['TrkObjectID']
|
||||
currentTargets.add(targetId)
|
||||
if targetId not in self.pts:
|
||||
self.pts[targetId] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[targetId].trackId = targetId
|
||||
distance = cpt['TrkRange']
|
||||
self.pts[targetId].dRel = distance # from front of car
|
||||
# From driver's pov, left is positive
|
||||
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
|
||||
self.pts[targetId].vRel = cpt['TrkRangeRate']
|
||||
self.pts[targetId].aRel = float('nan')
|
||||
self.pts[targetId].yvRel = float('nan')
|
||||
|
||||
for oldTarget in list(self.pts.keys()):
|
||||
if oldTarget not in currentTargets:
|
||||
del self.pts[oldTarget]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
0
selfdrive/car/gm/tests/__init__.py
Executable file
0
selfdrive/car/gm/tests/__init__.py
Executable file
28
selfdrive/car/gm/tests/test_gm.py
Executable file
28
selfdrive/car/gm/tests/test_gm.py
Executable file
@@ -0,0 +1,28 @@
|
||||
#!/usr/bin/env python3
|
||||
from parameterized import parameterized
|
||||
import unittest
|
||||
|
||||
from openpilot.selfdrive.car.gm.fingerprints import FINGERPRINTS
|
||||
from openpilot.selfdrive.car.gm.values import CAMERA_ACC_CAR, CAR, GM_RX_OFFSET
|
||||
|
||||
CAMERA_DIAGNOSTIC_ADDRESS = 0x24b
|
||||
|
||||
|
||||
class TestGMFingerprint(unittest.TestCase):
|
||||
@parameterized.expand(FINGERPRINTS.items())
|
||||
def test_can_fingerprints(self, car_model, fingerprints):
|
||||
self.assertGreater(len(fingerprints), 0)
|
||||
|
||||
# Trailblazer is in dashcam
|
||||
if car_model != CAR.TRAILBLAZER:
|
||||
self.assertTrue(all(len(finger) for finger in fingerprints))
|
||||
|
||||
# The camera can sometimes be communicating on startup
|
||||
if car_model in CAMERA_ACC_CAR - {CAR.TRAILBLAZER}:
|
||||
for finger in fingerprints:
|
||||
for required_addr in (CAMERA_DIAGNOSTIC_ADDRESS, CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET):
|
||||
self.assertEqual(finger.get(required_addr), 8, required_addr)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
354
selfdrive/car/gm/values.py
Executable file
354
selfdrive/car/gm/values.py
Executable file
@@ -0,0 +1,354 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car import dbc_dict, PlatformConfig, DbcDict, Platforms, CarSpecs
|
||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output
|
||||
STEER_STEP = 3 # Active control frames per command (~33hz)
|
||||
INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz)
|
||||
STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness
|
||||
STEER_DELTA_DOWN = 15
|
||||
STEER_DRIVER_ALLOWANCE = 65
|
||||
STEER_DRIVER_MULTIPLIER = 4
|
||||
STEER_DRIVER_FACTOR = 100
|
||||
NEAR_STOP_BRAKE_PHASE = 0.25 # m/s
|
||||
SNG_INTERCEPTOR_GAS = 18. / 255.
|
||||
SNG_TIME = 30 # frames until the above is reached
|
||||
|
||||
# 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_MAX_PLUS = 4. # m/s^2
|
||||
ACCEL_MIN = -4. # m/s^2
|
||||
|
||||
def __init__(self, CP):
|
||||
# Gas/brake lookups
|
||||
self.ZERO_GAS = 6144 # Coasting
|
||||
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
|
||||
|
||||
if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR:
|
||||
self.MAX_GAS = 7496
|
||||
self.MAX_GAS_PLUS = 8848
|
||||
self.MAX_ACC_REGEN = 5610
|
||||
self.INACTIVE_REGEN = 5650
|
||||
# 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.
|
||||
|
||||
if CP.carFingerprint in SLOW_ACC and Params().get_bool("GasRegenCmd"):
|
||||
self.MAX_GAS = 8650
|
||||
self.MAX_GAS_PLUS = 8650 # Don't Stack Extra Speed
|
||||
self.ACCEL_MAX_PLUS = 2
|
||||
|
||||
elif CP.carFingerprint in SDGM_CAR:
|
||||
self.MAX_GAS = 7496
|
||||
self.MAX_GAS_PLUS = 8848
|
||||
self.MAX_ACC_REGEN = 5610
|
||||
self.INACTIVE_REGEN = 5650
|
||||
max_regen_acceleration = 0.
|
||||
|
||||
else:
|
||||
self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill.
|
||||
self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max
|
||||
self.MAX_ACC_REGEN = 5500 # Max ACC regen is slightly less than max paddle regen
|
||||
self.INACTIVE_REGEN = 5500
|
||||
# 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_BP_PLUS = [max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
|
||||
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS]
|
||||
self.GAS_LOOKUP_V_PLUS = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS_PLUS]
|
||||
|
||||
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration]
|
||||
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
|
||||
|
||||
# determined by letting Volt regen to a stop in L gear from 89mph,
|
||||
# and by letting off gas and allowing car to creep, for determining
|
||||
# the positive threshold values at very low speed
|
||||
EV_GAS_BRAKE_THRESHOLD_BP = [1.29, 1.52, 1.55, 1.6, 1.7, 1.8, 2.0, 2.2, 2.5, 5.52, 9.6, 20.5, 23.5, 35.0] # [m/s]
|
||||
EV_GAS_BRAKE_THRESHOLD_V = [0.0, -0.14, -0.16, -0.18, -0.215, -0.255, -0.32, -0.41, -0.5, -0.72, -0.895, -1.125, -1.145, -1.16] # [m/s^s]
|
||||
|
||||
def update_ev_gas_brake_threshold(self, v_ego):
|
||||
gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V)
|
||||
self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX]
|
||||
self.EV_GAS_LOOKUP_BP_PLUS = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX_PLUS]
|
||||
self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold]
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
OBD_II = CarFootnote(
|
||||
'Requires a <a href="https://github.com/commaai/openpilot/wiki/GM#hardware" target="_blank">community built ASCM harness</a>. ' +
|
||||
'<b><i>NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).</i></b>',
|
||||
Column.MODEL)
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMCarDocs(CarDocs):
|
||||
package: str = "Adaptive Cruise Control (ACC)"
|
||||
|
||||
def init_make(self, CP: car.CarParams):
|
||||
if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera:
|
||||
self.car_parts = CarParts.common([CarHarness.gm])
|
||||
else:
|
||||
self.car_parts = CarParts.common([CarHarness.obd_ii])
|
||||
self.footnotes.append(Footnote.OBD_II)
|
||||
|
||||
|
||||
@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'))
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
HOLDEN_ASTRA = GMPlatformConfig(
|
||||
"HOLDEN ASTRA RS-V BK 2017",
|
||||
[GMCarDocs("Holden Astra 2017")],
|
||||
GMCarSpecs(mass=1363, wheelbase=2.662, steerRatio=15.7, centerToFrontRatio=0.4),
|
||||
)
|
||||
VOLT = GMPlatformConfig(
|
||||
"CHEVROLET VOLT PREMIER 2017",
|
||||
[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, minEnableSpeed=-1),
|
||||
dbc_dict=dbc_dict('gm_global_a_powertrain_volt', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')
|
||||
)
|
||||
CADILLAC_ATS = GMPlatformConfig(
|
||||
"CADILLAC ATS Premium Performance 2018",
|
||||
[GMCarDocs("Cadillac ATS Premium Performance 2018")],
|
||||
GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3),
|
||||
)
|
||||
MALIBU = GMPlatformConfig(
|
||||
"CHEVROLET MALIBU PREMIER 2017",
|
||||
[GMCarDocs("Chevrolet Malibu Premier 2017")],
|
||||
GMCarSpecs(mass=1496, wheelbase=2.83, steerRatio=15.8, centerToFrontRatio=0.4),
|
||||
)
|
||||
ACADIA = GMPlatformConfig(
|
||||
"GMC ACADIA DENALI 2018",
|
||||
[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 = GMPlatformConfig(
|
||||
"BUICK LACROSSE 2017",
|
||||
[GMCarDocs("Buick LaCrosse 2017-19", "Driver Confidence Package 2")],
|
||||
GMCarSpecs(mass=1712, wheelbase=2.91, steerRatio=15.8, centerToFrontRatio=0.4),
|
||||
)
|
||||
BUICK_REGAL = GMPlatformConfig(
|
||||
"BUICK REGAL ESSENCE 2018",
|
||||
[GMCarDocs("Buick Regal Essence 2018")],
|
||||
GMCarSpecs(mass=1714, wheelbase=2.83, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
ESCALADE = GMPlatformConfig(
|
||||
"CADILLAC ESCALADE 2017",
|
||||
[GMCarDocs("Cadillac Escalade 2017", "Driver Assist Package")],
|
||||
GMCarSpecs(mass=2564, wheelbase=2.95, steerRatio=17.3),
|
||||
)
|
||||
ESCALADE_ESV = GMPlatformConfig(
|
||||
"CADILLAC ESCALADE ESV 2016",
|
||||
[GMCarDocs("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
GMCarSpecs(mass=2739, wheelbase=3.302, steerRatio=17.3, tireStiffnessFactor=1.0),
|
||||
)
|
||||
ESCALADE_ESV_2019 = GMPlatformConfig(
|
||||
"CADILLAC ESCALADE ESV 2019",
|
||||
[GMCarDocs("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
ESCALADE_ESV.specs,
|
||||
)
|
||||
BOLT_EUV = GMPlatformConfig(
|
||||
"CHEVROLET BOLT EUV 2022",
|
||||
[
|
||||
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),
|
||||
)
|
||||
SILVERADO = GMPlatformConfig(
|
||||
"CHEVROLET SILVERADO 1500 2020",
|
||||
[
|
||||
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),
|
||||
)
|
||||
EQUINOX = GMPlatformConfig(
|
||||
"CHEVROLET EQUINOX 2019",
|
||||
[GMCarDocs("Chevrolet Equinox 2019-22")],
|
||||
GMCarSpecs(mass=1588, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
TRAILBLAZER = GMPlatformConfig(
|
||||
"CHEVROLET TRAILBLAZER 2021",
|
||||
[GMCarDocs("Chevrolet Trailblazer 2021-22")],
|
||||
GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
|
||||
)
|
||||
# Separate car def is required when there is no ASCM
|
||||
# (for now) unless there is a way to detect it when it has been unplugged...
|
||||
VOLT_CC = GMPlatformConfig(
|
||||
"CHEVROLET VOLT NO ACC",
|
||||
[GMCarDocs("Chevrolet Volt 2017-18")],
|
||||
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=1.0),
|
||||
)
|
||||
BOLT_CC = GMPlatformConfig(
|
||||
"CHEVROLET BOLT EV NO ACC",
|
||||
[GMCarDocs("Chevrolet Bolt No ACC")],
|
||||
GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
|
||||
)
|
||||
EQUINOX_CC = GMPlatformConfig(
|
||||
"CHEVROLET EQUINOX NO ACC",
|
||||
[GMCarDocs("Chevrolet Equinox No ACC")],
|
||||
GMCarSpecs(mass=3500, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
|
||||
)
|
||||
SUBURBAN = GMPlatformConfig(
|
||||
"CHEVROLET SUBURBAN PREMIER 2016",
|
||||
[GMCarDocs("Chevrolet Suburban Premier 2016-2020")],
|
||||
CarSpecs(mass=2731, wheelbase=3.302, steerRatio=17.3, centerToFrontRatio=0.49),
|
||||
)
|
||||
SUBURBAN_CC = GMPlatformConfig(
|
||||
"CHEVROLET SUBURBAN NO ACC",
|
||||
[GMCarDocs("Chevrolet Suburban No ACC")],
|
||||
GMCarSpecs(mass=2731, wheelbase=3.032, steerRatio=17.3, centerToFrontRatio=0.49, tireStiffnessFactor=1.0),
|
||||
)
|
||||
YUKON_CC = GMPlatformConfig(
|
||||
"GMC YUKON NO ACC",
|
||||
[GMCarDocs("GMC Yukon No ACC")],
|
||||
CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4),
|
||||
)
|
||||
CT6_CC = GMPlatformConfig(
|
||||
"CADILLAC CT6 NO ACC",
|
||||
[GMCarDocs("Cadillac CT6 No ACC")],
|
||||
CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4),
|
||||
)
|
||||
TRAILBLAZER_CC = GMPlatformConfig(
|
||||
"CHEVROLET TRAILBLAZER NO ACC",
|
||||
[GMCarDocs("Chevrolet Trailblazer 2024 No ACC")],
|
||||
GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
|
||||
)
|
||||
XT4 = GMPlatformConfig(
|
||||
"CADILLAC XT4 2023",
|
||||
[GMCarDocs("Cadillac XT4 2023", "Driver Assist Package")],
|
||||
CarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
BABYENCLAVE = GMPlatformConfig(
|
||||
"BUICK BABY ENCLAVE 2020",
|
||||
[GMCarDocs("Buick Baby Enclave 2020-23")],
|
||||
CarSpecs(mass=2050, wheelbase=2.86, steerRatio=16.0, centerToFrontRatio=0.5),
|
||||
)
|
||||
TRAX = GMPlatformConfig(
|
||||
"CHEVROLET TRAX 2024",
|
||||
[GMCarDocs("Chevrolet TRAX 2024")],
|
||||
CarSpecs(mass=1365, wheelbase=2.7, steerRatio=16.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
|
||||
|
||||
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
|
||||
|
||||
class GMFlags(IntFlag):
|
||||
PEDAL_LONG = 1
|
||||
CC_LONG = 2
|
||||
NO_CAMERA = 4
|
||||
NO_ACCELERATOR_POS_MSG = 8
|
||||
|
||||
# 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.VOLT, CAR.BOLT_EUV, CAR.VOLT_CC, CAR.BOLT_CC}
|
||||
CC_ONLY_CAR = {CAR.VOLT_CC, CAR.BOLT_CC, CAR.EQUINOX_CC, CAR.SUBURBAN_CC, CAR.YUKON_CC, CAR.CT6_CC, CAR.TRAILBLAZER_CC}
|
||||
|
||||
# We're integrated at the Safety Data Gateway Module on these cars
|
||||
SDGM_CAR = {CAR.XT4, CAR.BABYENCLAVE}
|
||||
|
||||
# Slow acceleration cars
|
||||
SLOW_ACC = {CAR.SILVERADO}
|
||||
|
||||
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
|
||||
CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX, CAR.TRAILBLAZER, CAR.TRAX}
|
||||
CAMERA_ACC_CAR.update({CAR.VOLT_CC, CAR.BOLT_CC, CAR.EQUINOX_CC, CAR.YUKON_CC, CAR.CT6_CC, CAR.TRAILBLAZER_CC})
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
0
selfdrive/car/honda/__init__.py
Executable file
0
selfdrive/car/honda/__init__.py
Executable file
275
selfdrive/car/honda/carcontroller.py
Executable file
275
selfdrive/car/honda/carcontroller.py
Executable file
@@ -0,0 +1,275 @@
|
||||
from collections import namedtuple
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import create_gas_interceptor_command
|
||||
from openpilot.selfdrive.car.honda import hondacan
|
||||
from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def compute_gb_honda_bosch(accel, speed):
|
||||
# TODO returns 0s, is unused
|
||||
return 0.0, 0.0
|
||||
|
||||
|
||||
def compute_gb_honda_nidec(accel, speed):
|
||||
creep_brake = 0.0
|
||||
creep_speed = 2.3
|
||||
creep_brake_value = 0.15
|
||||
if speed < creep_speed:
|
||||
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
|
||||
gb = float(accel) / 4.8 - creep_brake
|
||||
return clip(gb, 0.0, 1.0), clip(-gb, 0.0, 1.0)
|
||||
|
||||
|
||||
def compute_gas_brake(accel, speed, fingerprint):
|
||||
if fingerprint in HONDA_BOSCH:
|
||||
return compute_gb_honda_bosch(accel, speed)
|
||||
else:
|
||||
return compute_gb_honda_nidec(accel, speed)
|
||||
|
||||
|
||||
# TODO not clear this does anything useful
|
||||
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# hyst params
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
|
||||
|
||||
# *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
|
||||
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
|
||||
brake = 0.
|
||||
braking = brake > 0.
|
||||
|
||||
# for small brake oscillations within brake_hyst_gap, don't change the brake command
|
||||
if brake == 0.:
|
||||
brake_steady = 0.
|
||||
elif brake > brake_steady + brake_hyst_gap:
|
||||
brake_steady = brake - brake_hyst_gap
|
||||
elif brake < brake_steady - brake_hyst_gap:
|
||||
brake_steady = brake + brake_hyst_gap
|
||||
brake = brake_steady
|
||||
|
||||
return brake, braking, brake_steady
|
||||
|
||||
|
||||
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts):
|
||||
pump_on = False
|
||||
|
||||
# reset pump timer if:
|
||||
# - there is an increment in brake request
|
||||
# - we are applying steady state brakes and we haven't been running the pump
|
||||
# for more than 20s (to prevent pressure bleeding)
|
||||
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0):
|
||||
last_pump_ts = ts
|
||||
|
||||
# once the pump is on, run it for at least 0.2s
|
||||
if ts - last_pump_ts < 0.2 and apply_brake > 0:
|
||||
pump_on = True
|
||||
|
||||
return pump_on, last_pump_ts
|
||||
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
# initialize to no alert
|
||||
fcw_display = 0
|
||||
steer_required = 0
|
||||
acc_alert = 0
|
||||
|
||||
# priority is: FCW, steer required, all others
|
||||
if hud_alert == VisualAlert.fcw:
|
||||
fcw_display = VISUAL_HUD[hud_alert.raw]
|
||||
elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
|
||||
steer_required = VISUAL_HUD[hud_alert.raw]
|
||||
else:
|
||||
acc_alert = VISUAL_HUD[hud_alert.raw]
|
||||
|
||||
return fcw_display, steer_required, acc_alert
|
||||
|
||||
|
||||
HUDData = namedtuple("HUDData",
|
||||
["pcm_accel", "v_cruise", "lead_visible",
|
||||
"lanes_visible", "fcw", "acc_alert", "steer_required", "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, VM):
|
||||
self.CP = CP
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.params = CarControllerParams(CP)
|
||||
self.CAN = hondacan.CanBus(CP)
|
||||
self.frame = 0
|
||||
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.apply_brake_last = 0
|
||||
self.last_pump_ts = 0.
|
||||
self.stopping_counter = 0
|
||||
|
||||
self.accel = 0.0
|
||||
self.speed = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake = 0.0
|
||||
self.last_steer = 0.0
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric)
|
||||
hud_v_cruise = hud_control.setSpeed / conversion if hud_control.speedVisible else 255
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
if CC.longActive:
|
||||
accel = actuators.accel
|
||||
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
|
||||
else:
|
||||
accel = 0.0
|
||||
gas, brake = 0.0, 0.0
|
||||
|
||||
# *** rate limit steer ***
|
||||
limited_steer = rate_limit_steer(actuators.steer, self.last_steer)
|
||||
self.last_steer = limited_steer
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
|
||||
CS.out.vEgo, self.CP.carFingerprint)
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_steer = int(interp(-limited_steer * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
|
||||
# Send CAN commands
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
|
||||
|
||||
# Send steering command.
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, 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 self.CP.enableGasInterceptor or not CC.longActive:
|
||||
pcm_speed = 0.0
|
||||
pcm_accel = int(0.0)
|
||||
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX)
|
||||
else:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 2.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 2.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX)
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.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:
|
||||
if frogpilot_variables.sport_plus:
|
||||
self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX_PLUS)
|
||||
else:
|
||||
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
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
|
||||
# when you do enable.
|
||||
if CC.longActive:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
|
||||
else:
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if self.frame % 10 == 0:
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
|
||||
hud_control.lanesVisible, fcw_display, acc_alert, steer_required, 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, CC.latActive))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
|
||||
if not self.CP.enableGasInterceptor:
|
||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.speed = self.speed
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
new_actuators.brake = self.brake
|
||||
new_actuators.steer = self.last_steer
|
||||
new_actuators.steerOutputCan = apply_steer
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
317
selfdrive/car/honda/carstate.py
Executable file
317
selfdrive/car/honda/carstate.py
Executable file
@@ -0,0 +1,317 @@
|
||||
from collections import defaultdict
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.honda.hondacan import 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
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
|
||||
|
||||
def get_can_messages(CP, gearbox_msg):
|
||||
messages = [
|
||||
("ENGINE_DATA", 100),
|
||||
("WHEEL_SPEEDS", 50),
|
||||
("STEERING_SENSORS", 100),
|
||||
("SEATBELT_STATUS", 10),
|
||||
("CRUISE", 10),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
("CAR_SPEED", 10),
|
||||
("VSA_STATUS", 50),
|
||||
("STEER_STATUS", 100),
|
||||
("STEER_MOTOR_TORQUE", 0), # TODO: not on every car
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
messages += [
|
||||
("SCM_FEEDBACK", 25),
|
||||
("SCM_BUTTONS", 50),
|
||||
]
|
||||
else:
|
||||
messages += [
|
||||
("SCM_FEEDBACK", 10),
|
||||
("SCM_BUTTONS", 25),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E):
|
||||
messages.append((gearbox_msg, 50))
|
||||
else:
|
||||
messages.append((gearbox_msg, 100))
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_ALT_BRAKE:
|
||||
messages.append(("BRAKE_MODULE", 50))
|
||||
|
||||
if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CLARITY}):
|
||||
messages.append(("EPB_STATUS", 50))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
# these messages are on camera bus on radarless cars
|
||||
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
messages += [
|
||||
("ACC_HUD", 10),
|
||||
("ACC_CONTROL", 50),
|
||||
]
|
||||
else: # Nidec signals
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
messages.append(("CRUISE_PARAMS", 10))
|
||||
else:
|
||||
messages.append(("CRUISE_PARAMS", 50))
|
||||
|
||||
# TODO: clean this up
|
||||
if CP.carFingerprint in (CAR.ACCORD, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
|
||||
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
|
||||
pass
|
||||
elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
|
||||
pass
|
||||
else:
|
||||
messages.append(("DOORS_STATUS", 3))
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
messages.append(("GAS_SENSOR", 50))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
messages.append(("CRUISE_FAULT_STATUS", 50))
|
||||
elif CP.carFingerprint == CAR.CLARITY:
|
||||
messages.append(("BRAKE_ERROR", 100)),
|
||||
elif CP.openpilotLongitudinalControl:
|
||||
messages.append(("STANDSTILL", 50))
|
||||
|
||||
return messages
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.gearbox_msg = "GEARBOX"
|
||||
if CP.carFingerprint == CAR.ACCORD and CP.transmissionType == TransmissionType.cvt:
|
||||
self.gearbox_msg = "GEARBOX_15T"
|
||||
|
||||
self.main_on_sig_msg = "SCM_FEEDBACK"
|
||||
if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES:
|
||||
self.main_on_sig_msg = "SCM_BUTTONS"
|
||||
|
||||
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
|
||||
self.brake_switch_prev = False
|
||||
self.brake_switch_active = False
|
||||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
|
||||
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
|
||||
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
|
||||
self.dash_speed_seen = False
|
||||
|
||||
def update(self, cp, cp_cam, cp_body, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.prev_cruise_setting = self.cruise_setting
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
|
||||
|
||||
# used for car hud message
|
||||
self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"]
|
||||
|
||||
# ******************* parse out can *******************
|
||||
# STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED
|
||||
# panda checks if the signal is non-zero
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
|
||||
# TODO: find a common signal across all cars
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
|
||||
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
|
||||
elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
|
||||
else:
|
||||
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
|
||||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
|
||||
ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
|
||||
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
ret.accFaulted = bool(cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"])
|
||||
elif self.CP.carFingerprint == CAR.CLARITY:
|
||||
ret.accFaulted = bool(cp.vl["BRAKE_ERROR"]["BRAKE_ERROR_1"] or cp.vl["BRAKE_ERROR"]["BRAKE_ERROR_2"])
|
||||
else:
|
||||
# On some cars, these two signals are always 1, this flag is masking a bug in release
|
||||
# FIXME: find and set the ACC faulted signals on more platforms
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
ret.accFaulted = bool(cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"])
|
||||
|
||||
# Log non-critical stock ACC/LKAS faults if Nidec (camera)
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
ret.carFaultedNonCritical = bool(cp_cam.vl["ACC_HUD"]["ACC_PROBLEM"] or cp_cam.vl["LKAS_HUD"]["LKAS_PROBLEM"])
|
||||
|
||||
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
|
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
|
||||
)
|
||||
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3
|
||||
if self.dash_speed_seen:
|
||||
conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
|
||||
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
|
||||
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
|
||||
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
|
||||
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
|
||||
# TODO: set for all cars
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CLARITY}):
|
||||
ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
|
||||
|
||||
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
# Same threshold as panda, equivalent to 1e-5 with previous DBC scaling
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
||||
ret.gasPressed = ret.gas > 492
|
||||
else:
|
||||
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
|
||||
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200)
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# 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.PILOT, CAR.RIDGELINE):
|
||||
if ret.brake > 0.1:
|
||||
ret.brakePressed = True
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
|
||||
else:
|
||||
aeb_sig = "COMPUTER_BRAKE_ALT" if self.CP.carFingerprint == CAR.CLARITY else "COMPUTER_BRAKE"
|
||||
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"][aeb_sig] > 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
|
||||
|
||||
self.prev_distance_button = self.distance_button
|
||||
self.distance_button = self.cruise_setting == 3
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
self.lkas_enabled = self.cruise_setting == 1
|
||||
|
||||
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
|
||||
1447
selfdrive/car/honda/fingerprints.py
Executable file
1447
selfdrive/car/honda/fingerprints.py
Executable file
File diff suppressed because it is too large
Load Diff
217
selfdrive/car/honda/hondacan.py
Executable file
217
selfdrive/car/honda/hondacan.py
Executable file
@@ -0,0 +1,217 @@
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car import CanBusBase
|
||||
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 = {
|
||||
"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,
|
||||
}
|
||||
|
||||
if car_fingerprint == CAR.CLARITY:
|
||||
values["COMPUTER_BRAKE_ALT"] = apply_brake
|
||||
values["BRAKE_PUMP_REQUEST_ALT"] = apply_brake > 0
|
||||
else:
|
||||
values["COMPUTER_BRAKE"] = apply_brake
|
||||
values["BRAKE_PUMP_REQUEST"] = pump_on
|
||||
|
||||
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, lat_active):
|
||||
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': lat_active,
|
||||
'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.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)
|
||||
296
selfdrive/car/honda/interface.py
Executable file
296
selfdrive/car/honda/interface.py
Executable file
@@ -0,0 +1,296 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car, custom
|
||||
from panda import Panda
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.selfdrive.car.honda.hondacan import CanBus
|
||||
from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CruiseSettings, HondaFlags, CAR, HONDA_BOSCH, \
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu
|
||||
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.altButton1}
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_variables):
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
if frogpilot_variables.sport_plus:
|
||||
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX_PLUS
|
||||
else:
|
||||
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
|
||||
elif CP.enableGasInterceptor:
|
||||
if frogpilot_variables.sport_plus:
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX_PLUS
|
||||
else:
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX
|
||||
else:
|
||||
# NIDECs don't allow acceleration near cruise_speed,
|
||||
# so limit limits of pid to prevent windup
|
||||
ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2]
|
||||
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "honda"
|
||||
|
||||
CAN = CanBus(ret, fingerprint)
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)]
|
||||
ret.radarUnavailable = True
|
||||
# Disable the radar and let openpilot control longitudinal
|
||||
# WARNING: THIS DISABLES AEB!
|
||||
# If Bosch radarless, this blocks ACC messages from the camera
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = experimental_long
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)]
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[CAN.pt]
|
||||
ret.openpilotLongitudinalControl = not disable_openpilot_long
|
||||
|
||||
ret.pcmCruise = not ret.enableGasInterceptor
|
||||
|
||||
if candidate == CAR.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.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.longitudinalTuning.kpV = [0.25]
|
||||
ret.longitudinalTuning.kiV = [0.05]
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
|
||||
else:
|
||||
# default longitudinal tuning for all hondas
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.18, 0.12]
|
||||
|
||||
eps_modified = False
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||
eps_modified = True
|
||||
|
||||
if candidate == CAR.CIVIC:
|
||||
if eps_modified:
|
||||
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
|
||||
# stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
|
||||
# modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
|
||||
# stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
|
||||
# modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
|
||||
# note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
|
||||
|
||||
elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022):
|
||||
if eps_modified:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] # 2.5x Modded EPS
|
||||
else:
|
||||
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.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.CRV, CAR.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.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.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.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.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.HRV, CAR.HRV_3G):
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
|
||||
if candidate == CAR.HRV:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
else:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning
|
||||
|
||||
elif candidate == CAR.ACURA_RDX:
|
||||
ret.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.ODYSSEY, CAR.ODYSSEY_CHN):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
|
||||
if candidate == CAR.ODYSSEY_CHN:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
|
||||
elif candidate == CAR.PILOT:
|
||||
ret.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.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.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
|
||||
|
||||
elif candidate == CAR.CLARITY:
|
||||
if eps_modified:
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"-" not in fw.fwVersion and b"," in fw.fwVersion:
|
||||
ret.lateralTuning.pid.kf = 0.00004
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 0xA00, 0x3C00], [0, 2560, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.1575], [0.05175]]
|
||||
elif fw.ecu == "eps" and b"-" in fw.fwVersion and b"," in fw.fwVersion:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 0xA00, 0x2800], [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 = [[0.8], [0.24]]
|
||||
|
||||
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 == CAR.ACCORD:
|
||||
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 ret.enableGasInterceptor and candidate not in HONDA_BOSCH:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_GAS_INTERCEPTOR
|
||||
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
|
||||
# conflict with PCM acc
|
||||
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC, CAR.CLARITY}) or ret.enableGasInterceptor
|
||||
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerLimitTimer = 0.8
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl:
|
||||
disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03')
|
||||
|
||||
# returns a car.CarState
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
|
||||
*create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
# events
|
||||
events = self.create_common_events(ret, pcm_enable=False)
|
||||
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
|
||||
if self.CP.pcmCruise:
|
||||
# we engage when pcm is active (rising edge)
|
||||
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
|
||||
events.add(EventName.pcmEnable)
|
||||
elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl):
|
||||
# it can happen that car cruise disables while comma system is enabled: need to
|
||||
# keep braking if needed or if the speed is very low
|
||||
if ret.vEgo < self.CP.minEnableSpeed + 2.:
|
||||
# non loud alert if cruise disables below 25mph as expected (+ a little margin)
|
||||
events.add(EventName.speedTooLow)
|
||||
else:
|
||||
events.add(EventName.cruiseDisabled)
|
||||
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
84
selfdrive/car/honda/radar_interface.py
Executable file
84
selfdrive/car/honda/radar_interface.py
Executable file
@@ -0,0 +1,84 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
from openpilot.selfdrive.car.honda.values import DBC
|
||||
|
||||
|
||||
def _create_nidec_can_parser(car_fingerprint):
|
||||
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
|
||||
messages = [(m, 20) for m in radar_messages]
|
||||
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
self.track_id = 0
|
||||
self.radar_fault = False
|
||||
self.radar_wrong_config = False
|
||||
self.radar_off_can = CP.radarUnavailable
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
|
||||
self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar
|
||||
|
||||
# Nidec
|
||||
if self.radar_off_can:
|
||||
self.rcp = None
|
||||
else:
|
||||
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = 0x445
|
||||
self.updated_messages = set()
|
||||
|
||||
def update(self, can_strings):
|
||||
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
|
||||
# radard at 20Hz and return no points
|
||||
if self.radar_off_can:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
rr = self._update(self.updated_messages)
|
||||
self.updated_messages.clear()
|
||||
return rr
|
||||
|
||||
def _update(self, updated_messages):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
for ii in sorted(updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
if ii == 0x400:
|
||||
# check for radar faults
|
||||
self.radar_fault = cpt['RADAR_STATE'] != 0x79
|
||||
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
|
||||
elif cpt['LONG_DIST'] < 255:
|
||||
if ii not in self.pts or cpt['NEW_TRACK']:
|
||||
self.pts[ii] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
|
||||
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['REL_SPEED']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = True
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
errors = []
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
if self.radar_fault:
|
||||
errors.append("fault")
|
||||
if self.radar_wrong_config:
|
||||
errors.append("wrongConfig")
|
||||
ret.errors = errors
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
|
||||
return ret
|
||||
0
selfdrive/car/honda/tests/__init__.py
Executable file
0
selfdrive/car/honda/tests/__init__.py
Executable file
20
selfdrive/car/honda/tests/test_honda.py
Executable file
20
selfdrive/car/honda/tests/test_honda.py
Executable file
@@ -0,0 +1,20 @@
|
||||
#!/usr/bin/env python3
|
||||
import re
|
||||
import unittest
|
||||
|
||||
from openpilot.selfdrive.car.honda.fingerprints import FW_VERSIONS
|
||||
|
||||
HONDA_FW_VERSION_RE = br"\d{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$"
|
||||
|
||||
|
||||
class TestHondaFingerprint(unittest.TestCase):
|
||||
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:
|
||||
self.assertTrue(re.match(HONDA_FW_VERSION_RE, fw) is not None, fw)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
369
selfdrive/car/honda/values.py
Executable file
369
selfdrive/car/honda/values.py
Executable file
@@ -0,0 +1,369 @@
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from panda.python import uds
|
||||
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict
|
||||
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 = car.CarParams.Ecu
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
|
||||
# perform the closed loop control, and might need some
|
||||
# to apply some more braking if we're on a downhill slope.
|
||||
# Our controller should still keep the 2 second average above
|
||||
# -3.5 m/s^2 as per planner limits
|
||||
NIDEC_ACCEL_MIN = -4.0 # m/s^2
|
||||
NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons
|
||||
NIDEC_ACCEL_MAX_PLUS = 4.0 # m/s^2
|
||||
|
||||
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_ACCEL_MAX_PLUS = 4.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: car.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
|
||||
ACCORD = HondaBoschPlatformConfig(
|
||||
"HONDA ACCORD 2018",
|
||||
[
|
||||
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),
|
||||
)
|
||||
CIVIC_BOSCH = HondaBoschPlatformConfig(
|
||||
"HONDA CIVIC (BOSCH) 2019",
|
||||
[
|
||||
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),
|
||||
)
|
||||
CIVIC_BOSCH_DIESEL = HondaBoschPlatformConfig(
|
||||
"HONDA CIVIC SEDAN 1.6 DIESEL 2019",
|
||||
[], # don't show in docs
|
||||
CIVIC_BOSCH.specs,
|
||||
dbc_dict('honda_accord_2018_can_generated', None),
|
||||
)
|
||||
CIVIC_2022 = HondaBoschPlatformConfig(
|
||||
"HONDA CIVIC 2022",
|
||||
[
|
||||
HondaCarDocs("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
|
||||
HondaCarDocs("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
|
||||
],
|
||||
CIVIC_BOSCH.specs,
|
||||
dbc_dict('honda_civic_ex_2022_can_generated', None),
|
||||
flags=HondaFlags.BOSCH_RADARLESS,
|
||||
)
|
||||
CRV_5G = HondaBoschPlatformConfig(
|
||||
"HONDA CR-V 2017",
|
||||
[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,
|
||||
)
|
||||
CRV_HYBRID = HondaBoschPlatformConfig(
|
||||
"HONDA CR-V HYBRID 2019",
|
||||
[HondaCarDocs("Honda CR-V Hybrid 2017-20", 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),
|
||||
)
|
||||
HRV_3G = HondaBoschPlatformConfig(
|
||||
"HONDA HR-V 2023",
|
||||
[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 | HondaFlags.BOSCH_ALT_BRAKE,
|
||||
)
|
||||
ACURA_RDX_3G = HondaBoschPlatformConfig(
|
||||
"ACURA RDX 2020",
|
||||
[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,
|
||||
)
|
||||
INSIGHT = HondaBoschPlatformConfig(
|
||||
"HONDA INSIGHT 2019",
|
||||
[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(
|
||||
"HONDA E 2020",
|
||||
[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),
|
||||
)
|
||||
CLARITY = HondaBoschPlatformConfig(
|
||||
"HONDA CLARITY 2018",
|
||||
[HondaCarDocs("Honda Clarity 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)],
|
||||
CarSpecs(mass=4052. * CV.LB_TO_KG, wheelbase=2.75, centerToFrontRatio=0.41, steerRatio=16.50, tireStiffnessFactor=1.),
|
||||
dbc_dict('honda_clarity_hybrid_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
)
|
||||
|
||||
# Nidec Cars
|
||||
ACURA_ILX = HondaNidecPlatformConfig(
|
||||
"ACURA ILX 2016",
|
||||
[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,
|
||||
)
|
||||
CRV = HondaNidecPlatformConfig(
|
||||
"HONDA CR-V 2016",
|
||||
[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,
|
||||
)
|
||||
CRV_EU = HondaNidecPlatformConfig(
|
||||
"HONDA CR-V EU 2016",
|
||||
[], # Euro version of CRV Touring, don't show in docs
|
||||
CRV.specs,
|
||||
dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES,
|
||||
)
|
||||
FIT = HondaNidecPlatformConfig(
|
||||
"HONDA FIT 2018",
|
||||
[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,
|
||||
)
|
||||
FREED = HondaNidecPlatformConfig(
|
||||
"HONDA FREED 2020",
|
||||
[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,
|
||||
)
|
||||
HRV = HondaNidecPlatformConfig(
|
||||
"HONDA HRV 2019",
|
||||
[HondaCarDocs("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS)],
|
||||
HRV_3G.specs,
|
||||
dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES,
|
||||
)
|
||||
ODYSSEY = HondaNidecPlatformConfig(
|
||||
"HONDA ODYSSEY 2018",
|
||||
[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,
|
||||
)
|
||||
ODYSSEY_CHN = HondaNidecPlatformConfig(
|
||||
"HONDA ODYSSEY CHN 2019",
|
||||
[], # Chinese version of Odyssey, don't show in docs
|
||||
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(
|
||||
"ACURA RDX 2018",
|
||||
[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,
|
||||
)
|
||||
PILOT = HondaNidecPlatformConfig(
|
||||
"HONDA PILOT 2017",
|
||||
[
|
||||
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,
|
||||
)
|
||||
RIDGELINE = HondaNidecPlatformConfig(
|
||||
"HONDA RIDGELINE 2017",
|
||||
[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,
|
||||
)
|
||||
CIVIC = HondaNidecPlatformConfig(
|
||||
"HONDA CIVIC 2016",
|
||||
[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_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(0xF112)
|
||||
HONDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(0xF112)
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[
|
||||
# Currently used to fingerprint
|
||||
Request(
|
||||
[StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.UDS_VERSION_RESPONSE],
|
||||
bus=1,
|
||||
),
|
||||
|
||||
# Data collection requests:
|
||||
# Attempt to get the radarless Civic 2022+ camera FW
|
||||
Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
logging=True
|
||||
),
|
||||
# Log extra identifiers for current ECUs
|
||||
Request(
|
||||
[HONDA_VERSION_REQUEST],
|
||||
[HONDA_VERSION_RESPONSE],
|
||||
bus=1,
|
||||
logging=True,
|
||||
),
|
||||
# Nidec PT bus
|
||||
Request(
|
||||
[StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.UDS_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
),
|
||||
# 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
|
||||
non_essential_ecus={
|
||||
Ecu.programmedFuelInjection: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G],
|
||||
Ecu.transmission: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G],
|
||||
Ecu.srs: [CAR.ACCORD],
|
||||
Ecu.eps: [CAR.ACCORD],
|
||||
Ecu.vsa: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G],
|
||||
Ecu.combinationMeter: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G],
|
||||
Ecu.gateway: [CAR.ACCORD, CAR.CIVIC, CAR.CIVIC_BOSCH, CAR.CRV_5G],
|
||||
Ecu.electricBrakeBooster: [CAR.ACCORD, CAR.CIVIC_BOSCH, CAR.CRV_5G],
|
||||
Ecu.shiftByWire: [CAR.ACCORD], # existence correlates with transmission type for ICE
|
||||
Ecu.hud: [CAR.ACCORD], # existence correlates with trim level
|
||||
},
|
||||
extra_ecus=[
|
||||
# The only other ECU on PT bus accessible by camera on radarless Civic
|
||||
(Ecu.unknown, 0x18DAB3F1, None),
|
||||
],
|
||||
)
|
||||
|
||||
STEER_THRESHOLD = {
|
||||
# default is 1200, overrides go here
|
||||
CAR.ACURA_RDX: 400,
|
||||
CAR.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()
|
||||
0
selfdrive/car/hyundai/__init__.py
Executable file
0
selfdrive/car/hyundai/__init__.py
Executable file
253
selfdrive/car/hyundai/carcontroller.py
Executable file
253
selfdrive/car/hyundai/carcontroller.py
Executable file
@@ -0,0 +1,253 @@
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
|
||||
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
|
||||
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
|
||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.common.params import Params
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
|
||||
# All slightly below EPS thresholds to avoid fault
|
||||
MAX_ANGLE = 85
|
||||
MAX_ANGLE_FRAMES = 89
|
||||
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
|
||||
|
||||
|
||||
def process_hud_alert(enabled, fingerprint, hud_control):
|
||||
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
|
||||
|
||||
# initialize to no line visible
|
||||
# TODO: this is not accurate for all cars
|
||||
sys_state = 1
|
||||
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
|
||||
sys_state = 3 if enabled or sys_warning else 4
|
||||
elif hud_control.leftLaneVisible:
|
||||
sys_state = 5
|
||||
elif hud_control.rightLaneVisible:
|
||||
sys_state = 6
|
||||
|
||||
# initialize to no warnings
|
||||
left_lane_warning = 0
|
||||
right_lane_warning = 0
|
||||
if hud_control.leftLaneDepart:
|
||||
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
|
||||
if hud_control.rightLaneDepart:
|
||||
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
|
||||
|
||||
return sys_warning, sys_state, left_lane_warning, right_lane_warning
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.CAN = CanBus(CP)
|
||||
self.params = CarControllerParams(CP)
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.angle_limit_counter = 0
|
||||
self.frame = 0
|
||||
|
||||
self.accel_last = 0
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.last_button_frame = 0
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
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
|
||||
if frogpilot_variables.sport_plus:
|
||||
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
|
||||
else:
|
||||
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
|
||||
|
||||
# HUD messages
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
|
||||
hud_control)
|
||||
|
||||
can_sends = []
|
||||
|
||||
# *** common hyundai stuff ***
|
||||
|
||||
# tester present - w/ no response (keeps relevant ECU disabled)
|
||||
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
|
||||
# for longitudinal control, either radar or ADAS driving ECU
|
||||
addr, bus = 0x7d0, 0
|
||||
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
|
||||
addr, bus = 0x730, self.CAN.ECAN
|
||||
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
|
||||
|
||||
# for blinkers
|
||||
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
|
||||
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
|
||||
|
||||
# CAN-FD platforms
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
|
||||
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
|
||||
|
||||
# steering control
|
||||
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
|
||||
|
||||
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
|
||||
if self.frame % 5 == 0 and hda2:
|
||||
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
|
||||
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
|
||||
|
||||
# LFA and HDA icons
|
||||
if self.frame % 5 == 0 and (not hda2 or hda2_long):
|
||||
# CLEARPILOT TEST self.CS.lkas_enabled
|
||||
# can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CC.latActive))
|
||||
# params_memory = Params("/dev/shm/params")
|
||||
# lkas_icon = params_memory.get_bool("CPTLkasButtonAction") or CS.lkas_enabled
|
||||
# lkas_icon = CS.experimentalMode
|
||||
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, False, False))
|
||||
# can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CS.experimentalMode, False))
|
||||
|
||||
# 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))
|
||||
|
||||
# params_memory = Params("/dev/shm/params")
|
||||
# if params_memory.get_bool("CPTLkasButtonAction"):
|
||||
# if self.frame % 10 == 0:
|
||||
# for _ in range(20):
|
||||
# can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.SET_DECEL))
|
||||
# can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
|
||||
# can_sends.append(hyundaicanfd.create_acc_set_speed(self.packer, self.CP, self.CAN, CS.cruise_info, 50))
|
||||
# can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
|
||||
# print("Debug cancel executed")
|
||||
|
||||
if self.CP.openpilotLongitudinalControl: # or CS.experimentalMode:
|
||||
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:
|
||||
# Clearpilot
|
||||
# If cruise control was enabled or idle on start, force cancel
|
||||
# if CS.fix_main_enabled_cancel_main:
|
||||
# CS.fix_main_enabled_cancel_main = False
|
||||
# CC.cruiseControl.cancel = True
|
||||
# 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, CS.out.cruiseState.available))
|
||||
|
||||
# 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, CC.latActive))
|
||||
|
||||
# 5 Hz ACC options
|
||||
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(hyundaican.create_acc_opt(self.packer))
|
||||
|
||||
# 2 Hz front radar options
|
||||
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = apply_steer / self.params.STEER_MAX
|
||||
new_actuators.steerOutputCan = apply_steer
|
||||
new_actuators.accel = accel
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool):
|
||||
# hax
|
||||
|
||||
|
||||
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))
|
||||
CS.lkas_trigger_result = 5
|
||||
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:
|
||||
# params_memory = Params("/dev/shm/params")
|
||||
# if params_memory.get_bool("CPTLkasButtonAction"):
|
||||
# params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
# CC.cruiseControl.cancel = True
|
||||
# print("Debug cancel executed")
|
||||
# cruise cancel
|
||||
if CC.cruiseControl.cancel:
|
||||
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
|
||||
# pass
|
||||
# can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
|
||||
# for _ in range(20):
|
||||
# can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
|
||||
# if CS.cruise_can_msg:
|
||||
# can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL, CS.cruise_can_msg))
|
||||
# print("Try Cancel Button Alt")
|
||||
self.last_button_frame = self.frame
|
||||
CS.lkas_trigger_result = 1
|
||||
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
|
||||
CS.lkas_trigger_result = 2
|
||||
|
||||
# cruise standstill resume
|
||||
# elif CC.cruiseControl.resume:
|
||||
elif CC.cruiseControl.resume:
|
||||
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
|
||||
# TODO: resume for alt button cars
|
||||
print (CS.cruise_can_msg)
|
||||
CS.lkas_trigger_result = 3
|
||||
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
|
||||
CS.lkas_trigger_result = 4
|
||||
|
||||
return can_sends
|
||||
553
selfdrive/car/hyundai/carstate.py
Executable file
553
selfdrive/car/hyundai/carstate.py
Executable file
@@ -0,0 +1,553 @@
|
||||
from collections import deque
|
||||
import copy
|
||||
import math
|
||||
|
||||
import sys
|
||||
import json
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
|
||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
|
||||
CANFD_CAR, Buttons, CarControllerParams
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
|
||||
PREV_BUTTON_SAMPLES = 8
|
||||
CLUSTER_SAMPLE_RATE = 20 # frames
|
||||
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
|
||||
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
|
||||
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
|
||||
|
||||
self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
|
||||
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
|
||||
"GEAR_SHIFTER"
|
||||
if CP.carFingerprint in CANFD_CAR:
|
||||
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
|
||||
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
|
||||
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
|
||||
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
|
||||
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
|
||||
else: # preferred and elect gear methods use same definition
|
||||
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
|
||||
|
||||
self.accelerator_msg_canfd = "ACCELERATOR" if CP.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)
|
||||
|
||||
# FrogPilot variables
|
||||
self.main_enabled = False
|
||||
|
||||
# Clearpilot variables dev
|
||||
self.fix_main_enabled_check = True
|
||||
self.fix_main_enabled_executed = False
|
||||
self.fix_main_enabled_cancel_main = False
|
||||
self.lkas_was_pressed = False
|
||||
self.lkas_trigger_result = None
|
||||
|
||||
def calculate_speed_limit(self, cp, cp_cam):
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
|
||||
# print(speed_limit_bus.vl, sys.stderr)
|
||||
# sys.stderr.write(str({k: v for k, v in vars(speed_limit_bus).items() if isinstance(v, (int, float, str, bool))}) + '\n')
|
||||
# Clearpilot fix
|
||||
best_speed = 0
|
||||
# print(speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"])
|
||||
# if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]):
|
||||
# best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]
|
||||
if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
|
||||
if (best_speed == 0 or best_speed < speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
|
||||
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]
|
||||
if (best_speed == 0):
|
||||
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
|
||||
return best_speed
|
||||
else:
|
||||
if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]:
|
||||
return 0
|
||||
speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
|
||||
return speed_limit if speed_limit not in (0, 255) else 0
|
||||
|
||||
def update(self, cp, cp_cam, frogpilot_variables):
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
return self.update_canfd(cp, cp_cam, frogpilot_variables)
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
|
||||
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
|
||||
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
|
||||
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
|
||||
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
|
||||
|
||||
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
|
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
|
||||
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
|
||||
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
|
||||
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
|
||||
)
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
|
||||
|
||||
self.cluster_speed_counter += 1
|
||||
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
|
||||
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
|
||||
self.cluster_speed_counter = 0
|
||||
|
||||
# Mimic how dash converts to imperial.
|
||||
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
|
||||
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
|
||||
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
|
||||
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
|
||||
|
||||
ret.vEgoCluster = self.cluster_speed * speed_conv
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
|
||||
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
|
||||
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
|
||||
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
|
||||
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
|
||||
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
|
||||
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
|
||||
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
|
||||
|
||||
# cruise state
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
|
||||
ret.cruiseState.available = self.main_enabled
|
||||
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
|
||||
ret.cruiseState.standstill = False
|
||||
ret.cruiseState.nonAdaptive = False
|
||||
else:
|
||||
ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
|
||||
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
|
||||
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
|
||||
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
|
||||
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
|
||||
|
||||
# TODO: Find brake pressure
|
||||
ret.brake = 0
|
||||
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
|
||||
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
||||
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
||||
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
|
||||
if self.CP.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
|
||||
self.prev_cruise_buttons = self.cruise_buttons[-1]
|
||||
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
|
||||
self.prev_main_buttons = self.main_buttons[-1]
|
||||
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
|
||||
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
|
||||
self.main_enabled = not self.main_enabled
|
||||
|
||||
self.prev_distance_button = self.distance_button
|
||||
self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST
|
||||
|
||||
if self.CP.flags & HyundaiFlags.CAN_LFA_BTN:
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
|
||||
|
||||
# self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
|
||||
self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
|
||||
|
||||
|
||||
return ret
|
||||
|
||||
def update_canfd(self, cp, cp_cam, frogpilot_variables):
|
||||
|
||||
# ---- Preexisting unsorted frogpilot code ----
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
|
||||
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
|
||||
if self.CP.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.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 = self.main_enabled
|
||||
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
|
||||
|
||||
self.cruise_can_msg = copy.copy(cp.vl_all[self.cruise_btns_msg_canfd])
|
||||
# print(self.cruise_can_msg)
|
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons[-1]
|
||||
self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
|
||||
self.prev_main_buttons = self.main_buttons[-1]
|
||||
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
|
||||
|
||||
# ------ End old frogpilot code ------
|
||||
|
||||
# Clearpilot Activation button fixes:
|
||||
|
||||
# This code tracks the engaged state of openpilot based on when the user clicks the
|
||||
# cruise button. However, cruise only engages when the break is not pressed,
|
||||
# and cruise can already be enabled when openpilot starts. This compensates for that behavior.
|
||||
|
||||
# Block attempt to engage if already engaged according to cruise state
|
||||
# if ret.cruiseState.speed > 0 and self.main_enabled == False:
|
||||
# self.main_buttons[-1] = 0
|
||||
|
||||
# Block attempt to engage if breaks are pressed
|
||||
if self.main_buttons[-1] != 0 and ret.brakePressed and self.main_enabled == False:
|
||||
self.main_buttons[-1] = 0
|
||||
|
||||
# Swicth to enabled based on button if above conditions pass
|
||||
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
|
||||
self.main_enabled = not self.main_enabled
|
||||
|
||||
# --------- Resume preexisting frogpilot code ------------
|
||||
|
||||
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"])
|
||||
|
||||
self.prev_distance_button = self.distance_button
|
||||
self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
lkas_enabled = False
|
||||
try:
|
||||
lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]
|
||||
except:
|
||||
nothing = 0
|
||||
|
||||
self.lkas_enabled = lkas_enabled
|
||||
|
||||
# if self.lkas_enabled and not self.lkas_previously_enabled:
|
||||
# self.fix_main_enabled_cancel_main = True
|
||||
# self.lkas_was_pressed = True
|
||||
|
||||
# print('Hello World', file=sys.stderr)
|
||||
# sys.stderr.write(str({k: v for k, v in vars(ret).items() if isinstance(v, (int, float, str, bool))}) + '\n')
|
||||
# print(ret, sys.stderr)
|
||||
|
||||
# print(ret.cruiseState, sys.stderr)
|
||||
# print({"fix_main_enabled_check": self.fix_main_enabled_check}, sys.stderr)
|
||||
# print({"fix_main_enabled_cancel_main": self.fix_main_enabled_cancel_main}, sys.stderr)
|
||||
# print({"fix_main_enabled_executed": self.fix_main_enabled_executed}, sys.stderr)
|
||||
# print({"self.lkas_enabled": self.lkas_enabled}, sys.stderr)
|
||||
# print({"lkas_enabled": lkas_enabled}, sys.stderr)
|
||||
# print({"lkas_was_pressed": self.lkas_was_pressed}, sys.stderr)
|
||||
# print({"lkas_trigger_result": self.lkas_trigger_result}, sys.stderr)
|
||||
|
||||
# notes on self:
|
||||
# lkas_enabled = 1 = lkas button has been pressed
|
||||
# prev_cruise_buttons = 0 (none), 1, 2 - up down
|
||||
# new lane change works perfect
|
||||
# need to auto disable lat on turn lower than 20 + turn signal as default
|
||||
# ret: cruiseState.speed > 0 = and cruiseState.enabled = false = idle cruise.
|
||||
# press cruise to cancel it if not op engaged
|
||||
|
||||
# engaged at 25
|
||||
# ret from carstate:
|
||||
# . cruiseState = (
|
||||
# enabled = true,
|
||||
# speed = 11.176, # above 0 if disengaged but set
|
||||
# available = true,
|
||||
# speedOffset = 0,
|
||||
# standstill = false,
|
||||
# nonAdaptive = false,
|
||||
# speedCluster = 0 ),
|
||||
# .standstill = false,
|
||||
# steeringPressed = false,
|
||||
|
||||
# Interesting values:
|
||||
# 416.ACC_ObjDist - distance to lead radar?
|
||||
# 506.SPEED_LIMIT_2
|
||||
# print(speed_limit_bus.vl, sys.stderr)
|
||||
# {416: {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0, 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3':
|
||||
# 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0, 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0,
|
||||
# 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0, 'COUNTER': 173.0,
|
||||
# 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0, 'NEW_SIGNAL_2': 0.0,
|
||||
# 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0, 'DISTANCE_SETTING': 0.0,
|
||||
# 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0, 'aReqValue': 0.0, 'ZEROS_7': 0.0,
|
||||
# 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002, 'JerkLowerLimit': 0.0, 'StopReq': 0.0,
|
||||
# 'NEW_SIGNAL_15': 204.60000000000002, 'VSetDis': 0.0, 'MainMode_ACC': 0.0,
|
||||
# 'JerkUpperLimit': 0.0}, 'SCC_CONTROL': {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0,
|
||||
# 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3': 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0,
|
||||
# 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0, 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0,
|
||||
# 'COUNTER': 173.0, 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0,
|
||||
# 'NEW_SIGNAL_2': 0.0, 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0,
|
||||
# 'DISTANCE_SETTING': 0.0, 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0,
|
||||
# 'aReqValue': 0.0, 'ZEROS_7': 0.0, 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002,
|
||||
# 'JerkLowerLimit': 0.0, 'StopReq': 0.0, 'NEW_SIGNAL_15': 204.60000000000002,
|
||||
# 'VSetDis': 0.0, 'MainMode_ACC': 0.0, 'JerkUpperLimit': 0.0},
|
||||
# 506: {'SPEED_LIMIT_3': 38.0,
|
||||
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
|
||||
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
|
||||
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0},
|
||||
# 'CLUSTER_SPEED_LIMIT': {'SPEED_LIMIT_3': 38.0,
|
||||
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
|
||||
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
|
||||
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0}}
|
||||
# <_io.TextIOWrapper name='<stderr>' mode='w' encoding='utf-8'>
|
||||
|
||||
# ( enabled = false,
|
||||
# speed = 9.83488,
|
||||
# available = false,
|
||||
# speedOffset = 0,
|
||||
# standstill = false,
|
||||
# nonAdaptive = false,
|
||||
# speedCluster = 0 )
|
||||
|
||||
# print("Set limit")
|
||||
# print(self.calculate_speed_limit(cp, cp_cam))
|
||||
# self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
|
||||
|
||||
return ret
|
||||
|
||||
def get_can_parser(self, CP):
|
||||
if CP.carFingerprint in CANFD_CAR:
|
||||
return self.get_can_parser_canfd(CP)
|
||||
|
||||
messages = [
|
||||
# address, frequency
|
||||
("MDPS12", 50),
|
||||
("TCS13", 50),
|
||||
("TCS15", 10),
|
||||
("CLU11", 50),
|
||||
("CLU15", 5),
|
||||
("ESP12", 100),
|
||||
("CGW1", 10),
|
||||
("CGW2", 5),
|
||||
("CGW4", 5),
|
||||
("WHL_SPD11", 50),
|
||||
("SAS11", 100),
|
||||
]
|
||||
|
||||
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
|
||||
messages += [
|
||||
("SCC11", 50),
|
||||
("SCC12", 50),
|
||||
]
|
||||
if CP.flags & HyundaiFlags.USE_FCA.value:
|
||||
messages.append(("FCA11", 50))
|
||||
|
||||
if CP.enableBsm:
|
||||
messages.append(("LCA11", 50))
|
||||
|
||||
if CP.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))
|
||||
|
||||
if CP.flags & HyundaiFlags.CAN_LFA_BTN:
|
||||
messages.append(("BCM_PO_11", 50))
|
||||
|
||||
messages += [
|
||||
("Navi_HU", 5),
|
||||
]
|
||||
|
||||
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),
|
||||
]
|
||||
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2:
|
||||
messages.append(("CLUSTER_SPEED_LIMIT", 10))
|
||||
|
||||
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),
|
||||
]
|
||||
|
||||
if not (CP.flags & HyundaiFlags.CANFD_HDA2):
|
||||
messages.append(("CLUSTER_SPEED_LIMIT", 10))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)
|
||||
1719
selfdrive/car/hyundai/fingerprints.py
Executable file
1719
selfdrive/car/hyundai/fingerprints.py
Executable file
File diff suppressed because it is too large
Load Diff
213
selfdrive/car/hyundai/hyundaican.py
Executable file
213
selfdrive/car/hyundai/hyundaican.py
Executable file
@@ -0,0 +1,213 @@
|
||||
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.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
|
||||
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
|
||||
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022,
|
||||
CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022,
|
||||
CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED,
|
||||
CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN):
|
||||
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
|
||||
values["CF_Lkas_LdwsOpt_USM"] = 2
|
||||
|
||||
# FcwOpt_USM 5 = Orange blinking car + lanes
|
||||
# FcwOpt_USM 4 = Orange car + lanes
|
||||
# FcwOpt_USM 3 = Green blinking car + lanes
|
||||
# FcwOpt_USM 2 = Green car + lanes
|
||||
# FcwOpt_USM 1 = White car + lanes
|
||||
# FcwOpt_USM 0 = No car + lanes
|
||||
values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1
|
||||
|
||||
# SysWarning 4 = keep hands on wheel
|
||||
# SysWarning 5 = keep hands on wheel (red)
|
||||
# SysWarning 6 = keep hands on wheel (red) + beep
|
||||
# Note: the warning is hidden while the blinkers are on
|
||||
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
|
||||
|
||||
# Likely cars lacking the ability to show individual lane lines in the dash
|
||||
elif 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)[2]
|
||||
|
||||
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, lat_active, hda_set_speed=0):
|
||||
values = {
|
||||
"LFA_Icon_State": 2 if lat_active 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, cruise_available):
|
||||
commands = []
|
||||
|
||||
scc11_values = {
|
||||
"MainMode_ACC": 1 if cruise_available else 0,
|
||||
"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)[2]
|
||||
scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
|
||||
|
||||
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
|
||||
|
||||
scc14_values = {
|
||||
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ObjGap": 2 if 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)[2]
|
||||
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
|
||||
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
|
||||
|
||||
return commands
|
||||
|
||||
def create_acc_opt(packer):
|
||||
commands = []
|
||||
|
||||
scc13_values = {
|
||||
"SCCDrvModeRValue": 2,
|
||||
"SCC_Equip": 1,
|
||||
"Lead_Veh_Dep_Alert_USM": 2,
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
|
||||
|
||||
# TODO: this needs to be detected and conditionally sent on unsupported long cars
|
||||
fca12_values = {
|
||||
"FCA_DrvSetState": 2,
|
||||
"FCA_USM": 1, # AEB disabled
|
||||
}
|
||||
commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
|
||||
|
||||
return commands
|
||||
|
||||
def create_frt_radar_opt(packer):
|
||||
frt_radar11_values = {
|
||||
"CF_FCA_Equip_Front_Radar": 1,
|
||||
}
|
||||
return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)
|
||||
379
selfdrive/car/hyundai/hyundaicanfd.py
Executable file
379
selfdrive/car/hyundai/hyundaicanfd.py
Executable file
@@ -0,0 +1,379 @@
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.selfdrive.car import CanBusBase
|
||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
|
||||
|
||||
from openpilot.common.params import Params
|
||||
|
||||
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):
|
||||
|
||||
# Im sure there is a better way to do this
|
||||
params_memory = Params("/dev/shm/params")
|
||||
no_lat_lane_change = params_memory.get_int("no_lat_lane_change", 1)
|
||||
|
||||
ret = []
|
||||
|
||||
values = {
|
||||
"LKA_MODE": 2,
|
||||
"LKA_ICON": 0 if no_lat_lane_change else 2 if enabled else 1 if lat_active else 0, # right mode icon
|
||||
"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)
|
||||
|
||||
# null
|
||||
# {'CHECKSUM': [], 'COUNTER': [], 'NEW_SIGNAL_1': [], 'SET_ME_1': [], 'DISTANCE_UNIT': [],
|
||||
# 'NEW_SIGNAL_2': [], 'ADAPT
|
||||
# IVE_CRUISE_MAIN_BTN': [], 'NEW_SIGNAL_3': [], 'LFA_BTN': [], 'CRUISE_BUTTONS': [],
|
||||
# 'NEW_SIGNAL_4': [], 'NORMAL_CRUI
|
||||
# SE_MAIN_BTN': [], 'NEW_SIGNAL_5': [], 'SET_ME_2': [], 'NEW_SIGNAL_6': [], 'BYTE6': [],
|
||||
# 'BYTE7': [], 'BYTE8': [], 'B
|
||||
# YTE9': [], 'BYTE10': [], 'BYTE11': [], 'BYTE12': [], 'BYTE13': [], 'BYTE14': [],
|
||||
# 'BYTE15': []}
|
||||
|
||||
# holding down up button
|
||||
# {'CHECKSUM': [5774.0], 'COUNTER': [63.0], 'NEW_SIGNAL_1': [0.0], 'SET_ME_1':
|
||||
# [0.0], 'DISTANCE_UNIT': [1.0], 'NEW_SIGNAL_2': [4.0], 'ADAPTIVE_CRUISE_MAIN_BTN':
|
||||
# [0.0], 'NEW_SIGNAL_3': [0.0], 'LFA_BTN': [0.0], 'CRUISE_BUTTONS': [1.0],
|
||||
# 'NEW_SIGNAL_4': [0.0], 'NORMAL_CRUISE_MAIN_BTN': [0.0], 'NEW_SIGNAL_5': [0.0],
|
||||
# 'SET_ME_2': [2.0], 'NEW_SIGNAL_6': [1.0], 'BYTE6': [32.0], 'BYTE7': [0.0],
|
||||
# 'BYTE8': [26.0], 'BYTE9': [0.0], 'BYTE10': [0.0], 'BYTE11': [0.0], 'BYTE12':
|
||||
# [0.0], 'BYTE13': [0.0], 'BYTE14': [0.0], 'BYTE15': [0.0]}
|
||||
|
||||
# {'CHECKSUM': [14783.0], 'COUNTER': [150.0], 'NEW_SIGNAL_1': [0.0],
|
||||
# 'SET_ME_1': [0.0], 'DISTANCE_UNIT': [1.0], 'NEW_
|
||||
# SIGNAL_2': [6.0], 'ADAPTIVE_CRUISE_MAIN_BTN': [0.0], 'NEW_SIGNAL_3': [0.0],
|
||||
# 'LFA_BTN': [0.0], 'CRUISE_BUTTONS': [1.
|
||||
# 0], 'NEW_SIGNAL_4': [0.0], 'NORMAL_CRUISE_MAIN_BTN': [0.0], 'NEW_SIGNAL_5':
|
||||
# [0.0], 'SET_ME_2': [2.0], 'NEW_SIGNAL_6
|
||||
# ': [1.0], 'BYTE6': [37.0], 'BYTE7': [0.0], 'BYTE8': [30.0], 'BYTE9': [0.0],
|
||||
# 'BYTE10': [0.0], 'BYTE11': [0.0], 'BYTE
|
||||
# 12': [0.0], 'BYTE13': [0.0], 'BYTE14': [0.0], 'BYTE15': [0.0]}
|
||||
|
||||
# {'CHECKSUM': [61602.0], 'COUNTER': [153.0], 'NEW_SIGNAL_1': [0.0], 'SET_ME_1':
|
||||
# [0.0], 'DISTANCE_UNIT': [1.0], 'NEW_
|
||||
# SIGNAL_2': [0.0], 'ADAPTIVE_CRUISE_MAIN_BTN': [0.0], 'NEW_SIGNAL_3': [0.0],
|
||||
# 'LFA_BTN': [1.0], 'CRUISE_BUTTONS': [1.
|
||||
# 0], 'NEW_SIGNAL_4': [0.0], 'NORMAL_CRUISE_MAIN_BTN': [0.0], 'NEW_SIGNAL_5':
|
||||
# [0.0], 'SET_ME_2': [2.0], 'NEW_SIGNAL_6
|
||||
# ': [1.0], 'BYTE6': [38.0], 'BYTE7': [0.0], 'BYTE8': [31.0], 'BYTE9': [0.0],
|
||||
# 'BYTE10': [0.0], 'BYTE11': [0.0], 'BYTE
|
||||
# 12': [0.0], 'BYTE13': [0.0], 'BYTE14': [0.0], 'BYTE15': [0.0]}
|
||||
|
||||
|
||||
def create_buttons_alt(packer, CP, CAN, cnt, btn, template):
|
||||
return
|
||||
params_memory = Params("/dev/shm/params")
|
||||
CarCruiseDisplayActual = params_memory.get_float("CarCruiseDisplayActual")
|
||||
|
||||
values = {
|
||||
"COUNTER": cnt,
|
||||
"NEW_SIGNAL_1": 0.0,
|
||||
"DISTANCE_UNIT": 1.0,
|
||||
"SET_ME_1": 0.0,
|
||||
"NEW_SIGNAL_2": 0.0,
|
||||
"ADAPTIVE_CRUISE_MAIN_BTN": 0.0,
|
||||
"NEW_SIGNAL_3": 0.0,
|
||||
"CRUISE_BUTTONS": 1.0, #btn * 1.0,
|
||||
"NEW_SIGNAL_4": 0.0,
|
||||
"NORMAL_CRUISE_MAIN_BTN": 0.0,
|
||||
"NEW_SIGNAL_5": 0.0,
|
||||
"SET_ME_2": 2.0,
|
||||
"NEW_SIGNAL_5": 1.0,
|
||||
# "BYTE_6": CarCruiseDisplayActual+1, # Target
|
||||
# "BYTE_7": 0.0,
|
||||
# "BYTE_8": CarCruiseDisplayActual, # Current cruise sets
|
||||
# "BYTE_9": 0.0,
|
||||
# "BYTE_10": 0.0,
|
||||
# "BYTE_11": 0.0,
|
||||
# "BYTE_12": 0.0,
|
||||
# "BYTE_13": 0.0,
|
||||
# "BYTE_14": 0.0,
|
||||
# "BYTE_15": 0.0,
|
||||
}
|
||||
|
||||
bus = CAN.ECAN # if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
|
||||
return packer.make_can_msg("CRUISE_BUTTONS_ALT", bus, values)
|
||||
|
||||
|
||||
# def create_buttons_alt(packer, CP, CAN, cnt, btn, template):
|
||||
# template.update({
|
||||
# "CRUISE_BUTTONS": btn
|
||||
# })
|
||||
|
||||
# bus = CAN.ECAN # if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
|
||||
# return packer.make_can_msg("CRUISE_BUTTONS_ALT", bus, template)
|
||||
|
||||
|
||||
def create_acc_set_speed(packer, CP, CAN, cruise_info_copy, speed):
|
||||
# why are we executing this at all?
|
||||
# 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": 1, # testing 1 instead of 4
|
||||
"VSetDis": speed
|
||||
})
|
||||
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
|
||||
|
||||
|
||||
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
|
||||
# This does nothing on the tucson
|
||||
# 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, # testing 1 instead of 4
|
||||
"aReqRaw": 0.0,
|
||||
"aReqValue": 0.0,
|
||||
})
|
||||
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
|
||||
|
||||
# CLEARPILOT changed HDA icons
|
||||
# This doesn't appear to do anything on my tucson
|
||||
def create_lfahda_cluster(packer, CAN, experimental, lat_active):
|
||||
values = {
|
||||
"HDA_ICON": 0, # Literally shows HDA. Maybe we use this to indicate experimental mode
|
||||
"LFA_ICON": 2 if experimental 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_acc_control_alt(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)
|
||||
|
||||
# Disabled blinker messages
|
||||
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
|
||||
ret = []
|
||||
return 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
|
||||
191
selfdrive/car/hyundai/interface.py
Executable file
191
selfdrive/car/hyundai/interface.py
Executable file
@@ -0,0 +1,191 @@
|
||||
from cereal import car, custom
|
||||
from panda import Panda
|
||||
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
|
||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
|
||||
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, Buttons
|
||||
from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
GearShifter = car.CarState.GearShifter
|
||||
ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
|
||||
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
|
||||
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "hyundai"
|
||||
ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
|
||||
|
||||
# These cars have been put into dashcam only due to both a lack of users and test coverage.
|
||||
# These cars likely still work fine. Once a user confirms each car works and a test route is
|
||||
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
|
||||
# FIXME: the Optima Hybrid 2017 uses a different SCC12 checksum
|
||||
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
|
||||
|
||||
hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
|
||||
CAN = CanBus(None, hda2, fingerprint)
|
||||
|
||||
if candidate in CANFD_CAR:
|
||||
# detect 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)
|
||||
|
||||
# *** longitudinal control ***
|
||||
if candidate in CANFD_CAR:
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
|
||||
else:
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
|
||||
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.startingState = True
|
||||
ret.vEgoStarting = 0.1
|
||||
ret.startAccel = 1.0
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.5
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5
|
||||
|
||||
# *** feature detection ***
|
||||
if candidate in CANFD_CAR:
|
||||
ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN]
|
||||
else:
|
||||
ret.enableBsm = 0x58b in fingerprint[0]
|
||||
|
||||
# *** panda safety config ***
|
||||
if candidate in CANFD_CAR:
|
||||
cfgs = [get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd), ]
|
||||
if CAN.ECAN >= 4:
|
||||
cfgs.insert(0, get_safety_config(car.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(car.CarParams.SafetyModel.hyundaiLegacy)]
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundai, 0)]
|
||||
|
||||
if candidate in CAMERA_SCC_CAR:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
|
||||
|
||||
if 0x391 in fingerprint[0]:
|
||||
ret.flags |= HyundaiFlags.CAN_LFA_BTN.value
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN
|
||||
|
||||
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.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
|
||||
ret.flags |= HyundaiFlags.ALT_LIMITS.value
|
||||
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value):
|
||||
addr, bus = 0x7d0, 0
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
|
||||
addr, bus = 0x730, CanBus(CP).ECAN
|
||||
disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
|
||||
|
||||
# for blinkers
|
||||
if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
|
||||
disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
|
||||
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
|
||||
|
||||
# todo: this check probably needs to be removed on other cars
|
||||
# if self.CS.CP.openpilotLongitudinalControl:
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
|
||||
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
|
||||
# Main button also can trigger an engagement on these cars
|
||||
allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
|
||||
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.manumatic],
|
||||
pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
|
||||
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
self.low_speed_alert = True
|
||||
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
|
||||
self.low_speed_alert = False
|
||||
if self.low_speed_alert:
|
||||
events.add(car.CarEvent.EventName.belowSteerSpeed)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
79
selfdrive/car/hyundai/radar_interface.py
Executable file
79
selfdrive/car/hyundai/radar_interface.py
Executable file
@@ -0,0 +1,79 @@
|
||||
import math
|
||||
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
from openpilot.selfdrive.car.hyundai.values import DBC
|
||||
|
||||
RADAR_START_ADDR = 0x500
|
||||
RADAR_MSG_COUNT = 32
|
||||
|
||||
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
|
||||
|
||||
def get_radar_can_parser(CP):
|
||||
if DBC[CP.carFingerprint]['radar'] is None:
|
||||
return None
|
||||
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)]
|
||||
return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
self.updated_messages = set()
|
||||
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
|
||||
self.track_id = 0
|
||||
|
||||
self.radar_off_can = CP.radarUnavailable
|
||||
self.rcp = get_radar_can_parser(CP)
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.radar_off_can or (self.rcp is None):
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
rr = self._update(self.updated_messages)
|
||||
self.updated_messages.clear()
|
||||
|
||||
return rr
|
||||
|
||||
def _update(self, updated_messages):
|
||||
ret = car.RadarData.new_message()
|
||||
if self.rcp is None:
|
||||
return ret
|
||||
|
||||
errors = []
|
||||
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
ret.errors = errors
|
||||
|
||||
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
|
||||
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]
|
||||
|
||||
if addr not in self.pts:
|
||||
self.pts[addr] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[addr].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
|
||||
valid = msg['STATE'] in (3, 4)
|
||||
if valid:
|
||||
azimuth = math.radians(msg['AZIMUTH'])
|
||||
self.pts[addr].measured = True
|
||||
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
|
||||
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
|
||||
self.pts[addr].vRel = msg['REL_SPEED']
|
||||
self.pts[addr].aRel = msg['REL_ACCEL']
|
||||
self.pts[addr].yvRel = float('nan')
|
||||
|
||||
else:
|
||||
del self.pts[addr]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
0
selfdrive/car/hyundai/tests/__init__.py
Executable file
0
selfdrive/car/hyundai/tests/__init__.py
Executable file
22
selfdrive/car/hyundai/tests/print_platform_codes.py
Executable file
22
selfdrive/car/hyundai/tests/print_platform_codes.py
Executable file
@@ -0,0 +1,22 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.hyundai.values import PLATFORM_CODE_ECUS, get_platform_codes
|
||||
from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
|
||||
|
||||
if __name__ == "__main__":
|
||||
for car_model, ecus in FW_VERSIONS.items():
|
||||
print()
|
||||
print(car_model)
|
||||
for ecu in sorted(ecus, key=lambda x: int(x[0])):
|
||||
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_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):')
|
||||
print(f' Codes: {codes}')
|
||||
print(f' Dates: {dates}')
|
||||
224
selfdrive/car/hyundai/tests/test_hyundai.py
Executable file
224
selfdrive/car/hyundai/tests/test_hyundai.py
Executable file
@@ -0,0 +1,224 @@
|
||||
#!/usr/bin/env python3
|
||||
from hypothesis import settings, given, strategies as st
|
||||
import unittest
|
||||
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.fw_versions import build_fw_dict
|
||||
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, \
|
||||
get_platform_codes
|
||||
from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
|
||||
|
||||
# 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.SANTA_CRUZ_1ST_GEN,
|
||||
CAR.TUCSON_4TH_GEN,
|
||||
# CAN
|
||||
CAR.ELANTRA,
|
||||
CAR.ELANTRA_GT_I30,
|
||||
CAR.KIA_CEED,
|
||||
CAR.KIA_FORTE,
|
||||
CAR.KIA_OPTIMA_G4,
|
||||
CAR.KIA_OPTIMA_G4_FL,
|
||||
CAR.KIA_SORENTO,
|
||||
CAR.KONA,
|
||||
CAR.KONA_EV,
|
||||
CAR.KONA_EV_2022,
|
||||
CAR.KONA_HEV,
|
||||
CAR.SONATA_LF,
|
||||
CAR.VELOSTER,
|
||||
}
|
||||
|
||||
|
||||
class TestHyundaiFingerprint(unittest.TestCase):
|
||||
def test_can_features(self):
|
||||
# Test no EV/HEV in any gear lists (should all use ELECT_GEAR)
|
||||
self.assertEqual(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:
|
||||
self.assertNotIn(car_model, can_specific_feature_list, "CAN FD car unexpectedly found in a CAN feature list")
|
||||
|
||||
def test_hybrid_ev_sets(self):
|
||||
self.assertEqual(HYBRID_CAR & EV_CAR, set(), "Shared cars between hybrid and EV")
|
||||
self.assertEqual(CANFD_CAR & HYBRID_CAR, set(), "Hard coding CAN FD cars as hybrid is no longer supported")
|
||||
|
||||
def test_auxiliary_request_ecu_whitelist(self):
|
||||
# Asserts only auxiliary Ecus can exist in database for CAN-FD cars
|
||||
whitelisted_ecus = {ecu for r in FW_QUERY_CONFIG.requests for ecu in r.whitelist_ecus if r.auxiliary}
|
||||
|
||||
for car_model in CANFD_CAR:
|
||||
ecus = {fw[0] for fw in FW_VERSIONS[car_model].keys()}
|
||||
ecus_not_in_whitelist = ecus - whitelisted_ecus
|
||||
ecu_strings = ", ".join([f"Ecu.{ECU_NAME[ecu]}" for ecu in ecus_not_in_whitelist])
|
||||
self.assertEqual(len(ecus_not_in_whitelist), 0,
|
||||
f"{car_model}: Car model has ECUs not in auxiliary request whitelists: {ecu_strings}")
|
||||
|
||||
def test_blacklisted_parts(self):
|
||||
# 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 self.subTest(car_model=car_model.value):
|
||||
if car_model == CAR.SANTA_CRUZ_1ST_GEN:
|
||||
raise unittest.SkipTest("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]
|
||||
self.assertFalse(part.startswith(b'CW'), "Car has bad part number")
|
||||
|
||||
def test_correct_ecu_response_database(self):
|
||||
"""
|
||||
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 self.subTest(car_model=car_model.value):
|
||||
for ecu, fws in ecus.items():
|
||||
# TODO: enable for Ecu.fwdRadar, Ecu.abs, Ecu.eps, Ecu.transmission
|
||||
if ecu[0] in (Ecu.fwdCamera,):
|
||||
self.assertTrue(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):
|
||||
# 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 self.subTest(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.PALISADE:
|
||||
self.assertEqual(codes, {b"LX", b"ON"}, f"Car has unexpected platform codes: {car_model} {codes}")
|
||||
elif car_model == CAR.KONA_EV and ecu[0] == Ecu.fwdCamera:
|
||||
self.assertEqual(codes, {b"OE", b"OS"}, f"Car has unexpected platform codes: {car_model} {codes}")
|
||||
else:
|
||||
self.assertEqual(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):
|
||||
# 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.SONATA_LF, CAR.TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.ELANTRA}
|
||||
|
||||
# Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms
|
||||
for car_model, ecus in FW_VERSIONS.items():
|
||||
with self.subTest(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
|
||||
self.assertIn(platform_code_ecu, [e[0] for e in ecus])
|
||||
|
||||
def test_fw_format(self):
|
||||
# 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 self.subTest(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])
|
||||
self.assertEqual(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:
|
||||
self.assertTrue(all(date is None for _, date in codes))
|
||||
else:
|
||||
self.assertTrue(all(date is not None for _, date in codes))
|
||||
|
||||
if car_model == CAR.HYUNDAI_GENESIS:
|
||||
raise unittest.SkipTest("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"
|
||||
self.assertTrue(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"])
|
||||
self.assertEqual(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 "])
|
||||
self.assertEqual(results, {(b"AEhe-G2000", None)})
|
||||
|
||||
results = get_platform_codes([b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 "])
|
||||
self.assertEqual(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 ",
|
||||
])
|
||||
self.assertEqual(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",
|
||||
])
|
||||
self.assertEqual(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({"ecu": ecu_name, "fwVersion": fw, "address": addr,
|
||||
"subAddress": 0 if sub_addr is None else sub_addr})
|
||||
|
||||
CP = car.CarParams.new_message(carFw=car_fw)
|
||||
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), FW_VERSIONS)
|
||||
if len(matches) == 1:
|
||||
self.assertEqual(list(matches)[0], platform)
|
||||
else:
|
||||
platforms_with_shared_codes.add(platform)
|
||||
|
||||
self.assertEqual(platforms_with_shared_codes, excluded_platforms)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
860
selfdrive/car/hyundai/values.py
Executable file
860
selfdrive/car/hyundai/values.py
Executable file
@@ -0,0 +1,860 @@
|
||||
import re
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from cereal import car
|
||||
from panda.python import uds
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict
|
||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
ACCEL_MIN = -3.5 # m/s
|
||||
ACCEL_MAX = 2.0 # m/s
|
||||
ACCEL_MAX_PLUS = 4.0 # m/s
|
||||
|
||||
def __init__(self, CP):
|
||||
self.STEER_DELTA_UP = 3
|
||||
self.STEER_DELTA_DOWN = 7
|
||||
self.STEER_DRIVER_ALLOWANCE = 50
|
||||
self.STEER_DRIVER_MULTIPLIER = 2
|
||||
self.STEER_DRIVER_FACTOR = 1
|
||||
self.STEER_THRESHOLD = 150
|
||||
self.STEER_STEP = 1 # 100 Hz
|
||||
|
||||
if CP.carFingerprint in CANFD_CAR:
|
||||
self.STEER_MAX = 270
|
||||
self.STEER_DRIVER_ALLOWANCE = 250
|
||||
self.STEER_DRIVER_MULTIPLIER = 2
|
||||
self.STEER_THRESHOLD = 250
|
||||
self.STEER_DELTA_UP = 2
|
||||
self.STEER_DELTA_DOWN = 3
|
||||
|
||||
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
|
||||
# If the max stock LKAS request is <384, add your car to this list.
|
||||
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ,
|
||||
CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV,
|
||||
CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO):
|
||||
self.STEER_MAX = 255
|
||||
|
||||
# these cars have significantly more torque than most HKG; limit to 70% of max
|
||||
elif CP.flags & HyundaiFlags.ALT_LIMITS:
|
||||
self.STEER_MAX = 270
|
||||
self.STEER_DELTA_UP = 2
|
||||
self.STEER_DELTA_DOWN = 3
|
||||
|
||||
# Default for most HKG
|
||||
else:
|
||||
self.STEER_MAX = 384
|
||||
|
||||
|
||||
class HyundaiFlags(IntFlag):
|
||||
# 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
|
||||
|
||||
CAN_LFA_BTN = 2 ** 24
|
||||
|
||||
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: car.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
|
||||
AZERA_6TH_GEN = HyundaiPlatformConfig(
|
||||
"HYUNDAI AZERA 6TH GEN",
|
||||
[HyundaiCarDocs("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))],
|
||||
CarSpecs(mass=1600, wheelbase=2.885, steerRatio=14.5),
|
||||
)
|
||||
AZERA_HEV_6TH_GEN = HyundaiPlatformConfig(
|
||||
"HYUNDAI AZERA HYBRID 6TH GEN",
|
||||
[
|
||||
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,
|
||||
)
|
||||
ELANTRA = HyundaiPlatformConfig(
|
||||
"HYUNDAI ELANTRA 2017",
|
||||
[
|
||||
# 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,
|
||||
)
|
||||
ELANTRA_GT_I30 = HyundaiPlatformConfig(
|
||||
"HYUNDAI I30 N LINE 2019 & GT 2018 DCT",
|
||||
[
|
||||
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])),
|
||||
],
|
||||
ELANTRA.specs,
|
||||
flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH,
|
||||
)
|
||||
ELANTRA_2021 = HyundaiPlatformConfig(
|
||||
"HYUNDAI ELANTRA 2021",
|
||||
[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,
|
||||
)
|
||||
ELANTRA_HEV_2021 = HyundaiPlatformConfig(
|
||||
"HYUNDAI ELANTRA HYBRID 2021",
|
||||
[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(
|
||||
"HYUNDAI GENESIS 2015-2016",
|
||||
[
|
||||
# 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,
|
||||
)
|
||||
IONIQ = HyundaiPlatformConfig(
|
||||
"HYUNDAI IONIQ HYBRID 2017-2019",
|
||||
[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,
|
||||
)
|
||||
IONIQ_HEV_2022 = HyundaiPlatformConfig(
|
||||
"HYUNDAI IONIQ HYBRID 2020-2022",
|
||||
[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,
|
||||
)
|
||||
IONIQ_EV_LTD = HyundaiPlatformConfig(
|
||||
"HYUNDAI IONIQ ELECTRIC LIMITED 2019",
|
||||
[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,
|
||||
)
|
||||
IONIQ_EV_2020 = HyundaiPlatformConfig(
|
||||
"HYUNDAI IONIQ ELECTRIC 2020",
|
||||
[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,
|
||||
)
|
||||
IONIQ_PHEV_2019 = HyundaiPlatformConfig(
|
||||
"HYUNDAI IONIQ PLUG-IN HYBRID 2019",
|
||||
[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,
|
||||
)
|
||||
IONIQ_PHEV = HyundaiPlatformConfig(
|
||||
"HYUNDAI IONIQ PHEV 2020",
|
||||
[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,
|
||||
)
|
||||
KONA = HyundaiPlatformConfig(
|
||||
"HYUNDAI KONA 2020",
|
||||
[HyundaiCarDocs("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b]))],
|
||||
CarSpecs(mass=1275, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385),
|
||||
flags=HyundaiFlags.CLUSTER_GEARS,
|
||||
)
|
||||
KONA_EV = HyundaiPlatformConfig(
|
||||
"HYUNDAI KONA ELECTRIC 2019",
|
||||
[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,
|
||||
)
|
||||
KONA_EV_2022 = HyundaiPlatformConfig(
|
||||
"HYUNDAI KONA ELECTRIC 2022",
|
||||
[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,
|
||||
)
|
||||
KONA_EV_2ND_GEN = HyundaiCanFDPlatformConfig(
|
||||
"HYUNDAI KONA ELECTRIC 2ND GEN",
|
||||
[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,
|
||||
)
|
||||
KONA_HEV = HyundaiPlatformConfig(
|
||||
"HYUNDAI KONA HYBRID 2020",
|
||||
[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,
|
||||
)
|
||||
SANTA_FE = HyundaiPlatformConfig(
|
||||
"HYUNDAI SANTA FE 2019",
|
||||
[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,
|
||||
)
|
||||
SANTA_FE_2022 = HyundaiPlatformConfig(
|
||||
"HYUNDAI SANTA FE 2022",
|
||||
[HyundaiCarDocs("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4",
|
||||
car_parts=CarParts.common([CarHarness.hyundai_l]))],
|
||||
SANTA_FE.specs,
|
||||
flags=HyundaiFlags.CHECKSUM_CRC8,
|
||||
)
|
||||
SANTA_FE_HEV_2022 = HyundaiPlatformConfig(
|
||||
"HYUNDAI SANTA FE HYBRID 2022",
|
||||
[HyundaiCarDocs("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))],
|
||||
SANTA_FE.specs,
|
||||
flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID,
|
||||
)
|
||||
SANTA_FE_PHEV_2022 = HyundaiPlatformConfig(
|
||||
"HYUNDAI SANTA FE PlUG-IN HYBRID 2022",
|
||||
[HyundaiCarDocs("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))],
|
||||
SANTA_FE.specs,
|
||||
flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID,
|
||||
)
|
||||
SONATA = HyundaiPlatformConfig(
|
||||
"HYUNDAI SONATA 2020",
|
||||
[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,
|
||||
)
|
||||
SONATA_LF = HyundaiPlatformConfig(
|
||||
"HYUNDAI SONATA 2019",
|
||||
[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,
|
||||
)
|
||||
STARIA_4TH_GEN = HyundaiCanFDPlatformConfig(
|
||||
"HYUNDAI STARIA 4TH GEN",
|
||||
[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
|
||||
)
|
||||
TUCSON = HyundaiPlatformConfig(
|
||||
"HYUNDAI TUCSON 2019",
|
||||
[
|
||||
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,
|
||||
)
|
||||
PALISADE = HyundaiPlatformConfig(
|
||||
"HYUNDAI PALISADE 2020",
|
||||
[
|
||||
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,
|
||||
)
|
||||
VELOSTER = HyundaiPlatformConfig(
|
||||
"HYUNDAI VELOSTER 2019",
|
||||
[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,
|
||||
)
|
||||
SONATA_HYBRID = HyundaiPlatformConfig(
|
||||
"HYUNDAI SONATA HYBRID 2021",
|
||||
[HyundaiCarDocs("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))],
|
||||
SONATA.specs,
|
||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID,
|
||||
)
|
||||
IONIQ_5 = HyundaiCanFDPlatformConfig(
|
||||
"HYUNDAI IONIQ 5 2022",
|
||||
[
|
||||
HyundaiCarDocs("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])),
|
||||
HyundaiCarDocs("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])),
|
||||
HyundaiCarDocs("Hyundai Ioniq 5 (with HDA II) 2022-23", "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,
|
||||
)
|
||||
IONIQ_6 = HyundaiCanFDPlatformConfig(
|
||||
"HYUNDAI IONIQ 6 2023",
|
||||
[HyundaiCarDocs("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))],
|
||||
IONIQ_5.specs,
|
||||
flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE,
|
||||
)
|
||||
TUCSON_4TH_GEN = HyundaiCanFDPlatformConfig(
|
||||
"HYUNDAI TUCSON 4TH GEN",
|
||||
[
|
||||
HyundaiCarDocs("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])),
|
||||
HyundaiCarDocs("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
|
||||
HyundaiCarDocs("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
|
||||
],
|
||||
CarSpecs(mass=1630, wheelbase=2.756, steerRatio=13.7, tireStiffnessFactor=0.385),
|
||||
)
|
||||
SANTA_CRUZ_1ST_GEN = HyundaiCanFDPlatformConfig(
|
||||
"HYUNDAI SANTA CRUZ 1ST GEN",
|
||||
[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),
|
||||
)
|
||||
CUSTIN_1ST_GEN = HyundaiPlatformConfig(
|
||||
"HYUNDAI CUSTIN 1ST GEN",
|
||||
[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(
|
||||
"KIA FORTE E 2018 & GT 2021",
|
||||
[
|
||||
HyundaiCarDocs("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
|
||||
HyundaiCarDocs("Kia Forte 2023", 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(
|
||||
"KIA K5 2021",
|
||||
[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(
|
||||
"KIA K5 HYBRID 2020",
|
||||
[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(
|
||||
"KIA K8 HYBRID 1ST GEN",
|
||||
[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(
|
||||
"KIA NIRO EV 2020",
|
||||
[
|
||||
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(
|
||||
"KIA NIRO EV 2ND GEN",
|
||||
[HyundaiCarDocs("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))],
|
||||
KIA_NIRO_EV.specs,
|
||||
flags=HyundaiFlags.EV,
|
||||
)
|
||||
KIA_NIRO_PHEV = HyundaiPlatformConfig(
|
||||
"KIA NIRO HYBRID 2019",
|
||||
[
|
||||
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(
|
||||
"KIA NIRO PLUG-IN HYBRID 2022",
|
||||
[
|
||||
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(
|
||||
"KIA NIRO HYBRID 2021",
|
||||
[
|
||||
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(
|
||||
"KIA NIRO HYBRID 2ND GEN",
|
||||
[HyundaiCarDocs("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a]))],
|
||||
KIA_NIRO_EV.specs,
|
||||
)
|
||||
KIA_OPTIMA_G4 = HyundaiPlatformConfig(
|
||||
"KIA OPTIMA 4TH GEN",
|
||||
[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(
|
||||
"KIA OPTIMA 4TH GEN FACELIFT",
|
||||
[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(
|
||||
"KIA OPTIMA HYBRID 2017 & SPORTS 2019",
|
||||
[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(
|
||||
"KIA OPTIMA HYBRID 4TH GEN FACELIFT",
|
||||
[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(
|
||||
"KIA SELTOS 2021",
|
||||
[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(
|
||||
"KIA SPORTAGE 5TH GEN",
|
||||
[
|
||||
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(
|
||||
"KIA SORENTO GT LINE 2018",
|
||||
[
|
||||
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(
|
||||
"KIA SORENTO 4TH GEN",
|
||||
[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(
|
||||
"KIA SORENTO HYBRID 4TH GEN",
|
||||
[
|
||||
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(
|
||||
"KIA STINGER GT2 2018",
|
||||
[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(
|
||||
"KIA STINGER 2022",
|
||||
[HyundaiCarDocs("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))],
|
||||
KIA_STINGER.specs,
|
||||
)
|
||||
KIA_CEED = HyundaiPlatformConfig(
|
||||
"KIA CEED INTRO ED 2019",
|
||||
[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(
|
||||
"KIA EV6 2022",
|
||||
[
|
||||
HyundaiCarDocs("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])),
|
||||
HyundaiCarDocs("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])),
|
||||
HyundaiCarDocs("Kia EV6 (with HDA II) 2022-23", "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(
|
||||
"KIA CARNIVAL 4TH GEN",
|
||||
[
|
||||
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(
|
||||
"GENESIS GV60 ELECTRIC 1ST GEN",
|
||||
[
|
||||
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(
|
||||
"GENESIS G70 2018",
|
||||
[HyundaiCarDocs("Genesis G70 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_f]))],
|
||||
CarSpecs(mass=1640, wheelbase=2.84, steerRatio=13.56),
|
||||
flags=HyundaiFlags.LEGACY,
|
||||
)
|
||||
GENESIS_G70_2020 = HyundaiPlatformConfig(
|
||||
"GENESIS G70 2020",
|
||||
[HyundaiCarDocs("Genesis G70 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_f]))],
|
||||
CarSpecs(mass=3673 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=12.9),
|
||||
flags=HyundaiFlags.MANDO_RADAR,
|
||||
)
|
||||
GENESIS_GV70_1ST_GEN = HyundaiCanFDPlatformConfig(
|
||||
"GENESIS GV70 1ST GEN",
|
||||
[
|
||||
HyundaiCarDocs("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
|
||||
HyundaiCarDocs("Genesis GV70 (3.5T Trim) 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(
|
||||
"GENESIS G80 2017",
|
||||
[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(
|
||||
"GENESIS G90 2017",
|
||||
[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(
|
||||
"GENESIS GV80 2023",
|
||||
[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, 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_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
|
||||
p16(0xf100)
|
||||
|
||||
HYUNDAI_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]
|
||||
|
||||
ALL_HYUNDAI_ECUS = [Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.engine, Ecu.parkingAdas,
|
||||
Ecu.transmission, Ecu.adas, Ecu.hvac, Ecu.cornerRadar, Ecu.combinationMeter]
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[
|
||||
# TODO: minimize shared whitelists for CAN and cornerRadar for CAN-FD
|
||||
# CAN queries (OBD-II port)
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_LONG],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera],
|
||||
),
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_MULTI],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar],
|
||||
),
|
||||
|
||||
# CAN-FD queries (from camera)
|
||||
# TODO: combine shared whitelists with CAN requests
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_LONG],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac, Ecu.eps],
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
),
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_LONG],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac],
|
||||
bus=1,
|
||||
auxiliary=True,
|
||||
obd_multiplexing=False,
|
||||
),
|
||||
|
||||
# CAN & 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],
|
||||
whitelist_ecus=[Ecu.fwdCamera],
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
logging=True,
|
||||
),
|
||||
|
||||
# CAN & CAN FD logging queries (from camera)
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_LONG],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=ALL_HYUNDAI_ECUS,
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
logging=True,
|
||||
),
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_MULTI],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=ALL_HYUNDAI_ECUS,
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
logging=True,
|
||||
),
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_LONG],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=ALL_HYUNDAI_ECUS,
|
||||
bus=1,
|
||||
auxiliary=True,
|
||||
obd_multiplexing=False,
|
||||
logging=True,
|
||||
),
|
||||
|
||||
# CAN-FD alt request logging queries
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_ALT],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac],
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
logging=True,
|
||||
),
|
||||
Request(
|
||||
[HYUNDAI_VERSION_REQUEST_ALT],
|
||||
[HYUNDAI_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac],
|
||||
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.transmission: [CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.PALISADE, CAR.SONATA],
|
||||
Ecu.engine: [CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.PALISADE, CAR.SONATA],
|
||||
Ecu.abs: [CAR.PALISADE, CAR.SONATA],
|
||||
},
|
||||
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()
|
||||
|
||||
if __name__ == "__main__":
|
||||
CAR.print_debug(HyundaiFlags)
|
||||
738
selfdrive/car/interfaces.py
Executable file
738
selfdrive/car/interfaces.py
Executable file
@@ -0,0 +1,738 @@
|
||||
import json
|
||||
import os
|
||||
import numpy as np
|
||||
import tomllib
|
||||
from abc import abstractmethod, ABC
|
||||
from difflib import SequenceMatcher
|
||||
from enum import StrEnum
|
||||
from json import load
|
||||
from typing import Any, Callable, Dict, List, NamedTuple, Optional, Tuple, Union
|
||||
from collections.abc import Callable
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.simple_kalman import KF1D, get_kalman_gain
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
|
||||
from openpilot.selfdrive.car.values import PLATFORMS
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS, V_CRUISE_MAX, get_friction
|
||||
from openpilot.selfdrive.controls.lib.events import Events
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
||||
# Notes:
|
||||
|
||||
# CarInterfaceBase
|
||||
# Has multiple functions. Haven't figured them all out yet.
|
||||
# It turns car state into events.
|
||||
# It passes calls to update() to the specific cars interface.py (get car state)
|
||||
# It passes calls to apply() to the specific cars interface.py (apply car state, gen canbus commands)
|
||||
|
||||
# CarStateBase
|
||||
# carstate is variable (not event) based tracking of the current properties of the car
|
||||
# such as which buttons are pressed or if blinkers are enabled. It uses previous frame
|
||||
# to detect transitions (previous lkas button vs lkas button, if not equal, button just pressed)
|
||||
# - init - defines default values for carstate.
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
|
||||
ACCEL_MAX = 2.0
|
||||
ACCEL_MAX_PLUS = 4.0
|
||||
ACCEL_MIN = -3.5
|
||||
FRICTION_THRESHOLD = 0.3
|
||||
|
||||
TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.toml')
|
||||
TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.toml')
|
||||
TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.toml')
|
||||
TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/lat_models')
|
||||
|
||||
def similarity(s1:str, s2:str) -> float:
|
||||
return SequenceMatcher(None, s1, s2).ratio()
|
||||
|
||||
class LatControlInputs(NamedTuple):
|
||||
lateral_acceleration: float
|
||||
roll_compensation: float
|
||||
vego: float
|
||||
aego: float
|
||||
|
||||
|
||||
TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, car.CarParams.LateralTorqueTuning, float, float, bool, bool], float]
|
||||
|
||||
|
||||
def get_torque_params(candidate):
|
||||
with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f:
|
||||
sub = tomllib.load(f)
|
||||
if candidate in sub:
|
||||
candidate = sub[candidate]
|
||||
|
||||
with open(TORQUE_PARAMS_PATH, 'rb') as f:
|
||||
params = tomllib.load(f)
|
||||
with open(TORQUE_OVERRIDE_PATH, 'rb') as f:
|
||||
override = tomllib.load(f)
|
||||
|
||||
# Ensure no overlap
|
||||
if sum([candidate in x for x in [sub, params, override]]) > 1:
|
||||
raise RuntimeError(f'{candidate} is defined twice in torque config')
|
||||
|
||||
if candidate in override:
|
||||
out = override[candidate]
|
||||
elif candidate in params:
|
||||
out = params[candidate]
|
||||
else:
|
||||
raise NotImplementedError(f"Did not find torque params for {candidate}")
|
||||
return {key: out[i] for i, key in enumerate(params['legend'])}
|
||||
|
||||
|
||||
# Twilsonco's Lateral Neural Network Feedforward
|
||||
class FluxModel:
|
||||
# dict used to rename activation functions whose names aren't valid python identifiers
|
||||
activation_function_names = {'σ': 'sigmoid'}
|
||||
def __init__(self, params_file, zero_bias=False):
|
||||
with open(params_file, "r") as f:
|
||||
params = load(f)
|
||||
|
||||
self.input_size = params["input_size"]
|
||||
self.output_size = params["output_size"]
|
||||
self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
|
||||
self.input_std = np.array(params["input_std"], dtype=np.float32).T
|
||||
self.layers = []
|
||||
|
||||
for layer_params in params["layers"]:
|
||||
W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T
|
||||
b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T
|
||||
if zero_bias:
|
||||
b = np.zeros_like(b)
|
||||
activation = layer_params["activation"]
|
||||
for k, v in self.activation_function_names.items():
|
||||
activation = activation.replace(k, v)
|
||||
self.layers.append((W, b, activation))
|
||||
|
||||
self.validate_layers()
|
||||
self.check_for_friction_override()
|
||||
|
||||
# Begin activation functions.
|
||||
# These are called by name using the keys in the model json file
|
||||
def sigmoid(self, x):
|
||||
return 1 / (1 + np.exp(-x))
|
||||
|
||||
def identity(self, x):
|
||||
return x
|
||||
# End activation functions
|
||||
|
||||
def forward(self, x):
|
||||
for W, b, activation in self.layers:
|
||||
x = getattr(self, activation)(x.dot(W) + b)
|
||||
return x
|
||||
|
||||
def evaluate(self, input_array):
|
||||
in_len = len(input_array)
|
||||
if in_len != self.input_size:
|
||||
# If the input is length 2-4, then it's a simplified evaluation.
|
||||
# In that case, need to add on zeros to fill out the input array to match the correct length.
|
||||
if 2 <= in_len:
|
||||
input_array = input_array + [0] * (self.input_size - in_len)
|
||||
else:
|
||||
raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
|
||||
|
||||
input_array = np.array(input_array, dtype=np.float32)
|
||||
|
||||
# Rescale the input array using the input_mean and input_std
|
||||
input_array = (input_array - self.input_mean) / self.input_std
|
||||
|
||||
output_array = self.forward(input_array)
|
||||
|
||||
return float(output_array[0, 0])
|
||||
|
||||
def validate_layers(self):
|
||||
for W, b, activation in self.layers:
|
||||
if not hasattr(self, activation):
|
||||
raise ValueError(f"Unknown activation: {activation}")
|
||||
|
||||
def check_for_friction_override(self):
|
||||
y = self.evaluate([10.0, 0.0, 0.2])
|
||||
self.friction_override = (y < 0.1)
|
||||
|
||||
def get_nn_model_path(car, eps_firmware) -> Tuple[Union[str, None, float]]:
|
||||
def check_nn_path(check_model):
|
||||
model_path = None
|
||||
max_similarity = -1.0
|
||||
for f in os.listdir(TORQUE_NN_MODEL_PATH):
|
||||
if f.endswith(".json") and car in f:
|
||||
model = f.replace(".json", "").replace(f"{TORQUE_NN_MODEL_PATH}/", "")
|
||||
similarity_score = similarity(model, check_model)
|
||||
if similarity_score > max_similarity:
|
||||
max_similarity = similarity_score
|
||||
model_path = os.path.join(TORQUE_NN_MODEL_PATH, f)
|
||||
return model_path, max_similarity
|
||||
|
||||
if len(eps_firmware) > 3:
|
||||
eps_firmware = eps_firmware.replace("\\", "")
|
||||
check_model = f"{car} {eps_firmware}"
|
||||
else:
|
||||
check_model = car
|
||||
model_path, max_similarity = check_nn_path(check_model)
|
||||
if max_similarity < 0.9:
|
||||
check_model = car
|
||||
model_path, max_similarity = check_nn_path(check_model)
|
||||
if max_similarity < 0.9:
|
||||
model_path = None
|
||||
return model_path, max_similarity
|
||||
|
||||
def get_nn_model(car, eps_firmware) -> Tuple[Union[FluxModel, None, float]]:
|
||||
model, similarity_score = get_nn_model_path(car, eps_firmware)
|
||||
if model is not None:
|
||||
model = FluxModel(model)
|
||||
return model, similarity_score
|
||||
|
||||
# generic car and radar interfaces
|
||||
|
||||
class CarInterfaceBase(ABC):
|
||||
def __init__(self, CP, CarController, CarState):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
eps_firmware = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
|
||||
|
||||
self.frame = 0
|
||||
self.steering_unpressed = 0
|
||||
self.low_speed_alert = False
|
||||
self.no_steer_warning = False
|
||||
self.silent_steer_warning = True
|
||||
self.v_ego_cluster_seen = False
|
||||
|
||||
self.CS = None
|
||||
self.can_parsers = []
|
||||
if CarState is not None:
|
||||
self.CS = CarState(CP)
|
||||
|
||||
self.cp = self.CS.get_can_parser(CP)
|
||||
self.cp_cam = self.CS.get_cam_can_parser(CP)
|
||||
self.cp_adas = self.CS.get_adas_can_parser(CP)
|
||||
self.cp_body = self.CS.get_body_can_parser(CP)
|
||||
self.cp_loopback = self.CS.get_loopback_can_parser(CP)
|
||||
self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback]
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
|
||||
|
||||
# FrogPilot variables
|
||||
self.params = Params()
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
|
||||
lateral_tune = self.params.get_bool("LateralTune")
|
||||
nnff_supported = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware)
|
||||
use_comma_nnff = self.check_comma_nn_ff_support(CP.carFingerprint)
|
||||
self.use_nnff = not use_comma_nnff and nnff_supported and lateral_tune and self.params.get_bool("NNFF")
|
||||
self.use_nnff_lite = not use_comma_nnff and not nnff_supported and lateral_tune and self.params.get_bool("NNFFLite")
|
||||
|
||||
self.belowSteerSpeed_shown = False
|
||||
self.disable_belowSteerSpeed = False
|
||||
self.disable_resumeRequired = False
|
||||
self.prev_distance_button = False
|
||||
self.resumeRequired_shown = False
|
||||
self.traffic_mode_changed = False
|
||||
|
||||
self.gap_counter = 0
|
||||
|
||||
def get_ff_nn(self, x):
|
||||
return self.lat_torque_nn_model.evaluate(x)
|
||||
|
||||
def check_comma_nn_ff_support(self, car):
|
||||
try:
|
||||
with open("/data/openpilot/selfdrive/car/torque_data/neural_ff_weights.json", "r") as file:
|
||||
data = json.load(file)
|
||||
return car in data
|
||||
|
||||
except FileNotFoundError:
|
||||
print("Failed to open neural_ff_weights file.")
|
||||
return False
|
||||
|
||||
def initialize_lat_torque_nn(self, car, eps_firmware):
|
||||
self.lat_torque_nn_model, _ = get_nn_model(car, eps_firmware)
|
||||
return (self.lat_torque_nn_model is not None)
|
||||
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_variables):
|
||||
if frogpilot_variables.sport_plus:
|
||||
return ACCEL_MIN, ACCEL_MAX_PLUS
|
||||
else:
|
||||
return ACCEL_MIN, ACCEL_MAX
|
||||
|
||||
@classmethod
|
||||
def get_non_essential_params(cls, candidate: str):
|
||||
"""
|
||||
Parameters essential to controlling the car may be incomplete or wrong without FW versions or fingerprints.
|
||||
"""
|
||||
return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False, False)
|
||||
|
||||
@classmethod
|
||||
def get_params(cls, params, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], disable_openpilot_long: bool, experimental_long: bool, docs: bool):
|
||||
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, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs)
|
||||
|
||||
# Enable torque controller for all cars that do not use angle based steering
|
||||
if ret.steerControlType != car.CarParams.SteerControlType.angle and params.get_bool("LateralTune") and params.get_bool("NNFF"):
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
eps_firmware = str(next((fw.fwVersion for fw in car_fw if fw.ecu == "eps"), ""))
|
||||
model, similarity_score = get_nn_model_path(candidate, eps_firmware)
|
||||
if model is not None:
|
||||
params.put("NNFFModelName", candidate)
|
||||
|
||||
# Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
|
||||
if not ret.notCar:
|
||||
ret.mass = ret.mass + STD_CARGO_KG
|
||||
|
||||
# Set params dependent on values set by the car interface
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor)
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
@abstractmethod
|
||||
def _get_params(ret: car.CarParams, candidate, fingerprint: dict[int, dict[int, int]],
|
||||
car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool):
|
||||
raise NotImplementedError
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def get_steer_feedforward_default(desired_angle, v_ego):
|
||||
# Proportional to realigning tire momentum: lateral acceleration.
|
||||
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: car.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):
|
||||
ret = car.CarParams.new_message()
|
||||
ret.carFingerprint = candidate
|
||||
|
||||
# Car docs fields
|
||||
ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
|
||||
ret.autoResumeSng = True # describes whether car can resume from a stop automatically
|
||||
|
||||
# standard ALC params
|
||||
ret.tireStiffnessFactor = 1.0
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
ret.minSteerSpeed = 0.
|
||||
ret.wheelSpeedFactor = 1.0
|
||||
|
||||
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
|
||||
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
||||
ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA
|
||||
ret.openpilotLongitudinalControl = False
|
||||
ret.stopAccel = -2.0
|
||||
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
|
||||
ret.vEgoStopping = 0.5
|
||||
ret.vEgoStarting = 0.5
|
||||
ret.stoppingControl = True
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
ret.longitudinalTuning.kf = 1.
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [1.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [1.]
|
||||
# TODO estimate car specific lag, use .15s for now
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.15
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.15
|
||||
ret.steerLimitTimer = 1.0
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
|
||||
params = get_torque_params(candidate)
|
||||
|
||||
tune.init('torque')
|
||||
tune.torque.useSteeringAngle = use_steering_angle
|
||||
tune.torque.kp = 1.0
|
||||
tune.torque.kf = 1.0
|
||||
tune.torque.ki = 0.1
|
||||
tune.torque.friction = params['FRICTION']
|
||||
tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
|
||||
tune.torque.latAccelOffset = 0.0
|
||||
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
|
||||
|
||||
@abstractmethod
|
||||
def _update(self, c: car.CarControl) -> car.CarState:
|
||||
pass
|
||||
|
||||
def update(self, c: car.CarControl, can_strings: list[bytes], frogpilot_variables) -> car.CarState:
|
||||
# parse can
|
||||
for cp in self.can_parsers:
|
||||
if cp is not None:
|
||||
cp.update_strings(can_strings)
|
||||
|
||||
# get CarState
|
||||
ret = self._update(c, frogpilot_variables)
|
||||
|
||||
ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None)
|
||||
ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None)
|
||||
|
||||
if ret.vEgoCluster == 0.0 and not self.v_ego_cluster_seen:
|
||||
ret.vEgoCluster = ret.vEgo
|
||||
else:
|
||||
self.v_ego_cluster_seen = True
|
||||
|
||||
# Many cars apply hysteresis to the ego dash speed
|
||||
if self.CS is not None:
|
||||
ret.vEgoCluster = apply_hysteresis(ret.vEgoCluster, self.CS.out.vEgoCluster, self.CS.cluster_speed_hyst_gap)
|
||||
if abs(ret.vEgo) < self.CS.cluster_min_speed:
|
||||
ret.vEgoCluster = 0.0
|
||||
|
||||
if ret.cruiseState.speedCluster == 0:
|
||||
ret.cruiseState.speedCluster = ret.cruiseState.speed
|
||||
|
||||
distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed")
|
||||
self.params_memory.put_bool("DistanceLongPressed", self.frogpilot_distance_functions(distance_button, self.prev_distance_button, frogpilot_variables))
|
||||
self.prev_distance_button = distance_button
|
||||
|
||||
# copy back for next iteration
|
||||
reader = ret.as_reader()
|
||||
if self.CS is not None:
|
||||
self.CS.out = reader
|
||||
|
||||
return reader
|
||||
|
||||
@abstractmethod
|
||||
def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[bytes]]:
|
||||
pass
|
||||
|
||||
def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True,
|
||||
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
|
||||
events = Events()
|
||||
|
||||
if cs_out.doorOpen:
|
||||
events.add(EventName.doorOpen)
|
||||
if cs_out.seatbeltUnlatched:
|
||||
events.add(EventName.seatbeltNotLatched)
|
||||
if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
|
||||
cs_out.gearShifter not in extra_gears):
|
||||
events.add(EventName.wrongGear)
|
||||
if cs_out.gearShifter == GearShifter.reverse:
|
||||
events.add(EventName.reverseGear)
|
||||
if not cs_out.cruiseState.available:
|
||||
events.add(EventName.wrongCarMode)
|
||||
if cs_out.espDisabled:
|
||||
events.add(EventName.espDisabled)
|
||||
if cs_out.stockFcw:
|
||||
events.add(EventName.stockFcw)
|
||||
if cs_out.stockAeb:
|
||||
events.add(EventName.stockAeb)
|
||||
if cs_out.vEgo > MAX_CTRL_SPEED:
|
||||
events.add(EventName.speedTooHigh)
|
||||
if cs_out.cruiseState.nonAdaptive:
|
||||
events.add(EventName.wrongCruiseMode)
|
||||
if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
|
||||
events.add(EventName.brakeHold)
|
||||
if cs_out.parkingBrake:
|
||||
events.add(EventName.parkBrake)
|
||||
if cs_out.accFaulted:
|
||||
events.add(EventName.accFaulted)
|
||||
if cs_out.steeringPressed:
|
||||
events.add(EventName.steerOverride)
|
||||
|
||||
# Handle button presses
|
||||
for b in cs_out.buttonEvents:
|
||||
# Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
|
||||
if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
|
||||
events.add(EventName.buttonEnable)
|
||||
# Disable on rising and falling edge of cancel for both stock and OP long
|
||||
if b.type == ButtonType.cancel:
|
||||
events.add(EventName.buttonCancel)
|
||||
|
||||
# Handle permanent and temporary steering faults
|
||||
self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
|
||||
if cs_out.steerFaultTemporary:
|
||||
if cs_out.steeringPressed and (not self.CS.out.steerFaultTemporary or self.no_steer_warning):
|
||||
self.no_steer_warning = True
|
||||
else:
|
||||
self.no_steer_warning = False
|
||||
|
||||
# if the user overrode recently, show a less harsh alert
|
||||
if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
|
||||
self.silent_steer_warning = True
|
||||
events.add(EventName.steerTempUnavailableSilent)
|
||||
else:
|
||||
events.add(EventName.steerTempUnavailable)
|
||||
else:
|
||||
self.no_steer_warning = False
|
||||
self.silent_steer_warning = False
|
||||
if cs_out.steerFaultPermanent:
|
||||
events.add(EventName.steerUnavailable)
|
||||
|
||||
# we engage when pcm is active (rising edge)
|
||||
# enabling can optionally be blocked by the car interface
|
||||
if pcm_enable:
|
||||
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
|
||||
events.add(EventName.pcmEnable)
|
||||
elif not cs_out.cruiseState.enabled:
|
||||
events.add(EventName.pcmDisable)
|
||||
|
||||
return events
|
||||
|
||||
def frogpilot_distance_functions(self, distance_button, prev_distance_button, frogpilot_variables):
|
||||
if distance_button:
|
||||
self.gap_counter += 1
|
||||
elif not prev_distance_button:
|
||||
self.gap_counter = 0
|
||||
|
||||
if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance or self.traffic_mode_changed:
|
||||
if frogpilot_variables.conditional_experimental_mode:
|
||||
conditional_status = self.params_memory.get_int("CEStatus")
|
||||
override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2
|
||||
self.params_memory.put_int("CEStatus", override_value)
|
||||
else:
|
||||
experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
self.params.put_bool("ExperimentalMode", not experimental_mode)
|
||||
self.traffic_mode_changed = False
|
||||
|
||||
if self.gap_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode:
|
||||
traffic_mode = self.params_memory.get_bool("TrafficModeActive")
|
||||
self.params_memory.put_bool("TrafficModeActive", not traffic_mode)
|
||||
self.traffic_mode_changed = frogpilot_variables.experimental_mode_via_distance
|
||||
|
||||
return self.gap_counter >= CRUISE_LONG_PRESS
|
||||
|
||||
class RadarInterfaceBase(ABC):
|
||||
def __init__(self, CP):
|
||||
self.rcp = None
|
||||
self.pts = {}
|
||||
self.delay = 0
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
self.frame = 0
|
||||
|
||||
def update(self, can_strings):
|
||||
self.frame += 1
|
||||
if (self.frame % int(100 * self.radar_ts)) == 0:
|
||||
return car.RadarData.new_message()
|
||||
return None
|
||||
|
||||
|
||||
class CarStateBase(ABC):
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.out = car.CarState.new_message()
|
||||
|
||||
self.cruise_buttons = 0
|
||||
self.left_blinker_cnt = 0
|
||||
self.right_blinker_cnt = 0
|
||||
self.steering_pressed_cnt = 0
|
||||
self.left_blinker_prev = False
|
||||
self.right_blinker_prev = False
|
||||
self.cluster_speed_hyst_gap = 0.0
|
||||
self.cluster_min_speed = 0.0 # min speed before dropping to 0
|
||||
|
||||
Q = [[0.0, 0.0], [0.0, 100.0]]
|
||||
R = 0.3
|
||||
A = [[1.0, DT_CTRL], [0.0, 1.0]]
|
||||
C = [[1.0, 0.0]]
|
||||
x0=[[0.0], [0.0]]
|
||||
K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
|
||||
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
|
||||
|
||||
# FrogPilot variables
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
|
||||
self.cruise_decreased = False
|
||||
self.cruise_decreased_previously = False
|
||||
self.cruise_increased = False
|
||||
self.cruise_increased_previously = False
|
||||
self.lkas_enabled = False
|
||||
self.lkas_previously_enabled = False
|
||||
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
def update_speed_kf(self, v_ego_raw):
|
||||
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.set_x([[v_ego_raw], [0.0]])
|
||||
|
||||
v_ego_x = self.v_ego_kf.update(v_ego_raw)
|
||||
return float(v_ego_x[0]), float(v_ego_x[1])
|
||||
|
||||
def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS):
|
||||
factor = unit * self.CP.wheelSpeedFactor
|
||||
|
||||
wheelSpeeds = car.CarState.WheelSpeeds.new_message()
|
||||
wheelSpeeds.fl = fl * factor
|
||||
wheelSpeeds.fr = fr * factor
|
||||
wheelSpeeds.rl = rl * factor
|
||||
wheelSpeeds.rr = rr * factor
|
||||
return wheelSpeeds
|
||||
|
||||
def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
|
||||
"""Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
|
||||
iterations"""
|
||||
# TODO: Handle case when switching direction. Now both blinkers can be on at the same time
|
||||
self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0)
|
||||
self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0)
|
||||
return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0
|
||||
|
||||
def update_steering_pressed(self, steering_pressed, steering_pressed_min_count):
|
||||
"""Applies filtering on steering pressed for noisy driver torque signals."""
|
||||
self.steering_pressed_cnt += 1 if steering_pressed else -1
|
||||
self.steering_pressed_cnt = clip(self.steering_pressed_cnt, 0, steering_pressed_min_count * 2)
|
||||
return self.steering_pressed_cnt > steering_pressed_min_count
|
||||
|
||||
def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool):
|
||||
"""Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time,
|
||||
or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker
|
||||
is forced to the other side. On a rising edge of the stalk the timeout is reset."""
|
||||
|
||||
if left_blinker_stalk:
|
||||
self.right_blinker_cnt = 0
|
||||
if not self.left_blinker_prev:
|
||||
self.left_blinker_cnt = blinker_time
|
||||
|
||||
if right_blinker_stalk:
|
||||
self.left_blinker_cnt = 0
|
||||
if not self.right_blinker_prev:
|
||||
self.right_blinker_cnt = blinker_time
|
||||
|
||||
self.left_blinker_cnt = max(self.left_blinker_cnt - 1, 0)
|
||||
self.right_blinker_cnt = max(self.right_blinker_cnt - 1, 0)
|
||||
|
||||
self.left_blinker_prev = left_blinker_stalk
|
||||
self.right_blinker_prev = right_blinker_stalk
|
||||
|
||||
return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
|
||||
|
||||
@staticmethod
|
||||
def parse_gear_shifter(gear: str | None) -> car.CarState.GearShifter:
|
||||
if gear is None:
|
||||
return GearShifter.unknown
|
||||
|
||||
d: dict[str, car.CarState.GearShifter] = {
|
||||
'P': GearShifter.park, 'PARK': GearShifter.park,
|
||||
'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse,
|
||||
'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral,
|
||||
'E': GearShifter.eco, 'ECO': GearShifter.eco,
|
||||
'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic,
|
||||
'D': GearShifter.drive, 'DRIVE': GearShifter.drive,
|
||||
'S': GearShifter.sport, 'SPORT': GearShifter.sport,
|
||||
'L': GearShifter.low, 'LOW': GearShifter.low,
|
||||
'B': GearShifter.brake, 'BRAKE': GearShifter.brake,
|
||||
}
|
||||
return d.get(gear.upper(), GearShifter.unknown)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def get_adas_can_parser(CP):
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def get_body_can_parser(CP):
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def get_loopback_can_parser(CP):
|
||||
return None
|
||||
|
||||
|
||||
SendCan = tuple[int, int, bytes, int]
|
||||
|
||||
|
||||
class CarControllerBase(ABC):
|
||||
@abstractmethod
|
||||
def update(self, CC, CS, now_nanos) -> tuple[car.CarControl.Actuators, list[SendCan]]:
|
||||
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 + '/selfdrive/car')]):
|
||||
try:
|
||||
brand_name = car_folder.split('/')[-1]
|
||||
brand_values = __import__(f'openpilot.selfdrive.car.{brand_name}.{INTERFACE_ATTR_FILE.get(attr, "values")}', fromlist=[attr])
|
||||
if hasattr(brand_values, attr) or not ignore_none:
|
||||
attr_data = getattr(brand_values, attr, None)
|
||||
else:
|
||||
continue
|
||||
|
||||
if combine_brands:
|
||||
if isinstance(attr_data, dict):
|
||||
for f, v in attr_data.items():
|
||||
result[f] = v
|
||||
else:
|
||||
result[brand_name] = attr_data
|
||||
except (ImportError, OSError):
|
||||
pass
|
||||
|
||||
return result
|
||||
|
||||
|
||||
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
|
||||
173
selfdrive/car/isotp_parallel_query.py
Executable file
173
selfdrive/car/isotp_parallel_query.py
Executable file
@@ -0,0 +1,173 @@
|
||||
import time
|
||||
from collections import defaultdict
|
||||
from functools import partial
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from 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, sendcan: messaging.PubSocket, logcan: messaging.SubSocket, 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.sendcan = sendcan
|
||||
self.logcan = logcan
|
||||
self.bus = bus
|
||||
self.request = request
|
||||
self.response = response
|
||||
self.functional_addrs = functional_addrs or []
|
||||
self.debug = debug
|
||||
self.response_pending_timeout = response_pending_timeout
|
||||
|
||||
real_addrs = [a if isinstance(a, tuple) else (a, None) for a in addrs]
|
||||
for tx_addr, _ in real_addrs:
|
||||
assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}"
|
||||
|
||||
self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in real_addrs}
|
||||
self.msg_buffer: dict[int, list[tuple[int, int, bytes, int]]] = defaultdict(list)
|
||||
|
||||
def rx(self):
|
||||
"""Drain can socket and sort messages into buffers based on address"""
|
||||
can_packets = messaging.drain_sock(self.logcan, wait_for_one=True)
|
||||
|
||||
for packet in can_packets:
|
||||
for msg in packet.can:
|
||||
if msg.src == self.bus and msg.address in self.msg_addrs.values():
|
||||
self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
|
||||
|
||||
def _can_tx(self, tx_addr, dat, bus):
|
||||
"""Helper function to send single message"""
|
||||
msg = [tx_addr, 0, dat, bus]
|
||||
self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan'))
|
||||
|
||||
def _can_rx(self, addr, sub_addr=None):
|
||||
"""Helper function to retrieve message with specified address and subadress from buffer"""
|
||||
keep_msgs = []
|
||||
|
||||
if sub_addr is None:
|
||||
msgs = self.msg_buffer[addr]
|
||||
else:
|
||||
# Filter based on subadress
|
||||
msgs = []
|
||||
for m in self.msg_buffer[addr]:
|
||||
first_byte = m[2][0]
|
||||
if first_byte == sub_addr:
|
||||
msgs.append(m)
|
||||
else:
|
||||
keep_msgs.append(m)
|
||||
|
||||
self.msg_buffer[addr] = keep_msgs
|
||||
return msgs
|
||||
|
||||
def _drain_rx(self):
|
||||
messaging.drain_sock_raw(self.logcan)
|
||||
self.msg_buffer = defaultdict(list)
|
||||
|
||||
def _create_isotp_msg(self, tx_addr: 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:
|
||||
cloudlog.exception(f"Error processing UDS response: {tx_addr}")
|
||||
request_done[tx_addr] = True
|
||||
continue
|
||||
|
||||
# Extend timeout for each consecutive ISO-TP frame to avoid timing out on long responses
|
||||
if rx_in_progress:
|
||||
addrs_responded.add(tx_addr)
|
||||
response_timeouts[tx_addr] = time.monotonic() + timeout
|
||||
|
||||
if dat is None:
|
||||
continue
|
||||
|
||||
# Log unexpected empty responses
|
||||
if len(dat) == 0:
|
||||
cloudlog.error(f"iso-tp query empty response: {tx_addr}")
|
||||
request_done[tx_addr] = True
|
||||
continue
|
||||
|
||||
counter = request_counter[tx_addr]
|
||||
expected_response = self.response[counter]
|
||||
response_valid = dat.startswith(expected_response)
|
||||
|
||||
if response_valid:
|
||||
if counter + 1 < len(self.request):
|
||||
response_timeouts[tx_addr] = time.monotonic() + timeout
|
||||
msg.send(self.request[counter + 1])
|
||||
request_counter[tx_addr] += 1
|
||||
else:
|
||||
results[tx_addr] = dat[len(expected_response):]
|
||||
request_done[tx_addr] = True
|
||||
else:
|
||||
error_code = dat[2] if len(dat) > 2 else -1
|
||||
if error_code == 0x78:
|
||||
response_timeouts[tx_addr] = time.monotonic() + self.response_pending_timeout
|
||||
cloudlog.error(f"iso-tp query response pending: {tx_addr}")
|
||||
else:
|
||||
request_done[tx_addr] = True
|
||||
cloudlog.error(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}")
|
||||
|
||||
# Mark request done if address timed out
|
||||
cur_time = time.monotonic()
|
||||
for tx_addr in response_timeouts:
|
||||
if cur_time - response_timeouts[tx_addr] > 0:
|
||||
if not request_done[tx_addr]:
|
||||
if request_counter[tx_addr] > 0:
|
||||
cloudlog.error(f"iso-tp query timeout after receiving partial response: {tx_addr}")
|
||||
elif tx_addr in addrs_responded:
|
||||
cloudlog.error(f"iso-tp query timeout while receiving response: {tx_addr}")
|
||||
# TODO: handle functional addresses
|
||||
# else:
|
||||
# cloudlog.error(f"iso-tp query timeout with no response: {tx_addr}")
|
||||
request_done[tx_addr] = True
|
||||
|
||||
# Break if all requests are done (finished or timed out)
|
||||
if all(request_done.values()):
|
||||
break
|
||||
|
||||
if cur_time - start_time > total_timeout:
|
||||
cloudlog.error("iso-tp query timeout while receiving data")
|
||||
break
|
||||
|
||||
return results
|
||||
0
selfdrive/car/mazda/__init__.py
Executable file
0
selfdrive/car/mazda/__init__.py
Executable file
66
selfdrive/car/mazda/carcontroller.py
Executable file
66
selfdrive/car/mazda/carcontroller.py
Executable file
@@ -0,0 +1,66 @@
|
||||
from cereal import car
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.car.mazda import mazdacan
|
||||
from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.brake_counter = 0
|
||||
self.frame = 0
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
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 = CC.actuators.copy()
|
||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
|
||||
new_actuators.steerOutputCan = apply_steer
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
156
selfdrive/car/mazda/carstate.py
Executable file
156
selfdrive/car/mazda/carstate.py
Executable file
@@ -0,0 +1,156 @@
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags
|
||||
|
||||
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.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
def update(self, cp, cp_cam, frogpilot_variables):
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.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
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
self.lkas_enabled = not self.lkas_disabled
|
||||
|
||||
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)
|
||||
262
selfdrive/car/mazda/fingerprints.py
Executable file
262
selfdrive/car/mazda/fingerprints.py
Executable file
@@ -0,0 +1,262 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.mazda.values import CAR
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.CX5_2022: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PEW5-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PW67-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2H-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX85-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXFG-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'SH54-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'KGWD-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-U\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'PG69-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'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'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.CX5: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KCB8-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KJ01-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KJ01-3210X-M-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PA53-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PAR4-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2E-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2F-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2H-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2H-188K2-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX2K-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX68-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFA-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'SHKT-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KL2K-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KN0W-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'B61L-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PA66-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX39-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX39-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB1-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.CX9: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KJ01-3210X-L-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PX23-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX24-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXM4-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXN8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXN8-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYD8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFM-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'PXM4-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYD6-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYD6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYFM-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.MAZDA3: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYJW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYKE-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'B63C-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'B45A-437AS-0-08\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'B61L-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-Q\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'P52G-21PS1-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PY2S-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYKA-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYKE-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYKE-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.MAZDA6: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GBEF-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GFBC-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PA34-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PX4F-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYH7-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GBVH-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GDDM-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'PA28-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYH3-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.CX9_2021: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PXGW-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXGW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXM4-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PXM6-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
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',
|
||||
],
|
||||
(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',
|
||||
],
|
||||
},
|
||||
}
|
||||
57
selfdrive/car/mazda/interface.py
Executable file
57
selfdrive/car/mazda/interface.py
Executable file
@@ -0,0 +1,57 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car, custom
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "mazda"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
|
||||
ret.radarUnavailable = True
|
||||
|
||||
ret.dashcamOnly = False
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerLimitTimer = 0.8
|
||||
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
if candidate not in (CAR.CX5_2022, ):
|
||||
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
|
||||
|
||||
# TODO: add button types for inc and dec
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
# events
|
||||
events = self.create_common_events(ret)
|
||||
|
||||
if self.CS.lkas_disabled:
|
||||
events.add(EventName.lkasDisabled)
|
||||
elif self.CS.low_speed_alert:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
128
selfdrive/car/mazda/mazdacan.py
Executable file
128
selfdrive/car/mazda/mazdacan.py
Executable file
@@ -0,0 +1,128 @@
|
||||
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)
|
||||
5
selfdrive/car/mazda/radar_interface.py
Executable file
5
selfdrive/car/mazda/radar_interface.py
Executable file
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
110
selfdrive/car/mazda/values.py
Executable file
110
selfdrive/car/mazda/values.py
Executable file
@@ -0,0 +1,110 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict
|
||||
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
||||
# Steer torque limits
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_MAX = 800 # theoretical max_steer 2047
|
||||
STEER_DELTA_UP = 10 # torque increase per refresh
|
||||
STEER_DELTA_DOWN = 25 # torque decrease per refresh
|
||||
STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting
|
||||
STEER_DRIVER_MULTIPLIER = 1 # weight driver torque
|
||||
STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
STEER_STEP = 1 # 100 Hz
|
||||
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
|
||||
@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):
|
||||
CX5 = MazdaPlatformConfig(
|
||||
"MAZDA CX-5",
|
||||
[MazdaCarDocs("Mazda CX-5 2017-21")],
|
||||
MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5)
|
||||
)
|
||||
CX9 = MazdaPlatformConfig(
|
||||
"MAZDA CX-9",
|
||||
[MazdaCarDocs("Mazda CX-9 2016-20")],
|
||||
MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6)
|
||||
)
|
||||
MAZDA3 = MazdaPlatformConfig(
|
||||
"MAZDA 3",
|
||||
[MazdaCarDocs("Mazda 3 2017-18")],
|
||||
MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0)
|
||||
)
|
||||
MAZDA6 = MazdaPlatformConfig(
|
||||
"MAZDA 6",
|
||||
[MazdaCarDocs("Mazda 6 2017-20")],
|
||||
MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5)
|
||||
)
|
||||
CX9_2021 = MazdaPlatformConfig(
|
||||
"MAZDA CX-9 2021",
|
||||
[MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")],
|
||||
CX9.specs
|
||||
)
|
||||
CX5_2022 = MazdaPlatformConfig(
|
||||
"MAZDA CX-5 2022",
|
||||
[MazdaCarDocs("Mazda CX-5 2022-24")],
|
||||
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()
|
||||
0
selfdrive/car/mock/__init__.py
Executable file
0
selfdrive/car/mock/__init__.py
Executable file
35
selfdrive/car/mock/interface.py
Executable file
35
selfdrive/car/mock/interface.py
Executable file
@@ -0,0 +1,35 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
# mocked car interface for dashcam mode
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController, CarState):
|
||||
super().__init__(CP, CarController, CarState)
|
||||
|
||||
self.speed = 0.
|
||||
self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "mock"
|
||||
ret.mass = 1700.
|
||||
ret.wheelbase = 2.70
|
||||
ret.centerToFront = ret.wheelbase * 0.5
|
||||
ret.steerRatio = 13.
|
||||
return ret
|
||||
|
||||
def _update(self, c, frogpilot_variables):
|
||||
self.sm.update(0)
|
||||
gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
ret.vEgo = self.sm[gps_sock].speed
|
||||
ret.vEgoRaw = self.sm[gps_sock].speed
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
return actuators, []
|
||||
4
selfdrive/car/mock/radar_interface.py
Executable file
4
selfdrive/car/mock/radar_interface.py
Executable file
@@ -0,0 +1,4 @@
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
10
selfdrive/car/mock/values.py
Executable file
10
selfdrive/car/mock/values.py
Executable file
@@ -0,0 +1,10 @@
|
||||
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
MOCK = PlatformConfig(
|
||||
'mock',
|
||||
[],
|
||||
CarSpecs(mass=1700, wheelbase=2.7, steerRatio=13),
|
||||
{}
|
||||
)
|
||||
0
selfdrive/car/nissan/__init__.py
Executable file
0
selfdrive/car/nissan/__init__.py
Executable file
82
selfdrive/car/nissan/carcontroller.py
Executable file
82
selfdrive/car/nissan/carcontroller.py
Executable file
@@ -0,0 +1,82 @@
|
||||
from cereal import car
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_std_steer_angle_limits
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.car.nissan import nissancan
|
||||
from openpilot.selfdrive.car.nissan.values import CAR, CarControllerParams
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.frame = 0
|
||||
|
||||
self.lkas_max_torque = 0
|
||||
self.apply_angle_last = 0
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
can_sends = []
|
||||
|
||||
### STEER ###
|
||||
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
|
||||
if CC.latActive:
|
||||
# windup slower
|
||||
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CarControllerParams)
|
||||
|
||||
# Max torque from driver before EPS will give up and not apply torque
|
||||
if not bool(CS.out.steeringPressed):
|
||||
self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE
|
||||
else:
|
||||
# Scale max torque based on how much torque the driver is applying to the wheel
|
||||
self.lkas_max_torque = max(
|
||||
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum
|
||||
CarControllerParams.LKAS_MAX_TORQUE * 0.5,
|
||||
# Start scaling torque at STEER_THRESHOLD
|
||||
CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD)
|
||||
)
|
||||
|
||||
else:
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
self.lkas_max_torque = 0
|
||||
|
||||
self.apply_angle_last = apply_angle
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd:
|
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg))
|
||||
|
||||
# TODO: Find better way to cancel!
|
||||
# For some reason spamming the cancel button is unreliable on the Leaf
|
||||
# We now cancel by making propilot think the seatbelt is unlatched,
|
||||
# this generates a beep and a warning message every time you disengage
|
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0:
|
||||
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd))
|
||||
|
||||
can_sends.append(nissancan.create_steering_control(
|
||||
self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque))
|
||||
|
||||
# Below are the HUD messages. We copy the stock message and modify
|
||||
if self.CP.carFingerprint != CAR.ALTIMA:
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive))
|
||||
|
||||
if self.frame % 50 == 0:
|
||||
can_sends.append(nissancan.create_lkas_hud_info_msg(
|
||||
self.packer, CS.lkas_hud_info_msg, steer_hud_alert
|
||||
))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_angle
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
198
selfdrive/car/nissan/carstate.py
Executable file
198
selfdrive/car/nissan/carstate.py
Executable file
@@ -0,0 +1,198 @@
|
||||
import copy
|
||||
from collections import deque
|
||||
from cereal import car
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
|
||||
|
||||
TORQUE_SAMPLES = 12
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
|
||||
self.lkas_hud_msg = {}
|
||||
self.lkas_hud_info_msg = {}
|
||||
|
||||
self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES)
|
||||
self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
|
||||
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
def update(self, cp, cp_adas, cp_cam, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.prev_distance_button = self.distance_button
|
||||
self.distance_button = cp.vl["CRUISE_THROTTLE"]["FOLLOW_DISTANCE_BUTTON"]
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
|
||||
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
|
||||
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
|
||||
ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"]
|
||||
|
||||
ret.gasPressed = bool(ret.gas > 3)
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
|
||||
ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"])
|
||||
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
|
||||
ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"])
|
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"],
|
||||
cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"],
|
||||
cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"],
|
||||
cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"],
|
||||
)
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0
|
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA:
|
||||
ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"])
|
||||
else:
|
||||
ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"])
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL):
|
||||
ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0
|
||||
ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"])
|
||||
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
|
||||
if self.CP.carFingerprint == CAR.LEAF:
|
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0
|
||||
elif self.CP.carFingerprint == CAR.LEAF_IC:
|
||||
ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1
|
||||
ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"])
|
||||
elif self.CP.carFingerprint == CAR.ALTIMA:
|
||||
ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0
|
||||
ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"])
|
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA:
|
||||
speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"]
|
||||
else:
|
||||
speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"]
|
||||
|
||||
if speed != 255:
|
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
|
||||
conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS
|
||||
else:
|
||||
conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = speed * conversion
|
||||
ret.cruiseState.speedCluster = (speed - 1) * conversion # Speed on HUD is always 1 lower than actually sent on can bus
|
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA:
|
||||
ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
|
||||
else:
|
||||
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
|
||||
|
||||
self.steeringTorqueSamples.append(ret.steeringTorque)
|
||||
# Filtering driver torque to prevent steeringPressed false positives
|
||||
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD)
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
|
||||
|
||||
ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
|
||||
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])
|
||||
|
||||
ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"],
|
||||
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"],
|
||||
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FL"]])
|
||||
|
||||
ret.espDisabled = bool(cp.vl["ESP"]["ESP_DISABLED"])
|
||||
|
||||
can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
if self.CP.carFingerprint == CAR.ALTIMA:
|
||||
self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"])
|
||||
else:
|
||||
self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"])
|
||||
|
||||
self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"])
|
||||
|
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
|
||||
self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"])
|
||||
|
||||
if self.CP.carFingerprint != CAR.ALTIMA:
|
||||
self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"])
|
||||
self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"])
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
# sig_address, frequency
|
||||
("STEER_ANGLE_SENSOR", 100),
|
||||
("WHEEL_SPEEDS_REAR", 50),
|
||||
("WHEEL_SPEEDS_FRONT", 50),
|
||||
("ESP", 25),
|
||||
("GEARBOX", 25),
|
||||
("DOORS_LIGHTS", 10),
|
||||
("LIGHTS", 10),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
|
||||
messages += [
|
||||
("GAS_PEDAL", 100),
|
||||
("CRUISE_THROTTLE", 50),
|
||||
("HUD", 25),
|
||||
]
|
||||
|
||||
elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
|
||||
messages += [
|
||||
("BRAKE_PEDAL", 100),
|
||||
("CRUISE_THROTTLE", 50),
|
||||
("CANCEL_MSG", 50),
|
||||
("HUD_SETTINGS", 25),
|
||||
("SEATBELT", 10),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.ALTIMA:
|
||||
messages += [
|
||||
("CRUISE_STATE", 10),
|
||||
("LKAS_SETTINGS", 10),
|
||||
("PROPILOT_HUD", 50),
|
||||
]
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1)
|
||||
|
||||
messages.append(("STEER_TORQUE_SENSOR", 100))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_adas_can_parser(CP):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
|
||||
if CP.carFingerprint == CAR.ALTIMA:
|
||||
messages = [
|
||||
("LKAS", 100),
|
||||
("PRO_PILOT", 100),
|
||||
]
|
||||
else:
|
||||
messages = [
|
||||
("PROPILOT_HUD_INFO_MSG", 2),
|
||||
("LKAS_SETTINGS", 10),
|
||||
("CRUISE_STATE", 50),
|
||||
("PROPILOT_HUD", 50),
|
||||
("LKAS", 100),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
messages = []
|
||||
|
||||
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL):
|
||||
messages.append(("PRO_PILOT", 100))
|
||||
elif CP.carFingerprint == CAR.ALTIMA:
|
||||
messages.append(("STEER_TORQUE_SENSOR", 100))
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1)
|
||||
119
selfdrive/car/nissan/fingerprints.py
Executable file
119
selfdrive/car/nissan/fingerprints.py
Executable file
@@ -0,0 +1,119 @@
|
||||
# ruff: noqa: E501
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.nissan.values import CAR
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.XTRAIL: [{
|
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
},
|
||||
{
|
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3, 1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.LEAF: [{
|
||||
2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8
|
||||
},
|
||||
{
|
||||
2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8
|
||||
}],
|
||||
CAR.LEAF_IC: [{
|
||||
2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8
|
||||
}],
|
||||
CAR.ROGUE: [{
|
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
}],
|
||||
CAR.ALTIMA: [{
|
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 438: 8, 451: 8, 517: 8, 520: 2, 522: 8, 523: 6, 539: 8, 541: 7, 542: 8, 543: 8, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 570: 8, 576: 8, 577: 8, 582: 8, 583: 8, 584: 8, 586: 8, 587: 8, 588: 8, 589: 8, 590: 8, 591: 8, 592: 8, 600: 8, 601: 8, 610: 8, 611: 8, 612: 8, 614: 8, 615: 8, 616: 8, 617: 8, 622: 8, 623: 8, 634: 7, 638: 8, 645: 8, 648: 5, 654: 6, 658: 8, 659: 8, 660: 8, 661: 8, 665: 8, 666: 8, 674: 2, 675: 8, 676: 8, 682: 8, 683: 8, 684: 8, 685: 8, 686: 8, 687: 8, 689: 8, 690: 8, 703: 8, 708: 7, 709: 7, 711: 7, 712: 7, 713: 7, 714: 8, 715: 8, 716: 8, 717: 7, 718: 7, 719: 7, 720: 7, 723: 8, 726: 7, 727: 7, 728: 7, 735: 8, 746: 8, 748: 6, 749: 6, 750: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 779: 7, 781: 7, 782: 7, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1144: 7, 1145: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1258: 8, 1259: 8, 1266: 8, 1273: 7, 1306: 1, 1314: 8, 1323: 8, 1324: 8, 1342: 1, 1376: 8, 1401: 8, 1454: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
}],
|
||||
}
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.ALTIMA: {
|
||||
(Ecu.fwdCamera, 0x707, None): [
|
||||
b'284N86CA1D',
|
||||
],
|
||||
(Ecu.eps, 0x742, None): [
|
||||
b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'237109HE2B',
|
||||
],
|
||||
(Ecu.gateway, 0x18dad0f1, None): [
|
||||
b'284U29HE0A',
|
||||
],
|
||||
},
|
||||
CAR.LEAF: {
|
||||
(Ecu.abs, 0x740, None): [
|
||||
b'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.LEAF_IC: {
|
||||
(Ecu.fwdCamera, 0x707, None): [
|
||||
b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80',
|
||||
b'5SH4BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80',
|
||||
b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.abs, 0x740, None): [
|
||||
b'476605SD2E',
|
||||
b'476605SH1D',
|
||||
b'476605SK2A',
|
||||
],
|
||||
(Ecu.eps, 0x742, None): [
|
||||
b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SH2C\xb7A\x05\x02N123F\x15\xa3\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.gateway, 0x18dad0f1, None): [
|
||||
b'284U25SF0C',
|
||||
b'284U25SH3A',
|
||||
b'284U25SK2D',
|
||||
],
|
||||
},
|
||||
CAR.XTRAIL: {
|
||||
(Ecu.fwdCamera, 0x707, None): [
|
||||
b'284N86FR2A',
|
||||
],
|
||||
(Ecu.abs, 0x740, None): [
|
||||
b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80',
|
||||
b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.eps, 0x742, None): [
|
||||
b'6FP2A\x99A\x05\x02N123F\x18\x02\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.combinationMeter, 0x743, None): [
|
||||
b'6FR2A\x18B\x05\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'6FR9A\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'6FU9B\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.gateway, 0x18dad0f1, None): [
|
||||
b'284U26FR0E',
|
||||
],
|
||||
},
|
||||
}
|
||||
51
selfdrive/car/nissan/interface.py
Executable file
51
selfdrive/car/nissan/interface.py
Executable file
@@ -0,0 +1,51 @@
|
||||
from cereal import car, custom
|
||||
from panda import Panda
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
from openpilot.selfdrive.car.nissan.values import CAR
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
ret.carName = "nissan"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
|
||||
ret.autoResumeSng = False
|
||||
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.radarUnavailable = True
|
||||
|
||||
if candidate == CAR.ALTIMA:
|
||||
# Altima has EPS on C-CAN unlike the others that have it on V-CAN
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def _update(self, c, frogpilot_variables):
|
||||
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_variables)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake])
|
||||
|
||||
if self.CS.lkas_enabled:
|
||||
events.add(car.CarEvent.EventName.invalidLkasSetting)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
154
selfdrive/car/nissan/nissancan.py
Executable file
154
selfdrive/car/nissan/nissancan.py
Executable file
@@ -0,0 +1,154 @@
|
||||
import crcmod
|
||||
from openpilot.selfdrive.car.nissan.values import CAR
|
||||
|
||||
# TODO: add this checksum to the CANPacker
|
||||
nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque):
|
||||
values = {
|
||||
"COUNTER": frame % 0x10,
|
||||
"DESIRED_ANGLE": apply_steer,
|
||||
"SET_0x80_2": 0x80,
|
||||
"SET_0x80": 0x80,
|
||||
"MAX_TORQUE": lkas_max_torque if steer_on else 0,
|
||||
"LKA_ACTIVE": steer_on,
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("LKAS", 0, values)[2]
|
||||
|
||||
values["CHECKSUM"] = nissan_checksum(dat[:7])
|
||||
return packer.make_can_msg("LKAS", 0, values)
|
||||
|
||||
|
||||
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg):
|
||||
values = {s: cruise_throttle_msg[s] for s in [
|
||||
"COUNTER",
|
||||
"PROPILOT_BUTTON",
|
||||
"CANCEL_BUTTON",
|
||||
"GAS_PEDAL_INVERTED",
|
||||
"SET_BUTTON",
|
||||
"RES_BUTTON",
|
||||
"FOLLOW_DISTANCE_BUTTON",
|
||||
"NO_BUTTON_PRESSED",
|
||||
"GAS_PEDAL",
|
||||
"USER_BRAKE_PRESSED",
|
||||
"NEW_SIGNAL_2",
|
||||
"GAS_PRESSED_INVERTED",
|
||||
"unsure1",
|
||||
"unsure2",
|
||||
"unsure3",
|
||||
]}
|
||||
can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2
|
||||
|
||||
values["CANCEL_BUTTON"] = 1
|
||||
values["NO_BUTTON_PRESSED"] = 0
|
||||
values["PROPILOT_BUTTON"] = 0
|
||||
values["SET_BUTTON"] = 0
|
||||
values["RES_BUTTON"] = 0
|
||||
values["FOLLOW_DISTANCE_BUTTON"] = 0
|
||||
|
||||
return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values)
|
||||
|
||||
|
||||
def create_cancel_msg(packer, cancel_msg, cruise_cancel):
|
||||
values = {s: cancel_msg[s] for s in [
|
||||
"CANCEL_SEATBELT",
|
||||
"NEW_SIGNAL_1",
|
||||
"NEW_SIGNAL_2",
|
||||
"NEW_SIGNAL_3",
|
||||
]}
|
||||
|
||||
if cruise_cancel:
|
||||
values["CANCEL_SEATBELT"] = 1
|
||||
|
||||
return packer.make_can_msg("CANCEL_MSG", 2, values)
|
||||
|
||||
|
||||
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
|
||||
values = {s: lkas_hud_msg[s] for s in [
|
||||
"LARGE_WARNING_FLASHING",
|
||||
"SIDE_RADAR_ERROR_FLASHING1",
|
||||
"SIDE_RADAR_ERROR_FLASHING2",
|
||||
"LEAD_CAR",
|
||||
"LEAD_CAR_ERROR",
|
||||
"FRONT_RADAR_ERROR",
|
||||
"FRONT_RADAR_ERROR_FLASHING",
|
||||
"SIDE_RADAR_ERROR_FLASHING3",
|
||||
"LKAS_ERROR_FLASHING",
|
||||
"SAFETY_SHIELD_ACTIVE",
|
||||
"RIGHT_LANE_GREEN_FLASH",
|
||||
"LEFT_LANE_GREEN_FLASH",
|
||||
"FOLLOW_DISTANCE",
|
||||
"AUDIBLE_TONE",
|
||||
"SPEED_SET_ICON",
|
||||
"SMALL_STEERING_WHEEL_ICON",
|
||||
"unknown59",
|
||||
"unknown55",
|
||||
"unknown26",
|
||||
"unknown28",
|
||||
"unknown31",
|
||||
"SET_SPEED",
|
||||
"unknown43",
|
||||
"unknown08",
|
||||
"unknown05",
|
||||
"unknown02",
|
||||
]}
|
||||
|
||||
values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0
|
||||
values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0
|
||||
|
||||
values["LARGE_STEERING_WHEEL_ICON"] = 2 if lat_active else 0
|
||||
values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0
|
||||
values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active else 0
|
||||
|
||||
return packer.make_can_msg("PROPILOT_HUD", 0, values)
|
||||
|
||||
|
||||
def create_lkas_hud_info_msg(packer, lkas_hud_info_msg, steer_hud_alert):
|
||||
values = {s: lkas_hud_info_msg[s] for s in [
|
||||
"NA_HIGH_ACCEL_TEMP",
|
||||
"SIDE_RADAR_NA_HIGH_CABIN_TEMP",
|
||||
"SIDE_RADAR_MALFUNCTION",
|
||||
"LKAS_MALFUNCTION",
|
||||
"FRONT_RADAR_MALFUNCTION",
|
||||
"SIDE_RADAR_NA_CLEAN_REAR_CAMERA",
|
||||
"NA_POOR_ROAD_CONDITIONS",
|
||||
"CURRENTLY_UNAVAILABLE",
|
||||
"SAFETY_SHIELD_OFF",
|
||||
"FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION",
|
||||
"PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED",
|
||||
"SIDE_IMPACT_NA_RADAR_OBSTRUCTION",
|
||||
"WARNING_DO_NOT_ENTER",
|
||||
"SIDE_IMPACT_SYSTEM_OFF",
|
||||
"SIDE_IMPACT_MALFUNCTION",
|
||||
"FRONT_COLLISION_MALFUNCTION",
|
||||
"SIDE_RADAR_MALFUNCTION2",
|
||||
"LKAS_MALFUNCTION2",
|
||||
"FRONT_RADAR_MALFUNCTION2",
|
||||
"PROPILOT_NA_MSGS",
|
||||
"BOTTOM_MSG",
|
||||
"HANDS_ON_WHEEL_WARNING",
|
||||
"WARNING_STEP_ON_BRAKE_NOW",
|
||||
"PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED",
|
||||
"PROPILOT_NA_HIGH_CABIN_TEMP",
|
||||
"WARNING_PROPILOT_MALFUNCTION",
|
||||
"ACC_UNAVAILABLE_HIGH_CABIN_TEMP",
|
||||
"ACC_NA_FRONT_CAMERA_IMPARED",
|
||||
"unknown07",
|
||||
"unknown10",
|
||||
"unknown15",
|
||||
"unknown23",
|
||||
"unknown19",
|
||||
"unknown31",
|
||||
"unknown32",
|
||||
"unknown46",
|
||||
"unknown61",
|
||||
"unknown55",
|
||||
"unknown50",
|
||||
]}
|
||||
|
||||
if steer_hud_alert:
|
||||
values["HANDS_ON_WHEEL_WARNING"] = 1
|
||||
|
||||
return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values)
|
||||
4
selfdrive/car/nissan/radar_interface.py
Executable file
4
selfdrive/car/nissan/radar_interface.py
Executable file
@@ -0,0 +1,4 @@
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
111
selfdrive/car/nissan/values.py
Executable file
111
selfdrive/car/nissan/values.py
Executable file
@@ -0,0 +1,111 @@
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from cereal import car
|
||||
from panda.python import uds
|
||||
from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict
|
||||
from openpilot.selfdrive.car.docs_definitions import CarDocs, CarHarness, CarParts
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
|
||||
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
|
||||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass
|
||||
class NissanCarDocs(CarDocs):
|
||||
package: str = "ProPILOT Assist"
|
||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.nissan_a]))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NissanCarSpecs(CarSpecs):
|
||||
centerToFrontRatio: float = 0.44
|
||||
steerRatio: float = 17.
|
||||
|
||||
|
||||
@dataclass
|
||||
class NissanPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('nissan_x_trail_2017_generated', None))
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
XTRAIL = NissanPlatformConfig(
|
||||
"NISSAN X-TRAIL 2017",
|
||||
[NissanCarDocs("Nissan X-Trail 2017")],
|
||||
NissanCarSpecs(mass=1610, wheelbase=2.705)
|
||||
)
|
||||
LEAF = NissanPlatformConfig(
|
||||
"NISSAN LEAF 2018",
|
||||
[NissanCarDocs("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY")],
|
||||
NissanCarSpecs(mass=1610, wheelbase=2.705),
|
||||
dbc_dict('nissan_leaf_2018_generated', None),
|
||||
)
|
||||
# Leaf with ADAS ECU found behind instrument cluster instead of glovebox
|
||||
# Currently the only known difference between them is the inverted seatbelt signal.
|
||||
LEAF_IC = LEAF.override(platform_str="NISSAN LEAF 2018 Instrument Cluster", car_docs=[])
|
||||
ROGUE = NissanPlatformConfig(
|
||||
"NISSAN ROGUE 2019",
|
||||
[NissanCarDocs("Nissan Rogue 2018-20")],
|
||||
NissanCarSpecs(mass=1610, wheelbase=2.705)
|
||||
)
|
||||
ALTIMA = NissanPlatformConfig(
|
||||
"NISSAN ALTIMA 2020",
|
||||
[NissanCarDocs("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b]))],
|
||||
NissanCarSpecs(mass=1492, wheelbase=2.824)
|
||||
)
|
||||
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
|
||||
# Default diagnostic session
|
||||
NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81])
|
||||
NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0x81])
|
||||
|
||||
# Manufacturer specific
|
||||
NISSAN_DIAGNOSTIC_REQUEST_KWP_2 = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xda])
|
||||
NISSAN_DIAGNOSTIC_RESPONSE_KWP_2 = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xda])
|
||||
|
||||
NISSAN_VERSION_REQUEST_KWP = b'\x21\x83'
|
||||
NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83'
|
||||
|
||||
NISSAN_RX_OFFSET = 0x20
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[request for bus, logging in ((0, False), (1, True)) for request in [
|
||||
Request(
|
||||
[NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
|
||||
[NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
|
||||
bus=bus,
|
||||
logging=logging,
|
||||
),
|
||||
Request(
|
||||
[NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
|
||||
[NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
|
||||
rx_offset=NISSAN_RX_OFFSET,
|
||||
bus=bus,
|
||||
logging=logging,
|
||||
),
|
||||
# Rogue's engine solely responds to this
|
||||
Request(
|
||||
[NISSAN_DIAGNOSTIC_REQUEST_KWP_2, NISSAN_VERSION_REQUEST_KWP],
|
||||
[NISSAN_DIAGNOSTIC_RESPONSE_KWP_2, NISSAN_VERSION_RESPONSE_KWP],
|
||||
bus=bus,
|
||||
logging=logging,
|
||||
),
|
||||
Request(
|
||||
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
|
||||
rx_offset=NISSAN_RX_OFFSET,
|
||||
bus=bus,
|
||||
logging=logging,
|
||||
),
|
||||
]],
|
||||
)
|
||||
0
selfdrive/car/subaru/__init__.py
Executable file
0
selfdrive/car/subaru/__init__.py
Executable file
144
selfdrive/car/subaru/carcontroller.py
Executable file
144
selfdrive/car/subaru/carcontroller.py
Executable file
@@ -0,0 +1,144 @@
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.car.subaru import subarucan
|
||||
from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
|
||||
|
||||
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
|
||||
# involves the total steering angle change rather than rate, but these limits work well for now
|
||||
MAX_STEER_RATE = 25 # deg/s
|
||||
MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.frame = 0
|
||||
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_counter = 0
|
||||
|
||||
self.p = CarControllerParams(CP)
|
||||
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
can_sends = []
|
||||
|
||||
# *** steering ***
|
||||
if (self.frame % self.p.STEER_STEP) == 0:
|
||||
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
|
||||
|
||||
# limits due to driver torque
|
||||
|
||||
new_steer = int(round(apply_steer))
|
||||
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
|
||||
|
||||
if not CC.latActive:
|
||||
apply_steer = 0
|
||||
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive))
|
||||
else:
|
||||
apply_steer_req = CC.latActive
|
||||
|
||||
if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
|
||||
# Steering rate fault prevention
|
||||
self.steer_rate_counter, apply_steer_req = \
|
||||
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
|
||||
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
|
||||
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req))
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
# *** longitudinal ***
|
||||
|
||||
if CC.longActive:
|
||||
apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V)))
|
||||
apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V)))
|
||||
apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V)))
|
||||
|
||||
# limit min and max values
|
||||
cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
|
||||
cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
|
||||
cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
|
||||
else:
|
||||
cruise_throttle = CarControllerParams.THROTTLE_INACTIVE
|
||||
cruise_rpm = CarControllerParams.RPM_MIN
|
||||
cruise_brake = CarControllerParams.BRAKE_MIN
|
||||
|
||||
# *** alerts and pcm cancel ***
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
if self.frame % 5 == 0:
|
||||
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
|
||||
# disengage ACC when OP is disengaged
|
||||
if pcm_cancel_cmd:
|
||||
cruise_button = 1
|
||||
# turn main on if off and past start-up state
|
||||
elif not CS.out.cruiseState.available and CS.ready:
|
||||
cruise_button = 1
|
||||
else:
|
||||
cruise_button = CS.cruise_button
|
||||
|
||||
# unstick previous mocked button press
|
||||
if cruise_button == 1 and self.cruise_button_prev == 1:
|
||||
cruise_button = 0
|
||||
self.cruise_button_prev = cruise_button
|
||||
|
||||
can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
|
||||
|
||||
else:
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled,
|
||||
self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible))
|
||||
|
||||
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive))
|
||||
|
||||
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
|
||||
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 5 == 0:
|
||||
can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg,
|
||||
self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
|
||||
|
||||
can_sends.append(subarucan.create_es_brake(self.packer, self.frame // 5, CS.es_brake_msg,
|
||||
self.CP.openpilotLongitudinalControl, CC.longActive, cruise_brake))
|
||||
|
||||
can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd,
|
||||
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
|
||||
else:
|
||||
if pcm_cancel_cmd:
|
||||
if not (self.CP.flags & SubaruFlags.HYBRID):
|
||||
bus = CanBus.alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main
|
||||
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
|
||||
|
||||
if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT:
|
||||
# Tester present (keeps eyesight disabled)
|
||||
if self.frame % 100 == 0:
|
||||
can_sends.append([GLOBAL_ES_ADDR, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", CanBus.camera])
|
||||
|
||||
# Create all of the other eyesight messages to keep the rest of the car happy when eyesight is disabled
|
||||
if self.frame % 5 == 0:
|
||||
can_sends.append(subarucan.create_es_highbeamassist(self.packer))
|
||||
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append(subarucan.create_es_static_1(self.packer))
|
||||
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(subarucan.create_es_static_2(self.packer))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
|
||||
new_actuators.steerOutputCan = self.apply_steer_last
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
231
selfdrive/car/subaru/carstate.py
Executable file
231
selfdrive/car/subaru/carstate.py
Executable file
@@ -0,0 +1,231 @@
|
||||
import copy
|
||||
from cereal import car
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.subaru.values import DBC, CanBus, SubaruFlags
|
||||
from openpilot.selfdrive.car import CanSignalRateCalculator
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["Transmission"]["Gear"]
|
||||
|
||||
self.angle_rate_calulator = CanSignalRateCalculator(50)
|
||||
|
||||
def update(self, cp, cp_cam, cp_body, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_body.vl["Throttle_Hybrid"]
|
||||
ret.gas = throttle_msg["Throttle_Pedal"] / 255.
|
||||
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 0
|
||||
else:
|
||||
cp_brakes = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
||||
ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1
|
||||
|
||||
cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam
|
||||
if not (self.CP.flags & SubaruFlags.HYBRID):
|
||||
eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"])
|
||||
|
||||
# if openpilot is controlling long, an eyesight fault is a non-critical fault. otherwise it's an ACC fault
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
ret.carFaultedNonCritical = eyesight_fault
|
||||
else:
|
||||
ret.accFaulted = eyesight_fault
|
||||
|
||||
cp_wheels = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp_wheels.vl["Wheel_Speeds"]["FL"],
|
||||
cp_wheels.vl["Wheel_Speeds"]["FR"],
|
||||
cp_wheels.vl["Wheel_Speeds"]["RL"],
|
||||
cp_wheels.vl["Wheel_Speeds"]["RR"],
|
||||
)
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw == 0
|
||||
|
||||
# continuous blinker signals for assisted lane change
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
|
||||
cp.vl["Dashlights"]["RIGHT_BLINKER"])
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
|
||||
ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
|
||||
|
||||
cp_transmission = cp_body if self.CP.flags & SubaruFlags.HYBRID else cp
|
||||
can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
||||
|
||||
if not (self.CP.flags & SubaruFlags.PREGLOBAL):
|
||||
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
|
||||
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"])
|
||||
|
||||
ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
||||
ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
|
||||
|
||||
steer_threshold = 75 if self.CP.flags & SubaruFlags.PREGLOBAL else 80
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
|
||||
|
||||
cp_cruise = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
||||
if self.CP.flags & SubaruFlags.HYBRID:
|
||||
ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
|
||||
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
|
||||
else:
|
||||
ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
|
||||
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
|
||||
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
|
||||
|
||||
if (self.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \
|
||||
(not (self.CP.flags & SubaruFlags.PREGLOBAL) and cp.vl["Dashlights"]["UNITS"] == 1):
|
||||
ret.cruiseState.speed *= CV.MPH_TO_KPH
|
||||
|
||||
ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1
|
||||
ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"],
|
||||
cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
|
||||
cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
|
||||
cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
|
||||
ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
|
||||
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
|
||||
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
|
||||
else:
|
||||
ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
|
||||
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
|
||||
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
|
||||
ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
|
||||
(cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2)
|
||||
|
||||
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
|
||||
cp_es_brake = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
|
||||
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
|
||||
cp_es_status = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
|
||||
|
||||
# TODO: Hybrid cars don't have ES_Distance, need a replacement
|
||||
if not (self.CP.flags & SubaruFlags.HYBRID):
|
||||
# 8 is known AEB, there are a few other values related to AEB we ignore
|
||||
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
|
||||
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
|
||||
|
||||
self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
|
||||
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
|
||||
|
||||
if not (self.CP.flags & SubaruFlags.HYBRID):
|
||||
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
|
||||
|
||||
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
|
||||
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
|
||||
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
|
||||
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"]
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_common_global_body_messages(CP):
|
||||
messages = [
|
||||
("Wheel_Speeds", 50),
|
||||
("Brake_Status", 50),
|
||||
]
|
||||
|
||||
if not (CP.flags & SubaruFlags.HYBRID):
|
||||
messages.append(("CruiseControl", 20))
|
||||
|
||||
return messages
|
||||
|
||||
@staticmethod
|
||||
def get_common_global_es_messages(CP):
|
||||
messages = [
|
||||
("ES_Brake", 20),
|
||||
]
|
||||
|
||||
if not (CP.flags & SubaruFlags.HYBRID):
|
||||
messages += [
|
||||
("ES_Distance", 20),
|
||||
("ES_Status", 20)
|
||||
]
|
||||
|
||||
return messages
|
||||
|
||||
@staticmethod
|
||||
def get_common_preglobal_body_messages():
|
||||
messages = [
|
||||
("CruiseControl", 50),
|
||||
("Wheel_Speeds", 50),
|
||||
("Dash_State2", 1),
|
||||
]
|
||||
|
||||
return messages
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
# sig_address, frequency
|
||||
("Dashlights", 10),
|
||||
("Steering_Torque", 50),
|
||||
("BodyInfo", 1),
|
||||
("Brake_Pedal", 50),
|
||||
]
|
||||
|
||||
if not (CP.flags & SubaruFlags.HYBRID):
|
||||
messages += [
|
||||
("Throttle", 100),
|
||||
("Transmission", 100)
|
||||
]
|
||||
|
||||
if CP.enableBsm:
|
||||
messages.append(("BSD_RCTA", 17))
|
||||
|
||||
if not (CP.flags & SubaruFlags.PREGLOBAL):
|
||||
if not (CP.flags & SubaruFlags.GLOBAL_GEN2):
|
||||
messages += CarState.get_common_global_body_messages(CP)
|
||||
else:
|
||||
messages += CarState.get_common_preglobal_body_messages()
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.main)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
if CP.flags & SubaruFlags.PREGLOBAL:
|
||||
messages = [
|
||||
("ES_DashStatus", 20),
|
||||
("ES_Distance", 20),
|
||||
]
|
||||
else:
|
||||
messages = [
|
||||
("ES_DashStatus", 10),
|
||||
("ES_LKAS_State", 10),
|
||||
]
|
||||
|
||||
if not (CP.flags & SubaruFlags.GLOBAL_GEN2):
|
||||
messages += CarState.get_common_global_es_messages(CP)
|
||||
|
||||
if CP.flags & SubaruFlags.SEND_INFOTAINMENT:
|
||||
messages.append(("ES_Infotainment", 10))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.camera)
|
||||
|
||||
@staticmethod
|
||||
def get_body_can_parser(CP):
|
||||
messages = []
|
||||
|
||||
if CP.flags & SubaruFlags.GLOBAL_GEN2:
|
||||
messages += CarState.get_common_global_body_messages(CP)
|
||||
messages += CarState.get_common_global_es_messages(CP)
|
||||
|
||||
if CP.flags & SubaruFlags.HYBRID:
|
||||
messages += [
|
||||
("Throttle_Hybrid", 40),
|
||||
("Transmission", 100)
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt)
|
||||
551
selfdrive/car/subaru/fingerprints.py
Executable file
551
selfdrive/car/subaru/fingerprints.py
Executable file
@@ -0,0 +1,551 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.subaru.values import CAR
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.ASCENT: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa5 \x19\x02\x00',
|
||||
b'\xa5 !\x02\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x05\xc0\xd0\x00',
|
||||
b'\x85\xc0\xd0\x00',
|
||||
b'\x95\xc0\xd0\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00d\xb9\x00\x00\x00\x00',
|
||||
b'\x00\x00d\xb9\x1f@ \x10',
|
||||
b'\x00\x00e@\x00\x00\x00\x00',
|
||||
b'\x00\x00e@\x1f@ $',
|
||||
b"\x00\x00e~\x1f@ '",
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xbb,\xa0t\x07',
|
||||
b'\xd1,\xa0q\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\x00>\xf0\x00\x00',
|
||||
b'\x00\xfe\xf7\x00\x00',
|
||||
b'\x01\xfe\xf7\x00\x00',
|
||||
b'\x01\xfe\xf9\x00\x00',
|
||||
b'\x01\xfe\xfa\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.ASCENT_2023: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa5 #\x03\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'%\xc0\xd0\x11',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x05!\x08\x1dK\x05!\x08\x01/',
|
||||
],
|
||||
(Ecu.engine, 0x7a2, None): [
|
||||
b'\xe5,\xa0P\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7a3, None): [
|
||||
b'\x04\xfe\xf3\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.LEGACY: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa1 \x02\x01',
|
||||
b'\xa1 \x02\x02',
|
||||
b'\xa1 \x03\x03',
|
||||
b'\xa1 \x04\x01',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x9b\xc0\x11\x00',
|
||||
b'\x9b\xc0\x11\x02',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00e\x80\x00\x1f@ \x19\x00',
|
||||
b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xde"a0\x07',
|
||||
b'\xde,\xa0@\x07',
|
||||
b'\xe2"aq\x07',
|
||||
b'\xe2,\xa0@\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xa5\xf6\x05@\x00',
|
||||
b'\xa5\xfe\xc7@\x00',
|
||||
b'\xa7\xf6\x04@\x00',
|
||||
b'\xa7\xfe\xc4@\x00',
|
||||
],
|
||||
},
|
||||
CAR.IMPREZA: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'z\x84\x19\x90\x00',
|
||||
b'z\x94\x08\x90\x00',
|
||||
b'z\x94\x08\x90\x01',
|
||||
b'z\x94\x0c\x90\x00',
|
||||
b'z\x94\x0c\x90\x01',
|
||||
b'z\x94.\x90\x00',
|
||||
b'z\x94?\x90\x00',
|
||||
b'z\x9c\x19\x80\x01',
|
||||
b'\xa2 \x185\x00',
|
||||
b'\xa2 \x193\x00',
|
||||
b'\xa2 \x194\x00',
|
||||
b'\xa2 \x19`\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'z\xc0\x00\x00',
|
||||
b'z\xc0\x04\x00',
|
||||
b'z\xc0\x08\x00',
|
||||
b'z\xc0\n\x00',
|
||||
b'z\xc0\x0c\x00',
|
||||
b'\x8a\xc0\x00\x00',
|
||||
b'\x8a\xc0\x10\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00c\xf4\x00\x00\x00\x00',
|
||||
b'\x00\x00c\xf4\x1f@ \x07',
|
||||
b'\x00\x00d)\x00\x00\x00\x00',
|
||||
b'\x00\x00d)\x1f@ \x07',
|
||||
b'\x00\x00dd\x00\x00\x00\x00',
|
||||
b'\x00\x00dd\x1f@ \x0e',
|
||||
b'\x00\x00d\xb5\x1f@ \x0e',
|
||||
b'\x00\x00d\xdc\x00\x00\x00\x00',
|
||||
b'\x00\x00d\xdc\x1f@ \x0e',
|
||||
b'\x00\x00e\x02\x1f@ \x14',
|
||||
b'\x00\x00e\x1c\x00\x00\x00\x00',
|
||||
b'\x00\x00e\x1c\x1f@ \x14',
|
||||
b'\x00\x00e+\x00\x00\x00\x00',
|
||||
b'\x00\x00e+\x1f@ \x14',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xaa\x00Bu\x07',
|
||||
b'\xaa\x01bt\x07',
|
||||
b'\xaa!`u\x07',
|
||||
b'\xaa!au\x07',
|
||||
b'\xaa!av\x07',
|
||||
b'\xaa!aw\x07',
|
||||
b'\xaa!dq\x07',
|
||||
b'\xaa!ds\x07',
|
||||
b'\xaa!dt\x07',
|
||||
b'\xaaafs\x07',
|
||||
b'\xbe!as\x07',
|
||||
b'\xbe!at\x07',
|
||||
b'\xbeacr\x07',
|
||||
b'\xc5!`r\x07',
|
||||
b'\xc5!`s\x07',
|
||||
b'\xc5!ap\x07',
|
||||
b'\xc5!ar\x07',
|
||||
b'\xc5!as\x07',
|
||||
b'\xc5!dr\x07',
|
||||
b'\xc5!ds\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xe3\xd0\x081\x00',
|
||||
b'\xe3\xd5\x161\x00',
|
||||
b'\xe3\xe5F1\x00',
|
||||
b'\xe3\xf5\x06\x00\x00',
|
||||
b'\xe3\xf5\x07\x00\x00',
|
||||
b'\xe3\xf5C\x00\x00',
|
||||
b'\xe3\xf5F\x00\x00',
|
||||
b'\xe3\xf5G\x00\x00',
|
||||
b'\xe4\xe5\x061\x00',
|
||||
b'\xe4\xf5\x02\x00\x00',
|
||||
b'\xe4\xf5\x07\x00\x00',
|
||||
b'\xe5\xf5\x04\x00\x00',
|
||||
b'\xe5\xf5$\x00\x00',
|
||||
b'\xe5\xf5B\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.IMPREZA_2020: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa2 \x193\x00',
|
||||
b'\xa2 \x194\x00',
|
||||
b'\xa2 `\x00',
|
||||
b'\xa2 !3\x00',
|
||||
b'\xa2 !`\x00',
|
||||
b'\xa2 !i\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\n\xc0\x04\x00',
|
||||
b'\n\xc0\x04\x01',
|
||||
b'\x9a\xc0\x00\x00',
|
||||
b'\x9a\xc0\x04\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00eb\x1f@ "',
|
||||
b'\x00\x00eq\x00\x00\x00\x00',
|
||||
b'\x00\x00eq\x1f@ "',
|
||||
b'\x00\x00e\x8f\x00\x00\x00\x00',
|
||||
b'\x00\x00e\x8f\x1f@ )',
|
||||
b'\x00\x00e\x92\x00\x00\x00\x00',
|
||||
b'\x00\x00e\xa4\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xca!`0\x07',
|
||||
b'\xca!`p\x07',
|
||||
b'\xca!ap\x07',
|
||||
b'\xca!f@\x07',
|
||||
b'\xca!fp\x07',
|
||||
b'\xcc!`p\x07',
|
||||
b'\xcc!fp\x07',
|
||||
b'\xcc"f0\x07',
|
||||
b'\xe6!`@\x07',
|
||||
b'\xe6!fp\x07',
|
||||
b'\xe6"f0\x07',
|
||||
b'\xe6"fp\x07',
|
||||
b'\xf3"f@\x07',
|
||||
b'\xf3"fp\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xe6\xf5\x04\x00\x00',
|
||||
b'\xe6\xf5$\x00\x00',
|
||||
b'\xe6\xf5D0\x00',
|
||||
b'\xe7\xf5\x04\x00\x00',
|
||||
b'\xe7\xf5D0\x00',
|
||||
b'\xe7\xf6B0\x00',
|
||||
b'\xe9\xf5"\x00\x00',
|
||||
b'\xe9\xf5B0\x00',
|
||||
b'\xe9\xf6B0\x00',
|
||||
b'\xe9\xf6F0\x00',
|
||||
],
|
||||
},
|
||||
CAR.CROSSTREK_HYBRID: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa2 \x19e\x01',
|
||||
b'\xa2 !e\x01',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\n\xc2\x01\x00',
|
||||
b'\x9a\xc2\x01\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00el\x1f@ #',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xd7!`@\x07',
|
||||
b'\xd7!`p\x07',
|
||||
b'\xf4!`0\x07',
|
||||
],
|
||||
},
|
||||
CAR.FORESTER: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa3 \x18\x14\x00',
|
||||
b'\xa3 \x18&\x00',
|
||||
b'\xa3 \x19\x14\x00',
|
||||
b'\xa3 \x19&\x00',
|
||||
b'\xa3 \x14\x00',
|
||||
b'\xa3 \x14\x01',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x8d\xc0\x00\x00',
|
||||
b'\x8d\xc0\x04\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00e!\x00\x00\x00\x00',
|
||||
b'\x00\x00e!\x1f@ \x11',
|
||||
b'\x00\x00e^\x00\x00\x00\x00',
|
||||
b'\x00\x00e^\x1f@ !',
|
||||
b'\x00\x00e`\x1f@ ',
|
||||
b'\x00\x00e\x97\x00\x00\x00\x00',
|
||||
b'\x00\x00e\x97\x1f@ 0',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xb6"`A\x07',
|
||||
b'\xb6\xa2`A\x07',
|
||||
b'\xcb"`@\x07',
|
||||
b'\xcb"`p\x07',
|
||||
b'\xcf"`0\x07',
|
||||
b'\xcf"`p\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\x1a\xe6B1\x00',
|
||||
b'\x1a\xe6F1\x00',
|
||||
b'\x1a\xf6B0\x00',
|
||||
b'\x1a\xf6B`\x00',
|
||||
b'\x1a\xf6F`\x00',
|
||||
b'\x1a\xf6b0\x00',
|
||||
b'\x1a\xf6b`\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORESTER_HYBRID: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa3 \x19T\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x8d\xc2\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00eY\x1f@ !',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xd2\xa1`r\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\x1b\xa7@a\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORESTER_PREGLOBAL: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'm\x97\x14@',
|
||||
b'}\x97\x14@',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'm\xc0\x10\x00',
|
||||
b'}\xc0\x10\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00c\xe9\x00\x00\x00\x00',
|
||||
b'\x00\x00c\xe9\x1f@ \x03',
|
||||
b'\x00\x00d5\x1f@ \t',
|
||||
b'\x00\x00d\xd3\x1f@ \t',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xa7"@p\x07',
|
||||
b'\xa7)\xa0q\x07',
|
||||
b'\xba"@@\x07',
|
||||
b'\xba"@p\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\x1a\xf6F`\x00',
|
||||
b'\xda\xf2`\x80\x00',
|
||||
b'\xda\xfd\xe0\x80\x00',
|
||||
b'\xdc\xf2@`\x00',
|
||||
b'\xdc\xf2``\x00',
|
||||
b'\xdc\xf2`\x80\x00',
|
||||
b'\xdc\xf2`\x81\x00',
|
||||
],
|
||||
},
|
||||
CAR.LEGACY_PREGLOBAL: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'[\x97D\x00',
|
||||
b'[\xba\xc4\x03',
|
||||
b'k\x97D\x00',
|
||||
b'k\x9aD\x00',
|
||||
b'{\x97D\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'K\xb0\x00\x01',
|
||||
b'[\xb0\x00\x01',
|
||||
b'k\xb0\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00c\x94\x1f@\x10\x08',
|
||||
b'\x00\x00c\xb7\x1f@\x10\x16',
|
||||
b'\x00\x00c\xec\x1f@ \x04',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xa0"@q\x07',
|
||||
b'\xa0+@p\x07',
|
||||
b'\xab*@r\x07',
|
||||
b'\xab+@p\x07',
|
||||
b'\xb4"@0\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xbd\xf2\x00`\x00',
|
||||
b'\xbe\xf2\x00p\x00',
|
||||
b'\xbe\xfb\xc0p\x00',
|
||||
b'\xbf\xf2\x00\x80\x00',
|
||||
b'\xbf\xfb\xc0\x80\x00',
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK_PREGLOBAL: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'[\xba\xac\x03',
|
||||
b'[\xf7\xac\x00',
|
||||
b'[\xf7\xac\x03',
|
||||
b'[\xf7\xbc\x03',
|
||||
b'k\x97\xac\x00',
|
||||
b'k\x9a\xac\x00',
|
||||
b'{\x97\xac\x00',
|
||||
b'{\x9a\xac\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'K\xb0\x00\x00',
|
||||
b'K\xb0\x00\x02',
|
||||
b'[\xb0\x00\x00',
|
||||
b'k\xb0\x00\x00',
|
||||
b'{\xb0\x00\x01',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00c\x90\x1f@\x10\x0e',
|
||||
b'\x00\x00c\x94\x00\x00\x00\x00',
|
||||
b'\x00\x00c\x94\x1f@\x10\x08',
|
||||
b'\x00\x00c\xb7\x1f@\x10\x16',
|
||||
b'\x00\x00c\xd1\x1f@\x10\x17',
|
||||
b'\x00\x00c\xec\x1f@ \x04',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xa0"@\x80\x07',
|
||||
b'\xa0*@q\x07',
|
||||
b'\xa0*@u\x07',
|
||||
b'\xa0+@@\x07',
|
||||
b'\xa0bAq\x07',
|
||||
b'\xab"@@\x07',
|
||||
b'\xab"@s\x07',
|
||||
b'\xab*@@\x07',
|
||||
b'\xab+@@\x07',
|
||||
b'\xb4"@0\x07',
|
||||
b'\xb4"@p\x07',
|
||||
b'\xb4"@r\x07',
|
||||
b'\xb4+@p\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xbd\xf2@`\x00',
|
||||
b'\xbd\xf2@\x81\x00',
|
||||
b'\xbd\xfb\xe0\x80\x00',
|
||||
b'\xbe\xf2@p\x00',
|
||||
b'\xbe\xf2@\x80\x00',
|
||||
b'\xbe\xfb\xe0p\x00',
|
||||
b'\xbf\xe2@\x80\x00',
|
||||
b'\xbf\xf2@\x80\x00',
|
||||
b'\xbf\xfb\xe0b\x00',
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK_PREGLOBAL_2018: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x8b\x97\xac\x00',
|
||||
b'\x8b\x97\xbc\x00',
|
||||
b'\x8b\x99\xac\x00',
|
||||
b'\x8b\x9a\xac\x00',
|
||||
b'\x9b\x97\xac\x00',
|
||||
b'\x9b\x97\xbe\x10',
|
||||
b'\x9b\x9a\xac\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'{\xb0\x00\x00',
|
||||
b'{\xb0\x00\x01',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00df\x1f@ \n',
|
||||
b'\x00\x00d\x95\x00\x00\x00\x00',
|
||||
b'\x00\x00d\x95\x1f@ \x0f',
|
||||
b'\x00\x00d\xfe\x00\x00\x00\x00',
|
||||
b'\x00\x00d\xfe\x1f@ \x15',
|
||||
b'\x00\x00e\x19\x1f@ \x15',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xb5"@P\x07',
|
||||
b'\xb5"@p\x07',
|
||||
b'\xb5+@@\x07',
|
||||
b'\xb5b@1\x07',
|
||||
b'\xb5q\xe0@\x07',
|
||||
b'\xc4"@0\x07',
|
||||
b'\xc4+@0\x07',
|
||||
b'\xc4b@p\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xbb\xf2@`\x00',
|
||||
b'\xbb\xfb\xe0`\x00',
|
||||
b'\xbc\xaf\xe0`\x00',
|
||||
b'\xbc\xe2@\x80\x00',
|
||||
b'\xbc\xf2@\x80\x00',
|
||||
b'\xbc\xf2@\x81\x00',
|
||||
b'\xbc\xfb\xe0`\x00',
|
||||
b'\xbc\xfb\xe0\x80\x00',
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa1 \x06\x00',
|
||||
b'\xa1 \x06\x01',
|
||||
b'\xa1 \x06\x02',
|
||||
b'\xa1 \x07\x00',
|
||||
b'\xa1 \x07\x02',
|
||||
b'\xa1 \x07\x03',
|
||||
b'\xa1 \x08\x00',
|
||||
b'\xa1 \x08\x01',
|
||||
b'\xa1 \x08\x02',
|
||||
b'\xa1 "\t\x00',
|
||||
b'\xa1 "\t\x01',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x1b\xc0\x10\x00',
|
||||
b'\x9b\xc0\x10\x00',
|
||||
b'\x9b\xc0\x10\x02',
|
||||
b'\x9b\xc0 \x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00eJ\x00\x00\x00\x00\x00\x00',
|
||||
b'\x00\x00eJ\x00\x1f@ \x19\x00',
|
||||
b'\x00\x00e\x80\x00\x1f@ \x19\x00',
|
||||
b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00',
|
||||
b'\x00\x00e\x9a\x00\x1f@ 1\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xbc"`@\x07',
|
||||
b'\xbc"`q\x07',
|
||||
b'\xbc,\xa0q\x07',
|
||||
b'\xbc,\xa0u\x07',
|
||||
b'\xde"`0\x07',
|
||||
b'\xde,\xa0@\x07',
|
||||
b'\xe2"`0\x07',
|
||||
b'\xe2"`p\x07',
|
||||
b'\xe2"`q\x07',
|
||||
b'\xe3,\xa0@\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xa5\xf6D@\x00',
|
||||
b'\xa5\xfe\xf6@\x00',
|
||||
b'\xa5\xfe\xf7@\x00',
|
||||
b'\xa5\xfe\xf8@\x00',
|
||||
b'\xa7\x8e\xf40\x00',
|
||||
b'\xa7\xf6D@\x00',
|
||||
b'\xa7\xfe\xf4@\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORESTER_2022: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa3 !v\x00',
|
||||
b'\xa3 !x\x00',
|
||||
b'\xa3 "v\x00',
|
||||
b'\xa3 "x\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'-\xc0\x040',
|
||||
b'-\xc0%0',
|
||||
b'=\xc0%\x02',
|
||||
b'=\xc04\x02',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x04!\x01\x1eD\x07!\x00\x04,',
|
||||
b'\x04!\x08\x01.\x07!\x08\x022',
|
||||
b'\r!\x08\x017\n!\x08\x003',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xd5"`0\x07',
|
||||
b'\xd5"a0\x07',
|
||||
b'\xf1"`q\x07',
|
||||
b'\xf1"aq\x07',
|
||||
b'\xfa"ap\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\x1d\x86B0\x00',
|
||||
b'\x1d\xf6B0\x00',
|
||||
b'\x1e\x86B0\x00',
|
||||
b'\x1e\x86F0\x00',
|
||||
b'\x1e\xf6D0\x00',
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK_2023: {
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa1 #\x14\x00',
|
||||
b'\xa1 #\x17\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'+\xc0\x10\x11\x00',
|
||||
b'+\xc0\x12\x11\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\t!\x08\x046\x05!\x08\x01/',
|
||||
],
|
||||
(Ecu.engine, 0x7a2, None): [
|
||||
b'\xed,\xa0q\x07',
|
||||
b'\xed,\xa2q\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7a3, None): [
|
||||
b'\xa8\x8e\xf41\x00',
|
||||
b'\xa8\xfe\xf41\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
126
selfdrive/car/subaru/interface.py
Executable file
126
selfdrive/car/subaru/interface.py
Executable file
@@ -0,0 +1,126 @@
|
||||
from cereal import car, custom
|
||||
from panda import Panda
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags
|
||||
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret, params, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
crosstrek_torque_increase = params.get_bool("CrosstrekTorque")
|
||||
|
||||
ret.carName = "subaru"
|
||||
ret.radarUnavailable = True
|
||||
# for HYBRID CARS to be upstreamed, we need:
|
||||
# - replacement for ES_Distance so we can cancel the cruise control
|
||||
# - to find the Cruise_Activated bit from the car
|
||||
# - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
|
||||
ret.dashcamOnly = bool(ret.flags & (SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID))
|
||||
ret.autoResumeSng = False
|
||||
|
||||
# Detect infotainment message sent from the camera
|
||||
if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]:
|
||||
ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value
|
||||
|
||||
if ret.flags & SubaruFlags.PREGLOBAL:
|
||||
ret.enableBsm = 0x25c in fingerprint[0]
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)]
|
||||
else:
|
||||
ret.enableBsm = 0x228 in fingerprint[0]
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)]
|
||||
if ret.flags & SubaruFlags.GLOBAL_GEN2:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
if ret.flags & SubaruFlags.LKAS_ANGLE:
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
else:
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
if candidate in (CAR.ASCENT, CAR.ASCENT_2023):
|
||||
ret.steerActuatorDelay = 0.3 # end-to-end angle controller
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kf = 0.00003
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
|
||||
|
||||
elif candidate == CAR.IMPREZA:
|
||||
ret.steerActuatorDelay = 0.4 # end-to-end angle controller
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kf = 0.00003333 if crosstrek_torque_increase else 0.00005
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.133, 0.2], [0.0133, 0.02]] if crosstrek_torque_increase else [[0.2, 0.3], [0.02, 0.03]]
|
||||
|
||||
elif candidate == CAR.IMPREZA_2020:
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
|
||||
|
||||
elif candidate == CAR.CROSSTREK_HYBRID:
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
elif candidate in (CAR.FORESTER, CAR.FORESTER_2022, CAR.FORESTER_HYBRID):
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kf = 0.000038
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
|
||||
|
||||
elif candidate in (CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023):
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018):
|
||||
ret.safetyConfigs[0].safetyParam = Panda.FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE # Outback 2018-2019 and Forester have reversed driver torque signal
|
||||
|
||||
elif candidate == CAR.LEGACY_PREGLOBAL:
|
||||
ret.steerActuatorDelay = 0.15
|
||||
|
||||
elif candidate == CAR.OUTBACK_PREGLOBAL:
|
||||
pass
|
||||
else:
|
||||
raise ValueError(f"unknown car: {candidate}")
|
||||
|
||||
ret.experimentalLongitudinalAvailable = not (ret.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.PREGLOBAL |
|
||||
SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID))
|
||||
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
|
||||
|
||||
if ret.flags & SubaruFlags.GLOBAL_GEN2 and ret.openpilotLongitudinalControl:
|
||||
ret.flags |= SubaruFlags.DISABLE_EYESIGHT.value
|
||||
|
||||
if ret.openpilotLongitudinalControl:
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def _update(self, c, frogpilot_variables):
|
||||
|
||||
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
|
||||
]
|
||||
|
||||
ret.events = self.create_common_events(ret).to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.flags & SubaruFlags.DISABLE_EYESIGHT:
|
||||
disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01')
|
||||
|
||||
def apply(self, c, now_nanos, frogpilot_variables):
|
||||
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)
|
||||
4
selfdrive/car/subaru/radar_interface.py
Executable file
4
selfdrive/car/subaru/radar_interface.py
Executable file
@@ -0,0 +1,4 @@
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
323
selfdrive/car/subaru/subarucan.py
Executable file
323
selfdrive/car/subaru/subarucan.py
Executable file
@@ -0,0 +1,323 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.car.subaru.values import CanBus
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, steer_req):
|
||||
values = {
|
||||
"LKAS_Output": apply_steer,
|
||||
"LKAS_Request": steer_req,
|
||||
"SET_1": 1
|
||||
}
|
||||
return packer.make_can_msg("ES_LKAS", 0, values)
|
||||
|
||||
|
||||
def create_steering_control_angle(packer, apply_steer, steer_req):
|
||||
values = {
|
||||
"LKAS_Output": apply_steer,
|
||||
"LKAS_Request": steer_req,
|
||||
"SET_3": 3
|
||||
}
|
||||
return packer.make_can_msg("ES_LKAS_ANGLE", 0, values)
|
||||
|
||||
|
||||
def create_steering_status(packer):
|
||||
return packer.make_can_msg("ES_LKAS_State", 0, {})
|
||||
|
||||
def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
|
||||
values = {s: es_distance_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"Signal1",
|
||||
"Cruise_Fault",
|
||||
"Cruise_Throttle",
|
||||
"Signal2",
|
||||
"Car_Follow",
|
||||
"Low_Speed_Follow",
|
||||
"Cruise_Soft_Disable",
|
||||
"Signal7",
|
||||
"Cruise_Brake_Active",
|
||||
"Distance_Swap",
|
||||
"Cruise_EPB",
|
||||
"Signal4",
|
||||
"Close_Distance",
|
||||
"Signal5",
|
||||
"Cruise_Cancel",
|
||||
"Cruise_Set",
|
||||
"Cruise_Resume",
|
||||
"Signal6",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = frame % 0x10
|
||||
|
||||
if long_enabled:
|
||||
values["Cruise_Throttle"] = cruise_throttle
|
||||
|
||||
# Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
|
||||
values["Cruise_Soft_Disable"] = 0
|
||||
values["Cruise_Fault"] = 0
|
||||
|
||||
values["Cruise_Brake_Active"] = brake_cmd
|
||||
|
||||
if pcm_cancel_cmd:
|
||||
values["Cruise_Cancel"] = 1
|
||||
values["Cruise_Throttle"] = 1818 # inactive throttle
|
||||
|
||||
return packer.make_can_msg("ES_Distance", bus, values)
|
||||
|
||||
|
||||
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
|
||||
values = {s: es_lkas_state_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"LKAS_Alert_Msg",
|
||||
"Signal1",
|
||||
"LKAS_ACTIVE",
|
||||
"LKAS_Dash_State",
|
||||
"Signal2",
|
||||
"Backward_Speed_Limit_Menu",
|
||||
"LKAS_Left_Line_Enable",
|
||||
"LKAS_Left_Line_Light_Blink",
|
||||
"LKAS_Right_Line_Enable",
|
||||
"LKAS_Right_Line_Light_Blink",
|
||||
"LKAS_Left_Line_Visible",
|
||||
"LKAS_Right_Line_Visible",
|
||||
"LKAS_Alert",
|
||||
"Signal3",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = frame % 0x10
|
||||
|
||||
# Filter the stock LKAS "Keep hands on wheel" alert
|
||||
if values["LKAS_Alert_Msg"] == 1:
|
||||
values["LKAS_Alert_Msg"] = 0
|
||||
|
||||
# Filter the stock LKAS sending an audible alert when it turns off LKAS
|
||||
if values["LKAS_Alert"] == 27:
|
||||
values["LKAS_Alert"] = 0
|
||||
|
||||
# Filter the stock LKAS sending an audible alert when "Keep hands on wheel" alert is active (2020+ models)
|
||||
if values["LKAS_Alert"] == 28 and values["LKAS_Alert_Msg"] == 7:
|
||||
values["LKAS_Alert"] = 0
|
||||
|
||||
# Filter the stock LKAS sending an audible alert when "Keep hands on wheel OFF" alert is active (2020+ models)
|
||||
if values["LKAS_Alert"] == 30:
|
||||
values["LKAS_Alert"] = 0
|
||||
|
||||
# Filter the stock LKAS sending "Keep hands on wheel OFF" alert (2020+ models)
|
||||
if values["LKAS_Alert_Msg"] == 7:
|
||||
values["LKAS_Alert_Msg"] = 0
|
||||
|
||||
# Show Keep hands on wheel alert for openpilot steerRequired alert
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
values["LKAS_Alert_Msg"] = 1
|
||||
|
||||
# Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW)
|
||||
if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0:
|
||||
if left_lane_depart:
|
||||
values["LKAS_Alert"] = 12 # Left lane departure dash alert
|
||||
elif right_lane_depart:
|
||||
values["LKAS_Alert"] = 11 # Right lane departure dash alert
|
||||
|
||||
if lat_active:
|
||||
values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
|
||||
values["LKAS_Dash_State"] = 2 # Green enabled indicator
|
||||
values["LKAS_Left_Line_Enable"] = 1
|
||||
values["LKAS_Right_Line_Enable"] = 1
|
||||
else:
|
||||
values["LKAS_Dash_State"] = 0 # LKAS Not enabled
|
||||
|
||||
values["LKAS_Left_Line_Visible"] = int(left_line)
|
||||
values["LKAS_Right_Line_Visible"] = int(right_line)
|
||||
|
||||
return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
|
||||
|
||||
def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
|
||||
values = {s: dashstatus_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"PCB_Off",
|
||||
"LDW_Off",
|
||||
"Signal1",
|
||||
"Cruise_State_Msg",
|
||||
"LKAS_State_Msg",
|
||||
"Signal2",
|
||||
"Cruise_Soft_Disable",
|
||||
"Cruise_Status_Msg",
|
||||
"Signal3",
|
||||
"Cruise_Distance",
|
||||
"Signal4",
|
||||
"Conventional_Cruise",
|
||||
"Signal5",
|
||||
"Cruise_Disengaged",
|
||||
"Cruise_Activated",
|
||||
"Signal6",
|
||||
"Cruise_Set_Speed",
|
||||
"Cruise_Fault",
|
||||
"Cruise_On",
|
||||
"Display_Own_Car",
|
||||
"Brake_Lights",
|
||||
"Car_Follow",
|
||||
"Signal7",
|
||||
"Far_Distance",
|
||||
"Cruise_State",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = frame % 0x10
|
||||
|
||||
if long_enabled:
|
||||
values["Cruise_State"] = 0
|
||||
values["Cruise_Activated"] = enabled
|
||||
values["Cruise_Disengaged"] = 0
|
||||
values["Car_Follow"] = int(lead_visible)
|
||||
|
||||
values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
|
||||
values["LDW_Off"] = 0
|
||||
values["Cruise_Fault"] = 0
|
||||
|
||||
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
|
||||
if values["LKAS_State_Msg"] in (2, 3):
|
||||
values["LKAS_State_Msg"] = 0
|
||||
|
||||
return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
|
||||
|
||||
def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value):
|
||||
values = {s: es_brake_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"Signal1",
|
||||
"Brake_Pressure",
|
||||
"AEB_Status",
|
||||
"Cruise_Brake_Lights",
|
||||
"Cruise_Brake_Fault",
|
||||
"Cruise_Brake_Active",
|
||||
"Cruise_Activated",
|
||||
"Signal3",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = frame % 0x10
|
||||
|
||||
if long_enabled:
|
||||
values["Cruise_Brake_Fault"] = 0
|
||||
values["Cruise_Activated"] = long_active
|
||||
|
||||
values["Brake_Pressure"] = brake_value
|
||||
|
||||
values["Cruise_Brake_Active"] = brake_value > 0
|
||||
values["Cruise_Brake_Lights"] = brake_value >= 70
|
||||
|
||||
return packer.make_can_msg("ES_Brake", CanBus.main, values)
|
||||
|
||||
def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm):
|
||||
values = {s: es_status_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"Signal1",
|
||||
"Cruise_Fault",
|
||||
"Cruise_RPM",
|
||||
"Cruise_Activated",
|
||||
"Brake_Lights",
|
||||
"Cruise_Hold",
|
||||
"Signal3",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = frame % 0x10
|
||||
|
||||
if long_enabled:
|
||||
values["Cruise_RPM"] = cruise_rpm
|
||||
values["Cruise_Fault"] = 0
|
||||
|
||||
values["Cruise_Activated"] = long_active
|
||||
|
||||
return packer.make_can_msg("ES_Status", CanBus.main, values)
|
||||
|
||||
|
||||
def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
|
||||
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
|
||||
values = {s: es_infotainment_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"LKAS_State_Infotainment",
|
||||
"LKAS_Blue_Lines",
|
||||
"Signal1",
|
||||
"Signal2",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = frame % 0x10
|
||||
|
||||
if values["LKAS_State_Infotainment"] in (3, 4):
|
||||
values["LKAS_State_Infotainment"] = 0
|
||||
|
||||
# Show Keep hands on wheel alert for openpilot steerRequired alert
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
values["LKAS_State_Infotainment"] = 3
|
||||
|
||||
# Show Obstacle Detected for fcw
|
||||
if visual_alert == VisualAlert.fcw:
|
||||
values["LKAS_State_Infotainment"] = 2
|
||||
|
||||
return packer.make_can_msg("ES_Infotainment", CanBus.main, values)
|
||||
|
||||
|
||||
def create_es_highbeamassist(packer):
|
||||
values = {
|
||||
"HBA_Available": False,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ES_HighBeamAssist", CanBus.main, values)
|
||||
|
||||
|
||||
def create_es_static_1(packer):
|
||||
values = {
|
||||
"SET_3": 3,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ES_STATIC_1", CanBus.main, values)
|
||||
|
||||
|
||||
def create_es_static_2(packer):
|
||||
values = {
|
||||
"SET_3": 3,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ES_STATIC_2", CanBus.main, values)
|
||||
|
||||
|
||||
# *** Subaru Pre-global ***
|
||||
|
||||
def subaru_preglobal_checksum(packer, values, addr, checksum_byte=7):
|
||||
dat = packer.make_can_msg(addr, 0, values)[2]
|
||||
return (sum(dat[:checksum_byte]) + sum(dat[checksum_byte+1:])) % 256
|
||||
|
||||
|
||||
def create_preglobal_steering_control(packer, frame, apply_steer, steer_req):
|
||||
values = {
|
||||
"COUNTER": frame % 0x08,
|
||||
"LKAS_Command": apply_steer,
|
||||
"LKAS_Active": steer_req,
|
||||
}
|
||||
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS")
|
||||
|
||||
return packer.make_can_msg("ES_LKAS", CanBus.main, values)
|
||||
|
||||
|
||||
def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
|
||||
values = {s: es_distance_msg[s] for s in [
|
||||
"Cruise_Throttle",
|
||||
"Signal1",
|
||||
"Car_Follow",
|
||||
"Signal2",
|
||||
"Cruise_Brake_Active",
|
||||
"Distance_Swap",
|
||||
"Standstill",
|
||||
"Signal3",
|
||||
"Close_Distance",
|
||||
"Signal4",
|
||||
"Standstill_2",
|
||||
"Cruise_Fault",
|
||||
"Signal5",
|
||||
"COUNTER",
|
||||
"Signal6",
|
||||
"Cruise_Button",
|
||||
"Signal7",
|
||||
]}
|
||||
|
||||
values["Cruise_Button"] = cruise_button
|
||||
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance")
|
||||
|
||||
return packer.make_can_msg("ES_Distance", CanBus.main, values)
|
||||
20
selfdrive/car/subaru/tests/test_subaru.py
Executable file
20
selfdrive/car/subaru/tests/test_subaru.py
Executable file
@@ -0,0 +1,20 @@
|
||||
from cereal import car
|
||||
import unittest
|
||||
from openpilot.selfdrive.car.subaru.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
|
||||
|
||||
|
||||
class TestSubaruFingerprint(unittest.TestCase):
|
||||
def test_fw_version_format(self):
|
||||
for platform, fws_per_ecu in FW_VERSIONS.items():
|
||||
for (ecu, _, _), fws in fws_per_ecu.items():
|
||||
fw_size = len(fws[0])
|
||||
for fw in fws:
|
||||
self.assertEqual(len(fw), fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user