diff --git a/selfdrive/car/README b/selfdrive/car/README new file mode 100644 index 0000000000..bacc422acf --- /dev/null +++ b/selfdrive/car/README @@ -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 + diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py new file mode 100644 index 0000000000..6ad29d4b3a --- /dev/null +++ b/selfdrive/car/__init__.py @@ -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] + diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py new file mode 100644 index 0000000000..4467788611 --- /dev/null +++ b/selfdrive/car/car_helpers.py @@ -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// +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 diff --git a/selfdrive/car/chrysler/__init__.py b/selfdrive/car/chrysler/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py new file mode 100644 index 0000000000..4b8919d2df --- /dev/null +++ b/selfdrive/car/chrysler/carcontroller.py @@ -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 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py new file mode 100644 index 0000000000..54b4a5f4c6 --- /dev/null +++ b/selfdrive/car/chrysler/carstate.py @@ -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'] diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py new file mode 100644 index 0000000000..3c5a913fca --- /dev/null +++ b/selfdrive/car/chrysler/chryslercan.py @@ -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) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py new file mode 100755 index 0000000000..f96d95ae98 --- /dev/null +++ b/selfdrive/car/chrysler/interface.py @@ -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 diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py new file mode 100755 index 0000000000..0713bf1d16 --- /dev/null +++ b/selfdrive/car/chrysler/radar_interface.py @@ -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 diff --git a/selfdrive/car/chrysler/test_chryslercan.py b/selfdrive/car/chrysler/test_chryslercan.py new file mode 100644 index 0000000000..637cdc9ffa --- /dev/null +++ b/selfdrive/car/chrysler/test_chryslercan.py @@ -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() diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py new file mode 100644 index 0000000000..ebd7cad875 --- /dev/null +++ b/selfdrive/car/chrysler/values.py @@ -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 +} diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py new file mode 100644 index 0000000000..4ca9fa0931 --- /dev/null +++ b/selfdrive/car/fingerprints.py @@ -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()) diff --git a/selfdrive/car/ford/__init__.py b/selfdrive/car/ford/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py new file mode 100644 index 0000000000..db574f38a9 --- /dev/null +++ b/selfdrive/car/ford/carcontroller.py @@ -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 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py new file mode 100644 index 0000000000..7484e064ca --- /dev/null +++ b/selfdrive/car/ford/carstate.py @@ -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"]) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py new file mode 100644 index 0000000000..dd0c15415e --- /dev/null +++ b/selfdrive/car/ford/fordcan.py @@ -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) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py new file mode 100755 index 0000000000..70df1cf954 --- /dev/null +++ b/selfdrive/car/ford/interface.py @@ -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 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py new file mode 100755 index 0000000000..048f556fd3 --- /dev/null +++ b/selfdrive/car/ford/radar_interface.py @@ -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 diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py new file mode 100644 index 0000000000..a3fb576a3b --- /dev/null +++ b/selfdrive/car/ford/values.py @@ -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'), +} diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py new file mode 100755 index 0000000000..1175c59784 --- /dev/null +++ b/selfdrive/car/fw_versions.py @@ -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)) diff --git a/selfdrive/car/gm/__init__.py b/selfdrive/car/gm/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py new file mode 100644 index 0000000000..53eccf1f38 --- /dev/null +++ b/selfdrive/car/gm/carcontroller.py @@ -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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py new file mode 100644 index 0000000000..8a083cb535 --- /dev/null +++ b/selfdrive/car/gm/carstate.py @@ -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 diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py new file mode 100644 index 0000000000..68b129e743 --- /dev/null +++ b/selfdrive/car/gm/gmcan.py @@ -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) + +''' diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py new file mode 100755 index 0000000000..b58a1a9979 --- /dev/null +++ b/selfdrive/car/gm/interface.py @@ -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 diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py new file mode 100755 index 0000000000..d36db9422d --- /dev/null +++ b/selfdrive/car/gm/radar_interface.py @@ -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 diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py new file mode 100644 index 0000000000..126ae9e40b --- /dev/null +++ b/selfdrive/car/gm/values.py @@ -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'), +} diff --git a/selfdrive/car/honda/__init__.py b/selfdrive/car/honda/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py new file mode 100644 index 0000000000..9b0e0706ce --- /dev/null +++ b/selfdrive/car/honda/carcontroller.py @@ -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 diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py new file mode 100644 index 0000000000..39b91c8664 --- /dev/null +++ b/selfdrive/car/honda/carstate.py @@ -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"] diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py new file mode 100644 index 0000000000..38d099fdce --- /dev/null +++ b/selfdrive/car/honda/hondacan.py @@ -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) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py new file mode 100755 index 0000000000..b1abeed3dc --- /dev/null +++ b/selfdrive/car/honda/interface.py @@ -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 diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py new file mode 100755 index 0000000000..3c07cd74cd --- /dev/null +++ b/selfdrive/car/honda/radar_interface.py @@ -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 diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py new file mode 100644 index 0000000000..4301128bc6 --- /dev/null +++ b/selfdrive/car/honda/values.py @@ -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] diff --git a/selfdrive/car/hyundai/__init__.py b/selfdrive/car/hyundai/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py new file mode 100644 index 0000000000..0a6144bab6 --- /dev/null +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py new file mode 100644 index 0000000000..7ebb8d70d6 --- /dev/null +++ b/selfdrive/car/hyundai/carstate.py @@ -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"] diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py new file mode 100644 index 0000000000..910e05ad7c --- /dev/null +++ b/selfdrive/car/hyundai/hyundaican.py @@ -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) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py new file mode 100644 index 0000000000..50c31af3d2 --- /dev/null +++ b/selfdrive/car/hyundai/interface.py @@ -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 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py new file mode 100644 index 0000000000..b2f7651136 --- /dev/null +++ b/selfdrive/car/hyundai/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py new file mode 100644 index 0000000000..f2a800a9da --- /dev/null +++ b/selfdrive/car/hyundai/values.py @@ -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 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py new file mode 100644 index 0000000000..ca1972a167 --- /dev/null +++ b/selfdrive/car/interfaces.py @@ -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 diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py new file mode 100644 index 0000000000..cda36d50fb --- /dev/null +++ b/selfdrive/car/isotp_parallel_query.py @@ -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 diff --git a/selfdrive/car/mock/__init__.py b/selfdrive/car/mock/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py new file mode 100755 index 0000000000..09c5b5c612 --- /dev/null +++ b/selfdrive/car/mock/interface.py @@ -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 [] diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py new file mode 100755 index 0000000000..b2f7651136 --- /dev/null +++ b/selfdrive/car/mock/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/mock/values.py b/selfdrive/car/mock/values.py new file mode 100644 index 0000000000..0dd91565bd --- /dev/null +++ b/selfdrive/car/mock/values.py @@ -0,0 +1,2 @@ +class CAR: + MOCK = 'mock' diff --git a/selfdrive/car/subaru/__init__.py b/selfdrive/car/subaru/__init__.py new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/selfdrive/car/subaru/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py new file mode 100644 index 0000000000..69f573fef8 --- /dev/null +++ b/selfdrive/car/subaru/carcontroller.py @@ -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 diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py new file mode 100644 index 0000000000..57bd1bee79 --- /dev/null +++ b/selfdrive/car/subaru/carstate.py @@ -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"]) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py new file mode 100644 index 0000000000..d4388727bb --- /dev/null +++ b/selfdrive/car/subaru/interface.py @@ -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 diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py new file mode 100644 index 0000000000..b2f7651136 --- /dev/null +++ b/selfdrive/car/subaru/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py new file mode 100644 index 0000000000..96abfaafb3 --- /dev/null +++ b/selfdrive/car/subaru/subarucan.py @@ -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) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py new file mode 100644 index 0000000000..7292a135c0 --- /dev/null +++ b/selfdrive/car/subaru/values.py @@ -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), +} diff --git a/selfdrive/car/tests/.gitignore b/selfdrive/car/tests/.gitignore new file mode 100644 index 0000000000..192fb0945e --- /dev/null +++ b/selfdrive/car/tests/.gitignore @@ -0,0 +1 @@ +*.bz2 diff --git a/selfdrive/car/tests/test_carstates.py b/selfdrive/car/tests/test_carstates.py new file mode 100755 index 0000000000..c0ef060ac1 --- /dev/null +++ b/selfdrive/car/tests/test_carstates.py @@ -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() + diff --git a/selfdrive/car/tests/test_honda_carcontroller.py b/selfdrive/car/tests/test_honda_carcontroller.py new file mode 100755 index 0000000000..fb912ab5b4 --- /dev/null +++ b/selfdrive/car/tests/test_honda_carcontroller.py @@ -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() diff --git a/selfdrive/car/tests/test_toyota_carcontroller.py b/selfdrive/car/tests/test_toyota_carcontroller.py new file mode 100755 index 0000000000..432684de00 --- /dev/null +++ b/selfdrive/car/tests/test_toyota_carcontroller.py @@ -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() diff --git a/selfdrive/car/toyota/__init__.py b/selfdrive/car/toyota/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py new file mode 100644 index 0000000000..0a8f7979d4 --- /dev/null +++ b/selfdrive/car/toyota/carcontroller.py @@ -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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py new file mode 100644 index 0000000000..0fbf7a7970 --- /dev/null +++ b/selfdrive/car/toyota/carstate.py @@ -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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py new file mode 100755 index 0000000000..83f499f450 --- /dev/null +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py new file mode 100755 index 0000000000..80977b706c --- /dev/null +++ b/selfdrive/car/toyota/radar_interface.py @@ -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 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py new file mode 100644 index 0000000000..48d6229c95 --- /dev/null +++ b/selfdrive/car/toyota/toyotacan.py @@ -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) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py new file mode 100644 index 0000000000..f8edbf48bc --- /dev/null +++ b/selfdrive/car/toyota/values.py @@ -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 diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py new file mode 100755 index 0000000000..648f416511 --- /dev/null +++ b/selfdrive/car/vin.py @@ -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) diff --git a/selfdrive/car/volkswagen/__init__.py b/selfdrive/car/volkswagen/__init__.py new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/selfdrive/car/volkswagen/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py new file mode 100644 index 0000000000..610e1d5a09 --- /dev/null +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -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 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py new file mode 100644 index 0000000000..58a19274d3 --- /dev/null +++ b/selfdrive/car/volkswagen/carstate.py @@ -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'] + diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py new file mode 100644 index 0000000000..0e8b5206d3 --- /dev/null +++ b/selfdrive/car/volkswagen/interface.py @@ -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 diff --git a/selfdrive/car/volkswagen/radar_interface.py b/selfdrive/car/volkswagen/radar_interface.py new file mode 100644 index 0000000000..b2f7651136 --- /dev/null +++ b/selfdrive/car/volkswagen/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py new file mode 100644 index 0000000000..045e03ae33 --- /dev/null +++ b/selfdrive/car/volkswagen/values.py @@ -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), +} diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py new file mode 100644 index 0000000000..34f19003d8 --- /dev/null +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -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)