selfdrive/car
George Hotz
parent
978e0eb986
commit
71ead9adea
|
|
@ -0,0 +1,38 @@
|
|||
In here lies the car abstraction layer, and is part of what you will implement to add a new car
|
||||
|
||||
== Car State ==
|
||||
|
||||
Implement the following minimal set of sensors:
|
||||
Speed -- best estimate of the speed of the car
|
||||
SteeringAngle -- current angle of the steering wheel
|
||||
ControlSurface -- gas position, brake position, steering torque
|
||||
|
||||
Implement the following optional sensors:
|
||||
RadarPoints -- Currently not optional, but could be with VOACC. Data from the radar of the car.
|
||||
WheelSpeeds -- The speed of the 4 wheels of the car. To be used for odometry
|
||||
|
||||
Implement the following controls:
|
||||
ControlSurface -- gas position (optional if PCM), brake position, steering torque
|
||||
|
||||
If you don't have a pedal interceptor and you use the PCM cruise control:
|
||||
CruiseState -- If system is enabled, and current set speed
|
||||
CruiseControl -- Commands to modify system speed and acceleration
|
||||
|
||||
== Car UI ==
|
||||
|
||||
I suspect many cars with ACC and LKAS have similar UIs
|
||||
|
||||
HUDControl
|
||||
|
||||
== Car Buttons ==
|
||||
|
||||
Uses an evented protocol
|
||||
|
||||
* Blinkers
|
||||
* Cruise buttons
|
||||
* Other buttons
|
||||
|
||||
== Car Errors ==
|
||||
|
||||
See capnp for list of errors
|
||||
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
# functions common among cars
|
||||
from common.numpy_fast import clip
|
||||
|
||||
# kg of standard extra cargo to count for drive, gas, etc...
|
||||
STD_CARGO_KG = 136.
|
||||
|
||||
def gen_empty_fingerprint():
|
||||
return {i: {} for i in range(0, 4)}
|
||||
|
||||
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
|
||||
# scale unknown params for other cars
|
||||
class CivicParams:
|
||||
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 CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.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=1.0):
|
||||
center_to_rear = wheelbase - center_to_front
|
||||
tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \
|
||||
(center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE)
|
||||
|
||||
tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \
|
||||
(center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE)
|
||||
|
||||
return tire_stiffness_front, tire_stiffness_rear
|
||||
|
||||
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None):
|
||||
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc}
|
||||
|
||||
|
||||
def apply_std_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_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS):
|
||||
# limits due to comparison of commanded torque VS motor reported torque
|
||||
max_lim = min(max(motor_torque + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_ERROR_MAX), LIMITS.STEER_MAX)
|
||||
min_lim = max(min(motor_torque - LIMITS.STEER_ERROR_MAX, -LIMITS.STEER_ERROR_MAX), -LIMITS.STEER_MAX)
|
||||
|
||||
apply_torque = clip(apply_torque, min_lim, max_lim)
|
||||
|
||||
# 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 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 j in range(8):
|
||||
if ((crc & 0x80) != 0):
|
||||
crc = ((crc << 1) ^ poly) & 0xFF
|
||||
else:
|
||||
crc <<= 1
|
||||
return crc
|
||||
|
||||
|
||||
def create_gas_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 is_ecu_disconnected(fingerprint, fingerprint_list, ecu_fingerprint, car, ecu):
|
||||
# check if a stock ecu is disconnected by looking for specific CAN msgs in the fingerprint
|
||||
# return True if the reference car fingerprint contains the ecu fingerprint msg and
|
||||
# fingerprint does not contains messages normally sent by a given ecu
|
||||
ecu_in_car = False
|
||||
for car_finger in fingerprint_list[car]:
|
||||
if any(msg in car_finger for msg in ecu_fingerprint[ecu]):
|
||||
ecu_in_car = True
|
||||
|
||||
return ecu_in_car and not any(msg in fingerprint for msg in ecu_fingerprint[ecu])
|
||||
|
||||
|
||||
def make_can_msg(addr, dat, bus):
|
||||
return [addr, 0, dat, bus]
|
||||
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
import os
|
||||
from common.params import Params
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
|
||||
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
|
||||
from selfdrive.car.fw_versions import get_fw_versions
|
||||
from selfdrive.swaglog import cloudlog
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
def get_startup_alert(car_recognized, controller_available):
|
||||
alert = 'startup'
|
||||
if not car_recognized:
|
||||
alert = 'startupNoCar'
|
||||
elif car_recognized and not controller_available:
|
||||
alert = 'startupNoControl'
|
||||
return alert
|
||||
|
||||
|
||||
def load_interfaces(brand_names):
|
||||
ret = {}
|
||||
for brand_name in brand_names:
|
||||
path = ('selfdrive.car.%s' % brand_name)
|
||||
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
|
||||
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)
|
||||
return ret
|
||||
|
||||
|
||||
def _get_interface_names():
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car names that which we have an interface for
|
||||
# - values are lists of spefic car models for a given car
|
||||
brand_names = {}
|
||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||
try:
|
||||
brand_name = car_folder.split('/')[-1]
|
||||
model_names = __import__('selfdrive.car.%s.values' % brand_name, fromlist=['CAR']).CAR
|
||||
model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
|
||||
brand_names[brand_name] = model_names
|
||||
except (ImportError, IOError):
|
||||
pass
|
||||
|
||||
return brand_names
|
||||
|
||||
|
||||
# imports from directory selfdrive/car/<name>/
|
||||
interfaces = load_interfaces(_get_interface_names())
|
||||
|
||||
def only_toyota_left(candidate_cars):
|
||||
return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0
|
||||
|
||||
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
|
||||
# **** for use live only ****
|
||||
def fingerprint(logcan, sendcan, has_relay):
|
||||
if has_relay:
|
||||
# Vin query only reliably works thorugh OBDII
|
||||
bus = 1
|
||||
addr, vin = get_vin(logcan, sendcan, bus)
|
||||
_, car_fw = get_fw_versions(logcan, sendcan, bus)
|
||||
else:
|
||||
vin = VIN_UNKNOWN
|
||||
_, car_fw = set(), []
|
||||
|
||||
cloudlog.warning("VIN %s", vin)
|
||||
Params().put("CarVin", vin)
|
||||
|
||||
finger = gen_empty_fingerprint()
|
||||
candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
|
||||
frame = 0
|
||||
frame_fingerprint = 10 # 0.1s
|
||||
car_fingerprint = None
|
||||
done = False
|
||||
|
||||
while not done:
|
||||
a = messaging.get_one_can(logcan)
|
||||
|
||||
for can in a.can:
|
||||
# need to independently try to fingerprint both bus 0 and 1 to work
|
||||
# for the combo black_panda and honda_bosch. Ignore extended messages
|
||||
# and VIN query response.
|
||||
# Include bus 2 for toyotas to disambiguate cars using camera messages
|
||||
# (ideally should be done for all cars but we can't for Honda Bosch)
|
||||
if can.src in range(0, 4):
|
||||
finger[can.src][can.address] = len(can.dat)
|
||||
for b in candidate_cars:
|
||||
if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) 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:
|
||||
# Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
|
||||
if only_toyota_left(candidate_cars[b]):
|
||||
frame_fingerprint = 100 # 1s
|
||||
if len(candidate_cars[b]) == 1:
|
||||
if 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()) or frame > 200
|
||||
succeeded = car_fingerprint is not None
|
||||
done = failed or succeeded
|
||||
|
||||
frame += 1
|
||||
|
||||
cloudlog.warning("fingerprinted %s", car_fingerprint)
|
||||
return car_fingerprint, finger, vin, car_fw
|
||||
|
||||
|
||||
def get_car(logcan, sendcan, has_relay=False):
|
||||
candidate, fingerprints, vin, car_fw = fingerprint(logcan, sendcan, has_relay)
|
||||
|
||||
if candidate is None:
|
||||
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
|
||||
candidate = "mock"
|
||||
|
||||
CarInterface, CarController = interfaces[candidate]
|
||||
car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw)
|
||||
car_params.carVin = vin
|
||||
car_params.carFw = car_fw
|
||||
|
||||
return CarInterface(car_params, CarController), car_params
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
|
||||
create_wheel_buttons
|
||||
from selfdrive.car.chrysler.values import ECU, CAR, SteerLimitParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera):
|
||||
self.braking = False
|
||||
# redundant safety check with the board
|
||||
self.controls_allowed = True
|
||||
self.apply_steer_last = 0
|
||||
self.ccframe = 0
|
||||
self.prev_frame = -1
|
||||
self.hud_count = 0
|
||||
self.car_fingerprint = car_fingerprint
|
||||
self.alert_active = False
|
||||
self.gone_fast_yet = False
|
||||
self.steer_rate_limited = False
|
||||
|
||||
self.fake_ecus = set()
|
||||
if enable_camera:
|
||||
self.fake_ecus.add(ECU.CAM)
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
|
||||
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
|
||||
# this seems needed to avoid steering faults and to force the sync with the EPS counter
|
||||
frame = CS.lkas_counter
|
||||
if self.prev_frame == frame:
|
||||
return []
|
||||
|
||||
# *** compute control surfaces ***
|
||||
# steer torque
|
||||
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
|
||||
CS.steer_torque_motor, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
|
||||
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
self.gone_fast_yet = True
|
||||
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
|
||||
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
|
||||
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
|
||||
lkas_active = moving_fast and enabled
|
||||
|
||||
if not lkas_active:
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
can_sends = []
|
||||
|
||||
#*** control msgs ***
|
||||
|
||||
if pcm_cancel_cmd:
|
||||
# TODO: would be better to start from frame_2b3
|
||||
new_msg = create_wheel_buttons(self.ccframe)
|
||||
can_sends.append(new_msg)
|
||||
|
||||
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
|
||||
# frame is 100Hz (0.01s period)
|
||||
if (self.ccframe % 25 == 0): # 0.25s period
|
||||
if (CS.lkas_car_model != -1):
|
||||
new_msg = create_lkas_hud(
|
||||
self.packer, CS.gear_shifter, lkas_active, hud_alert,
|
||||
self.hud_count, CS.lkas_car_model)
|
||||
can_sends.append(new_msg)
|
||||
self.hud_count += 1
|
||||
|
||||
new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
|
||||
can_sends.append(new_msg)
|
||||
|
||||
self.ccframe += 1
|
||||
self.prev_frame = frame
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def parse_gear_shifter(can_gear):
|
||||
if can_gear == 0x1:
|
||||
return GearShifter.park
|
||||
elif can_gear == 0x2:
|
||||
return GearShifter.reverse
|
||||
elif can_gear == 0x3:
|
||||
return GearShifter.neutral
|
||||
elif can_gear == 0x4:
|
||||
return GearShifter.drive
|
||||
elif can_gear == 0x5:
|
||||
return GearShifter.low
|
||||
return GearShifter.unknown
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("PRNDL", "GEAR", 0),
|
||||
("DOOR_OPEN_FL", "DOORS", 0),
|
||||
("DOOR_OPEN_FR", "DOORS", 0),
|
||||
("DOOR_OPEN_RL", "DOORS", 0),
|
||||
("DOOR_OPEN_RR", "DOORS", 0),
|
||||
("BRAKE_PRESSED_2", "BRAKE_2", 0),
|
||||
("ACCEL_134", "ACCEL_GAS_134", 0),
|
||||
("SPEED_LEFT", "SPEED_1", 0),
|
||||
("SPEED_RIGHT", "SPEED_1", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("STEER_ANGLE", "STEERING", 0),
|
||||
("STEERING_RATE", "STEERING", 0),
|
||||
("TURN_SIGNALS", "STEERING_LEVERS", 0),
|
||||
("ACC_STATUS_2", "ACC_2", 0),
|
||||
("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0),
|
||||
("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0),
|
||||
("TORQUE_DRIVER", "EPS_STATUS", 0),
|
||||
("TORQUE_MOTOR", "EPS_STATUS", 0),
|
||||
("LKAS_STATE", "EPS_STATUS", 1),
|
||||
("COUNTER", "EPS_STATUS", -1),
|
||||
("TRACTION_OFF", "TRACTION_BUTTON", 0),
|
||||
("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0),
|
||||
("COUNTER", "WHEEL_BUTTONS", -1), # incrementing counter for 23b
|
||||
]
|
||||
|
||||
# It's considered invalid if it is not received for 10x the expected period (1/f).
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("BRAKE_2", 50),
|
||||
("EPS_STATUS", 100),
|
||||
("SPEED_1", 100),
|
||||
("WHEEL_SPEEDS", 50),
|
||||
("STEERING", 100),
|
||||
("ACC_2", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
def get_camera_parser(CP):
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
# TODO read in all the other values
|
||||
("COUNTER", "LKAS_COMMAND", -1),
|
||||
("CAR_MODEL", "LKAS_HUD", -1),
|
||||
("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1)
|
||||
]
|
||||
checks = []
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER'])
|
||||
self.frame = int(cp.vl["EPS_STATUS"]['COUNTER'])
|
||||
|
||||
self.door_all_closed = not any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
|
||||
|
||||
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
|
||||
self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
|
||||
self.car_gas = self.pedal_gas
|
||||
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
|
||||
|
||||
self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
|
||||
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
|
||||
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
|
||||
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
|
||||
v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = not v_wheel > 0.001
|
||||
|
||||
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
|
||||
self.gear_shifter = parse_gear_shifter(cp.vl['GEAR']['PRNDL'])
|
||||
self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
self.steer_torque_driver = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
|
||||
self.steer_torque_motor = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
|
||||
self.steer_error = steer_state == 4 or (steer_state == 0 and self.v_ego > self.CP.minSteerSpeed)
|
||||
|
||||
self.user_brake = 0
|
||||
self.brake_lights = self.brake_pressed
|
||||
self.v_cruise_pcm = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH']
|
||||
self.pcm_acc_status = self.main_on
|
||||
|
||||
self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
|
||||
|
||||
self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER']
|
||||
self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL']
|
||||
self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK']
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import make_can_msg
|
||||
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def calc_checksum(data):
|
||||
"""This function does not want the checksum byte in the input data.
|
||||
|
||||
jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf
|
||||
"""
|
||||
end_index = len(data)
|
||||
index = 0
|
||||
checksum = 0xFF
|
||||
temp_chk = 0
|
||||
bit_sum = 0
|
||||
if(end_index <= index):
|
||||
return False
|
||||
for index in range(0, end_index):
|
||||
shift = 0x80
|
||||
curr = data[index]
|
||||
iterate = 8
|
||||
while(iterate > 0):
|
||||
iterate -= 1
|
||||
bit_sum = curr & shift
|
||||
temp_chk = checksum & 0x80
|
||||
if (bit_sum != 0):
|
||||
bit_sum = 0x1C
|
||||
if (temp_chk != 0):
|
||||
bit_sum = 1
|
||||
checksum = checksum << 1
|
||||
temp_chk = checksum | 1
|
||||
bit_sum ^= temp_chk
|
||||
else:
|
||||
if (temp_chk != 0):
|
||||
bit_sum = 0x1D
|
||||
checksum = checksum << 1
|
||||
bit_sum ^= checksum
|
||||
checksum = bit_sum
|
||||
shift = shift >> 1
|
||||
return ~checksum & 0xFF
|
||||
|
||||
|
||||
def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model):
|
||||
# LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed.
|
||||
|
||||
if hud_alert == VisualAlert.steerRequired:
|
||||
msg = b'\x00\x00\x00\x03\x00\x00\x00\x00'
|
||||
return make_can_msg(0x2a6, msg, 0)
|
||||
|
||||
color = 1 # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019
|
||||
lines = 1
|
||||
alerts = 0
|
||||
|
||||
if hud_count < (1 *4): # first 3 seconds, 4Hz
|
||||
alerts = 1
|
||||
# CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID
|
||||
# had color = 1 and lines = 1 but trying 2017 hybrid style for now.
|
||||
if gear in (GearShifter.drive, GearShifter.reverse, GearShifter.low):
|
||||
if lkas_active:
|
||||
color = 2 # control active, display green.
|
||||
lines = 6
|
||||
else:
|
||||
color = 1 # control off, display white.
|
||||
lines = 1
|
||||
|
||||
values = {
|
||||
"LKAS_ICON_COLOR": color, # byte 0, last 2 bits
|
||||
"CAR_MODEL": lkas_car_model, # byte 1
|
||||
"LKAS_LANE_LINES": lines, # byte 2, last 4 bits
|
||||
"LKAS_ALERTS": alerts, # byte 3, last 4 bits
|
||||
}
|
||||
|
||||
return packer.make_can_msg("LKAS_HUD", 0, values) # 0x2a6
|
||||
|
||||
|
||||
def create_lkas_command(packer, apply_steer, moving_fast, frame):
|
||||
# LKAS_COMMAND 0x292 (658) Lane-keeping signal to turn the wheel.
|
||||
values = {
|
||||
"LKAS_STEERING_TORQUE": apply_steer,
|
||||
"LKAS_HIGH_TORQUE": int(moving_fast),
|
||||
"COUNTER": frame % 0x10,
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("LKAS_COMMAND", 0, values)[2]
|
||||
dat = dat[:-1]
|
||||
checksum = calc_checksum(dat)
|
||||
|
||||
values["CHECKSUM"] = checksum
|
||||
return packer.make_can_msg("LKAS_COMMAND", 0, values)
|
||||
|
||||
|
||||
def create_wheel_buttons(frame):
|
||||
# WHEEL_BUTTONS (571) Message sent to cancel ACC.
|
||||
start = b"\x01" # acc cancel set
|
||||
counter = (frame % 10) << 4
|
||||
dat = start + counter.to_bytes(1, 'little')
|
||||
dat = dat + calc_checksum(dat).to_bytes(1, 'little')
|
||||
return make_can_msg(0x23b, dat, 0)
|
||||
|
|
@ -0,0 +1,235 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser
|
||||
from selfdrive.car.chrysler.values import ECU, ECU_FINGERPRINT, CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
|
||||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
self.low_speed_alert = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_camera_parser(CP)
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "chrysler"
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.chrysler
|
||||
|
||||
# pedal
|
||||
ret.enableCruise = True
|
||||
|
||||
# Speed conversion: 20, 45 mph
|
||||
ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017
|
||||
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
|
||||
ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017
|
||||
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 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerRateCost = 0.7
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
|
||||
ret.wheelbase = 2.91 # in meters
|
||||
ret.steerRatio = 12.7
|
||||
ret.steerActuatorDelay = 0.2 # in seconds
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
ret.minSteerSpeed = 3.8 # m/s
|
||||
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
||||
if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
|
||||
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
|
||||
# TODO allow 2019 cars to steer down to 13 m/s if already engaged.
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
|
||||
|
||||
# no rear steering, at least on the listed cars above
|
||||
ret.steerRatioRear = 0.
|
||||
|
||||
# steer, gas, brake limitations VS speed
|
||||
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph
|
||||
ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.5]
|
||||
ret.brakeMaxBP = [5., 20.]
|
||||
ret.brakeMaxV = [1., 0.8]
|
||||
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
print("ECU Camera Simulated: {0}".format(ret.enableCamera))
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
ret.stoppingControl = False
|
||||
ret.startAccel = 0.0
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 9.]
|
||||
ret.longitudinalTuning.deadzoneV = [0., .15]
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
ret.gasPressed = self.CS.pedal_gas > 0
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = self.CS.main_on
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
|
||||
|
||||
ret.genericToggle = self.CS.generic_toggle
|
||||
#ret.lkasCounter = self.CS.lkas_counter
|
||||
#ret.lkasCarModel = self.CS.lkas_car_model
|
||||
|
||||
# events
|
||||
events = []
|
||||
if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
|
||||
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
elif not ret.cruiseState.enabled:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
# disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC
|
||||
# from a 3+ second stop.
|
||||
if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if self.low_speed_alert:
|
||||
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
if (self.CS.frame == -1):
|
||||
return [] # if we haven't seen a frame 220, then do not update.
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
from opendbc.can.parser import CANParser
|
||||
from cereal import car
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
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():
|
||||
dbc_f = 'chrysler_pacifica_2017_hybrid_private_fusion.dbc'
|
||||
msg_n = len(RADAR_MSGS_C)
|
||||
# list of [(signal name, message name or number, initial values), (...)]
|
||||
# [('RADAR_STATE', 1024, 0),
|
||||
# ('LONG_DIST', 1072, 255),
|
||||
# ('LONG_DIST', 1073, 255),
|
||||
# ('LONG_DIST', 1074, 255),
|
||||
# ('LONG_DIST', 1075, 255),
|
||||
|
||||
# The factor and offset are applied by the dbc parsing library, so the
|
||||
# default values should be after the factor/offset are applied.
|
||||
signals = list(zip(['LONG_DIST'] * msg_n +
|
||||
['LAT_DIST'] * msg_n +
|
||||
['REL_SPEED'] * msg_n,
|
||||
RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST
|
||||
RADAR_MSGS_D, # REL_SPEED
|
||||
[0] * msg_n + # LONG_DIST
|
||||
[-1000] * msg_n + # LAT_DIST
|
||||
[-146.278] * msg_n)) # REL_SPEED set to 0, factor/offset to this
|
||||
# TODO what are the checks actually used for?
|
||||
# honda only checks the last message,
|
||||
# toyota checks all the messages. Which do we want?
|
||||
checks = list(zip(RADAR_MSGS_C +
|
||||
RADAR_MSGS_D,
|
||||
[20]*msg_n + # 20Hz (0.05s)
|
||||
[20]*msg_n)) # 20Hz (0.05s)
|
||||
|
||||
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 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):
|
||||
self.pts = {}
|
||||
self.delay = 0 # Delay of radar #TUNE
|
||||
self.rcp = _create_radar_can_parser()
|
||||
self.updated_messages = set()
|
||||
self.trigger_msg = LAST_MSG
|
||||
|
||||
def update(self, can_strings):
|
||||
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
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
import unittest
|
||||
|
||||
from cereal import car
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car.chrysler import chryslercan
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
|
||||
|
||||
class TestChryslerCan(unittest.TestCase):
|
||||
|
||||
def test_checksum(self):
|
||||
self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20"))
|
||||
self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20"))
|
||||
|
||||
def test_hud(self):
|
||||
packer = CANPacker('chrysler_pacifica_2017_hybrid')
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, b'\x01\x00\x01\x01\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
GearShifter.park, False, False, 1, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
GearShifter.park, False, False, 5*4, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
GearShifter.park, False, False, 99999, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, b'\x02\x00\x06\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
GearShifter.drive, True, False, 99999, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, b'\x02\x64\x06\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
GearShifter.drive, True, False, 99999, 0x64))
|
||||
|
||||
def test_command(self):
|
||||
packer = CANPacker('chrysler_pacifica_2017_hybrid')
|
||||
self.assertEqual(
|
||||
[0x292, 0, b'\x14\x00\x00\x00\x10\x86', 0],
|
||||
chryslercan.create_lkas_command(
|
||||
packer,
|
||||
0, True, 1))
|
||||
self.assertEqual(
|
||||
[0x292, 0, b'\x04\x00\x00\x00\x80\x83', 0],
|
||||
chryslercan.create_lkas_command(
|
||||
packer,
|
||||
0, False, 8))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 261 # 262 faults
|
||||
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
|
||||
STEER_DELTA_DOWN = 3 # no faults on the way down it seems
|
||||
STEER_ERROR_MAX = 80
|
||||
|
||||
|
||||
|
||||
class CAR:
|
||||
PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017"
|
||||
PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018"
|
||||
PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019"
|
||||
PACIFICA_2018 = "CHRYSLER PACIFICA 2018" # Also covers Pacifica 2017.
|
||||
JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017.
|
||||
JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019"
|
||||
|
||||
# Unique can messages:
|
||||
# Only the hybrids have 270: 8
|
||||
# Only the gas have 55: 8, 416: 7
|
||||
# For 564, All 2017 have length 4, whereas 2018-19 have length 8.
|
||||
# For 924, Pacifica 2017 has length 3, whereas all 2018-19 have length 8.
|
||||
# For 560, All 2019 have length 8, whereas all 2017-18 have length 4.
|
||||
|
||||
# Jeep Grand Cherokee unique messages:
|
||||
# 2017 Trailhawk: 618: 8
|
||||
# For 924, Trailhawk 2017 has length 3, whereas 2018 V6 has length 8.
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.PACIFICA_2017_HYBRID: [
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8},
|
||||
],
|
||||
CAR.PACIFICA_2018: [
|
||||
{55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
|
||||
{55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
|
||||
],
|
||||
CAR.PACIFICA_2018_HYBRID: [
|
||||
{68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8},
|
||||
# based on 9ae7821dc4e92455|2019-07-01--16-42-55
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8},
|
||||
],
|
||||
CAR.PACIFICA_2019_HYBRID: [
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8},
|
||||
# Based on 0607d2516fc2148f|2019-02-13--23-03-16
|
||||
{
|
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
|
||||
},
|
||||
# Based on 3c7ce223e3571b54|2019-05-11--20-16-14
|
||||
{
|
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8
|
||||
}
|
||||
],
|
||||
CAR.JEEP_CHEROKEE: [
|
||||
# JEEP GRAND CHEROKEE V6 2018
|
||||
{55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
# Jeep Grand Cherokee 2017 Trailhawk
|
||||
{257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
],
|
||||
CAR.JEEP_CHEROKEE_2019: [
|
||||
# Jeep Grand Cherokee 2019 from Switzerland
|
||||
# 530: 8 is so far only in this Jeep.
|
||||
{55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
DBC = {
|
||||
CAR.PACIFICA_2017_HYBRID: dbc_dict(
|
||||
'chrysler_pacifica_2017_hybrid', # 'pt'
|
||||
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
|
||||
CAR.PACIFICA_2018: dbc_dict( # Same DBC file works.
|
||||
'chrysler_pacifica_2017_hybrid', # 'pt'
|
||||
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
|
||||
CAR.PACIFICA_2018_HYBRID: dbc_dict( # Same DBC file works.
|
||||
'chrysler_pacifica_2017_hybrid', # 'pt'
|
||||
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
|
||||
CAR.PACIFICA_2019_HYBRID: dbc_dict( # Same DBC file works.
|
||||
'chrysler_pacifica_2017_hybrid', # 'pt'
|
||||
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
|
||||
CAR.JEEP_CHEROKEE: dbc_dict( # Same DBC file works.
|
||||
'chrysler_pacifica_2017_hybrid', # 'pt'
|
||||
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
|
||||
CAR.JEEP_CHEROKEE_2019: dbc_dict( # Same DBC file works.
|
||||
'chrysler_pacifica_2017_hybrid', # 'pt'
|
||||
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 120
|
||||
|
||||
|
||||
class ECU:
|
||||
CAM = 0 # LKAS camera
|
||||
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [0x292], # lkas cmd
|
||||
}
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
import os
|
||||
from common.basedir import BASEDIR
|
||||
|
||||
def get_attr_from_cars(attr):
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car models
|
||||
# - values are attr values from all car folders
|
||||
result = {}
|
||||
|
||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||
try:
|
||||
car_name = car_folder.split('/')[-1]
|
||||
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=[attr])
|
||||
if hasattr(values, attr):
|
||||
attr_values = getattr(values, attr)
|
||||
else:
|
||||
continue
|
||||
|
||||
for f, v in attr_values.items():
|
||||
result[f] = v
|
||||
|
||||
except (ImportError, IOError):
|
||||
pass
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def get_fw_versions_list():
|
||||
return get_attr_from_cars('FW_VERSIONS')
|
||||
|
||||
|
||||
def get_fingerprint_list():
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car models for which we have a fingerprint
|
||||
# - values are lists dicts of messages that constitute the unique
|
||||
# CAN fingerprint of each car model and all its variants
|
||||
return get_attr_from_cars('FINGERPRINTS')
|
||||
|
||||
|
||||
FW_VERSIONS = get_fw_versions_list()
|
||||
_FINGERPRINTS = get_fingerprint_list()
|
||||
|
||||
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
||||
|
||||
def is_valid_for_fingerprint(msg, car_fingerprint):
|
||||
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:
|
||||
fingerprint.update(_DEBUG_ADDRESS) # add alien debug address
|
||||
|
||||
if is_valid_for_fingerprint(msg, fingerprint):
|
||||
compatible_cars.append(car_name)
|
||||
break
|
||||
|
||||
return compatible_cars
|
||||
|
||||
|
||||
def all_known_cars():
|
||||
"""Returns a list of all known car strings."""
|
||||
return list(_FINGERPRINTS.keys())
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import make_can_msg
|
||||
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
|
||||
MAX_STEER_DELTA = 1
|
||||
TOGGLE_DEBUG = False
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, enable_camera, vehicle_model):
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.enable_camera = enable_camera
|
||||
self.enabled_last = False
|
||||
self.main_on_last = False
|
||||
self.vehicle_model = vehicle_model
|
||||
self.generic_toggle_last = 0
|
||||
self.steer_alert_last = False
|
||||
self.lkas_action = 0
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
|
||||
|
||||
can_sends = []
|
||||
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
|
||||
|
||||
apply_steer = actuators.steer
|
||||
|
||||
if self.enable_camera:
|
||||
|
||||
if pcm_cancel:
|
||||
#print "CANCELING!!!!"
|
||||
can_sends.append(spam_cancel_button(self.packer))
|
||||
|
||||
if (frame % 3) == 0:
|
||||
|
||||
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego)
|
||||
|
||||
# The use of the toggle below is handy for trying out the various LKAS modes
|
||||
if TOGGLE_DEBUG:
|
||||
self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last)
|
||||
self.lkas_action &= 0xf
|
||||
else:
|
||||
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
|
||||
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
|
||||
CS.lkas_state, CS.angle_steers, curvature, self.lkas_action))
|
||||
self.generic_toggle_last = CS.generic_toggle
|
||||
|
||||
if (frame % 100) == 0:
|
||||
|
||||
can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
|
||||
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
|
||||
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
|
||||
(self.steer_alert_last != steer_alert):
|
||||
can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert))
|
||||
|
||||
if (frame % 200) == 0:
|
||||
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
|
||||
|
||||
if (frame % 10) == 0:
|
||||
|
||||
can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
|
||||
can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
|
||||
|
||||
can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
|
||||
can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
|
||||
can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
|
||||
can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
|
||||
|
||||
can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
|
||||
can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
|
||||
can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
|
||||
can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
|
||||
can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
|
||||
can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
|
||||
|
||||
static_msgs = range(1653, 1658)
|
||||
for addr in static_msgs:
|
||||
cnt = (frame % 10) + 1
|
||||
can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1))
|
||||
|
||||
self.enabled_last = enabled
|
||||
self.main_on_last = CS.main_on
|
||||
self.steer_alert_last = steer_alert
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
from opendbc.can.parser import CANParser
|
||||
from common.numpy_fast import mean
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.ford.values import DBC
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
WHEEL_RADIUS = 0.33
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("WhlRr_W_Meas", "WheelSpeed_CG1", 0.),
|
||||
("WhlRl_W_Meas", "WheelSpeed_CG1", 0.),
|
||||
("WhlFr_W_Meas", "WheelSpeed_CG1", 0.),
|
||||
("WhlFl_W_Meas", "WheelSpeed_CG1", 0.),
|
||||
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.),
|
||||
("Cruise_State", "Cruise_Status", 0.),
|
||||
("Set_Speed", "Cruise_Status", 0.),
|
||||
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0),
|
||||
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0),
|
||||
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0),
|
||||
("ApedPosScal_Pc_Actl", "EngineData_14", 0.),
|
||||
("Dist_Incr", "Steering_Buttons", 0.),
|
||||
("Brake_Drv_Appl", "Cruise_Status", 0.),
|
||||
("Brake_Lights", "BCM_to_HS_Body", 0.),
|
||||
]
|
||||
|
||||
checks = [
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp):
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = not v_wheel > 0.001
|
||||
|
||||
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
|
||||
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
|
||||
self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State']
|
||||
self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0
|
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl']
|
||||
# TODO: we also need raw driver torque, needed for Assisted Lane Change
|
||||
self.steer_override = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
|
||||
self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl']
|
||||
self.user_gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl']
|
||||
self.brake_pressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
|
||||
self.brake_lights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
|
||||
self.generic_toggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
from common.numpy_fast import clip
|
||||
from selfdrive.car.ford.values import MAX_ANGLE
|
||||
|
||||
|
||||
def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action):
|
||||
"""Creates a CAN message for the Ford Steer Command."""
|
||||
|
||||
#if enabled and lkas available:
|
||||
if enabled and lkas_state in [2,3]: #and (frame % 500) >= 3:
|
||||
action = lkas_action
|
||||
else:
|
||||
action = 0xf
|
||||
angle_cmd = angle_steers/MAX_ANGLE
|
||||
|
||||
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE)
|
||||
|
||||
values = {
|
||||
"Lkas_Action": action,
|
||||
"Lkas_Alert": 0xf, # no alerts
|
||||
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug?
|
||||
#"Lane_Curvature": 0, # is it just for debug?
|
||||
"Steer_Angle_Req": angle_cmd
|
||||
}
|
||||
return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values)
|
||||
|
||||
|
||||
def create_lkas_ui(packer, main_on, enabled, steer_alert):
|
||||
"""Creates a CAN message for the Ford Steer Ui."""
|
||||
|
||||
if not main_on:
|
||||
lines = 0xf
|
||||
elif enabled:
|
||||
lines = 0x3
|
||||
else:
|
||||
lines = 0x6
|
||||
|
||||
values = {
|
||||
"Set_Me_X80": 0x80,
|
||||
"Set_Me_X45": 0x45,
|
||||
"Set_Me_X30": 0x30,
|
||||
"Lines_Hud": lines,
|
||||
"Hands_Warning_W_Chime": steer_alert,
|
||||
}
|
||||
return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values)
|
||||
|
||||
def spam_cancel_button(packer):
|
||||
values = {
|
||||
"Cancel": 1
|
||||
}
|
||||
return packer.make_can_msg("Steering_Buttons", 0, values)
|
||||
|
|
@ -0,0 +1,180 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.ford.carstate import CarState, get_can_parser
|
||||
from selfdrive.car.ford.values import MAX_ANGLE, ECU, ECU_FINGERPRINT, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
|
||||
self.frame = 0
|
||||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
||||
self.cp = get_can_parser(CP)
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP.enableCamera, self.VM)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "ford"
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.ford
|
||||
ret.dashcamOnly = True
|
||||
|
||||
# pedal
|
||||
ret.enableCruise = True
|
||||
|
||||
ret.wheelbase = 2.85
|
||||
ret.steerRatio = 14.8
|
||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this
|
||||
ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
ret.steerLimitTimer = 0.8
|
||||
ret.steerRateCost = 1.0
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
tire_stiffness_factor = 0.5328
|
||||
|
||||
# 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.
|
||||
ret.minEnableSpeed = -1.
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
# no rear steering, at least on the listed cars above
|
||||
ret.steerRatioRear = 0.
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
|
||||
# steer, gas, brake limitations VS speed
|
||||
ret.steerMaxBP = [0.] # breakpoints at 1 and 40 kph
|
||||
ret.steerMaxV = [1.0] # 2/3rd torque allowed above 45 kph
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.5]
|
||||
ret.brakeMaxBP = [5., 20.]
|
||||
ret.brakeMaxV = [1., 0.8]
|
||||
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.openpilotLongitudinalControl = False
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
|
||||
ret.stoppingControl = False
|
||||
ret.startAccel = 0.0
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 9.]
|
||||
ret.longitudinalTuning.deadzoneV = [0., .15]
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.cp.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.user_gas / 100.
|
||||
ret.gasPressed = self.CS.user_gas > 0.0001
|
||||
ret.brakePressed = self.CS.brake_pressed
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
ret.cruiseState.enabled = not (self.CS.pcm_acc_status in [0, 3])
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm
|
||||
ret.cruiseState.available = self.CS.pcm_acc_status != 0
|
||||
|
||||
ret.genericToggle = self.CS.generic_toggle
|
||||
|
||||
# events
|
||||
events = []
|
||||
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
|
||||
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
|
||||
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
elif not ret.cruiseState.enabled:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
# disable on pedals rising edge or when brake is pressed and speed isn't zero
|
||||
if (ret.gasPressed and not self.gas_pressed_prev) or \
|
||||
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled:
|
||||
events.append(create_event('steerTempUnavailableMute', [ET.WARNING]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.hudControl.visualAlert, c.cruiseControl.cancel)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.ford.values import DBC
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_MSGS = list(range(0x500, 0x540))
|
||||
|
||||
def _create_radar_can_parser(car_fingerprint):
|
||||
dbc_f = DBC[car_fingerprint]['radar']
|
||||
msg_n = len(RADAR_MSGS)
|
||||
signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n,
|
||||
RADAR_MSGS * 3,
|
||||
[0] * msg_n + [0] * msg_n + [0] * msg_n))
|
||||
checks = list(zip(RADAR_MSGS, [20]*msg_n))
|
||||
|
||||
return CANParser(dbc_f, signals, checks, 1)
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.validCnt = {key: 0 for key in RADAR_MSGS}
|
||||
self.track_id = 0
|
||||
|
||||
self.delay = 0 # Delay of radar
|
||||
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = 0x53f
|
||||
self.updated_messages = set()
|
||||
|
||||
def update(self, can_strings):
|
||||
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 sorted(self.updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.validCnt[ii] = 0 # reset counter
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.validCnt[ii] += 1
|
||||
else:
|
||||
self.validCnt[ii] = max(self.validCnt[ii] -1, 0)
|
||||
#print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
|
||||
|
||||
# radar point only valid if there have been enough valid measurements
|
||||
if self.validCnt[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]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault
|
||||
|
||||
class CAR:
|
||||
FUSION = "FORD FUSION 2018"
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.FUSION: [{
|
||||
71: 8, 74: 8, 75: 8, 76: 8, 90: 8, 92: 8, 93: 8, 118: 8, 119: 8, 120: 8, 125: 8, 129: 8, 130: 8, 131: 8, 132: 8, 133: 8, 145: 8, 146: 8, 357: 8, 359: 8, 360: 8, 361: 8, 376: 8, 390: 8, 391: 8, 392: 8, 394: 8, 512: 8, 514: 8, 516: 8, 531: 8, 532: 8, 534: 8, 535: 8, 560: 8, 578: 8, 604: 8, 613: 8, 673: 8, 827: 8, 848: 8, 934: 8, 935: 8, 936: 8, 947: 8, 963: 8, 970: 8, 972: 8, 973: 8, 984: 8, 992: 8, 994: 8, 997: 8, 998: 8, 1003: 8, 1034: 8, 1045: 8, 1046: 8, 1053: 8, 1054: 8, 1058: 8, 1059: 8, 1068: 8, 1072: 8, 1073: 8, 1082: 8, 1107: 8, 1108: 8, 1109: 8, 1110: 8, 1200: 8, 1427: 8, 1430: 8, 1438: 8, 1459: 8
|
||||
}],
|
||||
}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [970, 973, 984]
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
|
||||
}
|
||||
|
|
@ -0,0 +1,192 @@
|
|||
#!/usr/bin/env python3
|
||||
import traceback
|
||||
import struct
|
||||
from tqdm import tqdm
|
||||
|
||||
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.car.fingerprints import FW_VERSIONS
|
||||
import panda.python.uds as uds
|
||||
|
||||
from cereal import car
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
def p16(val):
|
||||
return struct.pack("!H", val)
|
||||
|
||||
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])
|
||||
|
||||
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)
|
||||
|
||||
TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01'
|
||||
TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01'
|
||||
|
||||
OBD_VERSION_REQUEST = b'\x09\x04'
|
||||
OBD_VERSION_RESPONSE = b'\x49\x04'
|
||||
|
||||
|
||||
REQUESTS = [
|
||||
# Honda
|
||||
(
|
||||
[UDS_VERSION_REQUEST],
|
||||
[UDS_VERSION_RESPONSE]
|
||||
),
|
||||
# Toyota
|
||||
(
|
||||
[SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST],
|
||||
[SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE]
|
||||
),
|
||||
(
|
||||
[SHORT_TESTER_PRESENT_REQUEST, OBD_VERSION_REQUEST],
|
||||
[SHORT_TESTER_PRESENT_RESPONSE, OBD_VERSION_RESPONSE]
|
||||
),
|
||||
(
|
||||
[TESTER_PRESENT_REQUEST, DEFAULT_DIAGNOSTIC_REQUEST, EXTENDED_DIAGNOSTIC_REQUEST, UDS_VERSION_REQUEST],
|
||||
[TESTER_PRESENT_RESPONSE, DEFAULT_DIAGNOSTIC_RESPONSE, EXTENDED_DIAGNOSTIC_RESPONSE, UDS_VERSION_RESPONSE]
|
||||
)
|
||||
]
|
||||
|
||||
def chunks(l, n=128):
|
||||
for i in range(0, len(l), n):
|
||||
yield l[i:i + n]
|
||||
|
||||
def match_fw_to_car(fw_versions):
|
||||
candidates = FW_VERSIONS
|
||||
invalid = []
|
||||
|
||||
for candidate, fws in candidates.items():
|
||||
for ecu, expected_versions in fws.items():
|
||||
ecu_type = ecu[0]
|
||||
addr = ecu[1:]
|
||||
|
||||
found_version = fw_versions.get(addr, None)
|
||||
|
||||
# Allow DSU not being present
|
||||
if ecu_type in [Ecu.unknown, Ecu.dsu] and found_version is None:
|
||||
continue
|
||||
|
||||
if found_version not in expected_versions:
|
||||
invalid.append(candidate)
|
||||
break
|
||||
|
||||
return set(candidates.keys()) - set(invalid)
|
||||
|
||||
|
||||
def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, progress=False):
|
||||
ecu_types = {}
|
||||
|
||||
# Extract ECU adresses to query from fingerprints
|
||||
# ECUs using a subadress need be queried one by one, the rest can be done in parallel
|
||||
addrs = []
|
||||
parallel_addrs = []
|
||||
|
||||
versions = FW_VERSIONS
|
||||
if extra is not None:
|
||||
versions.update(extra)
|
||||
|
||||
for c in versions.values():
|
||||
for ecu_type, addr, sub_addr in c.keys():
|
||||
a = (addr, sub_addr)
|
||||
if a not in ecu_types:
|
||||
ecu_types[a] = ecu_type
|
||||
|
||||
if sub_addr is None:
|
||||
parallel_addrs.append(a)
|
||||
else:
|
||||
addrs.append([a])
|
||||
addrs.insert(0, parallel_addrs)
|
||||
|
||||
fw_versions = {}
|
||||
for i, addr in enumerate(tqdm(addrs, disable=not progress)):
|
||||
for addr_chunk in chunks(addr):
|
||||
for request, response in REQUESTS:
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, addr_chunk, request, response, debug=debug)
|
||||
t = 2 * timeout if i == 0 else timeout
|
||||
fw_versions.update(query.get_data(t))
|
||||
except Exception:
|
||||
cloudlog.warning(f"FW query exception: {traceback.format_exc()}")
|
||||
|
||||
# Build capnp list to put into CarParams
|
||||
car_fw = []
|
||||
for addr, version in fw_versions.items():
|
||||
f = car.CarParams.CarFw.new_message()
|
||||
|
||||
f.ecu = ecu_types[addr]
|
||||
f.fwVersion = version
|
||||
f.address = addr[0]
|
||||
|
||||
if addr[1] is not None:
|
||||
f.subAddress = addr[1]
|
||||
|
||||
car_fw.append(f)
|
||||
|
||||
candidates = match_fw_to_car(fw_versions)
|
||||
return candidates, car_fw
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
import argparse
|
||||
import cereal.messaging as messaging
|
||||
from 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')
|
||||
args = parser.parse_args()
|
||||
|
||||
logcan = messaging.sub_sock('can')
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
extra = None
|
||||
if args.scan:
|
||||
extra = {"DEBUG": {}}
|
||||
# Honda
|
||||
for i in range(256):
|
||||
extra["DEBUG"][(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = []
|
||||
extra["DEBUG"][(Ecu.unknown, 0x700 + i, None)] = []
|
||||
extra["DEBUG"][(Ecu.unknown, 0x750, i)] = []
|
||||
|
||||
time.sleep(1.)
|
||||
|
||||
t = time.time()
|
||||
print("Getting vin...")
|
||||
addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
|
||||
print(f"VIN: {vin}")
|
||||
print("Getting VIN took %.3f s" % (time.time() - t))
|
||||
print()
|
||||
|
||||
t = time.time()
|
||||
candidates, fw_vers = get_fw_versions(logcan, sendcan, 1, extra=extra, debug=args.debug, progress=True)
|
||||
|
||||
print()
|
||||
print("Found FW versions")
|
||||
print("{")
|
||||
for version in fw_vers:
|
||||
subaddr = None if version.subAddress == 0 else hex(version.subAddress)
|
||||
print(f" (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]")
|
||||
print("}")
|
||||
|
||||
|
||||
print()
|
||||
print("Possible matches:", candidates)
|
||||
print("Getting fw took %.3f s" % (time.time() - t))
|
||||
|
|
@ -0,0 +1,187 @@
|
|||
from cereal import car
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import interp
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.gm import gmcan
|
||||
from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarControllerParams():
|
||||
def __init__(self, car_fingerprint):
|
||||
if car_fingerprint in SUPERCRUISE_CARS:
|
||||
self.STEER_MAX = 150
|
||||
self.STEER_STEP = 1 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 2 # 0.75s time to peak torque
|
||||
self.STEER_DELTA_DOWN = 5 # 0.3s from peak torque to zero
|
||||
self.MIN_STEER_SPEED = -1. # can steer down to zero
|
||||
else:
|
||||
self.STEER_MAX = 300
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s)
|
||||
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
|
||||
self.MIN_STEER_SPEED = 3.
|
||||
|
||||
self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting
|
||||
self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily
|
||||
self.STEER_DRIVER_FACTOR = 100 # from dbc
|
||||
self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop
|
||||
|
||||
# Takes case of "Service Adaptive Cruise" and "Service Front Camera"
|
||||
# dashboard messages.
|
||||
self.ADAS_KEEPALIVE_STEP = 100
|
||||
self.CAMERA_KEEPALIVE_STEP = 100
|
||||
|
||||
# pedal lookups, only for Volt
|
||||
MAX_GAS = 3072 # Only a safety limit
|
||||
ZERO_GAS = 2048
|
||||
MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen
|
||||
self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle
|
||||
self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
|
||||
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
|
||||
self.BRAKE_LOOKUP_BP = [-1., -0.25]
|
||||
self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
|
||||
|
||||
|
||||
def actuator_hystereses(final_pedal, pedal_steady):
|
||||
# hyst params... TODO: move these to VehicleParams
|
||||
pedal_hyst_gap = 0.01 # don't change pedal command for small oscillations within this value
|
||||
|
||||
# for small pedal oscillations within pedal_hyst_gap, don't change the pedal command
|
||||
if final_pedal == 0.:
|
||||
pedal_steady = 0.
|
||||
elif final_pedal > pedal_steady + pedal_hyst_gap:
|
||||
pedal_steady = final_pedal - pedal_hyst_gap
|
||||
elif final_pedal < pedal_steady - pedal_hyst_gap:
|
||||
pedal_steady = final_pedal + pedal_hyst_gap
|
||||
final_pedal = pedal_steady
|
||||
|
||||
return final_pedal, pedal_steady
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
# initialize to no alert
|
||||
steer = 0
|
||||
if hud_alert == VisualAlert.steerRequired:
|
||||
steer = 1
|
||||
return steer
|
||||
|
||||
class CarController():
|
||||
def __init__(self, canbus, car_fingerprint):
|
||||
self.pedal_steady = 0.
|
||||
self.start_time = 0.
|
||||
self.steer_idx = 0
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = car_fingerprint
|
||||
self.lka_icon_status_last = (False, False)
|
||||
self.steer_rate_limited = False
|
||||
|
||||
# Setup detection helper. Routes commands to
|
||||
# an appropriate CAN bus number.
|
||||
self.canbus = canbus
|
||||
self.params = CarControllerParams(car_fingerprint)
|
||||
|
||||
self.packer_pt = CANPacker(DBC[car_fingerprint]['pt'])
|
||||
self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis'])
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, \
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
|
||||
P = self.params
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
canbus = self.canbus
|
||||
|
||||
alert_out = process_hud_alert(hud_alert)
|
||||
steer = alert_out
|
||||
|
||||
### STEER ###
|
||||
|
||||
if (frame % P.STEER_STEP) == 0:
|
||||
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
|
||||
if lkas_enabled:
|
||||
new_steer = actuators.steer * P.STEER_MAX
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
else:
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
idx = (frame // P.STEER_STEP) % 4
|
||||
|
||||
if self.car_fingerprint in SUPERCRUISE_CARS:
|
||||
can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
|
||||
canbus, apply_steer, CS.v_ego, idx, lkas_enabled)
|
||||
else:
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt,
|
||||
canbus.powertrain, apply_steer, idx, lkas_enabled))
|
||||
|
||||
### GAS/BRAKE ###
|
||||
|
||||
if self.car_fingerprint not in SUPERCRUISE_CARS:
|
||||
# no output if not enabled, but keep sending keepalive messages
|
||||
# treat pedals as one
|
||||
final_pedal = actuators.gas - actuators.brake
|
||||
|
||||
# *** apply pedal hysteresis ***
|
||||
final_brake, self.brake_steady = actuator_hystereses(
|
||||
final_pedal, self.pedal_steady)
|
||||
|
||||
if not enabled:
|
||||
# Stock ECU sends max regen when not enabled.
|
||||
apply_gas = P.MAX_ACC_REGEN
|
||||
apply_brake = 0
|
||||
else:
|
||||
apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
|
||||
apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
|
||||
|
||||
# Gas/regen and brakes - all at 25Hz
|
||||
if (frame % 4) == 0:
|
||||
idx = (frame // 4) % 4
|
||||
|
||||
at_full_stop = enabled and CS.standstill
|
||||
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
|
||||
|
||||
at_full_stop = enabled and CS.standstill
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
|
||||
|
||||
# Send dashboard UI commands (ACC status), 25hz
|
||||
if (frame % 4) == 0:
|
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car))
|
||||
|
||||
# Radar needs to know current speed and yaw rate (50hz),
|
||||
# and that ADAS is alive (10hz)
|
||||
time_and_headlights_step = 10
|
||||
tt = frame * DT_CTRL
|
||||
|
||||
if frame % time_and_headlights_step == 0:
|
||||
idx = (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(canbus.obstacle))
|
||||
|
||||
speed_and_accelerometer_step = 2
|
||||
if frame % speed_and_accelerometer_step == 0:
|
||||
idx = (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.v_ego, idx))
|
||||
|
||||
if frame % P.ADAS_KEEPALIVE_STEP == 0:
|
||||
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
|
||||
|
||||
# Show green icon when LKA torque is applied, and
|
||||
# alarming orange icon when approaching torque limit.
|
||||
# If not sent again, LKA icon disappears in about 5 seconds.
|
||||
# Conveniently, sending camera message periodically also works as a keepalive.
|
||||
lka_active = CS.lkas_status == 1
|
||||
lka_critical = lka_active and abs(actuators.steer) > 0.9
|
||||
lka_icon_status = (lka_active, lka_critical)
|
||||
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
|
||||
or lka_icon_status != self.lka_icon_status_last:
|
||||
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer))
|
||||
self.lka_icon_status_last = lka_icon_status
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,151 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \
|
||||
CruiseButtons, is_eps_status_ok, \
|
||||
STEER_THRESHOLD, SUPERCRUISE_CARS
|
||||
|
||||
def get_powertrain_can_parser(CP, canbus):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("BrakePedalPosition", "EBCMBrakePedalPosition", 0),
|
||||
("FrontLeftDoor", "BCMDoorBeltStatus", 0),
|
||||
("FrontRightDoor", "BCMDoorBeltStatus", 0),
|
||||
("RearLeftDoor", "BCMDoorBeltStatus", 0),
|
||||
("RearRightDoor", "BCMDoorBeltStatus", 0),
|
||||
("LeftSeatBelt", "BCMDoorBeltStatus", 0),
|
||||
("RightSeatBelt", "BCMDoorBeltStatus", 0),
|
||||
("TurnSignals", "BCMTurnSignals", 0),
|
||||
("AcceleratorPedal", "AcceleratorPedal", 0),
|
||||
("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
|
||||
("SteeringWheelAngle", "PSCMSteeringAngle", 0),
|
||||
("FLWheelSpd", "EBCMWheelSpdFront", 0),
|
||||
("FRWheelSpd", "EBCMWheelSpdFront", 0),
|
||||
("RLWheelSpd", "EBCMWheelSpdRear", 0),
|
||||
("RRWheelSpd", "EBCMWheelSpdRear", 0),
|
||||
("PRNDL", "ECMPRDNL", 0),
|
||||
("LKADriverAppldTrq", "PSCMStatus", 0),
|
||||
("LKATorqueDeliveredStatus", "PSCMStatus", 0),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.VOLT:
|
||||
signals += [
|
||||
("RegenPaddle", "EBCMRegenPaddle", 0),
|
||||
]
|
||||
if CP.carFingerprint in SUPERCRUISE_CARS:
|
||||
signals += [
|
||||
("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0)
|
||||
]
|
||||
else:
|
||||
signals += [
|
||||
("TractionControlOn", "ESPStatus", 0),
|
||||
("EPBClosed", "EPBStatus", 0),
|
||||
("CruiseMainOn", "ECMEngineStatus", 0),
|
||||
("CruiseState", "AcceleratorPedal2", 0),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP, canbus):
|
||||
self.CP = CP
|
||||
# initialize can parser
|
||||
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.cruise_buttons = CruiseButtons.UNPRESS
|
||||
self.left_blinker_on = False
|
||||
self.prev_left_blinker_on = False
|
||||
self.right_blinker_on = False
|
||||
self.prev_right_blinker_on = False
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.
|
||||
|
||||
def update(self, pt_cp):
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
|
||||
|
||||
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
|
||||
self.standstill = self.v_ego_raw < 0.01
|
||||
|
||||
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
|
||||
self.gear_shifter = parse_gear_shifter(pt_cp.vl["ECMPRDNL"]['PRNDL'])
|
||||
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
|
||||
|
||||
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
|
||||
self.user_gas_pressed = self.pedal_gas > 0
|
||||
|
||||
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
|
||||
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
|
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
|
||||
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint)
|
||||
|
||||
# 1 - open, 0 - closed
|
||||
self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0)
|
||||
|
||||
# 1 - latched
|
||||
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1
|
||||
|
||||
self.steer_error = False
|
||||
|
||||
self.brake_error = False
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
|
||||
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
|
||||
|
||||
if self.car_fingerprint in SUPERCRUISE_CARS:
|
||||
self.park_brake = False
|
||||
self.main_on = False
|
||||
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
|
||||
self.esp_disabled = False
|
||||
self.regen_pressed = False
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
else:
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
|
||||
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
|
||||
self.acc_active = False
|
||||
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
|
||||
else:
|
||||
self.regen_pressed = False
|
||||
|
||||
# Brake pedal's potentiometer returns near-zero reading
|
||||
# even when pedal is not pressed.
|
||||
if self.user_brake < 10:
|
||||
self.user_brake = 0
|
||||
|
||||
# Regen braking is braking
|
||||
self.brake_pressed = self.user_brake > 10 or self.regen_pressed
|
||||
|
||||
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
from selfdrive.car import make_can_msg
|
||||
|
||||
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_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled):
|
||||
|
||||
values = {
|
||||
"LKASteeringCmdActive": 1 if enabled else 0,
|
||||
"LKASteeringCmd": apply_steer,
|
||||
"RollingCounter": idx,
|
||||
"SetMe1": 1,
|
||||
"LKASVehicleSpeed": abs(v_ego * 3.6),
|
||||
"LKASMode": 2 if enabled else 0,
|
||||
"LKASteeringCmdChecksum": 0 # assume zero and then manually compute it
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2]
|
||||
# the checksum logic is weird
|
||||
values['LKASteeringCmdChecksum'] = (0x2a +
|
||||
sum(dat[:4]) +
|
||||
values['LKASMode']) & 0x3ff
|
||||
# pack again with checksum
|
||||
dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2]
|
||||
|
||||
return [0x152, 0, dat, canbus.powertrain], \
|
||||
[0x154, 0, dat, canbus.powertrain], \
|
||||
[0x151, 0, dat, canbus.chassis], \
|
||||
[0x153, 0, dat, canbus.chassis]
|
||||
|
||||
|
||||
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, acc_engaged, at_full_stop):
|
||||
values = {
|
||||
"GasRegenCmdActive": acc_engaged,
|
||||
"RollingCounter": idx,
|
||||
"GasRegenCmdActiveInv": 1 - acc_engaged,
|
||||
"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, near_stop, at_full_stop):
|
||||
|
||||
if apply_brake == 0:
|
||||
mode = 0x1
|
||||
else:
|
||||
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.
|
||||
#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, acc_engaged, target_speed_kph, lead_car_in_sight):
|
||||
# Not a bit shift, dash can round up based on low 4 bits.
|
||||
target_speed = int(target_speed_kph * 16) & 0xfff
|
||||
|
||||
values = {
|
||||
"ACCAlwaysOne" : 1,
|
||||
"ACCResumeButton" : 0,
|
||||
"ACCSpeedSetpoint" : target_speed,
|
||||
"ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive"
|
||||
"ACCCmdActive" : acc_engaged,
|
||||
"ACCAlwaysOne2" : 1,
|
||||
"ACCLeadCar" : lead_car_in_sight
|
||||
}
|
||||
|
||||
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(bus):
|
||||
return make_can_msg(0x310, b"\x42\x04", bus)
|
||||
|
||||
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)
|
||||
|
||||
# TODO: WIP
|
||||
'''
|
||||
def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop):
|
||||
|
||||
# counters loops across [0, 29, 42, 55] but checksum only considers 0, 1, 2, 3
|
||||
cntrs = [0, 29, 42, 55]
|
||||
if apply_brake == 0:
|
||||
mode = 0x1
|
||||
else:
|
||||
mode = 0xa
|
||||
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
elif near_stop:
|
||||
mode = 0xb
|
||||
|
||||
brake = (0x1000 - apply_brake) & 0xfff
|
||||
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
|
||||
|
||||
values = {
|
||||
"RollingCounter" : cntrs[idx],
|
||||
"FrictionBrakeMode" : mode,
|
||||
"FrictionBrakeChecksum": checksum,
|
||||
"FrictionBrakeCmd" : -apply_brake
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("EBCMFrictionBrakeCmd", 0, values)[2]
|
||||
# msg is 0x315 but we are doing the panda forwarding
|
||||
return make_can_msg(0x314, dat, 2)
|
||||
|
||||
def create_gas_regen_command_ct6(bus, throttle, idx, acc_engaged, at_full_stop):
|
||||
cntrs = [0, 7, 10, 13]
|
||||
eng_bit = 1 if acc_engaged else 0
|
||||
gas_high = (throttle >> 8) | 0x80
|
||||
gas_low = (throttle) & 0xff
|
||||
full_stop = 0x20 if at_full_stop else 0
|
||||
|
||||
chk1 = (0x100 - gas_high - 1) & 0xff
|
||||
chk2 = (0x100 - gas_low - idx) & 0xff
|
||||
dat = [(idx << 6) | eng_bit, 0xc2 | full_stop, gas_high, gas_low,
|
||||
(1 - eng_bit) | (cntrs[idx] << 1), 0x5d - full_stop, chk1, chk2]
|
||||
return make_can_msg(0x2cb, "".join(map(chr, dat)), bus)
|
||||
|
||||
'''
|
||||
|
|
@ -0,0 +1,340 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.gm.values import DBC, CAR, ECU, ECU_FINGERPRINT, \
|
||||
SUPERCRUISE_CARS, AccState, FINGERPRINTS
|
||||
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CanBus(CarInterfaceBase):
|
||||
def __init__(self):
|
||||
self.powertrain = 0
|
||||
self.obstacle = 1
|
||||
self.chassis = 2
|
||||
self.sw_gmlan = 3
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
|
||||
self.frame = 0
|
||||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.acc_active_prev = 0
|
||||
|
||||
# *** init the major players ***
|
||||
canbus = CanBus()
|
||||
self.CS = CarState(CP, canbus)
|
||||
self.VM = VehicleModel(CP)
|
||||
self.pt_cp = get_powertrain_can_parser(CP, canbus)
|
||||
self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis']
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(canbus, CP.carFingerprint)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "gm"
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.enableCruise = False
|
||||
# GM port is considered a community feature, since it disables AEB;
|
||||
# TODO: make a port that uses a car harness and it only intercepts the camera
|
||||
ret.communityFeature = True
|
||||
|
||||
# Presence of a camera on the object bus is ok.
|
||||
# Have to go to read_only if ASCM is online (ACC-enabled cars),
|
||||
# or camera is on powertrain bus (LKA cars without ACC).
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or \
|
||||
has_relay or \
|
||||
candidate == CAR.CADILLAC_CT6
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
|
||||
if candidate == CAR.VOLT:
|
||||
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.mass = 1607. + STD_CARGO_KG
|
||||
ret.safetyModel = car.CarParams.SafetyModel.gm
|
||||
ret.wheelbase = 2.69
|
||||
ret.steerRatio = 15.7
|
||||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
||||
|
||||
elif candidate == CAR.MALIBU:
|
||||
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.mass = 1496. + STD_CARGO_KG
|
||||
ret.safetyModel = car.CarParams.SafetyModel.gm
|
||||
ret.wheelbase = 2.83
|
||||
ret.steerRatio = 15.8
|
||||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
||||
|
||||
elif candidate == CAR.HOLDEN_ASTRA:
|
||||
ret.mass = 1363. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.662
|
||||
# Remaining parameters copied from Volt for now
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.safetyModel = car.CarParams.SafetyModel.gm
|
||||
ret.steerRatio = 15.7
|
||||
ret.steerRatioRear = 0.
|
||||
|
||||
elif candidate == CAR.ACADIA:
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.safetyModel = car.CarParams.SafetyModel.gm
|
||||
ret.wheelbase = 2.86
|
||||
ret.steerRatio = 14.4 #end to end is 13.46
|
||||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
|
||||
elif candidate == CAR.BUICK_REGAL:
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
|
||||
ret.safetyModel = car.CarParams.SafetyModel.gm
|
||||
ret.wheelbase = 2.83 #111.4 inches in meters
|
||||
ret.steerRatio = 14.4 # guess for tourx
|
||||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
|
||||
|
||||
elif candidate == CAR.CADILLAC_ATS:
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.mass = 1601. + STD_CARGO_KG
|
||||
ret.safetyModel = car.CarParams.SafetyModel.gm
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 15.3
|
||||
ret.steerRatioRear = 0.
|
||||
ret.centerToFront = ret.wheelbase * 0.49
|
||||
|
||||
elif candidate == CAR.CADILLAC_CT6:
|
||||
# engage speed is decided by pcm
|
||||
ret.minEnableSpeed = -1.
|
||||
ret.mass = 4016. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.safetyModel = car.CarParams.SafetyModel.cadillac
|
||||
ret.wheelbase = 3.11
|
||||
ret.steerRatio = 14.6 # it's 16.3 without rear active steering
|
||||
ret.steerRatioRear = 0. # TODO: there is RAS on this car!
|
||||
ret.centerToFront = ret.wheelbase * 0.465
|
||||
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
# same tuning for Volt and CT6 for now
|
||||
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.steerMaxBP = [0.] # m/s
|
||||
ret.steerMaxV = [1.]
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [.5]
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [1.]
|
||||
|
||||
ret.longitudinalTuning.kpBP = [5., 35.]
|
||||
ret.longitudinalTuning.kpV = [2.4, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.36]
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.startAccel = 0.8
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
ret.steerRateCost = 1.0
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
self.pt_cp.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.pt_cp)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.pt_cp.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gas pedal information.
|
||||
ret.gas = self.CS.pedal_gas / 254.0
|
||||
ret.gasPressed = self.CS.user_gas_pressed
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake / 0xd0
|
||||
ret.brakePressed = self.CS.brake_pressed
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
|
||||
# torque and user override. Driver awareness
|
||||
# timer resets when the user uses the steering wheel.
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
|
||||
ret.cruiseState.enabled = cruiseEnabled
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
|
||||
|
||||
ret.leftBlinker = self.CS.left_blinker_on
|
||||
ret.rightBlinker = self.CS.right_blinker_on
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
buttonEvents = []
|
||||
|
||||
# blinkers
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.unknown
|
||||
if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
|
||||
be.pressed = True
|
||||
but = self.CS.cruise_buttons
|
||||
else:
|
||||
be.pressed = False
|
||||
but = self.CS.prev_cruise_buttons
|
||||
if but == CruiseButtons.RES_ACCEL:
|
||||
if not (cruiseEnabled and self.CS.standstill):
|
||||
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
|
||||
elif but == CruiseButtons.DECEL_SET:
|
||||
be.type = ButtonType.decelCruise
|
||||
elif but == CruiseButtons.CANCEL:
|
||||
be.type = ButtonType.cancel
|
||||
elif but == CruiseButtons.MAIN:
|
||||
be.type = ButtonType.altButton3
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
|
||||
events = []
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if self.CS.steer_not_allowed:
|
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if self.CS.car_fingerprint in SUPERCRUISE_CARS:
|
||||
if self.CS.acc_active and not self.acc_active_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
if not self.CS.acc_active:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
else:
|
||||
if self.CS.brake_error:
|
||||
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if not self.CS.gear_shifter_valid:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if self.CS.gear_shifter == 3:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
|
||||
if self.CS.park_brake:
|
||||
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
# disable on pedals rising edge or when brake is pressed and speed isn't zero
|
||||
if (ret.gasPressed and not self.gas_pressed_prev) or \
|
||||
(ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
if ret.cruiseState.standstill:
|
||||
events.append(create_event('resumeRequired', [ET.WARNING]))
|
||||
if self.CS.pcm_acc_status == AccState.FAULTED:
|
||||
events.append(create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
|
||||
# handle button presses
|
||||
for b in ret.buttonEvents:
|
||||
# do enable on both accel and decel buttons
|
||||
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
|
||||
events.append(create_event('buttonEnable', [ET.ENABLE]))
|
||||
# do disable on button down
|
||||
if b.type == ButtonType.cancel and b.pressed:
|
||||
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
# update previous brake/gas pressed
|
||||
self.acc_active_prev = self.CS.acc_active
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
hud_v_cruise = c.hudControl.setSpeed
|
||||
if hud_v_cruise > 70:
|
||||
hud_v_cruise = 0
|
||||
|
||||
# For Openpilot, "enabled" includes pre-enable.
|
||||
# In GM, PCM faults out if ACC command overlaps user gas.
|
||||
enabled = c.enabled and not self.CS.user_gas_pressed
|
||||
|
||||
can_sends = self.CC.update(enabled, self.CS, self.frame, \
|
||||
c.actuators,
|
||||
hud_v_cruise, c.hudControl.lanesVisible, \
|
||||
c.hudControl.leadVisible, c.hudControl.visualAlert)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
#!/usr/bin/env python3
|
||||
from __future__ import print_function
|
||||
import math
|
||||
import time
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.gm.interface import CanBus
|
||||
from selfdrive.car.gm.values import DBC, CAR
|
||||
from selfdrive.config import Conversions as CV
|
||||
from 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(canbus, car_fingerprint):
|
||||
|
||||
dbc_f = DBC[car_fingerprint]['radar']
|
||||
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
|
||||
# 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,
|
||||
[0] * 7 +
|
||||
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
|
||||
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
|
||||
[0.0] * NUM_SLOTS + [0] * NUM_SLOTS))
|
||||
|
||||
checks = []
|
||||
|
||||
return CANParser(dbc_f, signals, checks, canbus.obstacle)
|
||||
else:
|
||||
return None
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
|
||||
self.delay = 0 # Delay of radar
|
||||
|
||||
canbus = CanBus()
|
||||
print("Using %d as obstacle CAN bus ID" % canbus.obstacle)
|
||||
self.rcp = create_radar_can_parser(canbus, 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:
|
||||
time.sleep(self.radar_ts) # nothing to do
|
||||
return car.RadarData.new_message()
|
||||
|
||||
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 not oldTarget in currentTargets:
|
||||
del self.pts[oldTarget]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
|
||||
class CAR:
|
||||
HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
|
||||
VOLT = "CHEVROLET VOLT PREMIER 2017"
|
||||
CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018"
|
||||
CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018"
|
||||
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
|
||||
ACADIA = "GMC ACADIA DENALI 2018"
|
||||
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
|
||||
|
||||
SUPERCRUISE_CARS = [CAR.CADILLAC_CT6]
|
||||
|
||||
class CruiseButtons:
|
||||
UNPRESS = 1
|
||||
RES_ACCEL = 2
|
||||
DECEL_SET = 3
|
||||
MAIN = 5
|
||||
CANCEL = 6
|
||||
|
||||
class AccState:
|
||||
OFF = 0
|
||||
ACTIVE = 1
|
||||
FAULTED = 3
|
||||
STANDSTILL = 4
|
||||
|
||||
def is_eps_status_ok(eps_status, car_fingerprint):
|
||||
valid_eps_status = []
|
||||
if car_fingerprint in SUPERCRUISE_CARS:
|
||||
valid_eps_status += [0, 1, 4, 5, 6]
|
||||
else:
|
||||
valid_eps_status += [0, 1]
|
||||
return eps_status in valid_eps_status
|
||||
|
||||
def parse_gear_shifter(can_gear):
|
||||
if can_gear == 0:
|
||||
return car.CarState.GearShifter.park
|
||||
elif can_gear == 1:
|
||||
return car.CarState.GearShifter.neutral
|
||||
elif can_gear == 2:
|
||||
return car.CarState.GearShifter.drive
|
||||
elif can_gear == 3:
|
||||
return car.CarState.GearShifter.reverse
|
||||
else:
|
||||
return car.CarState.GearShifter.unknown
|
||||
|
||||
FINGERPRINTS = {
|
||||
# Astra BK MY17, ASCM unplugged
|
||||
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: [
|
||||
# Volt Premier w/ ACC 2017
|
||||
{
|
||||
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
|
||||
},
|
||||
# Volt Premier w/ ACC 2018
|
||||
{
|
||||
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
|
||||
}],
|
||||
CAR.BUICK_REGAL : [
|
||||
# Regal TourX Essence w/ ACC 2018
|
||||
{
|
||||
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: [
|
||||
# Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018
|
||||
{
|
||||
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.CADILLAC_CT6: [{
|
||||
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, 313: 8, 320: 3, 322: 7, 328: 1, 336: 1, 338: 6, 340: 6, 352: 5, 354: 5, 356: 8, 368: 3, 372: 5, 381: 8, 386: 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, 458: 5, 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: 5, 532: 6, 534: 2, 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, 719: 5, 723: 2, 753: 5, 761: 7, 800: 6, 801: 8, 804: 3, 810: 8, 832: 8, 833: 8, 834: 8, 835: 6, 836: 5, 837: 8, 838: 8, 839: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 884: 8, 961: 8, 969: 8, 977: 8, 979: 8, 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: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1934: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.MALIBU: [
|
||||
# Malibu Premier w/ ACC 2017
|
||||
{
|
||||
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: [
|
||||
# Acadia Denali w/ /ACC 2018
|
||||
{
|
||||
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
|
||||
}],
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [384, 715] # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'),
|
||||
}
|
||||
|
|
@ -0,0 +1,190 @@
|
|||
from collections import namedtuple
|
||||
from cereal import car
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
from common.numpy_fast import clip
|
||||
from selfdrive.car import create_gas_command
|
||||
from selfdrive.car.honda import hondacan
|
||||
from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def actuator_hystereses(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
|
||||
|
||||
if (car_fingerprint in (CAR.ACURA_ILX, CAR.CRV)) and brake > 0.0:
|
||||
brake += 0.15
|
||||
|
||||
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 == VisualAlert.steerRequired:
|
||||
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", "car",
|
||||
"lanes", "fcw", "acc_alert", "steer_required"])
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP):
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.apply_brake_last = 0
|
||||
self.last_pump_ts = 0.
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.new_radar_config = False
|
||||
self.eps_modified = False
|
||||
for fw in CP.carFw:
|
||||
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||
print("EPS FW MODIFIED!")
|
||||
self.eps_modified = True
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, \
|
||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
|
||||
|
||||
# *** no output if not enabled ***
|
||||
if not enabled and CS.pcm_acc_status:
|
||||
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
|
||||
pcm_cancel_cmd = True
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
if hud_show_lanes:
|
||||
hud_lanes = 1
|
||||
else:
|
||||
hud_lanes = 0
|
||||
|
||||
if enabled:
|
||||
if hud_show_car:
|
||||
hud_car = 2
|
||||
else:
|
||||
hud_car = 1
|
||||
else:
|
||||
hud_car = 0
|
||||
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
|
||||
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
|
||||
hud_lanes, fcw_display, acc_alert, steer_required)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# *** compute control surfaces ***
|
||||
BRAKE_MAX = 1024//4
|
||||
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
|
||||
STEER_MAX = 0xF00
|
||||
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
|
||||
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value
|
||||
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
|
||||
STEER_MAX = 0x7FFF
|
||||
elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
|
||||
STEER_MAX = 0x1400
|
||||
else:
|
||||
STEER_MAX = 0x1000
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_gas = clip(actuators.gas, 0., 1.)
|
||||
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
||||
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
||||
|
||||
if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
|
||||
if apply_steer > 0xA00:
|
||||
apply_steer = (apply_steer - 0xA00) / 2 + 0xA00
|
||||
elif apply_steer < -0xA00:
|
||||
apply_steer = (apply_steer + 0xA00) / 2 - 0xA00
|
||||
|
||||
lkas_active = enabled and not CS.steer_not_allowed
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
|
||||
# Send steering command.
|
||||
idx = frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
|
||||
lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
idx = (frame//10) % 4
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud))
|
||||
|
||||
if CS.CP.radarOffCan:
|
||||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
|
||||
elif CS.stopped:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
idx = frame // 2
|
||||
ts = frame * DT_CTRL
|
||||
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake))
|
||||
self.apply_brake_last = apply_brake
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
# 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
|
||||
can_sends.append(create_gas_command(self.packer, apply_gas, idx))
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,383 @@
|
|||
from cereal import car
|
||||
from collections import defaultdict
|
||||
from common.numpy_fast import interp
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def parse_gear_shifter(gear, vals):
|
||||
|
||||
val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
|
||||
'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low}
|
||||
try:
|
||||
return val_to_capnp[vals[gear]]
|
||||
except KeyError:
|
||||
return "unknown"
|
||||
|
||||
|
||||
def calc_cruise_offset(offset, speed):
|
||||
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
|
||||
# constraints to solve for _K0, _K1, _K2 are:
|
||||
# - speed = 0m/s, out = -0.3
|
||||
# - speed = 34m/s, offset = 20, out = -0.25
|
||||
# - speed = 34m/s, offset = -2.5, out = -1.8
|
||||
_K0 = -0.3
|
||||
_K1 = -0.01879
|
||||
_K2 = 0.01013
|
||||
return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
|
||||
|
||||
|
||||
def get_can_signals(CP):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
signals = [
|
||||
("XMISSION_SPEED", "ENGINE_DATA", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("STEER_ANGLE", "STEERING_SENSORS", 0),
|
||||
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
|
||||
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
|
||||
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
|
||||
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
|
||||
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
|
||||
("GEAR", "GEARBOX", 0),
|
||||
("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1),
|
||||
("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0),
|
||||
("BRAKE_PRESSED", "POWERTRAIN_DATA", 0),
|
||||
("BRAKE_SWITCH", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_BUTTONS", "SCM_BUTTONS", 0),
|
||||
("ESP_DISABLED", "VSA_STATUS", 1),
|
||||
("USER_BRAKE", "VSA_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
|
||||
("STEER_STATUS", "STEER_STATUS", 5),
|
||||
("GEAR_SHIFTER", "GEARBOX", 0),
|
||||
("PEDAL_GAS", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_SETTING", "SCM_BUTTONS", 0),
|
||||
("ACC_STATUS", "POWERTRAIN_DATA", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("ENGINE_DATA", 100),
|
||||
("WHEEL_SPEEDS", 50),
|
||||
("STEERING_SENSORS", 100),
|
||||
("SEATBELT_STATUS", 10),
|
||||
("CRUISE", 10),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
("VSA_STATUS", 50),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
checks += [
|
||||
("SCM_FEEDBACK", 25),
|
||||
("SCM_BUTTONS", 50),
|
||||
]
|
||||
else:
|
||||
checks += [
|
||||
("SCM_FEEDBACK", 10),
|
||||
("SCM_BUTTONS", 25),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.CRV_HYBRID:
|
||||
checks += [
|
||||
("GEARBOX", 50),
|
||||
]
|
||||
else:
|
||||
checks += [
|
||||
("GEARBOX", 100),
|
||||
]
|
||||
|
||||
if CP.radarOffCan:
|
||||
# Civic is only bosch to use the same brake message as other hondas.
|
||||
if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
|
||||
signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)]
|
||||
checks += [("BRAKE_MODULE", 50)]
|
||||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_FEEDBACK", 0),
|
||||
("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0),
|
||||
("CRUISE_SPEED", "ACC_HUD", 0)]
|
||||
checks += [("GAS_PEDAL_2", 100)]
|
||||
else:
|
||||
# Nidec signals.
|
||||
signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
|
||||
("BRAKE_ERROR_2", "STANDSTILL", 1),
|
||||
("CRUISE_SPEED_PCM", "CRUISE", 0),
|
||||
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
|
||||
checks += [("STANDSTILL", 50)]
|
||||
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
checks += [("CRUISE_PARAMS", 10)]
|
||||
else:
|
||||
checks += [("CRUISE_PARAMS", 50)]
|
||||
|
||||
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
|
||||
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
|
||||
elif CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)]
|
||||
else:
|
||||
signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1),
|
||||
("DOOR_OPEN_FR", "DOORS_STATUS", 1),
|
||||
("DOOR_OPEN_RL", "DOORS_STATUS", 1),
|
||||
("DOOR_OPEN_RR", "DOORS_STATUS", 1),
|
||||
("WHEELS_MOVING", "STANDSTILL", 1)]
|
||||
checks += [("DOORS_STATUS", 3)]
|
||||
|
||||
if CP.carFingerprint == CAR.CIVIC:
|
||||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_FEEDBACK", 0),
|
||||
("IMPERIAL_UNIT", "HUD_SETTING", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0)]
|
||||
elif CP.carFingerprint == CAR.ACURA_ILX:
|
||||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_BUTTONS", 0)]
|
||||
elif CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE):
|
||||
signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
|
||||
elif CP.carFingerprint == CAR.FIT:
|
||||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_BUTTONS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
|
||||
elif CP.carFingerprint == CAR.ODYSSEY:
|
||||
signals += [("MAIN_ON", "SCM_FEEDBACK", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0)]
|
||||
checks += [("EPB_STATUS", 50)]
|
||||
elif CP.carFingerprint == CAR.PILOT:
|
||||
signals += [("MAIN_ON", "SCM_BUTTONS", 0),
|
||||
("CAR_GAS", "GAS_PEDAL_2", 0)]
|
||||
elif CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
signals += [("MAIN_ON", "SCM_BUTTONS", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0)]
|
||||
checks += [("EPB_STATUS", 50)]
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
|
||||
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
|
||||
checks.append(("GAS_SENSOR", 50))
|
||||
|
||||
return signals, checks
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
signals, checks = get_can_signals(CP)
|
||||
bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt)
|
||||
|
||||
|
||||
def get_cam_can_parser(CP):
|
||||
signals = []
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0),
|
||||
("AEB_STATUS", "ACC_CONTROL", 0)]
|
||||
else:
|
||||
signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0),
|
||||
("AEB_REQ_1", "BRAKE_COMMAND", 0),
|
||||
("FCW", "BRAKE_COMMAND", 0),
|
||||
("CHIME", "BRAKE_COMMAND", 0),
|
||||
("FCM_OFF", "ACC_HUD", 0),
|
||||
("FCM_OFF_2", "ACC_HUD", 0),
|
||||
("FCM_PROBLEM", "ACC_HUD", 0),
|
||||
("ICONS", "ACC_HUD", 0)]
|
||||
|
||||
|
||||
# all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering
|
||||
checks = [(0xe4, 100)]
|
||||
if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]:
|
||||
checks = [(0x194, 100)]
|
||||
|
||||
bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
|
||||
self.user_gas, self.user_gas_pressed = 0., 0
|
||||
self.brake_switch_prev = 0
|
||||
self.brake_switch_ts = 0
|
||||
|
||||
self.cruise_buttons = 0
|
||||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
self.blinker_on = 0
|
||||
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
|
||||
self.cruise_mode = 0
|
||||
self.stopped = 0
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
# 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.prev_blinker_on = self.blinker_on
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
# ******************* parse out can *******************
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc
|
||||
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']
|
||||
else:
|
||||
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
|
||||
self.door_all_closed = not 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']])
|
||||
self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']]
|
||||
self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT']
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
|
||||
self.steer_not_allowed = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_2']
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
self.steer_warning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2']
|
||||
|
||||
if self.CP.radarOffCan:
|
||||
self.brake_error = 0
|
||||
else:
|
||||
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
|
||||
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4.
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
|
||||
self.v_weight * v_wheel
|
||||
|
||||
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[speed], [0.0]]
|
||||
|
||||
self.v_ego_raw = speed
|
||||
v_ego_x = self.v_ego_kf.update(speed)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
|
||||
# this is a hack for the interceptor. This is now only used in the simulation
|
||||
# TODO: Replace tests by toyota so this can go away
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||
|
||||
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
|
||||
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
|
||||
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
|
||||
|
||||
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
|
||||
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
|
||||
|
||||
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
else:
|
||||
self.park_brake = 0 # TODO
|
||||
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
|
||||
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
|
||||
self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.shifter_values)
|
||||
|
||||
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
|
||||
# crv doesn't include cruise control
|
||||
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN):
|
||||
self.car_gas = self.pedal_gas
|
||||
else:
|
||||
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
|
||||
|
||||
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
|
||||
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
|
||||
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
|
||||
if self.CP.radarOffCan:
|
||||
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
|
||||
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
|
||||
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
|
||||
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID):
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
|
||||
(self.brake_switch and self.brake_switch_prev and \
|
||||
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||
self.brake_switch_prev = self.brake_switch
|
||||
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
else:
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
|
||||
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
|
||||
self.v_cruise_pcm_prev = self.v_cruise_pcm
|
||||
else:
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
|
||||
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
|
||||
# brake switch has shown some single time step noise, so only considered when
|
||||
# switch is on for at least 2 consecutive CAN samples
|
||||
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
|
||||
(self.brake_switch and self.brake_switch_prev and \
|
||||
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||
self.brake_switch_prev = self.brake_switch
|
||||
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
|
||||
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
|
||||
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
|
||||
|
||||
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
|
||||
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
|
||||
if self.user_brake > 0.05:
|
||||
self.brake_pressed = 1
|
||||
|
||||
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
|
||||
self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.stock_aeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
|
||||
else:
|
||||
self.stock_aeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.stock_hud = False
|
||||
self.stock_fcw = False
|
||||
else:
|
||||
self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0)
|
||||
self.stock_hud = cp_cam.vl["ACC_HUD"]
|
||||
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.honda.values import HONDA_BOSCH
|
||||
|
||||
|
||||
def get_pt_bus(car_fingerprint, has_relay):
|
||||
return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0
|
||||
|
||||
|
||||
def get_lkas_cmd_bus(car_fingerprint, has_relay):
|
||||
return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0
|
||||
|
||||
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay, stock_brake):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
pcm_fault_cmd = False
|
||||
|
||||
values = {
|
||||
"COMPUTER_BRAKE": apply_brake,
|
||||
"BRAKE_PUMP_REQUEST": pump_on,
|
||||
"CRUISE_OVERRIDE": pcm_override,
|
||||
"CRUISE_FAULT_CMD": pcm_fault_cmd,
|
||||
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
|
||||
"COMPUTER_BRAKE_REQUEST": brake_rq,
|
||||
"SET_ME_1": 1,
|
||||
"BRAKE_LIGHTS": brakelights,
|
||||
"CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw
|
||||
"FCW": fcw << 1, # TODO: Why are there two bits for fcw?
|
||||
"AEB_REQ_1": 0,
|
||||
"AEB_REQ_2": 0,
|
||||
"AEB_STATUS": 0,
|
||||
}
|
||||
bus = get_pt_bus(car_fingerprint, has_relay)
|
||||
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay):
|
||||
values = {
|
||||
"STEER_TORQUE": apply_steer if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
}
|
||||
bus = get_lkas_cmd_bus(car_fingerprint, has_relay)
|
||||
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
|
||||
|
||||
|
||||
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, stock_hud):
|
||||
commands = []
|
||||
bus_pt = get_pt_bus(car_fingerprint, has_relay)
|
||||
bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay)
|
||||
|
||||
if car_fingerprint not in HONDA_BOSCH:
|
||||
acc_hud_values = {
|
||||
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
|
||||
'PCM_GAS': hud.pcm_accel,
|
||||
'CRUISE_SPEED': hud.v_cruise,
|
||||
'ENABLE_MINI_CAR': 1,
|
||||
'HUD_LEAD': hud.car,
|
||||
'HUD_DISTANCE': 3, # max distance setting on display
|
||||
'IMPERIAL_UNIT': int(not is_metric),
|
||||
'SET_ME_X01_2': 1,
|
||||
'SET_ME_X01': 1,
|
||||
"FCM_OFF": stock_hud["FCM_OFF"],
|
||||
"FCM_OFF_2": stock_hud["FCM_OFF_2"],
|
||||
"FCM_PROBLEM": stock_hud["FCM_PROBLEM"],
|
||||
"ICONS": stock_hud["ICONS"],
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
|
||||
|
||||
lkas_hud_values = {
|
||||
'SET_ME_X41': 0x41,
|
||||
'SET_ME_X48': 0x48,
|
||||
'STEERING_REQUIRED': hud.steer_required,
|
||||
'SOLID_LANES': hud.lanes,
|
||||
'BEEP': 0,
|
||||
}
|
||||
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def spam_buttons_command(packer, button_val, idx, car_fingerprint, has_relay):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': button_val,
|
||||
'CRUISE_SETTING': 0,
|
||||
}
|
||||
bus = get_pt_bus(car_fingerprint, has_relay)
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values, idx)
|
||||
|
|
@ -0,0 +1,597 @@
|
|||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, ECU, ECU_FINGERPRINT, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def compute_gb_honda(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
|
||||
return float(accel) / 4.8 - creep_brake
|
||||
|
||||
|
||||
def get_compute_gb_acura():
|
||||
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
|
||||
# where -1.0 is max brake and 1.0 is max gas
|
||||
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
|
||||
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
|
||||
[ 1.03691769, 0.78210306, -0.41343188]])
|
||||
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
|
||||
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
|
||||
[ 0.79973811, 0.13178682, 0.08550351],
|
||||
[-0.15651935, -0.44360259, 0.76910877]])
|
||||
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
|
||||
w4 = np.array([[-0.31521443],
|
||||
[-0.38626176],
|
||||
[ 0.52667892]])
|
||||
b4 = np.array([-0.02922216])
|
||||
|
||||
def compute_output(dat, w0, b0, w2, b2, w4, b4):
|
||||
m0 = np.dot(dat, w0) + b0
|
||||
m0 = leakyrelu(m0, 0.1)
|
||||
m2 = np.dot(m0, w2) + b2
|
||||
m2 = leakyrelu(m2, 0.1)
|
||||
m4 = np.dot(m2, w4) + b4
|
||||
return m4
|
||||
|
||||
def leakyrelu(x, alpha):
|
||||
return np.maximum(x, alpha * x)
|
||||
|
||||
def _compute_gb_acura(accel, speed):
|
||||
# linearly extrap below v1 using v1 and v2 data
|
||||
v1 = 5.
|
||||
v2 = 10.
|
||||
dat = np.array([accel, speed])
|
||||
if speed > 5.:
|
||||
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
|
||||
else:
|
||||
dat[1] = v1
|
||||
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
|
||||
dat[1] = v2
|
||||
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
|
||||
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
|
||||
return float(m4)
|
||||
|
||||
return _compute_gb_acura
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
|
||||
self.frame = 0
|
||||
self.last_enable_pressed = 0
|
||||
self.last_enable_sent = 0
|
||||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
|
||||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_cam_can_parser(CP)
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
self.VM = VehicleModel(CP)
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP)
|
||||
|
||||
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
|
||||
self.compute_gb = get_compute_gb_acura()
|
||||
else:
|
||||
self.compute_gb = compute_gb_honda
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
|
||||
# normalized max accel. Allowing max accel at low speed causes speed overshoots
|
||||
max_accel_bp = [10, 20] # m/s
|
||||
max_accel_v = [0.714, 1.0] # unit of max accel
|
||||
max_accel = interp(v_ego, max_accel_bp, max_accel_v)
|
||||
|
||||
# limit the pcm accel cmd if:
|
||||
# - v_ego exceeds v_target, or
|
||||
# - a_ego exceeds a_target and v_ego is close to v_target
|
||||
|
||||
eA = a_ego - a_target
|
||||
valuesA = [1.0, 0.1]
|
||||
bpA = [0.3, 1.1]
|
||||
|
||||
eV = v_ego - v_target
|
||||
valuesV = [1.0, 0.1]
|
||||
bpV = [0.0, 0.5]
|
||||
|
||||
valuesRangeV = [1., 0.]
|
||||
bpRangeV = [-1., 0.]
|
||||
|
||||
# only limit if v_ego is close to v_target
|
||||
speedLimiter = interp(eV, bpV, valuesV)
|
||||
accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV))
|
||||
|
||||
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
|
||||
# unless aTargetMax is very high and then we scale with it; this help in quicker restart
|
||||
|
||||
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
ret.carName = "honda"
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe
|
||||
rdr_bus = 0 if has_relay else 2
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.radarOffCan = True
|
||||
ret.openpilotLongitudinalControl = False
|
||||
else:
|
||||
ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
|
||||
ret.enableCruise = not ret.enableGasInterceptor
|
||||
ret.communityFeature = ret.enableGasInterceptor
|
||||
|
||||
# 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.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
||||
|
||||
eps_modified = False
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||
eps_modified = True
|
||||
|
||||
if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]:
|
||||
stop_and_go = True
|
||||
ret.mass = CivicParams.MASS
|
||||
ret.wheelbase = CivicParams.WHEELBASE
|
||||
ret.centerToFront = CivicParams.CENTER_TO_FRONT
|
||||
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
||||
tire_stiffness_factor = 1.
|
||||
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if eps_modified else [[0.8], [0.24]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
|
||||
stop_and_go = True
|
||||
if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch
|
||||
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
||||
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.83
|
||||
ret.centerToFront = ret.wheelbase * 0.39
|
||||
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
|
||||
tire_stiffness_factor = 0.8467
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.ACURA_ILX:
|
||||
stop_and_go = False
|
||||
ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.67
|
||||
ret.centerToFront = ret.wheelbase * 0.37
|
||||
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
|
||||
tire_stiffness_factor = 0.72
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.CRV:
|
||||
stop_and_go = False
|
||||
ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.62
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 16.89 # as spec
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.CRV_5G:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
||||
ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.66
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
|
||||
tire_stiffness_factor = 0.677
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.CRV_HYBRID:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
||||
ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg
|
||||
ret.wheelbase = 2.66
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
|
||||
tire_stiffness_factor = 0.677
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.FIT:
|
||||
stop_and_go = False
|
||||
ret.mass = 2644. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.53
|
||||
ret.centerToFront = ret.wheelbase * 0.39
|
||||
ret.steerRatio = 13.06
|
||||
tire_stiffness_factor = 0.75
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.ACURA_RDX:
|
||||
stop_and_go = False
|
||||
ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.68
|
||||
ret.centerToFront = ret.wheelbase * 0.38
|
||||
ret.steerRatio = 15.0 # as spec
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.ODYSSEY:
|
||||
stop_and_go = False
|
||||
ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 3.00
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 14.35 # as spec
|
||||
tire_stiffness_factor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.ODYSSEY_CHN:
|
||||
stop_and_go = False
|
||||
ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg
|
||||
ret.wheelbase = 2.90
|
||||
ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY
|
||||
ret.steerRatio = 14.35
|
||||
tire_stiffness_factor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
|
||||
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]
|
||||
|
||||
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
|
||||
stop_and_go = False
|
||||
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
|
||||
ret.wheelbase = 2.82
|
||||
ret.centerToFront = ret.wheelbase * 0.428
|
||||
ret.steerRatio = 17.25 # as spec
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
||||
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]
|
||||
|
||||
elif candidate == CAR.RIDGELINE:
|
||||
stop_and_go = False
|
||||
ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 3.18
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 15.59 # as spec
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
||||
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]
|
||||
|
||||
else:
|
||||
raise ValueError("unsupported car %s" % candidate)
|
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
||||
# 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.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
# no rear steering, at least on the listed cars above
|
||||
ret.steerRatioRear = 0.
|
||||
|
||||
# no max steer limit VS speed
|
||||
ret.steerMaxBP = [0.] # m/s
|
||||
ret.steerMaxV = [1.] # max steer allowed
|
||||
|
||||
ret.gasMaxBP = [0.] # m/s
|
||||
ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed
|
||||
ret.brakeMaxBP = [5., 20.] # m/s
|
||||
ret.brakeMaxV = [1., 0.8] # max brake allowed
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.startAccel = 0.5
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerRateCost = 0.5
|
||||
ret.steerLimitTimer = 0.8
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas / 256.0
|
||||
if not self.CP.enableGasInterceptor:
|
||||
ret.gasPressed = self.CS.pedal_gas > 0
|
||||
else:
|
||||
ret.gasPressed = self.CS.user_gas_pressed
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
# FIXME: read sendcan for brakelights
|
||||
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
|
||||
ret.brakeLights = bool(self.CS.brake_switch or
|
||||
c.actuators.brake > brakelights_threshold)
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate
|
||||
|
||||
# gear shifter lever
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringTorqueEps = self.CS.steer_torque_motor
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode)
|
||||
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
ret.stockAeb = self.CS.stock_aeb
|
||||
ret.stockFcw = self.CS.stock_fcw
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.unknown
|
||||
if self.CS.cruise_buttons != 0:
|
||||
be.pressed = True
|
||||
but = self.CS.cruise_buttons
|
||||
else:
|
||||
be.pressed = False
|
||||
but = self.CS.prev_cruise_buttons
|
||||
if but == CruiseButtons.RES_ACCEL:
|
||||
be.type = ButtonType.accelCruise
|
||||
elif but == CruiseButtons.DECEL_SET:
|
||||
be.type = ButtonType.decelCruise
|
||||
elif but == CruiseButtons.CANCEL:
|
||||
be.type = ButtonType.cancel
|
||||
elif but == CruiseButtons.MAIN:
|
||||
be.type = ButtonType.altButton3
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.unknown
|
||||
if self.CS.cruise_setting != 0:
|
||||
be.pressed = True
|
||||
but = self.CS.cruise_setting
|
||||
else:
|
||||
be.pressed = False
|
||||
but = self.CS.prev_cruise_setting
|
||||
if but == 1:
|
||||
be.type = ButtonType.altButton1
|
||||
# TODO: more buttons?
|
||||
buttonEvents.append(be)
|
||||
ret.buttonEvents = buttonEvents
|
||||
|
||||
# events
|
||||
events = []
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
elif self.CS.steer_warning:
|
||||
events.append(create_event('steerTempUnavailable', [ET.WARNING]))
|
||||
if self.CS.brake_error:
|
||||
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if not ret.gearShifter == GearShifter.drive:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on or self.CS.cruise_mode:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
|
||||
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if self.CS.park_brake:
|
||||
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
|
||||
|
||||
# disable on pedals rising edge or when brake is pressed and speed isn't zero
|
||||
if (ret.gasPressed and not self.gas_pressed_prev) or \
|
||||
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
# 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 self.CP.enableCruise and not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl):
|
||||
# non loud alert if cruise disbales below 25mph as expected (+ a little margin)
|
||||
if ret.vEgo < self.CP.minEnableSpeed + 2.:
|
||||
events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
|
||||
else:
|
||||
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
|
||||
events.append(create_event('manualRestart', [ET.WARNING]))
|
||||
|
||||
cur_time = self.frame * DT_CTRL
|
||||
enable_pressed = False
|
||||
# handle button presses
|
||||
for b in ret.buttonEvents:
|
||||
|
||||
# do enable on both accel and decel buttons
|
||||
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
|
||||
self.last_enable_pressed = cur_time
|
||||
enable_pressed = True
|
||||
|
||||
# do disable on button down
|
||||
if b.type == "cancel" and b.pressed:
|
||||
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
|
||||
|
||||
if self.CP.enableCruise:
|
||||
# KEEP THIS EVENT LAST! send enable event if button is pressed and there are
|
||||
# NO_ENTRY events, so controlsd will display alerts. Also not send enable events
|
||||
# too close in time, so a no_entry will not be followed by another one.
|
||||
# TODO: button press should be the only thing that triggers enable
|
||||
if ((cur_time - self.last_enable_pressed) < 0.2 and
|
||||
(cur_time - self.last_enable_sent) > 0.2 and
|
||||
ret.cruiseState.enabled) or \
|
||||
(enable_pressed and get_events(events, [ET.NO_ENTRY])):
|
||||
events.append(create_event('buttonEnable', [ET.ENABLE]))
|
||||
self.last_enable_sent = cur_time
|
||||
elif enable_pressed:
|
||||
events.append(create_event('buttonEnable', [ET.ENABLE]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
# update previous brake/gas pressed
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
if c.hudControl.speedVisible:
|
||||
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
|
||||
else:
|
||||
hud_v_cruise = 255
|
||||
|
||||
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
|
||||
c.actuators,
|
||||
c.cruiseControl.speedOverride,
|
||||
c.cruiseControl.override,
|
||||
c.cruiseControl.cancel,
|
||||
pcm_accel,
|
||||
hud_v_cruise,
|
||||
c.hudControl.lanesVisible,
|
||||
hud_show_car=c.hudControl.leadVisible,
|
||||
hud_alert=c.hudControl.visualAlert)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
def _create_nidec_can_parser():
|
||||
dbc_f = 'acura_ilx_2016_nidec.dbc'
|
||||
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
|
||||
signals = list(zip(['RADAR_STATE'] +
|
||||
['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
|
||||
['REL_SPEED'] * 16,
|
||||
[0x400] + radar_messages[1:] * 4,
|
||||
[0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16))
|
||||
checks = list(zip([0x445], [20]))
|
||||
fn = os.path.splitext(dbc_f)[0].encode('utf8')
|
||||
return CANParser(fn, signals, checks, 1)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.track_id = 0
|
||||
self.radar_fault = False
|
||||
self.radar_wrong_config = False
|
||||
self.radar_off_can = CP.radarOffCan
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
|
||||
self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar
|
||||
|
||||
# Nidec
|
||||
self.rcp = _create_nidec_can_parser()
|
||||
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:
|
||||
if 'NO_RADAR_SLEEP' not in os.environ:
|
||||
time.sleep(self.radar_ts)
|
||||
return car.RadarData.new_message()
|
||||
|
||||
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,0 +1,216 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# Car button codes
|
||||
class CruiseButtons:
|
||||
RES_ACCEL = 4
|
||||
DECEL_SET = 3
|
||||
CANCEL = 2
|
||||
MAIN = 1
|
||||
|
||||
# See dbc files for info on values"
|
||||
VISUAL_HUD = {
|
||||
VisualAlert.none: 0,
|
||||
VisualAlert.fcw: 1,
|
||||
VisualAlert.steerRequired: 1,
|
||||
VisualAlert.brakePressed: 10,
|
||||
VisualAlert.wrongGear: 6,
|
||||
VisualAlert.seatbeltUnbuckled: 5,
|
||||
VisualAlert.speedTooHigh: 8}
|
||||
|
||||
class ECU:
|
||||
CAM = Ecu.fwdCamera
|
||||
|
||||
class CAR:
|
||||
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
|
||||
ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T"
|
||||
ACCORDH = "HONDA ACCORD 2018 HYBRID TOURING"
|
||||
CIVIC = "HONDA CIVIC 2016 TOURING"
|
||||
CIVIC_BOSCH = "HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019"
|
||||
ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS"
|
||||
CRV = "HONDA CR-V 2016 TOURING"
|
||||
CRV_5G = "HONDA CR-V 2017 EX"
|
||||
CRV_HYBRID = "HONDA CR-V 2019 HYBRID"
|
||||
FIT = "HONDA FIT 2018 EX"
|
||||
ODYSSEY = "HONDA ODYSSEY 2018 EX-L"
|
||||
ODYSSEY_CHN = "HONDA ODYSSEY 2019 EXCLUSIVE CHN"
|
||||
ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS"
|
||||
PILOT = "HONDA PILOT 2017 TOURING"
|
||||
PILOT_2019 = "HONDA PILOT 2019 ELITE"
|
||||
RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION"
|
||||
|
||||
# diag message that in some Nidec cars only appear with 1s freq if VIN query is performed
|
||||
DIAG_MSGS = {1600: 5, 1601: 8}
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.ACCORD: [{
|
||||
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
|
||||
}],
|
||||
CAR.ACCORD_15: [{
|
||||
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
|
||||
}],
|
||||
CAR.ACCORDH: [{
|
||||
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
|
||||
}],
|
||||
CAR.ACURA_ILX: [{
|
||||
57: 3, 145: 8, 228: 5, 304: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 419: 8, 420: 8, 422: 8, 428: 8, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1030: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5,
|
||||
}],
|
||||
# Acura RDX w/ Added Comma Pedal Support (512L & 513L)
|
||||
CAR.ACURA_RDX: [{
|
||||
57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1
|
||||
}],
|
||||
CAR.CIVIC: [{
|
||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 450: 8, 464: 8, 470: 2, 476: 7, 487: 4, 490: 8, 493: 5, 506: 8, 512: 6, 513: 6, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1633: 8,
|
||||
}],
|
||||
CAR.CIVIC_BOSCH: [{
|
||||
# 2017 Civic Hatchback EX, 2019 Civic Sedan Touring Canadian, and 2018 Civic Hatchback Executive Premium 1.0L CVT European
|
||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1629: 5, 1633: 8,
|
||||
}],
|
||||
CAR.CRV: [{
|
||||
57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8,
|
||||
}],
|
||||
CAR.CRV_5G: [{
|
||||
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
|
||||
}],
|
||||
CAR.CRV_HYBRID: [{
|
||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5
|
||||
}],
|
||||
CAR.FIT: [{
|
||||
57: 3, 145: 8, 228: 5, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 422: 8, 427: 3, 428: 8, 432: 7, 464: 8, 487: 4, 490: 8, 506: 8, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8
|
||||
}],
|
||||
# 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L)
|
||||
CAR.ODYSSEY: [{
|
||||
57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5
|
||||
},
|
||||
# 2018 Odyssey Elite w/ Added Comma Pedal Support (512L & 513L)
|
||||
{
|
||||
57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5
|
||||
}],
|
||||
CAR.ODYSSEY_CHN: [{
|
||||
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
|
||||
}],
|
||||
# 2017 Pilot Touring AND 2016 Pilot EX-L w/ Added Comma Pedal Support (512L & 513L)
|
||||
CAR.PILOT: [{
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5
|
||||
}],
|
||||
# this fingerprint also includes the Passport 2019
|
||||
CAR.PILOT_2019: [{
|
||||
57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
|
||||
},
|
||||
# 2019 Pilot EX-L
|
||||
{
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
|
||||
}],
|
||||
# Ridgeline w/ Added Comma Pedal Support (512L & 513L)
|
||||
CAR.RIDGELINE: [{
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 471: 3, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1613: 5, 1616: 5, 1618: 5, 1668: 5, 2015: 3
|
||||
},
|
||||
# 2019 Ridgeline
|
||||
{
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5
|
||||
}]
|
||||
}
|
||||
|
||||
# add DIAG_MSGS to fingerprints
|
||||
for c in FINGERPRINTS:
|
||||
for f, _ in enumerate(FINGERPRINTS[c]):
|
||||
for d in DIAG_MSGS:
|
||||
FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
|
||||
|
||||
# TODO: Figure out what is relevant
|
||||
FW_VERSIONS = {
|
||||
CAR.CIVIC: {
|
||||
(Ecu.unknown, 0x18da10f1, None): [b'37805-5AA-L660\x00\x00'],
|
||||
(Ecu.unknown, 0x18da1ef1, None): [b'28101-5CG-A050\x00\x00'],
|
||||
(Ecu.unknown, 0x18da28f1, None): [b'57114-TBA-A550\x00\x00'],
|
||||
(Ecu.eps, 0x18da30f1, None): [b'39990-TBA-A030\x00\x00', b'39990-TBA,A030\x00\x00'],
|
||||
(Ecu.unknown, 0x18da53f1, None): [b'77959-TBA-A030\x00\x00'],
|
||||
(Ecu.unknown, 0x18da60f1, None): [b'78109-TBC-A310\x00\x00'],
|
||||
(Ecu.unknown, 0x18dab0f1, None): [b'36161-TBC-A030\x00\x00'],
|
||||
(Ecu.unknown, 0x18daeff1, None): [b'38897-TBA-A020\x00\x00'],
|
||||
|
||||
},
|
||||
CAR.ACCORD: {
|
||||
(Ecu.unknown, 0x18da10f1, None): [b'37805-6B2-A650\x00\x00'],
|
||||
(Ecu.unknown, 0x18da0bf1, None): [b'54008-TVC-A910\x00\x00'],
|
||||
(Ecu.unknown, 0x18da1ef1, None): [b'28102-6B8-A560\x00\x00'],
|
||||
(Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00'],
|
||||
(Ecu.unknown, 0x18da28f1, None): [b'57114-TVA-C050\x00\x00'],
|
||||
(Ecu.eps, 0x18da30f1, None): [b'39990-TVA-A150\x00\x00'],
|
||||
(Ecu.unknown, 0x18da3af1, None): [b'39390-TVA-A020\x00\x00'],
|
||||
(Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00'],
|
||||
(Ecu.unknown, 0x18da60f1, None): [b'78109-TVC-A210\x00\x00'],
|
||||
(Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'],
|
||||
(Ecu.unknown, 0x18dab0f1, None): [b'36802-TVA-A160\x00\x00'],
|
||||
(Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00'],
|
||||
(Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A010\x00\x00'],
|
||||
}
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
|
||||
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
|
||||
CAR.ACCORDH: dbc_dict('honda_accord_s2t_2018_can_generated', None),
|
||||
CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None),
|
||||
CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None),
|
||||
CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None),
|
||||
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = {
|
||||
CAR.ACCORD: 1200,
|
||||
CAR.ACCORD_15: 1200,
|
||||
CAR.ACCORDH: 1200,
|
||||
CAR.ACURA_ILX: 1200,
|
||||
CAR.ACURA_RDX: 400,
|
||||
CAR.CIVIC: 1200,
|
||||
CAR.CIVIC_BOSCH: 1200,
|
||||
CAR.CRV: 1200,
|
||||
CAR.CRV_5G: 1200,
|
||||
CAR.CRV_HYBRID: 1200,
|
||||
CAR.FIT: 1200,
|
||||
CAR.ODYSSEY: 1200,
|
||||
CAR.ODYSSEY_CHN: 1200,
|
||||
CAR.PILOT: 1200,
|
||||
CAR.PILOT_2019: 1200,
|
||||
CAR.RIDGELINE: 1200,
|
||||
}
|
||||
|
||||
SPEED_FACTOR = {
|
||||
CAR.ACCORD: 1.,
|
||||
CAR.ACCORD_15: 1.,
|
||||
CAR.ACCORDH: 1.,
|
||||
CAR.ACURA_ILX: 1.,
|
||||
CAR.ACURA_RDX: 1.,
|
||||
CAR.CIVIC: 1.,
|
||||
CAR.CIVIC_BOSCH: 1.,
|
||||
CAR.CRV: 1.025,
|
||||
CAR.CRV_5G: 1.025,
|
||||
CAR.CRV_HYBRID: 1.025,
|
||||
CAR.FIT: 1.,
|
||||
CAR.ODYSSEY: 1.,
|
||||
CAR.ODYSSEY_CHN: 1.,
|
||||
CAR.PILOT: 1.,
|
||||
CAR.PILOT_2019: 1.,
|
||||
CAR.RIDGELINE: 1.,
|
||||
}
|
||||
|
||||
# msgs sent for steering controller by camera module on can 0.
|
||||
# those messages are mutually exclusive on CRV and non-CRV cars
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [0xE4, 0x194], # steer torque cmd
|
||||
}
|
||||
|
||||
HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID]
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11
|
||||
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint):
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = car_fingerprint
|
||||
self.lkas11_cnt = 0
|
||||
self.cnt = 0
|
||||
self.last_resume_cnt = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.steer_rate_limited = False
|
||||
|
||||
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
|
||||
|
||||
### Steering Torque
|
||||
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
if not enabled:
|
||||
apply_steer = 0
|
||||
|
||||
steer_req = 1 if enabled else 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
can_sends = []
|
||||
|
||||
self.lkas11_cnt = self.cnt % 0x10
|
||||
self.clu11_cnt = self.cnt % 0x10
|
||||
|
||||
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
|
||||
enabled, CS.lkas11, hud_alert, keep_stock=True))
|
||||
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
|
||||
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
|
||||
self.last_resume_cnt = self.cnt
|
||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
|
||||
|
||||
self.cnt += 1
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,251 @@
|
|||
from cereal import car
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("WHL_SPD_FL", "WHL_SPD11", 0),
|
||||
("WHL_SPD_FR", "WHL_SPD11", 0),
|
||||
("WHL_SPD_RL", "WHL_SPD11", 0),
|
||||
("WHL_SPD_RR", "WHL_SPD11", 0),
|
||||
|
||||
("YAW_RATE", "ESP12", 0),
|
||||
|
||||
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
|
||||
|
||||
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
|
||||
("CF_Gway_TSigLHSw", "CGW1", 0),
|
||||
("CF_Gway_TurnSigLh", "CGW1", 0),
|
||||
("CF_Gway_TSigRHSw", "CGW1", 0),
|
||||
("CF_Gway_TurnSigRh", "CGW1", 0),
|
||||
("CF_Gway_ParkBrakeSw", "CGW1", 0),
|
||||
|
||||
("BRAKE_ACT", "EMS12", 0),
|
||||
("PV_AV_CAN", "EMS12", 0),
|
||||
("TPS", "EMS12", 0),
|
||||
|
||||
("CYL_PRES", "ESP12", 0),
|
||||
|
||||
("CF_Clu_CruiseSwState", "CLU11", 0),
|
||||
("CF_Clu_CruiseSwMain", "CLU11", 0),
|
||||
("CF_Clu_SldMainSW", "CLU11", 0),
|
||||
("CF_Clu_ParityBit1", "CLU11", 0),
|
||||
("CF_Clu_VanzDecimal" , "CLU11", 0),
|
||||
("CF_Clu_Vanz", "CLU11", 0),
|
||||
("CF_Clu_SPEED_UNIT", "CLU11", 0),
|
||||
("CF_Clu_DetentOut", "CLU11", 0),
|
||||
("CF_Clu_RheostatLevel", "CLU11", 0),
|
||||
("CF_Clu_CluInfo", "CLU11", 0),
|
||||
("CF_Clu_AmpInfo", "CLU11", 0),
|
||||
("CF_Clu_AliveCnt1", "CLU11", 0),
|
||||
|
||||
("CF_Clu_InhibitD", "CLU15", 0),
|
||||
("CF_Clu_InhibitP", "CLU15", 0),
|
||||
("CF_Clu_InhibitN", "CLU15", 0),
|
||||
("CF_Clu_InhibitR", "CLU15", 0),
|
||||
|
||||
("CF_Lvr_Gear", "LVR12",0),
|
||||
("CUR_GR", "TCU12",0),
|
||||
|
||||
("ACCEnable", "TCS13", 0),
|
||||
("ACC_REQ", "TCS13", 0),
|
||||
("DriverBraking", "TCS13", 0),
|
||||
("DriverOverride", "TCS13", 0),
|
||||
|
||||
("ESC_Off_Step", "TCS15", 0),
|
||||
|
||||
("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
|
||||
|
||||
("CR_Mdps_DrvTq", "MDPS11", 0),
|
||||
|
||||
("CR_Mdps_StrColTq", "MDPS12", 0),
|
||||
("CF_Mdps_ToiActive", "MDPS12", 0),
|
||||
("CF_Mdps_ToiUnavail", "MDPS12", 0),
|
||||
("CF_Mdps_FailStat", "MDPS12", 0),
|
||||
("CR_Mdps_OutTq", "MDPS12", 0),
|
||||
|
||||
("VSetDis", "SCC11", 0),
|
||||
("SCCInfoDisplay", "SCC11", 0),
|
||||
("ACCMode", "SCC12", 1),
|
||||
|
||||
("SAS_Angle", "SAS11", 0),
|
||||
("SAS_Speed", "SAS11", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
# address, frequency
|
||||
("MDPS12", 50),
|
||||
("MDPS11", 100),
|
||||
("TCS15", 10),
|
||||
("TCS13", 50),
|
||||
("CLU11", 50),
|
||||
("ESP12", 100),
|
||||
("EMS12", 100),
|
||||
("CGW1", 10),
|
||||
("CGW4", 5),
|
||||
("WHL_SPD11", 50),
|
||||
("SCC11", 50),
|
||||
("SCC12", 50),
|
||||
("SAS11", 100)
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
def get_camera_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("CF_Lkas_LdwsSysState", "LKAS11", 0),
|
||||
("CF_Lkas_SysWarning", "LKAS11", 0),
|
||||
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
|
||||
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
|
||||
("CF_Lkas_HbaLamp", "LKAS11", 0),
|
||||
("CF_Lkas_FcwBasReq", "LKAS11", 0),
|
||||
("CF_Lkas_ToiFlt", "LKAS11", 0),
|
||||
("CF_Lkas_HbaSysState", "LKAS11", 0),
|
||||
("CF_Lkas_FcwOpt", "LKAS11", 0),
|
||||
("CF_Lkas_HbaOpt", "LKAS11", 0),
|
||||
("CF_Lkas_FcwSysState", "LKAS11", 0),
|
||||
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
|
||||
("CF_Lkas_FusionState", "LKAS11", 0),
|
||||
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
|
||||
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
|
||||
]
|
||||
|
||||
checks = []
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
self.left_blinker_on = 0
|
||||
self.left_blinker_flash = 0
|
||||
self.right_blinker_on = 0
|
||||
self.right_blinker_flash = 0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# update prevs, update must run once per Loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
self.door_all_closed = True
|
||||
self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw']
|
||||
|
||||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
|
||||
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
|
||||
|
||||
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
|
||||
self.main_on = True
|
||||
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
|
||||
self.low_speed_lockout = v_wheel < 1.0
|
||||
|
||||
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
|
||||
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
|
||||
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
|
||||
self.standstill = not v_wheel > 0.1
|
||||
|
||||
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
|
||||
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
|
||||
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
|
||||
self.main_on = True
|
||||
self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
|
||||
self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh']
|
||||
self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
|
||||
self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh']
|
||||
self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD
|
||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
|
||||
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
|
||||
self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq']
|
||||
self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
|
||||
|
||||
self.user_brake = 0
|
||||
|
||||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
|
||||
self.brake_lights = bool(self.brake_pressed)
|
||||
if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1):
|
||||
self.pedal_gas = 0
|
||||
else:
|
||||
self.pedal_gas = cp.vl["EMS12"]['TPS']
|
||||
self.car_gas = cp.vl["EMS12"]['TPS']
|
||||
|
||||
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
||||
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
||||
if gear == 5:
|
||||
self.gear_shifter = GearShifter.drive
|
||||
elif gear == 6:
|
||||
self.gear_shifter = GearShifter.neutral
|
||||
elif gear == 0:
|
||||
self.gear_shifter = GearShifter.park
|
||||
elif gear == 7:
|
||||
self.gear_shifter = GearShifter.reverse
|
||||
else:
|
||||
self.gear_shifter = GearShifter.unknown
|
||||
|
||||
# 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 cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.drive
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.neutral
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.park
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
|
||||
self.gear_shifter_cluster = GearShifter.reverse
|
||||
else:
|
||||
self.gear_shifter_cluster = GearShifter.unknown
|
||||
|
||||
# Gear Selecton via TCU12
|
||||
gear2 = cp.vl["TCU12"]["CUR_GR"]
|
||||
if gear2 == 0:
|
||||
self.gear_tcu = GearShifter.park
|
||||
elif gear2 == 14:
|
||||
self.gear_tcu = GearShifter.reverse
|
||||
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
|
||||
self.gear_tcu = GearShifter.drive
|
||||
else:
|
||||
self.gear_tcu = GearShifter.unknown
|
||||
|
||||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = cp_cam.vl["LKAS11"]
|
||||
self.clu11 = cp.vl["CLU11"]
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
import crcmod
|
||||
from selfdrive.car.hyundai.values import CHECKSUM
|
||||
|
||||
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
|
||||
|
||||
def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False):
|
||||
values = {
|
||||
"CF_Lkas_Bca_R": 3 if enabled else 0,
|
||||
"CF_Lkas_LdwsSysState": 3 if steer_req else 1,
|
||||
"CF_Lkas_SysWarning": hud_alert,
|
||||
"CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,
|
||||
"CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"] if keep_stock else 0,
|
||||
"CF_Lkas_HbaLamp": lkas11["CF_Lkas_HbaLamp"] if keep_stock else 0,
|
||||
"CF_Lkas_FcwBasReq": lkas11["CF_Lkas_FcwBasReq"] if keep_stock else 0,
|
||||
"CR_Lkas_StrToqReq": apply_steer,
|
||||
"CF_Lkas_ActToi": steer_req,
|
||||
"CF_Lkas_ToiFlt": 0,
|
||||
"CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"] if keep_stock else 1,
|
||||
"CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"] if keep_stock else 0,
|
||||
"CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"] if keep_stock else 3,
|
||||
"CF_Lkas_MsgCount": cnt,
|
||||
"CF_Lkas_FcwSysState": lkas11["CF_Lkas_FcwSysState"] if keep_stock else 0,
|
||||
"CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"] if keep_stock else 0,
|
||||
"CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"] if keep_stock else 0,
|
||||
"CF_Lkas_Chksum": 0,
|
||||
"CF_Lkas_FcwOpt_USM": 2 if enabled else 1,
|
||||
"CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"] if keep_stock else 3,
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("LKAS11", 0, values)[2]
|
||||
|
||||
if car_fingerprint in CHECKSUM["crc8"]:
|
||||
# CRC Checksum as seen on 2019 Hyundai Santa Fe
|
||||
dat = dat[:6] + dat[7:8]
|
||||
checksum = hyundai_checksum(dat)
|
||||
elif car_fingerprint in CHECKSUM["6B"]:
|
||||
# Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
|
||||
checksum = sum(dat[:6]) % 256
|
||||
elif car_fingerprint in CHECKSUM["7B"]:
|
||||
# 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, clu11, button):
|
||||
values = {
|
||||
"CF_Clu_CruiseSwState": button,
|
||||
"CF_Clu_CruiseSwMain": clu11["CF_Clu_CruiseSwMain"],
|
||||
"CF_Clu_SldMainSW": clu11["CF_Clu_SldMainSW"],
|
||||
"CF_Clu_ParityBit1": clu11["CF_Clu_ParityBit1"],
|
||||
"CF_Clu_VanzDecimal": clu11["CF_Clu_VanzDecimal"],
|
||||
"CF_Clu_Vanz": clu11["CF_Clu_Vanz"],
|
||||
"CF_Clu_SPEED_UNIT": clu11["CF_Clu_SPEED_UNIT"],
|
||||
"CF_Clu_DetentOut": clu11["CF_Clu_DetentOut"],
|
||||
"CF_Clu_RheostatLevel": clu11["CF_Clu_RheostatLevel"],
|
||||
"CF_Clu_CluInfo": clu11["CF_Clu_CluInfo"],
|
||||
"CF_Clu_AmpInfo": clu11["CF_Clu_AmpInfo"],
|
||||
"CF_Clu_AliveCnt1": 0,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("CLU11", 0, values)
|
||||
|
|
@ -0,0 +1,282 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser
|
||||
from selfdrive.car.hyundai.values import ECU, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
self.idx = 0
|
||||
self.lanes = 0
|
||||
self.lkas_request = 0
|
||||
|
||||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
self.low_speed_alert = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_camera_parser(CP)
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "hyundai"
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
ret.radarOffCan = True
|
||||
ret.safetyModel = car.CarParams.SafetyModel.hyundai
|
||||
ret.enableCruise = True # stock acc
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay
|
||||
ret.steerRateCost = 0.5
|
||||
ret.steerLimitTimer = 0.4
|
||||
tire_stiffness_factor = 1.
|
||||
|
||||
if candidate == CAR.SANTA_FE:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.766
|
||||
|
||||
# Values from optimizer
|
||||
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
|
||||
tire_stiffness_factor = 0.82
|
||||
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
|
||||
ret.minSteerSpeed = 0.
|
||||
elif candidate == CAR.KIA_SORENTO:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1985. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 0.
|
||||
elif candidate == CAR.ELANTRA:
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
ret.mass = 1275. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.7
|
||||
ret.steerRatio = 13.73 #Spec
|
||||
tire_stiffness_factor = 0.385
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.GENESIS:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 2060. + STD_CARGO_KG
|
||||
ret.wheelbase = 3.01
|
||||
ret.steerRatio = 16.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
|
||||
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.KIA_OPTIMA:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.80
|
||||
ret.steerRatio = 13.75
|
||||
tire_stiffness_factor = 0.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate == CAR.KIA_STINGER:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1825. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
|
||||
# no rear steering, at least on the listed cars above
|
||||
ret.steerRatioRear = 0.
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
||||
# steer, gas, brake limitations VS speed
|
||||
ret.steerMaxBP = [0.]
|
||||
ret.steerMaxV = [1.0]
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [1.]
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [1.]
|
||||
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
ret.stoppingControl = False
|
||||
ret.startAccel = 0.0
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.yawRate = self.CS.yaw_rate
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
||||
ret.gearShifter = self.CS.gear_shifter_cluster
|
||||
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
||||
ret.gearShifter = self.CS.gear_tcu
|
||||
else:
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
|
||||
if self.CS.pcm_acc_status != 0:
|
||||
ret.cruiseState.speed = self.CS.cruise_set_speed
|
||||
else:
|
||||
ret.cruiseState.speed = 0
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
# 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
|
||||
|
||||
events = []
|
||||
if not ret.gearShifter == GearShifter.drive:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
|
||||
|
||||
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
elif not ret.cruiseState.enabled:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
# disable on pedals rising edge or when brake is pressed and speed isn't zero
|
||||
if (ret.gasPressed and not self.gas_pressed_prev) or \
|
||||
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
if self.low_speed_alert:
|
||||
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
|
||||
def apply(self, c):
|
||||
|
||||
hud_alert = get_hud_alerts(c.hudControl.visualAlert)
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, c.actuators,
|
||||
c.cruiseControl.cancel, hud_alert)
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def get_hud_alerts(visual_alert):
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
return 5
|
||||
else:
|
||||
return 0
|
||||
|
||||
# Steer torque limits
|
||||
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 255 # 409 is the max, 255 is stock
|
||||
STEER_DELTA_UP = 3
|
||||
STEER_DELTA_DOWN = 7
|
||||
STEER_DRIVER_ALLOWANCE = 50
|
||||
STEER_DRIVER_MULTIPLIER = 2
|
||||
STEER_DRIVER_FACTOR = 1
|
||||
|
||||
class CAR:
|
||||
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
|
||||
GENESIS = "HYUNDAI GENESIS 2018"
|
||||
KIA_OPTIMA = "KIA OPTIMA SX 2019"
|
||||
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
|
||||
KIA_STINGER = "KIA STINGER GT2 2018"
|
||||
SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019"
|
||||
|
||||
class Buttons:
|
||||
NONE = 0
|
||||
RES_ACCEL = 1
|
||||
SET_DECEL = 2
|
||||
CANCEL = 4
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.ELANTRA: [{
|
||||
66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
}],
|
||||
CAR.GENESIS: [{
|
||||
67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
|
||||
},
|
||||
{
|
||||
67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
|
||||
}],
|
||||
CAR.KIA_OPTIMA: [{
|
||||
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
}],
|
||||
CAR.KIA_SORENTO: [{
|
||||
67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1
|
||||
}],
|
||||
CAR.KIA_STINGER: [{
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8
|
||||
}],
|
||||
CAR.SANTA_FE: [{
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8
|
||||
},
|
||||
{
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8
|
||||
}
|
||||
],
|
||||
}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [832, 1156, 1191, 1342]
|
||||
}
|
||||
|
||||
CHECKSUM = {
|
||||
"crc8": [CAR.SANTA_FE],
|
||||
"6B": [CAR.KIA_SORENTO, CAR.GENESIS],
|
||||
"7B": [CAR.KIA_STINGER, CAR.ELANTRA, CAR.KIA_OPTIMA],
|
||||
}
|
||||
|
||||
FEATURES = {
|
||||
"use_cluster_gears": [CAR.ELANTRA], # Use Cluster for Gear Selection, rather than Transmission
|
||||
"use_tcu_gears": [CAR.KIA_OPTIMA], # Use TCU Message for Gear Selection
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.GENESIS: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None),
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 100
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
# generic car and radar interfaces
|
||||
|
||||
class CarInterfaceBase():
|
||||
def __init__(self, CP, CarController):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
raise NotImplementedError
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
raise NotImplementedError
|
||||
|
||||
# returns a car.CarState, pass in car.CarControl
|
||||
def update(self, c, can_strings):
|
||||
raise NotImplementedError
|
||||
|
||||
# return sendcan, pass in a car.CarControl
|
||||
def apply(self, c):
|
||||
raise NotImplementedError
|
||||
|
||||
class RadarInterfaceBase():
|
||||
def __init__(self, CP):
|
||||
self.pts = {}
|
||||
self.delay = 0
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
|
||||
def update(self, can_strings):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
if 'NO_RADAR_SLEEP' not in os.environ:
|
||||
time.sleep(self.radar_ts) # radard runs on RI updates
|
||||
|
||||
return ret
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
import time
|
||||
from collections import defaultdict
|
||||
from functools import partial
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr
|
||||
|
||||
|
||||
class IsoTpParallelQuery():
|
||||
def __init__(self, sendcan, logcan, bus, addrs, request, response, functional_addr=False, debug=False):
|
||||
self.sendcan = sendcan
|
||||
self.logcan = logcan
|
||||
self.bus = bus
|
||||
self.request = request
|
||||
self.response = response
|
||||
self.debug = debug
|
||||
self.functional_addr = functional_addr
|
||||
|
||||
self.real_addrs = []
|
||||
for a in addrs:
|
||||
if isinstance(a, tuple):
|
||||
self.real_addrs.append(a)
|
||||
else:
|
||||
self.real_addrs.append((a, None))
|
||||
|
||||
self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0]) for tx_addr in self.real_addrs}
|
||||
self.msg_buffer = defaultdict(list)
|
||||
|
||||
def rx(self):
|
||||
"""Drain can socket and sort messages into buffers based on address"""
|
||||
can_packets = messaging.drain_sock(self.logcan, wait_for_one=True)
|
||||
|
||||
for packet in can_packets:
|
||||
for msg in packet.can:
|
||||
if msg.src == self.bus:
|
||||
if self.functional_addr:
|
||||
if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF):
|
||||
fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32)
|
||||
self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src))
|
||||
elif 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(self.logcan)
|
||||
self.msg_buffer = defaultdict(list)
|
||||
|
||||
def get_data(self, timeout):
|
||||
self._drain_rx()
|
||||
|
||||
# Create message objects
|
||||
msgs = {}
|
||||
request_counter = {}
|
||||
request_done = {}
|
||||
for tx_addr, rx_addr in self.msg_addrs.items():
|
||||
# rx_addr not set when using functional tx addr
|
||||
id_addr = rx_addr or tx_addr[0]
|
||||
sub_addr = tx_addr[1]
|
||||
|
||||
can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr, self.bus, sub_addr=sub_addr, debug=self.debug)
|
||||
|
||||
max_len = 8 if sub_addr is None else 7
|
||||
|
||||
msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug)
|
||||
msg.send(self.request[0])
|
||||
|
||||
msgs[tx_addr] = msg
|
||||
request_counter[tx_addr] = 0
|
||||
request_done[tx_addr] = False
|
||||
|
||||
results = {}
|
||||
start_time = time.time()
|
||||
while True:
|
||||
self.rx()
|
||||
|
||||
if all(request_done.values()):
|
||||
break
|
||||
|
||||
for tx_addr, msg in msgs.items():
|
||||
dat = msg.recv()
|
||||
|
||||
if not dat:
|
||||
continue
|
||||
|
||||
counter = request_counter[tx_addr]
|
||||
expected_response = self.response[counter]
|
||||
response_valid = dat[:len(expected_response)] == expected_response
|
||||
|
||||
if response_valid:
|
||||
if counter + 1 < len(self.request):
|
||||
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:
|
||||
request_done[tx_addr] = True
|
||||
cloudlog.warning(f"iso-tp query bad response: 0x{bytes.hex(dat)}")
|
||||
|
||||
if time.time() - start_time > timeout:
|
||||
break
|
||||
|
||||
return results
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.swaglog import cloudlog
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
# mocked car interface to work with chffrplus
|
||||
TS = 0.01 # 100Hz
|
||||
YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
|
||||
# low pass gain
|
||||
LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.CC = CarController
|
||||
|
||||
cloudlog.debug("Using Mock Car Interface")
|
||||
|
||||
# TODO: subscribe to phone sensor
|
||||
self.sensor = messaging.sub_sock('sensorEvents')
|
||||
self.gps = messaging.sub_sock('gpsLocation')
|
||||
|
||||
self.speed = 0.
|
||||
self.prev_speed = 0.
|
||||
self.yaw_rate = 0.
|
||||
self.yaw_rate_meas = 0.
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return accel
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "mock"
|
||||
ret.carFingerprint = candidate
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.noOutput
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
|
||||
# scale unknown params for other cars
|
||||
ret.mass = 1700.
|
||||
ret.rotationalInertia = 2500.
|
||||
ret.wheelbase = 2.70
|
||||
ret.centerToFront = ret.wheelbase * 0.5
|
||||
ret.steerRatio = 13. # reasonable
|
||||
ret.tireStiffnessFront = 1e6 # very stiff to neglect slip
|
||||
ret.tireStiffnessRear = 1e6 # very stiff to neglect slip
|
||||
ret.steerRatioRear = 0.
|
||||
|
||||
ret.steerMaxBP = [0.]
|
||||
ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.]
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [0.]
|
||||
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
ret.steerActuatorDelay = 0.
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# get basic data from phone and gps since CAN isn't connected
|
||||
sensors = messaging.recv_sock(self.sensor)
|
||||
if sensors is not None:
|
||||
for sensor in sensors.sensorEvents:
|
||||
if sensor.type == 4: # gyro
|
||||
self.yaw_rate_meas = -sensor.gyro.v[0]
|
||||
|
||||
gps = messaging.recv_sock(self.gps)
|
||||
if gps is not None:
|
||||
self.prev_speed = self.speed
|
||||
self.speed = gps.gpsLocation.speed
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.speed
|
||||
ret.vEgoRaw = self.speed
|
||||
a = self.speed - self.prev_speed
|
||||
|
||||
ret.aEgo = a
|
||||
ret.brakePressed = a < -0.5
|
||||
|
||||
self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
|
||||
ret.yawRate = self.yaw_rate
|
||||
ret.standstill = self.speed < 0.01
|
||||
ret.wheelSpeeds.fl = self.speed
|
||||
ret.wheelSpeeds.fr = self.speed
|
||||
ret.wheelSpeeds.rl = self.speed
|
||||
ret.wheelSpeeds.rr = self.speed
|
||||
curvature = self.yaw_rate / max(self.speed, 1.)
|
||||
ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
|
||||
|
||||
events = []
|
||||
ret.events = events
|
||||
|
||||
return ret.as_reader()
|
||||
|
||||
def apply(self, c):
|
||||
# in mock no carcontrols
|
||||
return []
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
class CAR:
|
||||
MOCK = 'mock'
|
||||
|
|
@ -0,0 +1 @@
|
|||
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
#from common.numpy_fast import clip
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.subaru import subarucan
|
||||
from selfdrive.car.subaru.values import DBC
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
|
||||
class CarControllerParams():
|
||||
def __init__(self, car_fingerprint):
|
||||
self.STEER_MAX = 2047 # max_steer 4095
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
|
||||
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
|
||||
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
|
||||
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
|
||||
self.STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, car_fingerprint):
|
||||
self.lkas_active = False
|
||||
self.steer_idx = 0
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = car_fingerprint
|
||||
self.es_distance_cnt = -1
|
||||
self.es_lkas_cnt = -1
|
||||
self.steer_rate_limited = False
|
||||
|
||||
# Setup detection helper. Routes commands to
|
||||
# an appropriate CAN bus number.
|
||||
self.params = CarControllerParams(car_fingerprint)
|
||||
self.packer = CANPacker(DBC[car_fingerprint]['pt'])
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
|
||||
""" Controls thread """
|
||||
|
||||
P = self.params
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
|
||||
### STEER ###
|
||||
|
||||
if (frame % P.STEER_STEP) == 0:
|
||||
|
||||
final_steer = actuators.steer if enabled else 0.
|
||||
apply_steer = int(round(final_steer * P.STEER_MAX))
|
||||
|
||||
# limits due to driver torque
|
||||
|
||||
new_steer = int(round(apply_steer))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
lkas_enabled = enabled and not CS.steer_not_allowed
|
||||
|
||||
if not lkas_enabled:
|
||||
apply_steer = 0
|
||||
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
|
||||
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
|
||||
self.es_distance_cnt = CS.es_distance_msg["Counter"]
|
||||
|
||||
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
|
||||
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,153 @@
|
|||
import copy
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
|
||||
|
||||
def get_powertrain_can_parser(CP):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("Steer_Torque_Sensor", "Steering_Torque", 0),
|
||||
("Steering_Angle", "Steering_Torque", 0),
|
||||
("Cruise_On", "CruiseControl", 0),
|
||||
("Cruise_Activated", "CruiseControl", 0),
|
||||
("Brake_Pedal", "Brake_Pedal", 0),
|
||||
("Throttle_Pedal", "Throttle", 0),
|
||||
("LEFT_BLINKER", "Dashlights", 0),
|
||||
("RIGHT_BLINKER", "Dashlights", 0),
|
||||
("SEATBELT_FL", "Dashlights", 0),
|
||||
("FL", "Wheel_Speeds", 0),
|
||||
("FR", "Wheel_Speeds", 0),
|
||||
("RL", "Wheel_Speeds", 0),
|
||||
("RR", "Wheel_Speeds", 0),
|
||||
("DOOR_OPEN_FR", "BodyInfo", 1),
|
||||
("DOOR_OPEN_FL", "BodyInfo", 1),
|
||||
("DOOR_OPEN_RR", "BodyInfo", 1),
|
||||
("DOOR_OPEN_RL", "BodyInfo", 1),
|
||||
("Units", "Dash_State", 1),
|
||||
]
|
||||
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("Dashlights", 10),
|
||||
("CruiseControl", 20),
|
||||
("Wheel_Speeds", 50),
|
||||
("Steering_Torque", 50),
|
||||
("BodyInfo", 10),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
def get_camera_can_parser(CP):
|
||||
signals = [
|
||||
("Cruise_Set_Speed", "ES_DashStatus", 0),
|
||||
|
||||
("Counter", "ES_Distance", 0),
|
||||
("Signal1", "ES_Distance", 0),
|
||||
("Signal2", "ES_Distance", 0),
|
||||
("Main", "ES_Distance", 0),
|
||||
("Signal3", "ES_Distance", 0),
|
||||
|
||||
("Checksum", "ES_LKAS_State", 0),
|
||||
("Counter", "ES_LKAS_State", 0),
|
||||
("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
|
||||
("Empty_Box", "ES_LKAS_State", 0),
|
||||
("Signal1", "ES_LKAS_State", 0),
|
||||
("LKAS_ACTIVE", "ES_LKAS_State", 0),
|
||||
("Signal2", "ES_LKAS_State", 0),
|
||||
("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
|
||||
("LKAS_ENABLE_3", "ES_LKAS_State", 0),
|
||||
("Signal3", "ES_LKAS_State", 0),
|
||||
("LKAS_ENABLE_2", "ES_LKAS_State", 0),
|
||||
("Signal4", "ES_LKAS_State", 0),
|
||||
("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
|
||||
("Signal6", "ES_LKAS_State", 0),
|
||||
("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
|
||||
("Signal7", "ES_LKAS_State", 0),
|
||||
("FCW_Cont_Beep", "ES_LKAS_State", 0),
|
||||
("FCW_Repeated_Beep", "ES_LKAS_State", 0),
|
||||
("Throttle_Management_Activated", "ES_LKAS_State", 0),
|
||||
("Traffic_light_Ahead", "ES_LKAS_State", 0),
|
||||
("Right_Depart", "ES_LKAS_State", 0),
|
||||
("Signal5", "ES_LKAS_State", 0),
|
||||
|
||||
]
|
||||
|
||||
checks = [
|
||||
("ES_DashStatus", 10),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
# initialize can parser
|
||||
self.CP = CP
|
||||
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.left_blinker_on = False
|
||||
self.prev_left_blinker_on = False
|
||||
self.right_blinker_on = False
|
||||
self.prev_right_blinker_on = False
|
||||
self.steer_torque_driver = 0
|
||||
self.steer_not_allowed = False
|
||||
self.main_on = False
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
|
||||
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
|
||||
self.user_gas_pressed = self.pedal_gas > 0
|
||||
self.brake_pressed = self.brake_pressure > 0
|
||||
self.brake_lights = bool(self.brake_pressed)
|
||||
|
||||
self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
|
||||
|
||||
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed']
|
||||
# 1 = imperial, 6 = metric
|
||||
if cp.vl["Dash_State"]['Units'] == 1:
|
||||
self.v_cruise_pcm *= CV.MPH_TO_KPH
|
||||
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = self.v_ego_raw < 0.01
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
self.left_blinker_on = cp.vl["Dashlights"]['LEFT_BLINKER'] == 1
|
||||
self.right_blinker_on = cp.vl["Dashlights"]['RIGHT_BLINKER'] == 1
|
||||
self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
|
||||
self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
|
||||
self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated']
|
||||
self.main_on = cp.vl["CruiseControl"]['Cruise_On']
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint]
|
||||
self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle']
|
||||
self.door_open = 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']])
|
||||
|
||||
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
|
||||
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.subaru.values import CAR
|
||||
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
|
||||
self.frame = 0
|
||||
self.acc_active_prev = 0
|
||||
self.gas_pressed_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
self.VM = VehicleModel(CP)
|
||||
self.pt_cp = get_powertrain_can_parser(CP)
|
||||
self.cam_cp = get_camera_can_parser(CP)
|
||||
|
||||
self.gas_pressed_prev = False
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(CP.carFingerprint)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "subaru"
|
||||
ret.radarOffCan = True
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
ret.safetyModel = car.CarParams.SafetyModel.subaru
|
||||
|
||||
ret.enableCruise = True
|
||||
|
||||
# force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches)
|
||||
# was never released
|
||||
ret.enableCamera = True
|
||||
|
||||
ret.steerRateCost = 0.7
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
if candidate in [CAR.IMPREZA]:
|
||||
ret.mass = 1568. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.67
|
||||
ret.centerToFront = ret.wheelbase * 0.5
|
||||
ret.steerRatio = 15
|
||||
ret.steerActuatorDelay = 0.4 # end-to-end angle controller
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
|
||||
ret.steerMaxBP = [0.] # m/s
|
||||
ret.steerMaxV = [1.]
|
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
ret.steerRatioRear = 0.
|
||||
# testing tuning
|
||||
|
||||
# No long control in subaru
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.]
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [0.]
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
|
||||
# end from gm
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
self.pt_cp.update_strings(can_strings)
|
||||
self.cam_cp.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.pt_cp, self.cam_cp)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
|
||||
# torque and user override. Driver awareness
|
||||
# timer resets when the user uses the steering wheel.
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
ret.gas = self.CS.pedal_gas / 255.
|
||||
ret.gasPressed = self.CS.user_gas_pressed
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = bool(self.CS.acc_active)
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
|
||||
ret.leftBlinker = self.CS.left_blinker_on
|
||||
ret.rightBlinker = self.CS.right_blinker_on
|
||||
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
|
||||
ret.doorOpen = self.CS.door_open
|
||||
|
||||
buttonEvents = []
|
||||
|
||||
# blinkers
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.accelCruise
|
||||
buttonEvents.append(be)
|
||||
|
||||
|
||||
events = []
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if self.CS.acc_active and not self.acc_active_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
if not self.CS.acc_active:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
# disable on gas pedal rising edge
|
||||
if (ret.gasPressed and not self.gas_pressed_prev):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
# update previous brake/gas pressed
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.acc_active_prev = self.CS.acc_active
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
import copy
|
||||
from cereal import car
|
||||
from selfdrive.car.subaru.values import CAR
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def subaru_checksum(packer, values, addr):
|
||||
dat = packer.make_can_msg(addr, 0, values)[2]
|
||||
return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff
|
||||
|
||||
def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step):
|
||||
|
||||
if car_fingerprint == CAR.IMPREZA:
|
||||
#counts from 0 to 15 then back to 0 + 16 for enable bit
|
||||
idx = ((frame // steer_step) % 16)
|
||||
|
||||
values = {
|
||||
"Counter": idx,
|
||||
"LKAS_Output": apply_steer,
|
||||
"LKAS_Request": 1 if apply_steer != 0 else 0,
|
||||
"SET_1": 1
|
||||
}
|
||||
values["Checksum"] = subaru_checksum(packer, values, 0x122)
|
||||
|
||||
return packer.make_can_msg("ES_LKAS", 0, values)
|
||||
|
||||
def create_steering_status(packer, car_fingerprint, apply_steer, frame, steer_step):
|
||||
|
||||
if car_fingerprint == CAR.IMPREZA:
|
||||
values = {}
|
||||
values["Checksum"] = subaru_checksum(packer, {}, 0x322)
|
||||
|
||||
return packer.make_can_msg("ES_LKAS_State", 0, values)
|
||||
|
||||
def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd):
|
||||
|
||||
values = copy.copy(es_distance_msg)
|
||||
if pcm_cancel_cmd:
|
||||
values["Main"] = 1
|
||||
|
||||
values["Checksum"] = subaru_checksum(packer, values, 545)
|
||||
|
||||
return packer.make_can_msg("ES_Distance", 0, values)
|
||||
|
||||
def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line):
|
||||
|
||||
values = copy.copy(es_lkas_msg)
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
values["Keep_Hands_On_Wheel"] = 1
|
||||
|
||||
values["LKAS_Left_Line_Visible"] = int(left_line)
|
||||
values["LKAS_Right_Line_Visible"] = int(right_line)
|
||||
|
||||
values["Checksum"] = subaru_checksum(packer, values, 802)
|
||||
|
||||
return packer.make_can_msg("ES_LKAS_State", 0, values)
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
class CAR:
|
||||
IMPREZA = "SUBARU IMPREZA LIMITED 2019"
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.IMPREZA: [{
|
||||
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
|
||||
},
|
||||
# Crosstrek 2018 (same platform as Impreza)
|
||||
{
|
||||
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 256: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8
|
||||
}],
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = {
|
||||
CAR.IMPREZA: 80,
|
||||
}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [290, 356], # steer torque cmd
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.IMPREZA: dbc_dict('subaru_global_2017', None),
|
||||
}
|
||||
|
|
@ -0,0 +1 @@
|
|||
*.bz2
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import unittest
|
||||
import requests
|
||||
from cereal import car
|
||||
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
from selfdrive.car.honda.values import CAR as HONDA
|
||||
from selfdrive.car.honda.interface import CarInterface as HondaCarInterface
|
||||
from selfdrive.car.honda.carcontroller import CarController as HondaCarController
|
||||
from selfdrive.car.honda.radar_interface import RadarInterface as HondaRadarInterface
|
||||
|
||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from selfdrive.car.toyota.interface import CarInterface as ToyotaCarInterface
|
||||
from selfdrive.car.toyota.carcontroller import CarController as ToyotaCarController
|
||||
from selfdrive.car.toyota.radar_interface import RadarInterface as ToyotaRadarInterface
|
||||
|
||||
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
|
||||
|
||||
def run_route(route, car_name, CarInterface, CarController):
|
||||
lr = LogReader("/tmp/"+route + ".bz2")
|
||||
print(lr)
|
||||
|
||||
cps = []
|
||||
def CANParserHook(dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
|
||||
cp = CANParser(dbc_name, signals, checks, bus, sendcan, "", timeout)
|
||||
cps.append(cp)
|
||||
return cp
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController, CANParserHook)
|
||||
print(CI)
|
||||
|
||||
i = 0
|
||||
last_monotime = 0
|
||||
for msg in lr:
|
||||
if msg.which() == 'can':
|
||||
msg_bytes = msg.as_builder().to_bytes()
|
||||
monotime = msg.logMonoTime
|
||||
for x in cps:
|
||||
x.update_string(monotime, msg_bytes)
|
||||
|
||||
if (monotime-last_monotime) > 0.01:
|
||||
control = car.CarControl.new_message()
|
||||
CS = CI.update(control)
|
||||
if i % 100 == 0:
|
||||
print('\033[2J\033[H'+str(CS))
|
||||
last_monotime = monotime
|
||||
i += 1
|
||||
|
||||
return True
|
||||
|
||||
def run_route_radar(route, car_name, RadarInterface, CarInterface):
|
||||
lr = LogReader("/tmp/"+route + ".bz2")
|
||||
print(lr)
|
||||
|
||||
cps = []
|
||||
def CANParserHook(dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
|
||||
cp = CANParser(dbc_name, signals, checks, bus, sendcan, "", timeout)
|
||||
print(signals)
|
||||
cps.append(cp)
|
||||
return cp
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
RI = RadarInterface(params, CANParserHook)
|
||||
|
||||
i = 0
|
||||
updated_messages = set()
|
||||
for msg in lr:
|
||||
if msg.which() == 'can':
|
||||
msg_bytes = msg.as_builder().to_bytes()
|
||||
_, vls = cps[0].update_string(msg.logMonoTime, msg_bytes)
|
||||
updated_messages.update(vls)
|
||||
if RI.trigger_msg in updated_messages:
|
||||
ret = RI._update(updated_messages)
|
||||
if i % 10 == 0:
|
||||
print('\033[2J\033[H'+str(ret))
|
||||
updated_messages = set()
|
||||
i += 1
|
||||
|
||||
return True
|
||||
|
||||
|
||||
# TODO: make this generic
|
||||
class TestCarInterface(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.routes = {
|
||||
HONDA.CIVIC: "b0c9d2329ad1606b|2019-05-30--20-23-57",
|
||||
HONDA.ACCORD: "0375fdf7b1ce594d|2019-05-21--20-10-33",
|
||||
TOYOTA.PRIUS: "38bfd238edecbcd7|2019-06-07--10-15-25",
|
||||
TOYOTA.RAV4: "02ec6bea180a4d36|2019-04-17--11-21-35"
|
||||
}
|
||||
|
||||
for route in self.routes.values():
|
||||
route_filename = route + ".bz2"
|
||||
if not os.path.isfile("/tmp/"+route_filename):
|
||||
with open("/tmp/"+route + ".bz2", "w") as f:
|
||||
f.write(requests.get(BASE_URL + route_filename).content)
|
||||
|
||||
def test_parser_civic(self):
|
||||
#self.assertTrue(run_route(self.routes[HONDA.CIVIC], HONDA.CIVIC, HondaCarInterface, HondaCarController))
|
||||
pass
|
||||
|
||||
def test_parser_accord(self):
|
||||
# one honda
|
||||
#self.assertTrue(run_route(self.routes[HONDA.ACCORD], HONDA.ACCORD, HondaCarInterface, HondaCarController))
|
||||
pass
|
||||
|
||||
def test_parser_prius(self):
|
||||
#self.assertTrue(run_route(self.routes[TOYOTA.PRIUS], TOYOTA.PRIUS, ToyotaCarInterface, ToyotaCarController))
|
||||
pass
|
||||
|
||||
def test_parser_rav4(self):
|
||||
# hmm, rav4 is broken
|
||||
#self.assertTrue(run_route(self.routes[TOYOTA.RAV4], TOYOTA.RAV4, ToyotaCarInterface, ToyotaCarController))
|
||||
pass
|
||||
|
||||
def test_radar_civic(self):
|
||||
#self.assertTrue(run_route_radar(self.routes[HONDA.CIVIC], HONDA.CIVIC, HondaRadarInterface, HondaCarInterface))
|
||||
pass
|
||||
|
||||
def test_radar_prius(self):
|
||||
self.assertTrue(run_route_radar(self.routes[TOYOTA.PRIUS], TOYOTA.PRIUS, ToyotaRadarInterface, ToyotaCarInterface))
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
|
|
@ -0,0 +1,435 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import unittest
|
||||
from cereal import car, log
|
||||
from selfdrive.car.honda.values import CAR as HONDA
|
||||
from selfdrive.car.honda.carcontroller import CarController
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.config import Conversions as CV
|
||||
import cereal.messaging as messaging
|
||||
from cereal.services import service_list
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
import zmq
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
|
||||
class TestHondaCarcontroller(unittest.TestCase):
|
||||
def test_honda_lkas_hud(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
car_name = HONDA.CIVIC
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('SET_ME_X41', 'LKAS_HUD', 0),
|
||||
('SET_ME_X48', 'LKAS_HUD', 0),
|
||||
('STEERING_REQUIRED', 'LKAS_HUD', 0),
|
||||
('SOLID_LANES', 'LKAS_HUD', 0),
|
||||
('LEAD_SPEED', 'RADAR_HUD', 0),
|
||||
('LEAD_STATE', 'RADAR_HUD', 0),
|
||||
('LEAD_DISTANCE', 'RADAR_HUD', 0),
|
||||
('ACC_ALERTS', 'RADAR_HUD', 0),
|
||||
]
|
||||
|
||||
VA = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
|
||||
alerts = {
|
||||
VA.none: 0,
|
||||
VA.brakePressed: 10,
|
||||
VA.wrongGear: 6,
|
||||
VA.seatbeltUnbuckled: 5,
|
||||
VA.speedTooHigh: 8,
|
||||
}
|
||||
|
||||
for steer_required in [True, False]:
|
||||
for lanes in [True, False]:
|
||||
for alert in alerts.keys():
|
||||
control = car.CarControl.new_message()
|
||||
hud = car.CarControl.HUDControl.new_message()
|
||||
|
||||
control.enabled = True
|
||||
|
||||
if steer_required:
|
||||
hud.visualAlert = VA.steerRequired
|
||||
else:
|
||||
hud.visualAlert = alert
|
||||
|
||||
hud.lanesVisible = lanes
|
||||
control.hudControl = hud
|
||||
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertEqual(0x41, parser.vl['LKAS_HUD']['SET_ME_X41'])
|
||||
self.assertEqual(0x48, parser.vl['LKAS_HUD']['SET_ME_X48'])
|
||||
self.assertEqual(steer_required, parser.vl['LKAS_HUD']['STEERING_REQUIRED'])
|
||||
self.assertEqual(lanes, parser.vl['LKAS_HUD']['SOLID_LANES'])
|
||||
|
||||
self.assertEqual(0x1fe, parser.vl['RADAR_HUD']['LEAD_SPEED'])
|
||||
self.assertEqual(0x7, parser.vl['RADAR_HUD']['LEAD_STATE'])
|
||||
self.assertEqual(0x1e, parser.vl['RADAR_HUD']['LEAD_DISTANCE'])
|
||||
self.assertEqual(alerts[alert] if not steer_required else 0, parser.vl['RADAR_HUD']['ACC_ALERTS'])
|
||||
|
||||
def test_honda_ui_cruise_speed(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
car_name = HONDA.CIVIC
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
# 780 - 0x30c
|
||||
('CRUISE_SPEED', 'ACC_HUD', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for cruise_speed in np.linspace(0, 50, 20):
|
||||
for visible in [False, True]:
|
||||
control = car.CarControl.new_message()
|
||||
hud = car.CarControl.HUDControl.new_message()
|
||||
hud.setSpeed = float(cruise_speed)
|
||||
hud.speedVisible = visible
|
||||
control.enabled = True
|
||||
control.hudControl = hud
|
||||
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
expected_cruise_speed = round(cruise_speed * CV.MS_TO_KPH)
|
||||
if not visible:
|
||||
expected_cruise_speed = 255
|
||||
|
||||
self.assertAlmostEqual(parser.vl['ACC_HUD']['CRUISE_SPEED'], expected_cruise_speed, msg="Car: %s, speed: %.2f" % (car_name, cruise_speed))
|
||||
|
||||
def test_honda_ui_pcm_accel(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
car_name = HONDA.CIVIC
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
# 780 - 0x30c
|
||||
('PCM_GAS', 'ACC_HUD', 0),
|
||||
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for pcm_accel in np.linspace(0, 1, 25):
|
||||
cc = car.CarControl.CruiseControl.new_message()
|
||||
cc.accelOverride = float(pcm_accel)
|
||||
control = car.CarControl.new_message()
|
||||
control.enabled = True
|
||||
control.cruiseControl = cc
|
||||
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertAlmostEqual(parser.vl['ACC_HUD']['PCM_GAS'], int(0xc6 * pcm_accel), msg="Car: %s, accel: %.2f" % (car_name, pcm_accel))
|
||||
|
||||
def test_honda_ui_pcm_speed(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
car_name = HONDA.CIVIC
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
# 780 - 0x30c
|
||||
('PCM_SPEED', 'ACC_HUD', 99),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for pcm_speed in np.linspace(0, 100, 20):
|
||||
cc = car.CarControl.CruiseControl.new_message()
|
||||
cc.speedOverride = float(pcm_speed * CV.KPH_TO_MS)
|
||||
control = car.CarControl.new_message()
|
||||
control.enabled = True
|
||||
control.cruiseControl = cc
|
||||
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertAlmostEqual(parser.vl['ACC_HUD']['PCM_SPEED'], round(pcm_speed, 2), msg="Car: %s, speed: %.2f" % (car_name, pcm_speed))
|
||||
|
||||
def test_honda_ui_hud_lead(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
for car_name in [HONDA.CIVIC]:
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
# 780 - 0x30c
|
||||
# 3: acc off, 2: solid car (hud_show_car), 1: dashed car (enabled, not hud show car), 0: no car (not enabled)
|
||||
('HUD_LEAD', 'ACC_HUD', 99),
|
||||
('SET_ME_X03', 'ACC_HUD', 99),
|
||||
('SET_ME_X03_2', 'ACC_HUD', 99),
|
||||
('SET_ME_X01', 'ACC_HUD', 99),
|
||||
('ENABLE_MINI_CAR', 'ACC_HUD', 99),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for enabled in [True, False]:
|
||||
for leadVisible in [True, False]:
|
||||
|
||||
control = car.CarControl.new_message()
|
||||
hud = car.CarControl.HUDControl.new_message()
|
||||
hud.leadVisible = leadVisible
|
||||
control.enabled = enabled
|
||||
control.hudControl = hud
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
if not enabled:
|
||||
hud_lead = 0
|
||||
else:
|
||||
hud_lead = 2 if leadVisible else 1
|
||||
self.assertEqual(int(parser.vl['ACC_HUD']['HUD_LEAD']), hud_lead, msg="Car: %s, lead: %s, enabled %s" % (car_name, leadVisible, enabled))
|
||||
self.assertTrue(parser.vl['ACC_HUD']['ENABLE_MINI_CAR'])
|
||||
self.assertEqual(0x3, parser.vl['ACC_HUD']['SET_ME_X03'])
|
||||
self.assertEqual(0x3, parser.vl['ACC_HUD']['SET_ME_X03_2'])
|
||||
self.assertEqual(0x1, parser.vl['ACC_HUD']['SET_ME_X01'])
|
||||
|
||||
|
||||
def test_honda_steering(self):
|
||||
self.longMessage = True
|
||||
limits = {
|
||||
HONDA.CIVIC: 0x1000,
|
||||
HONDA.ODYSSEY: 0x1000,
|
||||
HONDA.PILOT: 0x1000,
|
||||
HONDA.CRV: 0x3e8,
|
||||
HONDA.ACURA_ILX: 0xF00,
|
||||
HONDA.ACURA_RDX: 0x3e8,
|
||||
}
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
for car_name in limits.keys():
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('STEER_TORQUE', 'STEERING_CONTROL', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for steer in np.linspace(-1., 1., 25):
|
||||
control = car.CarControl.new_message()
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
actuators.steer = float(steer)
|
||||
control.enabled = True
|
||||
control.actuators = actuators
|
||||
CI.update(control)
|
||||
|
||||
CI.CS.steer_not_allowed = False
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
torque = parser.vl['STEERING_CONTROL']['STEER_TORQUE']
|
||||
self.assertAlmostEqual(int(limits[car_name] * -actuators.steer), torque, msg="Car: %s, steer %.2f" % (car_name, steer))
|
||||
|
||||
sendcan.close()
|
||||
|
||||
def test_honda_gas(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
car_name = HONDA.ACURA_ILX
|
||||
|
||||
params = CarInterface.get_params(car_name, {0: {0x201: 6}, 1: {}, 2: {}}) # Add interceptor to fingerprint
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('GAS_COMMAND', 'GAS_COMMAND', -1),
|
||||
('GAS_COMMAND2', 'GAS_COMMAND', -1),
|
||||
('ENABLE', 'GAS_COMMAND', -1),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for gas in np.linspace(0., 0.95, 25):
|
||||
control = car.CarControl.new_message()
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
actuators.gas = float(gas)
|
||||
control.enabled = True
|
||||
control.actuators = actuators
|
||||
CI.update(control)
|
||||
|
||||
CI.CS.steer_not_allowed = False
|
||||
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
gas_command = parser.vl['GAS_COMMAND']['GAS_COMMAND'] / 255.0
|
||||
gas_command2 = parser.vl['GAS_COMMAND']['GAS_COMMAND2'] / 255.0
|
||||
enabled = gas > 0.001
|
||||
self.assertEqual(enabled, parser.vl['GAS_COMMAND']['ENABLE'], msg="Car: %s, gas %.2f" % (car_name, gas))
|
||||
if enabled:
|
||||
self.assertAlmostEqual(gas, gas_command, places=2, msg="Car: %s, gas %.2f" % (car_name, gas))
|
||||
self.assertAlmostEqual(gas, gas_command2, places=2, msg="Car: %s, gas %.2f" % (car_name, gas))
|
||||
|
||||
sendcan.close()
|
||||
|
||||
def test_honda_brake(self):
|
||||
self.longMessage = True
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
car_name = HONDA.CIVIC
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('COMPUTER_BRAKE', 'BRAKE_COMMAND', 0),
|
||||
('BRAKE_PUMP_REQUEST', 'BRAKE_COMMAND', 0), # pump_on
|
||||
('CRUISE_OVERRIDE', 'BRAKE_COMMAND', 0), # pcm_override
|
||||
('CRUISE_FAULT_CMD', 'BRAKE_COMMAND', 0), # pcm_fault_cmd
|
||||
('CRUISE_CANCEL_CMD', 'BRAKE_COMMAND', 0), # pcm_cancel_cmd
|
||||
('COMPUTER_BRAKE_REQUEST', 'BRAKE_COMMAND', 0), # brake_rq
|
||||
('SET_ME_0X80', 'BRAKE_COMMAND', 0),
|
||||
('BRAKE_LIGHTS', 'BRAKE_COMMAND', 0), # brakelights
|
||||
('FCW', 'BRAKE_COMMAND', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
VA = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
for override in [True, False]:
|
||||
for cancel in [True, False]:
|
||||
for fcw in [True, False]:
|
||||
steps = 25 if not override and not cancel else 2
|
||||
for brake in np.linspace(0., 0.95, steps):
|
||||
control = car.CarControl.new_message()
|
||||
|
||||
hud = car.CarControl.HUDControl.new_message()
|
||||
if fcw:
|
||||
hud.visualAlert = VA.fcw
|
||||
|
||||
cruise = car.CarControl.CruiseControl.new_message()
|
||||
cruise.cancel = cancel
|
||||
cruise.override = override
|
||||
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
actuators.brake = float(brake)
|
||||
|
||||
control.enabled = True
|
||||
control.actuators = actuators
|
||||
control.hudControl = hud
|
||||
control.cruiseControl = cruise
|
||||
|
||||
CI.update(control)
|
||||
|
||||
CI.CS.steer_not_allowed = False
|
||||
|
||||
for _ in range(20):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
brake_command = parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE']
|
||||
min_expected_brake = int(1024 / 4 * (actuators.brake - 0.02))
|
||||
max_expected_brake = int(1024 / 4 * (actuators.brake + 0.02))
|
||||
braking = actuators.brake > 0
|
||||
|
||||
braking_ok = min_expected_brake <= brake_command <= max_expected_brake
|
||||
if steps == 2:
|
||||
braking_ok = True
|
||||
|
||||
self.assertTrue(braking_ok, msg="Car: %s, brake %.2f" % (car_name, brake))
|
||||
self.assertEqual(0x80, parser.vl['BRAKE_COMMAND']['SET_ME_0X80'])
|
||||
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_PUMP_REQUEST'])
|
||||
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE_REQUEST'])
|
||||
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_LIGHTS'])
|
||||
self.assertFalse(parser.vl['BRAKE_COMMAND']['CRUISE_FAULT_CMD'])
|
||||
self.assertEqual(override, parser.vl['BRAKE_COMMAND']['CRUISE_OVERRIDE'])
|
||||
self.assertEqual(cancel, parser.vl['BRAKE_COMMAND']['CRUISE_CANCEL_CMD'])
|
||||
self.assertEqual(fcw, bool(parser.vl['BRAKE_COMMAND']['FCW']))
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,348 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import unittest
|
||||
from cereal import car, log
|
||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from selfdrive.car.toyota.carcontroller import CarController
|
||||
from selfdrive.car.toyota.interface import CarInterface
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.config import Conversions as CV
|
||||
import cereal.messaging as messaging
|
||||
from cereal.services import service_list
|
||||
from opendbc.can.parser import CANParser
|
||||
import zmq
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
|
||||
class TestToyotaCarcontroller(unittest.TestCase):
|
||||
def test_fcw(self):
|
||||
# TODO: This message has a 0xc1 setme which is not yet checked or in the dbc file
|
||||
self.longMessage = True
|
||||
car_name = TOYOTA.RAV4
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('FCW', 'ACC_HUD', 0),
|
||||
('SET_ME_X20', 'ACC_HUD', 0),
|
||||
('SET_ME_X10', 'ACC_HUD', 0),
|
||||
('SET_ME_X80', 'ACC_HUD', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
VA = car.CarControl.HUDControl.VisualAlert
|
||||
for fcw in [True, False]:
|
||||
control = car.CarControl.new_message()
|
||||
control.enabled = True
|
||||
|
||||
hud = car.CarControl.HUDControl.new_message()
|
||||
if fcw:
|
||||
hud.visualAlert = VA.fcw
|
||||
control.hudControl = hud
|
||||
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(200):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertEqual(fcw, parser.vl['ACC_HUD']['FCW'])
|
||||
self.assertEqual(0x20, parser.vl['ACC_HUD']['SET_ME_X20'])
|
||||
self.assertEqual(0x10, parser.vl['ACC_HUD']['SET_ME_X10'])
|
||||
self.assertEqual(0x80, parser.vl['ACC_HUD']['SET_ME_X80'])
|
||||
|
||||
def test_ui(self):
|
||||
self.longMessage = True
|
||||
car_name = TOYOTA.RAV4
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('BARRIERS', 'LKAS_HUD', -1),
|
||||
('RIGHT_LINE', 'LKAS_HUD', 0),
|
||||
('LEFT_LINE', 'LKAS_HUD', 0),
|
||||
('SET_ME_X01', 'LKAS_HUD', 0),
|
||||
('SET_ME_X01_2', 'LKAS_HUD', 0),
|
||||
('LDA_ALERT', 'LKAS_HUD', -1),
|
||||
('SET_ME_X0C', 'LKAS_HUD', 0),
|
||||
('SET_ME_X2C', 'LKAS_HUD', 0),
|
||||
('SET_ME_X38', 'LKAS_HUD', 0),
|
||||
('SET_ME_X02', 'LKAS_HUD', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
VA = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
for left_lane in [True, False]:
|
||||
for right_lane in [True, False]:
|
||||
for steer in [True, False]:
|
||||
control = car.CarControl.new_message()
|
||||
control.enabled = True
|
||||
|
||||
hud = car.CarControl.HUDControl.new_message()
|
||||
if steer:
|
||||
hud.visualAlert = VA.steerRequired
|
||||
|
||||
hud.leftLaneVisible = left_lane
|
||||
hud.rightLaneVisible = right_lane
|
||||
|
||||
control.hudControl = hud
|
||||
CI.update(control)
|
||||
|
||||
for _ in range(200): # UI is only sent at 1Hz
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertEqual(0x0c, parser.vl['LKAS_HUD']['SET_ME_X0C'])
|
||||
self.assertEqual(0x2c, parser.vl['LKAS_HUD']['SET_ME_X2C'])
|
||||
self.assertEqual(0x38, parser.vl['LKAS_HUD']['SET_ME_X38'])
|
||||
self.assertEqual(0x02, parser.vl['LKAS_HUD']['SET_ME_X02'])
|
||||
self.assertEqual(0, parser.vl['LKAS_HUD']['BARRIERS'])
|
||||
self.assertEqual(1 if right_lane else 2, parser.vl['LKAS_HUD']['RIGHT_LINE'])
|
||||
self.assertEqual(1 if left_lane else 2, parser.vl['LKAS_HUD']['LEFT_LINE'])
|
||||
self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01'])
|
||||
self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01_2'])
|
||||
self.assertEqual(steer, parser.vl['LKAS_HUD']['LDA_ALERT'])
|
||||
|
||||
def test_standstill_and_cancel(self):
|
||||
self.longMessage = True
|
||||
car_name = TOYOTA.RAV4
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('RELEASE_STANDSTILL', 'ACC_CONTROL', 0),
|
||||
('CANCEL_REQ', 'ACC_CONTROL', 0),
|
||||
('SET_ME_X3', 'ACC_CONTROL', 0),
|
||||
('SET_ME_1', 'ACC_CONTROL', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
control = car.CarControl.new_message()
|
||||
control.enabled = True
|
||||
|
||||
CI.update(control)
|
||||
|
||||
CI.CS.pcm_acc_status = 8 # Active
|
||||
CI.CS.standstill = True
|
||||
can_sends = CI.apply(control)
|
||||
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertEqual(0x3, parser.vl['ACC_CONTROL']['SET_ME_X3'])
|
||||
self.assertEqual(1, parser.vl['ACC_CONTROL']['SET_ME_1'])
|
||||
self.assertFalse(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL'])
|
||||
self.assertFalse(parser.vl['ACC_CONTROL']['CANCEL_REQ'])
|
||||
|
||||
CI.CS.pcm_acc_status = 7 # Standstill
|
||||
|
||||
for _ in range(10):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertTrue(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL'])
|
||||
|
||||
cruise = car.CarControl.CruiseControl.new_message()
|
||||
cruise.cancel = True
|
||||
control.cruiseControl = cruise
|
||||
|
||||
for _ in range(10):
|
||||
can_sends = CI.apply(control)
|
||||
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
self.assertTrue(parser.vl['ACC_CONTROL']['CANCEL_REQ'])
|
||||
|
||||
@unittest.skip("IPAS logic changed, fix test")
|
||||
def test_steering_ipas(self):
|
||||
self.longMessage = True
|
||||
car_name = TOYOTA.RAV4
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
params.enableApgs = True
|
||||
CI = CarInterface(params, CarController)
|
||||
CI.CC.angle_control = True
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('SET_ME_X10', 'STEERING_IPAS', 0),
|
||||
('SET_ME_X40', 'STEERING_IPAS', 0),
|
||||
('ANGLE', 'STEERING_IPAS', 0),
|
||||
('STATE', 'STEERING_IPAS', 0),
|
||||
('DIRECTION_CMD', 'STEERING_IPAS', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for enabled in [True, False]:
|
||||
for steer in np.linspace(-510., 510., 25):
|
||||
control = car.CarControl.new_message()
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
actuators.steerAngle = float(steer)
|
||||
control.enabled = enabled
|
||||
control.actuators = actuators
|
||||
CI.update(control)
|
||||
|
||||
CI.CS.steer_not_allowed = False
|
||||
|
||||
for _ in range(1000 if steer < -505 else 25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
|
||||
self.assertEqual(0x10, parser.vl['STEERING_IPAS']['SET_ME_X10'])
|
||||
self.assertEqual(0x40, parser.vl['STEERING_IPAS']['SET_ME_X40'])
|
||||
|
||||
expected_state = 3 if enabled else 1
|
||||
self.assertEqual(expected_state, parser.vl['STEERING_IPAS']['STATE'])
|
||||
|
||||
if steer < 0:
|
||||
direction = 3
|
||||
elif steer > 0:
|
||||
direction = 1
|
||||
else:
|
||||
direction = 2
|
||||
|
||||
if not enabled:
|
||||
direction = 2
|
||||
self.assertEqual(direction, parser.vl['STEERING_IPAS']['DIRECTION_CMD'])
|
||||
|
||||
expected_steer = int(round(steer / 1.5)) * 1.5 if enabled else 0
|
||||
self.assertAlmostEqual(expected_steer, parser.vl['STEERING_IPAS']['ANGLE'])
|
||||
|
||||
sendcan.close()
|
||||
|
||||
def test_steering(self):
|
||||
self.longMessage = True
|
||||
car_name = TOYOTA.RAV4
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
limit = 1500
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('STEER_REQUEST', 'STEERING_LKA', 0),
|
||||
('SET_ME_1', 'STEERING_LKA', 0),
|
||||
('STEER_TORQUE_CMD', 'STEERING_LKA', -1),
|
||||
('LKA_STATE', 'STEERING_LKA', -1),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for steer in np.linspace(-1., 1., 25):
|
||||
control = car.CarControl.new_message()
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
actuators.steer = float(steer)
|
||||
control.enabled = True
|
||||
control.actuators = actuators
|
||||
CI.update(control)
|
||||
|
||||
CI.CS.steer_not_allowed = False
|
||||
CI.CS.steer_torque_motor = limit * steer
|
||||
|
||||
# More control applies for the first one because of rate limits
|
||||
for _ in range(1000 if steer < -0.99 else 25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
|
||||
self.assertEqual(1, parser.vl['STEERING_LKA']['SET_ME_1'])
|
||||
self.assertEqual(True, parser.vl['STEERING_LKA']['STEER_REQUEST'])
|
||||
self.assertAlmostEqual(round(steer * limit), parser.vl['STEERING_LKA']['STEER_TORQUE_CMD'])
|
||||
self.assertEqual(0, parser.vl['STEERING_LKA']['LKA_STATE'])
|
||||
|
||||
sendcan.close()
|
||||
|
||||
def test_accel(self):
|
||||
self.longMessage = True
|
||||
car_name = TOYOTA.RAV4
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
params = CarInterface.get_params(car_name)
|
||||
CI = CarInterface(params, CarController)
|
||||
|
||||
# Get parser
|
||||
parser_signals = [
|
||||
('ACCEL_CMD', 'ACC_CONTROL', 0),
|
||||
]
|
||||
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
|
||||
time.sleep(0.2) # Slow joiner syndrome
|
||||
|
||||
for accel in np.linspace(-3., 1.5, 25):
|
||||
control = car.CarControl.new_message()
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
|
||||
gas = accel / 3. if accel > 0. else 0.
|
||||
brake = -accel / 3. if accel < 0. else 0.
|
||||
|
||||
actuators.gas = float(gas)
|
||||
actuators.brake = float(brake)
|
||||
control.enabled = True
|
||||
control.actuators = actuators
|
||||
CI.update(control)
|
||||
|
||||
# More control applies for the first one because of rate limits
|
||||
for _ in range(25):
|
||||
can_sends = CI.apply(control)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
for _ in range(5):
|
||||
parser.update(int(sec_since_boot() * 1e9), False)
|
||||
time.sleep(0.01)
|
||||
|
||||
min_accel = accel - 0.061
|
||||
max_accel = accel + 0.061
|
||||
sent_accel = parser.vl['ACC_CONTROL']['ACCEL_CMD']
|
||||
accel_ok = min_accel <= sent_accel <= max_accel
|
||||
self.assertTrue(accel_ok, msg="%.2f <= %.2f <= %.2f" % (min_accel, sent_accel, max_accel))
|
||||
sendcan.close()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,247 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg
|
||||
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
|
||||
create_ipas_steer_command, create_accel_command, \
|
||||
create_acc_cancel_command, create_fcw_command
|
||||
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, SteerLimitParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# Accel limits
|
||||
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
||||
ACCEL_MAX = 1.5 # 1.5 m/s2
|
||||
ACCEL_MIN = -3.0 # 3 m/s2
|
||||
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||
|
||||
|
||||
# Steer angle limits (tested at the Crows Landing track and considered ok)
|
||||
ANGLE_MAX_BP = [0., 5.]
|
||||
ANGLE_MAX_V = [510., 300.]
|
||||
ANGLE_DELTA_BP = [0., 5., 15.]
|
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
|
||||
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
|
||||
0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
|
||||
0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
|
||||
0x383]
|
||||
|
||||
|
||||
def accel_hysteresis(accel, accel_steady, enabled):
|
||||
|
||||
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
|
||||
if not enabled:
|
||||
# send 0 when disabled, otherwise acc faults
|
||||
accel_steady = 0.
|
||||
elif accel > accel_steady + ACCEL_HYST_GAP:
|
||||
accel_steady = accel - ACCEL_HYST_GAP
|
||||
elif accel < accel_steady - ACCEL_HYST_GAP:
|
||||
accel_steady = accel + ACCEL_HYST_GAP
|
||||
accel = accel_steady
|
||||
|
||||
return accel, accel_steady
|
||||
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
# initialize to no alert
|
||||
steer = 0
|
||||
fcw = 0
|
||||
|
||||
if hud_alert == VisualAlert.fcw:
|
||||
fcw = 1
|
||||
elif hud_alert == VisualAlert.steerRequired:
|
||||
steer = 1
|
||||
|
||||
return steer, fcw
|
||||
|
||||
|
||||
def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter):
|
||||
|
||||
if enabled and not steer_angle_enabled:
|
||||
#ipas_reset_counter = max(0, ipas_reset_counter - 1)
|
||||
#if ipas_reset_counter == 0:
|
||||
# steer_angle_enabled = True
|
||||
#else:
|
||||
# steer_angle_enabled = False
|
||||
#return steer_angle_enabled, ipas_reset_counter
|
||||
return True, 0
|
||||
|
||||
elif enabled and steer_angle_enabled:
|
||||
if steer_angle_enabled and not ipas_active:
|
||||
ipas_reset_counter += 1
|
||||
else:
|
||||
ipas_reset_counter = 0
|
||||
if ipas_reset_counter > 10: # try every 0.1s
|
||||
steer_angle_enabled = False
|
||||
return steer_angle_enabled, ipas_reset_counter
|
||||
|
||||
else:
|
||||
return False, 0
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg):
|
||||
self.braking = False
|
||||
# redundant safety check with the board
|
||||
self.controls_allowed = True
|
||||
self.last_steer = 0
|
||||
self.last_angle = 0
|
||||
self.accel_steady = 0.
|
||||
self.car_fingerprint = car_fingerprint
|
||||
self.alert_active = False
|
||||
self.last_standstill = False
|
||||
self.standstill_req = False
|
||||
self.angle_control = False
|
||||
|
||||
self.steer_angle_enabled = False
|
||||
self.ipas_reset_counter = 0
|
||||
self.last_fault_frame = -200
|
||||
self.steer_rate_limited = False
|
||||
|
||||
self.fake_ecus = set()
|
||||
if enable_camera: self.fake_ecus.add(ECU.CAM)
|
||||
if enable_dsu: self.fake_ecus.add(ECU.DSU)
|
||||
if enable_apg: self.fake_ecus.add(ECU.APGS)
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
|
||||
left_line, right_line, lead, left_lane_depart, right_lane_depart):
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
# gas and brake
|
||||
|
||||
apply_gas = clip(actuators.gas, 0., 1.)
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
# send only negative accel if interceptor is detected. otherwise, send the regular value
|
||||
# +0.06 offset to reduce ABS pump usage when OP is engaged
|
||||
apply_accel = 0.06 - actuators.brake
|
||||
else:
|
||||
apply_accel = actuators.gas - actuators.brake
|
||||
|
||||
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
|
||||
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
|
||||
|
||||
# steer torque
|
||||
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# only cut torque when steer state is a known fault
|
||||
if CS.steer_state in [9, 25]:
|
||||
self.last_fault_frame = frame
|
||||
|
||||
# Cut steering for 2s after fault
|
||||
if not enabled or (frame - self.last_fault_frame < 200):
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
else:
|
||||
apply_steer_req = 1
|
||||
|
||||
self.steer_angle_enabled, self.ipas_reset_counter = \
|
||||
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
|
||||
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
|
||||
|
||||
# steer angle
|
||||
if self.steer_angle_enabled and CS.ipas_active:
|
||||
apply_angle = actuators.steerAngle
|
||||
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
|
||||
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
|
||||
|
||||
# windup slower
|
||||
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
|
||||
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
|
||||
else:
|
||||
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
|
||||
|
||||
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
|
||||
else:
|
||||
apply_angle = CS.angle_steers
|
||||
|
||||
if not enabled and CS.pcm_acc_status:
|
||||
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
|
||||
pcm_cancel_cmd = 1
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
if CS.standstill and not self.last_standstill:
|
||||
self.standstill_req = True
|
||||
if CS.pcm_acc_status != 8:
|
||||
# pcm entered standstill or it's disabled
|
||||
self.standstill_req = False
|
||||
|
||||
self.last_steer = apply_steer
|
||||
self.last_angle = apply_angle
|
||||
self.last_accel = apply_accel
|
||||
self.last_standstill = CS.standstill
|
||||
|
||||
can_sends = []
|
||||
|
||||
#*** control msgs ***
|
||||
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
|
||||
|
||||
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
|
||||
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
|
||||
# on consecutive messages
|
||||
if ECU.CAM in self.fake_ecus:
|
||||
if self.angle_control:
|
||||
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
|
||||
else:
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
|
||||
|
||||
if self.angle_control:
|
||||
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
|
||||
ECU.APGS in self.fake_ecus))
|
||||
elif ECU.APGS in self.fake_ecus:
|
||||
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
|
||||
|
||||
# we can spam can to cancel the system even if we are using lat only control
|
||||
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
|
||||
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
|
||||
|
||||
# Lexus IS uses a different cancellation message
|
||||
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
can_sends.append(create_acc_cancel_command(self.packer))
|
||||
elif CS.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
|
||||
else:
|
||||
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
|
||||
|
||||
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
|
||||
# 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
|
||||
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
|
||||
|
||||
# ui mesg is at 100Hz but we send asap if:
|
||||
# - there is something to display
|
||||
# - there is something to stop displaying
|
||||
alert_out = process_hud_alert(hud_alert)
|
||||
steer, fcw = alert_out
|
||||
|
||||
if (any(alert_out) and not self.alert_active) or \
|
||||
(not any(alert_out) and self.alert_active):
|
||||
send_ui = True
|
||||
self.alert_active = not self.alert_active
|
||||
else:
|
||||
send_ui = False
|
||||
|
||||
# disengage msg causes a bad fault sound so play a good sound instead
|
||||
if pcm_cancel_cmd:
|
||||
send_ui = True
|
||||
|
||||
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
|
||||
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
|
||||
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw))
|
||||
|
||||
#*** static msgs ***
|
||||
|
||||
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
|
||||
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
|
||||
can_sends.append(make_can_msg(addr, vl, bus))
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,202 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def parse_gear_shifter(gear, vals):
|
||||
|
||||
val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
|
||||
'D': GearShifter.drive, 'B': GearShifter.brake}
|
||||
try:
|
||||
return val_to_capnp[vals[gear]]
|
||||
except KeyError:
|
||||
return GearShifter.unknown
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
|
||||
("GEAR", "GEAR_PACKET", 0),
|
||||
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
|
||||
("GAS_PEDAL", "GAS_PEDAL", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
|
||||
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
|
||||
("TC_DISABLED", "ESP_CONTROL", 1),
|
||||
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
|
||||
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
|
||||
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
|
||||
("CRUISE_STATE", "PCM_CRUISE", 0),
|
||||
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
|
||||
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
|
||||
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
|
||||
("LKA_STATE", "EPS_STATUS", 0),
|
||||
("IPAS_STATE", "EPS_STATUS", 1),
|
||||
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
|
||||
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("BRAKE_MODULE", 40),
|
||||
("GAS_PEDAL", 33),
|
||||
("WHEEL_SPEEDS", 80),
|
||||
("STEER_ANGLE_SENSOR", 80),
|
||||
("PCM_CRUISE", 33),
|
||||
("STEER_TORQUE_SENSOR", 50),
|
||||
("EPS_STATUS", 25),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.LEXUS_IS:
|
||||
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
|
||||
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
|
||||
checks.append(("DSU_CRUISE", 5))
|
||||
else:
|
||||
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
|
||||
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
|
||||
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
|
||||
checks.append(("PCM_CRUISE_2", 33))
|
||||
|
||||
if CP.carFingerprint in NO_DSU_CAR:
|
||||
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
|
||||
|
||||
if CP.carFingerprint == CAR.PRIUS:
|
||||
signals += [("STATE", "AUTOPARK_STATUS", 0)]
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
|
||||
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
|
||||
checks.append(("GAS_SENSOR", 50))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
def get_cam_can_parser(CP):
|
||||
|
||||
signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)]
|
||||
|
||||
# use steering message to check if panda is connected to frc
|
||||
checks = [("STEERING_LKA", 42)]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
self.angle_offset = 0.
|
||||
self.init_angle_offset = False
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
else:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
self.car_gas = self.pedal_gas
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = not v_wheel > 0.001
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR:
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
elif self.CP.carFingerprint in NO_DSU_CAR:
|
||||
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
|
||||
# need to apply an offset as soon as the steering angle measurements are both received
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
|
||||
self.init_angle_offset = True
|
||||
self.angle_offset = self.angle_steers - angle_wheel
|
||||
else:
|
||||
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
|
||||
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
|
||||
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
|
||||
else:
|
||||
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
|
||||
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
|
||||
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
|
||||
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
|
||||
# we could use the override bit from dbc, but it's triggered at too high torque values
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
|
||||
self.user_brake = 0
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
|
||||
self.low_speed_lockout = False
|
||||
else:
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
if self.CP.carFingerprint == CAR.PRIUS:
|
||||
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
|
||||
else:
|
||||
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
|
||||
|
||||
self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
|
|
@ -0,0 +1,435 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.toyota.values import ECU, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
|
||||
self.frame = 0
|
||||
self.gas_pressed_prev = False
|
||||
self.brake_pressed_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
||||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_cam_can_parser(CP)
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "toyota"
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.toyota
|
||||
|
||||
ret.enableCruise = True
|
||||
|
||||
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
|
||||
if candidate == CAR.PRIUS:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 15.74 # unknown end-to-end spec
|
||||
tire_stiffness_factor = 0.6371 # hand-tune
|
||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
|
||||
ret.lateralTuning.init('indi')
|
||||
ret.lateralTuning.indi.innerLoopGain = 4.0
|
||||
ret.lateralTuning.indi.outerLoopGain = 3.0
|
||||
ret.lateralTuning.indi.timeConstant = 1.0
|
||||
ret.lateralTuning.indi.actuatorEffectiveness = 1.0
|
||||
|
||||
# TODO: Determine if this is better than INDI
|
||||
# ret.lateralTuning.init('lqr')
|
||||
# ret.lateralTuning.lqr.scale = 1500.0
|
||||
# ret.lateralTuning.lqr.ki = 0.01
|
||||
|
||||
# ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
|
||||
# ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
|
||||
# ret.lateralTuning.lqr.c = [1., 0.]
|
||||
# ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
|
||||
# ret.lateralTuning.lqr.l = [0.03233671, 0.03185757]
|
||||
# ret.lateralTuning.lqr.dcGain = 0.002237852961363602
|
||||
|
||||
ret.steerActuatorDelay = 0.5
|
||||
|
||||
elif candidate in [CAR.RAV4, CAR.RAV4H]:
|
||||
stop_and_go = True if (candidate in CAR.RAV4H) else False
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.65
|
||||
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
|
||||
tire_stiffness_factor = 0.5533
|
||||
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.init('lqr')
|
||||
|
||||
ret.lateralTuning.lqr.scale = 1500.0
|
||||
ret.lateralTuning.lqr.ki = 0.05
|
||||
|
||||
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
|
||||
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
|
||||
ret.lateralTuning.lqr.c = [1., 0.]
|
||||
ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
|
||||
ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
|
||||
ret.lateralTuning.lqr.dcGain = 0.002237852961363602
|
||||
|
||||
elif candidate == CAR.COROLLA:
|
||||
stop_and_go = False
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 18.27
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
|
||||
elif candidate == CAR.LEXUS_RXH:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.79
|
||||
ret.steerRatio = 16. # 14.8 is spec end-to-end
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
|
||||
elif candidate in [CAR.CHR, CAR.CHRH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.63906
|
||||
ret.steerRatio = 13.6
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate in [CAR.CAMRY, CAR.CAMRYH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.82448
|
||||
ret.steerRatio = 13.7
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 16.0
|
||||
tire_stiffness_factor = 0.8
|
||||
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
|
||||
ret.lateralTuning.pid.kf = 0.00012 # community tuning
|
||||
|
||||
elif candidate == CAR.AVALON:
|
||||
stop_and_go = False
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.82
|
||||
ret.steerRatio = 14.8 #Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
|
||||
tire_stiffness_factor = 0.7983
|
||||
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate == CAR.RAV4_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.68986
|
||||
ret.steerRatio = 14.3
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.63906
|
||||
ret.steerRatio = 13.9
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.8702
|
||||
ret.steerRatio = 16.0 # not optimized
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
elif candidate == CAR.SIENNA:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 3.03
|
||||
ret.steerRatio = 16.0
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
elif candidate == CAR.LEXUS_IS:
|
||||
stop_and_go = False
|
||||
ret.safetyParam = 77
|
||||
ret.wheelbase = 2.79908
|
||||
ret.steerRatio = 13.3
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate == CAR.LEXUS_CTH:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.60
|
||||
ret.steerRatio = 18.6
|
||||
tire_stiffness_factor = 0.517
|
||||
ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007
|
||||
|
||||
ret.steerRateCost = 1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
# no rear steering, at least on the listed cars above
|
||||
ret.steerRatioRear = 0.
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
||||
# steer, gas, brake limitations VS speed
|
||||
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph
|
||||
ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [1.]
|
||||
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
# In TSS2 cars the camera does long control
|
||||
ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) and candidate not in TSS2_CAR
|
||||
ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.APGS)
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera and (ret.enableDsu or candidate in TSS2_CAR)
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
|
||||
cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs)
|
||||
cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
|
||||
# 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.
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
|
||||
|
||||
# removing the DSU disables AEB and it's considered a community maintained feature
|
||||
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 9.]
|
||||
ret.longitudinalTuning.deadzoneV = [0., .15]
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.stoppingControl = False
|
||||
ret.startAccel = 0.0
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
ret.gasMaxBP = [0., 9., 35]
|
||||
ret.gasMaxV = [0.2, 0.5, 0.7]
|
||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalTuning.kiV = [0.18, 0.12]
|
||||
else:
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.5]
|
||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
if self.CP.enableGasInterceptor:
|
||||
# use interceptor values to disengage on pedal press
|
||||
ret.gasPressed = self.CS.pedal_gas > 15
|
||||
else:
|
||||
ret.gasPressed = self.CS.pedal_gas > 0
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringTorqueEps = self.CS.steer_torque_motor
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_active
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
|
||||
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
|
||||
# ignore standstill in hybrid vehicles, since pcm allows to restart without
|
||||
# receiving any special command
|
||||
# also if interceptor is detected
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
|
||||
|
||||
buttonEvents = []
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
ret.genericToggle = self.CS.generic_toggle
|
||||
ret.stockAeb = self.CS.stock_aeb
|
||||
|
||||
# events
|
||||
events = []
|
||||
|
||||
if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera:
|
||||
events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT]))
|
||||
if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.steer_error:
|
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
|
||||
if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
|
||||
if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
|
||||
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
|
||||
if c.actuators.gas > 0.1:
|
||||
# some margin on the actuator to not false trigger cancellation while stopping
|
||||
events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
|
||||
if ret.vEgo < 0.001:
|
||||
# while in standstill, send a user alert
|
||||
events.append(create_event('manualRestart', [ET.WARNING]))
|
||||
|
||||
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
|
||||
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
elif not ret.cruiseState.enabled:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
# disable on pedals rising edge or when brake is pressed and speed isn't zero
|
||||
if (ret.gasPressed and not self.gas_pressed_prev) or \
|
||||
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
ret.events = events
|
||||
|
||||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,111 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
from opendbc.can.parser import CANParser
|
||||
from cereal import car
|
||||
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
def _create_radar_can_parser(car_fingerprint):
|
||||
dbc_f = DBC[car_fingerprint]['radar']
|
||||
|
||||
if car_fingerprint in TSS2_CAR:
|
||||
RADAR_A_MSGS = list(range(0x180, 0x190))
|
||||
RADAR_B_MSGS = list(range(0x190, 0x1a0))
|
||||
else:
|
||||
RADAR_A_MSGS = list(range(0x210, 0x220))
|
||||
RADAR_B_MSGS = list(range(0x220, 0x230))
|
||||
|
||||
msg_a_n = len(RADAR_A_MSGS)
|
||||
msg_b_n = len(RADAR_B_MSGS)
|
||||
|
||||
signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
|
||||
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
|
||||
RADAR_A_MSGS * 5 + RADAR_B_MSGS,
|
||||
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n))
|
||||
|
||||
checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)))
|
||||
|
||||
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.track_id = 0
|
||||
|
||||
self.delay = 0 # Delay of radar
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
|
||||
if CP.carFingerprint in TSS2_CAR:
|
||||
self.RADAR_A_MSGS = list(range(0x180, 0x190))
|
||||
self.RADAR_B_MSGS = list(range(0x190, 0x1a0))
|
||||
else:
|
||||
self.RADAR_A_MSGS = list(range(0x210, 0x220))
|
||||
self.RADAR_B_MSGS = list(range(0x220, 0x230))
|
||||
|
||||
self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
|
||||
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = self.RADAR_B_MSGS[-1]
|
||||
self.updated_messages = set()
|
||||
|
||||
# No radar dbc for cars without DSU which are not TSS 2.0
|
||||
# TODO: make a adas dbc file for dsu-less models
|
||||
self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.no_radar:
|
||||
time.sleep(self.radar_ts)
|
||||
return car.RadarData.new_message()
|
||||
|
||||
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()
|
||||
errors = []
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
ret.errors = errors
|
||||
|
||||
for ii in sorted(updated_messages):
|
||||
if ii in self.RADAR_A_MSGS:
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']:
|
||||
self.valid_cnt[ii] = 0 # reset counter
|
||||
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
|
||||
self.valid_cnt[ii] += 1
|
||||
else:
|
||||
self.valid_cnt[ii] = max(self.valid_cnt[ii] -1, 0)
|
||||
|
||||
score = self.rcp.vl[ii+16]['SCORE']
|
||||
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
|
||||
|
||||
# radar point only valid if it's a valid measurement and score is above 50
|
||||
if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
|
||||
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 = bool(cpt['VALID'])
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
def create_ipas_steer_command(packer, steer, enabled, apgs_enabled):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
if steer < 0:
|
||||
direction = 3
|
||||
elif steer > 0:
|
||||
direction = 1
|
||||
else:
|
||||
direction = 2
|
||||
|
||||
mode = 3 if enabled else 1
|
||||
|
||||
values = {
|
||||
"STATE": mode,
|
||||
"DIRECTION_CMD": direction,
|
||||
"ANGLE": steer,
|
||||
"SET_ME_X10": 0x10,
|
||||
"SET_ME_X40": 0x40
|
||||
}
|
||||
if apgs_enabled:
|
||||
return packer.make_can_msg("STEERING_IPAS", 0, values)
|
||||
else:
|
||||
return packer.make_can_msg("STEERING_IPAS_COMMA", 0, values)
|
||||
|
||||
|
||||
def create_steer_command(packer, steer, steer_req, raw_cnt):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
|
||||
values = {
|
||||
"STEER_REQUEST": steer_req,
|
||||
"STEER_TORQUE_CMD": steer,
|
||||
"COUNTER": raw_cnt,
|
||||
"SET_ME_1": 1,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_LKA", 0, values)
|
||||
|
||||
|
||||
def create_lta_steer_command(packer, steer, steer_req, raw_cnt, angle):
|
||||
"""Creates a CAN message for the Toyota LTA Steer Command."""
|
||||
|
||||
values = {
|
||||
"COUNTER": raw_cnt,
|
||||
"SETME_X3": 3,
|
||||
"PERCENTAGE" : 100,
|
||||
"SETME_X64": 0x64,
|
||||
"ANGLE": angle,
|
||||
"STEER_ANGLE_CMD": steer,
|
||||
"STEER_REQUEST": steer_req,
|
||||
"BIT": 0,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_LTA", 0, values)
|
||||
|
||||
|
||||
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
|
||||
# TODO: find the exact canceling bit that does not create a chime
|
||||
values = {
|
||||
"ACCEL_CMD": accel,
|
||||
"SET_ME_X01": 1,
|
||||
"DISTANCE": 0,
|
||||
"MINI_CAR": lead,
|
||||
"SET_ME_X3": 3,
|
||||
"SET_ME_1": 1,
|
||||
"RELEASE_STANDSTILL": not standstill_req,
|
||||
"CANCEL_REQ": pcm_cancel,
|
||||
}
|
||||
return packer.make_can_msg("ACC_CONTROL", 0, values)
|
||||
|
||||
|
||||
def create_acc_cancel_command(packer):
|
||||
values = {
|
||||
"GAS_RELEASED": 0,
|
||||
"CRUISE_ACTIVE": 0,
|
||||
"STANDSTILL_ON": 0,
|
||||
"ACCEL_NET": 0,
|
||||
"CRUISE_STATE": 0,
|
||||
"CANCEL_REQ": 1,
|
||||
}
|
||||
return packer.make_can_msg("PCM_CRUISE", 0, values)
|
||||
|
||||
|
||||
def create_fcw_command(packer, fcw):
|
||||
values = {
|
||||
"FCW": fcw,
|
||||
"SET_ME_X20": 0x20,
|
||||
"SET_ME_X10": 0x10,
|
||||
"SET_ME_X80": 0x80,
|
||||
}
|
||||
return packer.make_can_msg("ACC_HUD", 0, values)
|
||||
|
||||
|
||||
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart):
|
||||
values = {
|
||||
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
|
||||
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
|
||||
"BARRIERS" : 3 if left_lane_depart or right_lane_depart else 0,
|
||||
"SET_ME_X0C": 0x0c,
|
||||
"SET_ME_X2C": 0x2c,
|
||||
"SET_ME_X38": 0x38,
|
||||
"SET_ME_X02": 0x02,
|
||||
"SET_ME_X01": 1,
|
||||
"SET_ME_X01_2": 1,
|
||||
"REPEATED_BEEPS": 0,
|
||||
"TWO_BEEPS": chime,
|
||||
"LDA_ALERT": steer,
|
||||
}
|
||||
return packer.make_can_msg("LKAS_HUD", 0, values)
|
||||
|
|
@ -0,0 +1,273 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
from cereal import car
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
# Steer torque limits
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
class CAR:
|
||||
PRIUS = "TOYOTA PRIUS 2017"
|
||||
RAV4H = "TOYOTA RAV4 HYBRID 2017"
|
||||
RAV4 = "TOYOTA RAV4 2017"
|
||||
COROLLA = "TOYOTA COROLLA 2017"
|
||||
LEXUS_RXH = "LEXUS RX HYBRID 2017"
|
||||
CHR = "TOYOTA C-HR 2018"
|
||||
CHRH = "TOYOTA C-HR HYBRID 2018"
|
||||
CAMRY = "TOYOTA CAMRY 2018"
|
||||
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
|
||||
HIGHLANDER = "TOYOTA HIGHLANDER 2017"
|
||||
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
|
||||
AVALON = "TOYOTA AVALON 2016"
|
||||
RAV4_TSS2 = "TOYOTA RAV4 2019"
|
||||
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
|
||||
COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019"
|
||||
LEXUS_ES_TSS2 = "LEXUS ES 2019"
|
||||
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
|
||||
SIENNA = "TOYOTA SIENNA XLE 2018"
|
||||
LEXUS_IS = "LEXUS IS300 2018"
|
||||
LEXUS_CTH = "LEXUS CT 200H 2018"
|
||||
|
||||
|
||||
class ECU:
|
||||
CAM = Ecu.fwdCamera # camera
|
||||
DSU = Ecu.dsu # driving support unit
|
||||
APGS = Ecu.apgs # advanced parking guidance system
|
||||
|
||||
|
||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||
STATIC_MSGS = [
|
||||
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
|
||||
(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 2, b'\x00\x00\x00\x46'),
|
||||
(0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
|
||||
(0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
|
||||
(0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
|
||||
(0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
|
||||
(0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
(0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
(0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
|
||||
(0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'),
|
||||
(0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
|
||||
(0x292, ECU.APGS, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'),
|
||||
(0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x396, ECU.APGS, (CAR.PRIUS), 0, 100, b'\xBD\x00\x00\x00\x60\x0F\x02\x00'),
|
||||
(0x43A, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x84\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x43B, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x497, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x4CC, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x0D\x00\x00\x00\x00\x00\x00\x00'),
|
||||
]
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [0x2e4], # steer torque cmd
|
||||
ECU.DSU: [0x343], # accel cmd
|
||||
ECU.APGS: [0x835], # angle cmd
|
||||
}
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.RAV4: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8
|
||||
}],
|
||||
CAR.RAV4H: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# Chinese RAV4
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
|
||||
}],
|
||||
CAR.PRIUS: [{
|
||||
# with ipas
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#2019 LE
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#2020 Prius Prime Limited
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
|
||||
}],
|
||||
#Corolla w/ added Pedal Support (512L and 513L)
|
||||
CAR.COROLLA: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
|
||||
}],
|
||||
CAR.LEXUS_RXH: [{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
|
||||
},
|
||||
# RX450HL
|
||||
# TODO: get proper fingerprint in stock mode
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# RX540H 2019 with color hud
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.CHR: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 913: 8, 918: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8
|
||||
}],
|
||||
CAR.CHRH: [{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 800: 8, 810: 2, 812: 8, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.CAMRY: [
|
||||
#XLE and LE
|
||||
{
|
||||
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#XSE and SE
|
||||
# TODO: get proper fingerprint in stock mode
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.CAMRYH: [
|
||||
#SE, LE and LE with Blindspot Monitor
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#SL
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#XLE
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.HIGHLANDER: [{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2019 Highlander XLE
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2017 Highlander Limited
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.HIGHLANDERH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
{
|
||||
# 2019 Highlander Hybrid Limited Platinum
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.AVALON: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.RAV4_TSS2: [
|
||||
# LE
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# XLE, Limited, and AWD
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.COROLLA_TSS2: [
|
||||
# hatch 2019+ and sedan 2020+
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8
|
||||
}],
|
||||
CAR.COROLLAH_TSS2: [
|
||||
# 2019 Taiwan Altis Hybrid
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 918: 7, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1082: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
},
|
||||
# 2019 Chinese Levin Hybrid
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
}
|
||||
],
|
||||
CAR.LEXUS_ES_TSS2: [{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8
|
||||
}],
|
||||
CAR.LEXUS_ESH_TSS2: [
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.SIENNA: [
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# XLE AWD 2018
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_IS: [
|
||||
# IS300 2018
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# IS300H 2017
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_CTH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1116: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}]
|
||||
}
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.COROLLA_TSS2: {
|
||||
(Ecu.engine, 0x700, None): [b'\x01896630ZG5000\x00\x00\x00\x00'],
|
||||
(Ecu.eps, 0x7a1, None): [b'\x018965B12350\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.esp, 0x7b0, None): [b'\x01F152602280\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301100\x00\x00\x00\x00'],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00'],
|
||||
},
|
||||
CAR.PRIUS: {
|
||||
(Ecu.engine, 0x700, None): [b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00'],
|
||||
(Ecu.eps, 0x7a1, None): [b'8965B47023\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.esp, 0x7b0, None): [b'F152647416\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.dsu, 0x791, None): [b'881514703100\x00\x00\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4702100\x00\x00\x00\x00'],
|
||||
},
|
||||
CAR.RAV4: {
|
||||
(Ecu.engine, 0x7e0, None): [b'\x02342Q2100\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.eps, 0x7a1, None): [b'8965B42083\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.esp, 0x7b0, None): [b'F15260R103\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.dsu, 0x791, None): [b'881514201400\x00\x00\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4202100\x00\x00\x00\x00'],
|
||||
}
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 100
|
||||
|
||||
DBC = {
|
||||
CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
|
||||
CAR.CHRH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'),
|
||||
CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
|
||||
CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
|
||||
}
|
||||
|
||||
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
|
||||
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
|
||||
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
#!/usr/bin/env python3
|
||||
import traceback
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from panda.python.uds import FUNCTIONAL_ADDRS
|
||||
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
VIN_REQUEST = b'\x09\x02'
|
||||
VIN_RESPONSE = b'\x49\x02\x01'
|
||||
VIN_UNKNOWN = "0" * 17
|
||||
|
||||
|
||||
def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [VIN_REQUEST], [VIN_RESPONSE], functional_addr=True, debug=debug)
|
||||
for addr, vin in query.get_data(timeout).items():
|
||||
return addr[0], vin.decode()
|
||||
print(f"vin query retry ({i+1}) ...")
|
||||
except Exception:
|
||||
cloudlog.warning(f"VIN query exception: {traceback.format_exc()}")
|
||||
|
||||
return 0, VIN_UNKNOWN
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
logcan = messaging.sub_sock('can')
|
||||
time.sleep(1)
|
||||
addr, vin = get_vin(logcan, sendcan, 1, debug=False)
|
||||
print(hex(addr), vin)
|
||||
|
|
@ -0,0 +1 @@
|
|||
|
||||
|
|
@ -0,0 +1,191 @@
|
|||
from cereal import car
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.volkswagen import volkswagencan
|
||||
from selfdrive.car.volkswagen.values import DBC, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, canbus, car_fingerprint):
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = car_fingerprint
|
||||
|
||||
# Setup detection helper. Routes commands to an appropriate CAN bus number.
|
||||
self.canbus = canbus
|
||||
self.packer_pt = CANPacker(DBC[car_fingerprint]['pt'])
|
||||
|
||||
self.hcaSameTorqueCount = 0
|
||||
self.hcaEnabledFrameCount = 0
|
||||
self.graButtonStatesToSend = None
|
||||
self.graMsgSentCount = 0
|
||||
self.graMsgStartFramePrev = 0
|
||||
self.graMsgBusCounterPrev = 0
|
||||
|
||||
self.steer_rate_limited = False
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible):
|
||||
""" Controls thread """
|
||||
|
||||
P = CarControllerParams
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
canbus = self.canbus
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
# #
|
||||
# Prepare HCA_01 Heading Control Assist messages with steering torque. #
|
||||
# #
|
||||
#--------------------------------------------------------------------------
|
||||
|
||||
# The factory camera sends at 50Hz while steering and 1Hz when not. When
|
||||
# OP is active, Panda filters HCA_01 from the factory camera and OP emits
|
||||
# HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and
|
||||
# doesn't seem to add value at this time. The rack will accept HCA_01 at
|
||||
# 100Hz if we want to control at finer resolution in the future.
|
||||
if frame % P.HCA_STEP == 0:
|
||||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop
|
||||
# commanding HCA if there's a fault, so the steering rack recovers.
|
||||
if enabled and not (CS.standstill or CS.steeringFault):
|
||||
|
||||
# FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This
|
||||
# is inherently handled by scaling to STEER_MAX. The rack doesn't seem
|
||||
# to care about up/down rate, but we have some evidence it may do its
|
||||
# own rate limiting, and matching OP helps for accurate tuning.
|
||||
new_steer = int(round(actuators.steer * P.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steeringTorque, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
|
||||
# a single frame with HCA disabled is an effective workaround.
|
||||
if apply_steer == 0:
|
||||
# We can usually reset the timer for free, just by disabling HCA
|
||||
# when apply_steer is exactly zero, which happens by chance during
|
||||
# many steer torque direction changes. This could be expanded with
|
||||
# a small dead-zone to capture all zero crossings, but not seeing a
|
||||
# major need at this time.
|
||||
hcaEnabled = False
|
||||
self.hcaEnabledFrameCount = 0
|
||||
else:
|
||||
self.hcaEnabledFrameCount += 1
|
||||
if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s
|
||||
# The Kansas I-70 Crosswind Problem: if we truly do need to steer
|
||||
# in one direction for > 360 seconds, we have to disable HCA for a
|
||||
# frame while actively steering. Testing shows we can just set the
|
||||
# disabled flag, and keep sending non-zero torque, which keeps the
|
||||
# Panda torque rate limiting safety happy. Do so 3x within the 360
|
||||
# second window for safety and redundancy.
|
||||
hcaEnabled = False
|
||||
self.hcaEnabledFrameCount = 0
|
||||
else:
|
||||
hcaEnabled = True
|
||||
# FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds.
|
||||
# This is to detect the sending camera being stuck or frozen. OP
|
||||
# can trip this on a curve if steering is saturated. Avoid this by
|
||||
# reducing torque 0.01 Nm for one frame. Do so 3x within the 6
|
||||
# second period for safety and redundancy.
|
||||
if self.apply_steer_last == apply_steer:
|
||||
self.hcaSameTorqueCount += 1
|
||||
if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s
|
||||
apply_steer -= (1, -1)[apply_steer < 0]
|
||||
self.hcaSameTorqueCount = 0
|
||||
else:
|
||||
self.hcaSameTorqueCount = 0
|
||||
|
||||
else:
|
||||
# Continue sending HCA_01 messages, with the enable flags turned off.
|
||||
hcaEnabled = False
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
idx = (frame / P.HCA_STEP) % 16
|
||||
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, canbus.pt, apply_steer,
|
||||
idx, hcaEnabled))
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
# #
|
||||
# Prepare LDW_02 HUD messages with lane borders, confidence levels, and #
|
||||
# the LKAS status LED. #
|
||||
# #
|
||||
#--------------------------------------------------------------------------
|
||||
|
||||
# The factory camera emits this message at 10Hz. When OP is active, Panda
|
||||
# filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
|
||||
|
||||
if frame % P.LDW_STEP == 0:
|
||||
hcaEnabled = True if enabled and not CS.standstill else False
|
||||
|
||||
if visual_alert == VisualAlert.steerRequired:
|
||||
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
|
||||
else:
|
||||
hud_alert = MQB_LDW_MESSAGES["none"]
|
||||
|
||||
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled,
|
||||
CS.steeringPressed, hud_alert, leftLaneVisible,
|
||||
rightLaneVisible))
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
# #
|
||||
# Prepare GRA_ACC_01 ACC control messages with button press events. #
|
||||
# #
|
||||
#--------------------------------------------------------------------------
|
||||
|
||||
# The car sends this message at 33hz. OP sends it on-demand only for
|
||||
# virtual button presses.
|
||||
#
|
||||
# First create any virtual button press event needed by openpilot, to sync
|
||||
# stock ACC with OP disengagement, or to auto-resume from stop.
|
||||
|
||||
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
|
||||
if not enabled and CS.accEnabled:
|
||||
# Cancel ACC if it's engaged with OP disengaged.
|
||||
self.graButtonStatesToSend = BUTTON_STATES.copy()
|
||||
self.graButtonStatesToSend["cancel"] = True
|
||||
elif enabled and CS.standstill:
|
||||
# Blip the Resume button if we're engaged at standstill.
|
||||
# FIXME: This is a naive implementation, improve with visiond or radar input.
|
||||
# A subset of MQBs like to "creep" too aggressively with this implementation.
|
||||
self.graButtonStatesToSend = BUTTON_STATES.copy()
|
||||
self.graButtonStatesToSend["resumeCruise"] = True
|
||||
|
||||
# OP/Panda can see this message but can't filter it when integrated at the
|
||||
# R242 LKAS camera. It could do so if integrated at the J533 gateway, but
|
||||
# we need a generalized solution that works for either. The message is
|
||||
# counter-protected, so we need to time our transmissions very precisely
|
||||
# to achieve fast and fault-free switching between message flows accepted
|
||||
# at the J428 ACC radar.
|
||||
#
|
||||
# Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP):
|
||||
#
|
||||
# CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6
|
||||
# EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^
|
||||
#
|
||||
# If OP needs to send a button press, it waits to see a GRA_ACC_01 message
|
||||
# counter change, and then immediately follows up with the next increment.
|
||||
# The OP message will be sent within about 1ms of the car's message, which
|
||||
# is about 2ms before the car's next message is expected. OP sends for an
|
||||
# arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new
|
||||
# message from the car.
|
||||
#
|
||||
# Because OP's counter is synced to the car, J428 immediately accepts the
|
||||
# OP messages as valid. Further messages from the car get discarded as
|
||||
# duplicates without a fault. When OP stops sending, the extra time gap
|
||||
# (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428
|
||||
# tolerates the gap just fine and control returns to the car immediately.
|
||||
|
||||
if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
|
||||
self.graMsgBusCounterPrev = CS.graMsgBusCounter
|
||||
if self.graButtonStatesToSend is not None:
|
||||
if self.graMsgSentCount == 0:
|
||||
self.graMsgStartFramePrev = frame
|
||||
idx = (CS.graMsgBusCounter + 1) % 16
|
||||
can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, canbus.pt, self.graButtonStatesToSend, CS, idx))
|
||||
self.graMsgSentCount += 1
|
||||
if self.graMsgSentCount >= P.GRA_VBP_COUNT:
|
||||
self.graButtonStatesToSend = None
|
||||
self.graMsgSentCount = 0
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,233 @@
|
|||
import numpy as np
|
||||
from cereal import car
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams
|
||||
|
||||
GEAR = car.CarState.GearShifter
|
||||
|
||||
def get_mqb_pt_can_parser(CP, canbus):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle
|
||||
("LWI_VZ_Lenkradwinkel", "LWI_01", 0), # Steering angle sign
|
||||
("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate
|
||||
("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign
|
||||
("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left
|
||||
("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right
|
||||
("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left
|
||||
("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right
|
||||
("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate
|
||||
("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign
|
||||
("ZV_FT_offen", "Gateway_72", 0), # Door open, driver
|
||||
("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger
|
||||
("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left
|
||||
("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right
|
||||
("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open
|
||||
("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on
|
||||
("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on
|
||||
("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position
|
||||
("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver
|
||||
("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger
|
||||
("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed
|
||||
("ESP_Status_Bremsdruck", "ESP_05", 0), # Brakes applied
|
||||
("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied
|
||||
("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value
|
||||
("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch
|
||||
("Driver_Strain", "EPS_01", 0), # Absolute driver torque input
|
||||
("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign
|
||||
("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured
|
||||
("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled
|
||||
("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display
|
||||
("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied
|
||||
("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator
|
||||
("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status
|
||||
("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go)
|
||||
("SetSpeed", "ACC_02", 0), # ACC set speed
|
||||
("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off
|
||||
("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel
|
||||
("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set
|
||||
("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel
|
||||
("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel
|
||||
("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume
|
||||
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj
|
||||
("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type
|
||||
("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type
|
||||
("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type
|
||||
("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter
|
||||
]
|
||||
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("LWI_01", 100), # From J500 Steering Assist with integrated sensors
|
||||
("EPS_01", 100), # From J500 Steering Assist with integrated sensors
|
||||
("ESP_19", 100), # From J104 ABS/ESP controller
|
||||
("ESP_05", 50), # From J104 ABS/ESP controller
|
||||
("ESP_21", 50), # From J104 ABS/ESP controller
|
||||
("ACC_06", 50), # From J428 ACC radar control module
|
||||
("Motor_20", 50), # From J623 Engine control module
|
||||
("GRA_ACC_01", 33), # From J??? steering wheel control buttons
|
||||
("ACC_02", 17), # From J428 ACC radar control module
|
||||
("Getriebe_11", 20), # From J743 Auto transmission control module
|
||||
("Gateway_72", 10), # From J533 CAN gateway (aggregated data)
|
||||
("Motor_14", 10), # From J623 Engine control module
|
||||
("Airbag_02", 5), # From J234 Airbag control module
|
||||
("Kombi_01", 2), # From J285 Instrument cluster
|
||||
("Motor_16", 2), # From J623 Engine control module
|
||||
("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.pt)
|
||||
|
||||
# A single signal is monitored from the camera CAN bus, and then ignored,
|
||||
# so the presence of CAN traffic can be verified with cam_cp.valid.
|
||||
|
||||
def get_mqb_cam_can_parser(CP, canbus):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED
|
||||
]
|
||||
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("LDW_02", 10) # From R242 Driver assistance camera
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam)
|
||||
|
||||
def parse_gear_shifter(gear, vals):
|
||||
# Return mapping of gearshift position to selected gear.
|
||||
|
||||
val_to_capnp = {'P': GEAR.park, 'R': GEAR.reverse, 'N': GEAR.neutral,
|
||||
'D': GEAR.drive, 'E': GEAR.eco, 'S': GEAR.sport, 'T': GEAR.manumatic}
|
||||
try:
|
||||
return val_to_capnp[vals[gear]]
|
||||
except KeyError:
|
||||
return "unknown"
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP, canbus):
|
||||
# initialize can parser
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe']
|
||||
|
||||
self.buttonStates = BUTTON_STATES.copy()
|
||||
|
||||
# vEgo Kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
|
||||
def update(self, pt_cp):
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedFR = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedRL = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
|
||||
self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR]))
|
||||
v_ego_x = self.v_ego_kf.update(self.vEgoRaw)
|
||||
self.vEgo = float(v_ego_x[0])
|
||||
self.aEgo = float(v_ego_x[1])
|
||||
self.standstill = self.vEgoRaw < 0.1
|
||||
|
||||
# Update steering angle, rate, yaw rate, and driver input torque. VW send
|
||||
# the sign/direction in a separate signal so they must be recombined.
|
||||
self.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
self.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
self.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
|
||||
self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
self.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
self.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
||||
self.gasPressed = self.gas > 0
|
||||
self.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
self.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
||||
self.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'])
|
||||
self.gearShifter = parse_gear_shifter(can_gear_shifter, self.shifter_values)
|
||||
|
||||
# Update door and trunk/hatch lid open status.
|
||||
self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_BT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HD_offen']])
|
||||
|
||||
# Update seatbelt fastened status.
|
||||
self.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True
|
||||
|
||||
# Update driver preference for metric. VW stores many different unit
|
||||
# preferences, including separate units for for distance vs. speed.
|
||||
# We use the speed preference for OP.
|
||||
self.displayMetricUnits = not pt_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"]
|
||||
|
||||
# Update ACC radar status.
|
||||
accStatus = pt_cp.vl["ACC_06"]['ACC_Status_ACC']
|
||||
if accStatus == 1:
|
||||
# ACC okay but disabled
|
||||
self.accFault = False
|
||||
self.accAvailable = False
|
||||
self.accEnabled = False
|
||||
elif accStatus == 2:
|
||||
# ACC okay and enabled, but not currently engaged
|
||||
self.accFault = False
|
||||
self.accAvailable = True
|
||||
self.accEnabled = False
|
||||
elif accStatus in [3, 4, 5]:
|
||||
# ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5)
|
||||
self.accFault = False
|
||||
self.accAvailable = True
|
||||
self.accEnabled = True
|
||||
else:
|
||||
# ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc.
|
||||
self.accFault = True
|
||||
self.accAvailable = False
|
||||
self.accEnabled = False
|
||||
|
||||
# Update ACC setpoint. When the setpoint is zero or there's an error, the
|
||||
# radar sends a set-speed of ~90.69 m/s / 203mph.
|
||||
self.accSetSpeed = pt_cp.vl["ACC_02"]['SetSpeed']
|
||||
if self.accSetSpeed > 90: self.accSetSpeed = 0
|
||||
|
||||
# Update control button states for turn signals and ACC controls.
|
||||
self.buttonStates["leftBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li'])
|
||||
self.buttonStates["rightBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re'])
|
||||
self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch'])
|
||||
self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter'])
|
||||
self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Abbrechen'])
|
||||
self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen'])
|
||||
self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme'])
|
||||
self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke'])
|
||||
|
||||
# Read ACC hardware button type configuration info that has to pass thru
|
||||
# to the radar. Ends up being different for steering wheel buttons vs
|
||||
# third stalk type controls.
|
||||
self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]['GRA_Hauptschalter']
|
||||
self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]['GRA_Typ_Hauptschalter']
|
||||
self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]['GRA_ButtonTypeInfo']
|
||||
self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Stufe_2']
|
||||
# Pick up the GRA_ACC_01 CAN message counter so we can sync to it for
|
||||
# later cruise-control button spamming.
|
||||
self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]['COUNTER']
|
||||
|
||||
# Check to make sure the electric power steering rack is configured to
|
||||
# accept and respond to HCA_01 messages and has not encountered a fault.
|
||||
self.steeringFault = not pt_cp.vl["EPS_01"]["HCA_Ready"]
|
||||
|
||||
# Additional safety checks performed in CarInterface.
|
||||
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
|
||||
self.stabilityControlDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv']
|
||||
|
||||
|
|
@ -0,0 +1,243 @@
|
|||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES
|
||||
from selfdrive.car.volkswagen.carstate import CarState, get_mqb_pt_can_parser, get_mqb_cam_can_parser
|
||||
from common.params import Params
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
GEAR = car.CarState.GearShifter
|
||||
|
||||
class CANBUS:
|
||||
pt = 0
|
||||
cam = 2
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.CC = None
|
||||
|
||||
self.frame = 0
|
||||
|
||||
self.gasPressedPrev = False
|
||||
self.brakePressedPrev = False
|
||||
self.cruiseStateEnabledPrev = False
|
||||
self.displayMetricUnitsPrev = None
|
||||
self.buttonStatesPrev = BUTTON_STATES.copy()
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP, CANBUS)
|
||||
self.VM = VehicleModel(CP)
|
||||
self.pt_cp = get_mqb_pt_can_parser(CP, CANBUS)
|
||||
self.cam_cp = get_mqb_cam_can_parser(CP, CANBUS)
|
||||
|
||||
# sending if read only is False
|
||||
if CarController is not None:
|
||||
self.CC = CarController(CANBUS, CP.carFingerprint)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
if candidate == CAR.GOLF:
|
||||
# Set common MQB parameters that will apply globally
|
||||
ret.carName = "volkswagen"
|
||||
ret.safetyModel = car.CarParams.SafetyModel.volkswagen
|
||||
ret.enableCruise = True # Stock ACC still controls acceleration and braking
|
||||
ret.openpilotLongitudinalControl = False
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
||||
# Additional common MQB parameters that may be overridden per-vehicle
|
||||
ret.steerRateCost = 0.5
|
||||
ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.steerMaxBP = [0.] # m/s
|
||||
ret.steerMaxV = [1.]
|
||||
|
||||
# As a starting point for speed-adjusted lateral tuning, use the example
|
||||
# map speed breakpoints from a VW Tiguan (SSP 399 page 9). It's unclear
|
||||
# whether the driver assist map breakpoints have any direct bearing on
|
||||
# HCA assist torque, but if they're good breakpoints for the driver,
|
||||
# they're probably good breakpoints for HCA as well. OP won't be driving
|
||||
# 250kph/155mph but it provides interpolation scaling above 100kmh/62mph.
|
||||
ret.lateralTuning.pid.kpBP = [0., 15 * CV.KPH_TO_MS, 50 * CV.KPH_TO_MS]
|
||||
ret.lateralTuning.pid.kiBP = [0., 15 * CV.KPH_TO_MS, 50 * CV.KPH_TO_MS]
|
||||
|
||||
# FIXME: Per-vehicle parameters need to be reintegrated.
|
||||
# For the time being, per-vehicle stuff is being archived since we
|
||||
# can't auto-detect very well yet. Now that tuning is figured out,
|
||||
# averaged params should work reasonably on a range of cars. Owners
|
||||
# can tweak here, as needed, until we have car type auto-detection.
|
||||
|
||||
ret.mass = 1700 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.75
|
||||
ret.centerToFront = ret.wheelbase * 0.45
|
||||
ret.steerRatio = 15.6
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
ret.lateralTuning.pid.kpV = [0.15, 0.25, 0.60]
|
||||
ret.lateralTuning.pid.kiV = [0.05, 0.05, 0.05]
|
||||
tire_stiffness_factor = 0.6
|
||||
|
||||
ret.enableCamera = True # Stock camera detection doesn't apply to VW
|
||||
ret.transmissionType = car.CarParams.TransmissionType.automatic
|
||||
ret.steerRatioRear = 0.
|
||||
|
||||
# No support for OP longitudinal control on Volkswagen at this time.
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.]
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [0.]
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.]
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.]
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# 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
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
canMonoTimes = []
|
||||
events = []
|
||||
buttonEvents = []
|
||||
params = Params()
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# Process the most recent CAN message traffic, and check for validity
|
||||
# The camera CAN has no signals we use at this time, but we process it
|
||||
# anyway so we can test connectivity with can_valid
|
||||
self.pt_cp.update_strings(can_strings)
|
||||
self.cam_cp.update_strings(can_strings)
|
||||
self.CS.update(self.pt_cp)
|
||||
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
|
||||
|
||||
# Wheel and vehicle speed, yaw rate
|
||||
ret.wheelSpeeds.fl = self.CS.wheelSpeedFL
|
||||
ret.wheelSpeeds.fr = self.CS.wheelSpeedFR
|
||||
ret.wheelSpeeds.rl = self.CS.wheelSpeedRL
|
||||
ret.wheelSpeeds.rr = self.CS.wheelSpeedRR
|
||||
ret.vEgoRaw = self.CS.vEgoRaw
|
||||
ret.vEgo = self.CS.vEgo
|
||||
ret.aEgo = self.CS.aEgo
|
||||
ret.standstill = self.CS.standstill
|
||||
|
||||
# Steering wheel position, movement, yaw rate, and driver input
|
||||
ret.steeringAngle = self.CS.steeringAngle
|
||||
ret.steeringRate = self.CS.steeringRate
|
||||
ret.steeringTorque = self.CS.steeringTorque
|
||||
ret.steeringPressed = self.CS.steeringPressed
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
ret.yawRate = self.CS.yawRate
|
||||
|
||||
# Gas, brakes and shifting
|
||||
ret.gas = self.CS.gas
|
||||
ret.gasPressed = self.CS.gasPressed
|
||||
ret.brake = self.CS.brake
|
||||
ret.brakePressed = self.CS.brakePressed
|
||||
ret.brakeLights = self.CS.brakeLights
|
||||
ret.gearShifter = self.CS.gearShifter
|
||||
|
||||
# Doors open, seatbelt unfastened
|
||||
ret.doorOpen = self.CS.doorOpen
|
||||
ret.seatbeltUnlatched = self.CS.seatbeltUnlatched
|
||||
|
||||
# Update the EON metric configuration to match the car at first startup,
|
||||
# or if there's been a change.
|
||||
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
|
||||
params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0")
|
||||
|
||||
# Blinker switch updates
|
||||
ret.leftBlinker = self.CS.buttonStates["leftBlinker"]
|
||||
ret.rightBlinker = self.CS.buttonStates["rightBlinker"]
|
||||
|
||||
# ACC cruise state
|
||||
ret.cruiseState.available = self.CS.accAvailable
|
||||
ret.cruiseState.enabled = self.CS.accEnabled
|
||||
ret.cruiseState.speed = self.CS.accSetSpeed
|
||||
|
||||
# Check for and process state-change events (button press or release) from
|
||||
# the turn stalk switch or ACC steering wheel/control stalk buttons.
|
||||
for button in self.CS.buttonStates:
|
||||
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = button
|
||||
be.pressed = self.CS.buttonStates[button]
|
||||
buttonEvents.append(be)
|
||||
|
||||
# Vehicle operation safety checks and events
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.gearShifter == GEAR.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if not ret.gearShifter in [GEAR.drive, GEAR.eco, GEAR.sport]:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.stabilityControlDisabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.parkingBrakeSet:
|
||||
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
|
||||
# Vehicle health safety checks and events
|
||||
if self.CS.accFault:
|
||||
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.steeringFault:
|
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
|
||||
|
||||
# Per the Comma safety model, disable on pedals rising edge or when brake
|
||||
# is pressed and speed isn't zero.
|
||||
if (ret.gasPressed and not self.gasPressedPrev) or \
|
||||
(ret.brakePressed and (not self.brakePressedPrev or not ret.standstill)):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
# Engagement and longitudinal control using stock ACC. Make sure OP is
|
||||
# disengaged if stock ACC is disengaged.
|
||||
if not ret.cruiseState.enabled:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
# Attempt OP engagement only on rising edge of stock ACC engagement.
|
||||
elif not self.cruiseStateEnabledPrev:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
|
||||
ret.events = events
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.canMonoTimes = canMonoTimes
|
||||
|
||||
# update previous car states
|
||||
self.gasPressedPrev = ret.gasPressed
|
||||
self.brakePressedPrev = ret.brakePressed
|
||||
self.cruiseStateEnabledPrev = ret.cruiseState.enabled
|
||||
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
|
||||
self.buttonStatesPrev = self.CS.buttonStates.copy()
|
||||
|
||||
# cast to reader so it can't be modified
|
||||
return ret.as_reader()
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.hudControl.visualAlert,
|
||||
c.hudControl.audibleAlert,
|
||||
c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
class CarControllerParams:
|
||||
HCA_STEP = 2 # HCA_01 message frequency 50Hz
|
||||
LDW_STEP = 10 # LDW_02 message frequency 10Hz
|
||||
GRA_ACC_STEP = 3 # GRA_ACC_01 message frequency 33Hz
|
||||
|
||||
GRA_VBP_STEP = 100 # Send ACC virtual button presses once a second
|
||||
GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16)
|
||||
|
||||
# Observed documented MQB limits: 3.00 Nm max, rate of change 5.00 Nm/sec.
|
||||
# Limiting both torque and rate-of-change based on real-world testing and
|
||||
# Comma's safety requirements for minimum time to lane departure.
|
||||
STEER_MAX = 250 # Max heading control assist torque 2.50 Nm
|
||||
STEER_DELTA_UP = 4 # Max HCA reached in 1.25s (STEER_MAX / (50Hz * 1.25))
|
||||
STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
|
||||
STEER_DRIVER_ALLOWANCE = 80
|
||||
STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily
|
||||
STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
|
||||
BUTTON_STATES = {
|
||||
"leftBlinker": False,
|
||||
"rightBlinker": False,
|
||||
"accelCruise": False,
|
||||
"decelCruise": False,
|
||||
"cancel": False,
|
||||
"setCruise": False,
|
||||
"resumeCruise": False,
|
||||
"gapAdjustCruise": False
|
||||
}
|
||||
|
||||
MQB_LDW_MESSAGES = {
|
||||
"none": 0, # Nothing to display
|
||||
"laneAssistUnavailChime": 1, # "Lane Assist currently not available." with chime
|
||||
"laneAssistUnavailNoSensorChime": 3, # "Lane Assist not available. No sensor view." with chime
|
||||
"laneAssistTakeOverUrgent": 4, # "Lane Assist: Please Take Over Steering" with urgent beep
|
||||
"emergencyAssistUrgent": 6, # "Emergency Assist: Please Take Over Steering" with urgent beep
|
||||
"laneAssistTakeOverChime": 7, # "Lane Assist: Please Take Over Steering" with chime
|
||||
"laneAssistTakeOverSilent": 8, # "Lane Assist: Please Take Over Steering" silent
|
||||
"emergencyAssistChangingLanes": 9, # "Emergency Assist: Changing lanes..." with urgent beep
|
||||
"laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward
|
||||
}
|
||||
|
||||
class CAR:
|
||||
GOLF = "Volkswagen Golf"
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.GOLF: [
|
||||
# 76b83eb0245de90e|2019-10-21--17-40-42 - jyoung8607 car
|
||||
{64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
|
||||
}],
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.GOLF: dbc_dict('vw_mqb_2010', None),
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
# CAN controls for MQB platform Volkswagen, Audi, Skoda and SEAT.
|
||||
# PQ35/PQ46/NMS, and any future MLB, to come later.
|
||||
|
||||
def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled):
|
||||
values = {
|
||||
"SET_ME_0X3": 0x3,
|
||||
"Assist_Torque": abs(apply_steer),
|
||||
"Assist_Requested": lkas_enabled,
|
||||
"Assist_VZ": 1 if apply_steer < 0 else 0,
|
||||
"HCA_Available": 1,
|
||||
"HCA_Standby": not lkas_enabled,
|
||||
"HCA_Active": lkas_enabled,
|
||||
"SET_ME_0XFE": 0xFE,
|
||||
"SET_ME_0X07": 0x07,
|
||||
}
|
||||
return packer.make_can_msg("HCA_01", bus, values, idx)
|
||||
|
||||
def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, leftLaneVisible, rightLaneVisible):
|
||||
|
||||
if hca_enabled:
|
||||
leftlanehud = 3 if leftLaneVisible else 1
|
||||
rightlanehud = 3 if rightLaneVisible else 1
|
||||
else:
|
||||
leftlanehud = 2 if leftLaneVisible else 1
|
||||
rightlanehud = 2 if rightLaneVisible else 1
|
||||
|
||||
values = {
|
||||
"LDW_Unknown": 2, # FIXME: possible speed or attention relationship
|
||||
"Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0,
|
||||
"Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0,
|
||||
"Left_Lane_Status": leftlanehud,
|
||||
"Right_Lane_Status": rightlanehud,
|
||||
"Alert_Message": hud_alert,
|
||||
}
|
||||
return packer.make_can_msg("LDW_02", bus, values)
|
||||
|
||||
def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
|
||||
values = {
|
||||
"GRA_Hauptschalter": CS.graHauptschalter,
|
||||
"GRA_Abbrechen": buttonStatesToSend["cancel"],
|
||||
"GRA_Tip_Setzen": buttonStatesToSend["setCruise"],
|
||||
"GRA_Tip_Hoch": buttonStatesToSend["accelCruise"],
|
||||
"GRA_Tip_Runter": buttonStatesToSend["decelCruise"],
|
||||
"GRA_Tip_Wiederaufnahme": buttonStatesToSend["resumeCruise"],
|
||||
"GRA_Verstellung_Zeitluecke": 3 if buttonStatesToSend["gapAdjustCruise"] else 0,
|
||||
"GRA_Typ_Hauptschalter": CS.graTypHauptschalter,
|
||||
"GRA_Codierung": 2,
|
||||
"GRA_Tip_Stufe_2": CS.graTipStufe2,
|
||||
"GRA_ButtonTypeInfo": CS.graButtonTypeInfo
|
||||
}
|
||||
|
||||
return packer.make_can_msg("GRA_ACC_01", bus, values, idx)
|
||||
Loading…
Reference in New Issue