version: dragonpilot v0.8.16 beta for EON/C2 date: 2022-08-18T23:02:08 dp-dev(priv2) master commit: 96dd81dde40b92d0686ec5761cfaaab24300b027pull/166/head
Dragonpilot Team
3 years ago
committed by
Comma Device
parent
4d7a40310f
commit
b437663252
105 changed files with 9548 additions and 3644 deletions
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1 +1 @@ |
||||
#define COMMA_VERSION "2022.08.11" |
||||
#define COMMA_VERSION "2022.08.17" |
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,7 @@ |
||||
def create_control(packer, torque_l, torque_r): |
||||
values = { |
||||
"TORQUE_L": torque_l, |
||||
"TORQUE_R": torque_r, |
||||
} |
||||
|
||||
return packer.make_can_msg("TORQUE_CMD", 0, values) |
||||
@ -0,0 +1,90 @@ |
||||
import numpy as np |
||||
|
||||
from common.realtime import DT_CTRL |
||||
from opendbc.can.packer import CANPacker |
||||
from selfdrive.car.body import bodycan |
||||
from selfdrive.car.body.values import SPEED_FROM_RPM |
||||
from selfdrive.controls.lib.pid import PIDController |
||||
|
||||
|
||||
MAX_TORQUE = 500 |
||||
MAX_TORQUE_RATE = 50 |
||||
MAX_ANGLE_ERROR = np.radians(7) |
||||
MAX_POS_INTEGRATOR = 0.2 # meters |
||||
MAX_TURN_INTEGRATOR = 0.1 # meters |
||||
|
||||
|
||||
class CarController: |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.frame = 0 |
||||
self.packer = CANPacker(dbc_name) |
||||
|
||||
# Speed, balance and turn PIDs |
||||
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL) |
||||
self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL) |
||||
self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) |
||||
|
||||
self.torque_r_filtered = 0. |
||||
self.torque_l_filtered = 0. |
||||
|
||||
@staticmethod |
||||
def deadband_filter(torque, deadband): |
||||
if torque > 0: |
||||
torque += deadband |
||||
else: |
||||
torque -= deadband |
||||
return torque |
||||
|
||||
def update(self, CC, CS): |
||||
|
||||
torque_l = 0 |
||||
torque_r = 0 |
||||
|
||||
llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0 |
||||
if CC.enabled and llk_valid: |
||||
# Read these from the joystick |
||||
# TODO: this isn't acceleration, okay? |
||||
speed_desired = CC.actuators.accel / 5. |
||||
speed_diff_desired = -CC.actuators.steer |
||||
|
||||
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. |
||||
speed_error = speed_desired - speed_measured |
||||
|
||||
freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or |
||||
(speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR)) |
||||
angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator) |
||||
|
||||
# Clip angle error, this is enough to get up from stands |
||||
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR) |
||||
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.) |
||||
torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate) |
||||
|
||||
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) |
||||
turn_error = speed_diff_measured - speed_diff_desired |
||||
freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or |
||||
(turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR)) |
||||
torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator) |
||||
|
||||
# Combine 2 PIDs outputs |
||||
torque_r = torque + torque_diff |
||||
torque_l = torque - torque_diff |
||||
|
||||
# Torque rate limits |
||||
self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), |
||||
self.torque_r_filtered - MAX_TORQUE_RATE, |
||||
self.torque_r_filtered + MAX_TORQUE_RATE) |
||||
self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), |
||||
self.torque_l_filtered - MAX_TORQUE_RATE, |
||||
self.torque_l_filtered + MAX_TORQUE_RATE) |
||||
torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) |
||||
torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) |
||||
|
||||
can_sends = [] |
||||
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) |
||||
|
||||
new_actuators = CC.actuators.copy() |
||||
new_actuators.accel = torque_l |
||||
new_actuators.steer = torque_r |
||||
|
||||
self.frame += 1 |
||||
return new_actuators, can_sends |
||||
@ -0,0 +1,60 @@ |
||||
from cereal import car |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from selfdrive.car.body.values import DBC |
||||
|
||||
STARTUP_TICKS = 100 |
||||
|
||||
class CarState(CarStateBase): |
||||
def update(self, cp): |
||||
ret = car.CarState.new_message() |
||||
|
||||
ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L'] |
||||
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R'] |
||||
|
||||
ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor |
||||
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.standstill = False |
||||
|
||||
ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'], |
||||
cp.vl['VAR_VALUES']['FAULT']]) |
||||
|
||||
ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1 |
||||
ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100 |
||||
|
||||
# irrelevant for non-car |
||||
ret.gearShifter = car.CarState.GearShifter.drive |
||||
ret.cruiseState.enabled = True |
||||
ret.cruiseState.available = True |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("SPEED_L", "MOTORS_DATA"), |
||||
("SPEED_R", "MOTORS_DATA"), |
||||
("ELEC_ANGLE_L", "MOTORS_DATA"), |
||||
("ELEC_ANGLE_R", "MOTORS_DATA"), |
||||
("COUNTER", "MOTORS_DATA"), |
||||
("CHECKSUM", "MOTORS_DATA"), |
||||
("IGNITION", "VAR_VALUES"), |
||||
("ENABLE_MOTORS", "VAR_VALUES"), |
||||
("FAULT", "VAR_VALUES"), |
||||
("MOTOR_ERR_L", "VAR_VALUES"), |
||||
("MOTOR_ERR_R", "VAR_VALUES"), |
||||
("MCU_TEMP", "BODY_DATA"), |
||||
("BATT_VOLTAGE", "BODY_DATA"), |
||||
("BATT_PERCENTAGE", "BODY_DATA"), |
||||
("CHARGER_CONNECTED", "BODY_DATA"), |
||||
] |
||||
|
||||
checks = [ |
||||
("MOTORS_DATA", 100), |
||||
("VAR_VALUES", 10), |
||||
("BODY_DATA", 1), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) |
||||
@ -0,0 +1,54 @@ |
||||
#!/usr/bin/env python3 |
||||
import math |
||||
from cereal import car |
||||
from common.realtime import DT_CTRL |
||||
from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
from selfdrive.car.body.values import SPEED_FROM_RPM |
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=None, car_fw=None, disable_radar=False): |
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
|
||||
ret.notCar = True |
||||
ret.carName = "body" |
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] |
||||
|
||||
ret.minSteerSpeed = -math.inf |
||||
ret.maxLateralAccel = math.inf # TODO: set to a reasonable value |
||||
ret.steerRatio = 0.5 |
||||
ret.steerLimitTimer = 1.0 |
||||
ret.steerActuatorDelay = 0. |
||||
|
||||
ret.mass = 9 |
||||
ret.wheelbase = 0.406 |
||||
ret.wheelSpeedFactor = SPEED_FROM_RPM |
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
|
||||
ret.radarOffCan = True |
||||
ret.openpilotLongitudinalControl = True |
||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) |
||||
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) |
||||
|
||||
return ret |
||||
|
||||
def _update(self, c): |
||||
ret = self.CS.update(self.cp) |
||||
|
||||
# wait for everything to init first |
||||
if self.frame > int(5. / DT_CTRL): |
||||
# body always wants to enable |
||||
ret.init('events', 1) |
||||
ret.events[0].name = car.CarEvent.EventName.pcmEnable |
||||
ret.events[0].enable = True |
||||
self.frame += 1 |
||||
|
||||
return ret |
||||
|
||||
def apply(self, c): |
||||
return self.CC.update(c, self.CS) |
||||
@ -0,0 +1,5 @@ |
||||
#!/usr/bin/env python3 |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
pass |
||||
@ -0,0 +1,38 @@ |
||||
from typing import Dict |
||||
|
||||
from cereal import car |
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarInfo, Harness |
||||
Ecu = car.CarParams.Ecu |
||||
|
||||
SPEED_FROM_RPM = 0.008587 |
||||
|
||||
class CarControllerParams: |
||||
ANGLE_DELTA_BP = [0., 5., 15.] |
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit |
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit |
||||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower |
||||
STEER_THRESHOLD = 1.0 |
||||
|
||||
class CAR: |
||||
BODY = "COMMA BODY" |
||||
|
||||
CAR_INFO: Dict[str, CarInfo] = { |
||||
CAR.BODY: CarInfo("comma body", package="All", harness=Harness.none), |
||||
} |
||||
|
||||
FW_VERSIONS = { |
||||
CAR.BODY: { |
||||
(Ecu.engine, 0x720, None): [ |
||||
b'0.0.01', |
||||
b'02/27/2022' |
||||
], |
||||
(Ecu.debug, 0x721, None): [ |
||||
b'166bd860' # git hash of the firmware used |
||||
], |
||||
}, |
||||
} |
||||
|
||||
DBC = { |
||||
CAR.BODY: dbc_dict('comma_body', None), |
||||
} |
||||
@ -0,0 +1,81 @@ |
||||
from opendbc.can.packer import CANPacker |
||||
from common.realtime import DT_CTRL |
||||
from selfdrive.car import apply_toyota_steer_torque_limits |
||||
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons |
||||
from selfdrive.car.chrysler.values import CAR, RAM_CARS, CarControllerParams |
||||
|
||||
|
||||
class CarController: |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.CP = CP |
||||
self.apply_steer_last = 0 |
||||
self.frame = 0 |
||||
|
||||
self.hud_count = 0 |
||||
self.last_lkas_falling_edge = 0 |
||||
self.lkas_control_bit_prev = False |
||||
self.last_button_frame = 0 |
||||
|
||||
self.packer = CANPacker(dbc_name) |
||||
self.params = CarControllerParams(CP) |
||||
|
||||
def update(self, CC, CS): |
||||
can_sends = [] |
||||
|
||||
# TODO: can we make this more sane? why is it different for all the cars? |
||||
lkas_control_bit = self.lkas_control_bit_prev |
||||
if CS.out.vEgo > self.CP.minSteerSpeed: |
||||
lkas_control_bit = True |
||||
elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): |
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0): |
||||
lkas_control_bit = False |
||||
elif self.CP.carFingerprint in RAM_CARS: |
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5): |
||||
lkas_control_bit = False |
||||
|
||||
# EPS faults if LKAS re-enables too quickly |
||||
lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200) |
||||
lkas_active = CC.latActive and self.lkas_control_bit_prev |
||||
|
||||
# *** control msgs *** |
||||
|
||||
# cruise buttons |
||||
if (self.frame - self.last_button_frame)*DT_CTRL > 0.05: |
||||
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0 |
||||
|
||||
# ACC cancellation |
||||
if CC.cruiseControl.cancel: |
||||
self.last_button_frame = self.frame |
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True)) |
||||
|
||||
# ACC resume from standstill |
||||
elif CC.cruiseControl.resume: |
||||
self.last_button_frame = self.frame |
||||
can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True)) |
||||
|
||||
# HUD alerts |
||||
if self.frame % 25 == 0: |
||||
if CS.lkas_car_model != -1: |
||||
can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) |
||||
self.hud_count += 1 |
||||
|
||||
# steering |
||||
if self.frame % 2 == 0: |
||||
# steer torque |
||||
new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX)) |
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) |
||||
if not lkas_active: |
||||
apply_steer = 0 |
||||
self.apply_steer_last = apply_steer |
||||
|
||||
can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit)) |
||||
|
||||
self.frame += 1 |
||||
if not lkas_control_bit and self.lkas_control_bit_prev: |
||||
self.last_lkas_falling_edge = self.frame |
||||
self.lkas_control_bit_prev = lkas_control_bit |
||||
|
||||
new_actuators = CC.actuators.copy() |
||||
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX |
||||
|
||||
return new_actuators, can_sends |
||||
@ -0,0 +1,205 @@ |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from opendbc.can.parser import CANParser |
||||
from opendbc.can.can_define import CANDefine |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS |
||||
|
||||
|
||||
class CarState(CarStateBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
self.CP = CP |
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) |
||||
|
||||
self.auto_high_beam = 0 |
||||
self.button_counter = 0 |
||||
self.lkas_car_model = -1 |
||||
|
||||
if CP.carFingerprint in RAM_CARS: |
||||
self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"] |
||||
else: |
||||
self.shifter_values = can_define.dv["GEAR"]["PRNDL"] |
||||
|
||||
def update(self, cp, cp_cam): |
||||
|
||||
ret = car.CarState.new_message() |
||||
|
||||
# lock info |
||||
ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], |
||||
cp.vl["BCM_1"]["DOOR_OPEN_FR"], |
||||
cp.vl["BCM_1"]["DOOR_OPEN_RL"], |
||||
cp.vl["BCM_1"]["DOOR_OPEN_RR"]]) |
||||
ret.seatbeltUnlatched = cp.vl["ORC_1"]["SEATBELT_DRIVER_UNLATCHED"] == 1 |
||||
|
||||
# brake pedal |
||||
ret.brake = 0 |
||||
ret.brakePressed = cp.vl["ESP_1"]['Brake_Pedal_State'] == 1 # Physical brake pedal switch |
||||
|
||||
# gas pedal |
||||
ret.gas = cp.vl["ECM_5"]["Accelerator_Position"] |
||||
ret.gasPressed = ret.gas > 1e-5 |
||||
|
||||
# car speed |
||||
if self.CP.carFingerprint in RAM_CARS: |
||||
ret.vEgoRaw = cp.vl["ESP_8"]["Vehicle_Speed"] * CV.KPH_TO_MS |
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["Transmission_Status"]["Gear_State"], None)) |
||||
else: |
||||
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2. |
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None)) |
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.standstill = not ret.vEgoRaw > 0.001 |
||||
ret.wheelSpeeds = self.get_wheel_speeds( |
||||
cp.vl["ESP_6"]["WHEEL_SPEED_FL"], |
||||
cp.vl["ESP_6"]["WHEEL_SPEED_FR"], |
||||
cp.vl["ESP_6"]["WHEEL_SPEED_RL"], |
||||
cp.vl["ESP_6"]["WHEEL_SPEED_RR"], |
||||
unit=1, |
||||
) |
||||
|
||||
# button presses |
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1 |
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2 |
||||
ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1 |
||||
|
||||
# steering wheel |
||||
ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"] + cp.vl["STEERING"]["STEERING_ANGLE_HP"] |
||||
ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"] |
||||
ret.steeringTorque = cp.vl["EPS_2"]["COLUMN_TORQUE"] |
||||
ret.steeringTorqueEps = cp.vl["EPS_2"]["EPS_TORQUE_MOTOR"] |
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD |
||||
|
||||
# cruise state |
||||
cp_cruise = cp_cam if self.CP.carFingerprint in RAM_CARS else cp |
||||
|
||||
ret.cruiseState.available = cp_cruise.vl["DAS_3"]["ACC_AVAILABLE"] == 1 |
||||
ret.cruiseState.enabled = cp_cruise.vl["DAS_3"]["ACC_ACTIVE"] == 1 |
||||
ret.cruiseState.speed = cp_cruise.vl["DAS_4"]["ACC_SET_SPEED_KPH"] * CV.KPH_TO_MS |
||||
ret.cruiseState.nonAdaptive = cp_cruise.vl["DAS_4"]["ACC_STATE"] in (1, 2) # 1 NormalCCOn and 2 NormalCCSet |
||||
ret.cruiseState.standstill = cp_cruise.vl["DAS_3"]["ACC_STANDSTILL"] == 1 |
||||
ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0 |
||||
|
||||
if self.CP.carFingerprint in RAM_CARS: |
||||
self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON'] # Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message |
||||
ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1 |
||||
else: |
||||
ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4 |
||||
|
||||
# blindspot sensors |
||||
if self.CP.enableBsm: |
||||
ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1 |
||||
ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1 |
||||
|
||||
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] |
||||
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def get_cruise_signals(): |
||||
signals = [ |
||||
("ACC_AVAILABLE", "DAS_3"), |
||||
("ACC_ACTIVE", "DAS_3"), |
||||
("ACC_FAULTED", "DAS_3"), |
||||
("ACC_STANDSTILL", "DAS_3"), |
||||
("COUNTER", "DAS_3"), |
||||
("ACC_SET_SPEED_KPH", "DAS_4"), |
||||
("ACC_STATE", "DAS_4"), |
||||
] |
||||
checks = [ |
||||
("DAS_3", 50), |
||||
("DAS_4", 50), |
||||
] |
||||
return signals, checks |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("DOOR_OPEN_FL", "BCM_1"), |
||||
("DOOR_OPEN_FR", "BCM_1"), |
||||
("DOOR_OPEN_RL", "BCM_1"), |
||||
("DOOR_OPEN_RR", "BCM_1"), |
||||
("Brake_Pedal_State", "ESP_1"), |
||||
("Accelerator_Position", "ECM_5"), |
||||
("WHEEL_SPEED_FL", "ESP_6"), |
||||
("WHEEL_SPEED_RR", "ESP_6"), |
||||
("WHEEL_SPEED_RL", "ESP_6"), |
||||
("WHEEL_SPEED_FR", "ESP_6"), |
||||
("STEERING_ANGLE", "STEERING"), |
||||
("STEERING_ANGLE_HP", "STEERING"), |
||||
("STEERING_RATE", "STEERING"), |
||||
("TURN_SIGNALS", "STEERING_LEVERS"), |
||||
("HIGH_BEAM_PRESSED", "STEERING_LEVERS"), |
||||
("SEATBELT_DRIVER_UNLATCHED", "ORC_1"), |
||||
("COUNTER", "EPS_2",), |
||||
("COLUMN_TORQUE", "EPS_2"), |
||||
("EPS_TORQUE_MOTOR", "EPS_2"), |
||||
("LKAS_STATE", "EPS_2"), |
||||
("COUNTER", "CRUISE_BUTTONS"), |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("ESP_1", 50), |
||||
("EPS_2", 100), |
||||
("ESP_6", 50), |
||||
("STEERING", 100), |
||||
("ECM_5", 50), |
||||
("CRUISE_BUTTONS", 50), |
||||
("STEERING_LEVERS", 10), |
||||
("ORC_1", 2), |
||||
("BCM_1", 1), |
||||
] |
||||
|
||||
if CP.enableBsm: |
||||
signals += [ |
||||
("RIGHT_STATUS", "BSM_1"), |
||||
("LEFT_STATUS", "BSM_1"), |
||||
] |
||||
checks.append(("BSM_1", 2)) |
||||
|
||||
if CP.carFingerprint in RAM_CARS: |
||||
signals += [ |
||||
("DASM_FAULT", "EPS_3"), |
||||
("Vehicle_Speed", "ESP_8"), |
||||
("Gear_State", "Transmission_Status"), |
||||
] |
||||
checks += [ |
||||
("ESP_8", 50), |
||||
("EPS_3", 50), |
||||
("Transmission_Status", 50), |
||||
] |
||||
else: |
||||
signals += [ |
||||
("PRNDL", "GEAR"), |
||||
("SPEED_LEFT", "SPEED_1"), |
||||
("SPEED_RIGHT", "SPEED_1"), |
||||
] |
||||
checks += [ |
||||
("GEAR", 50), |
||||
("SPEED_1", 100), |
||||
] |
||||
signals += CarState.get_cruise_signals()[0] |
||||
checks += CarState.get_cruise_signals()[1] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) |
||||
|
||||
@staticmethod |
||||
def get_cam_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("CAR_MODEL", "DAS_6"), |
||||
] |
||||
checks = [ |
||||
("DAS_6", 4), |
||||
] |
||||
|
||||
if CP.carFingerprint in RAM_CARS: |
||||
signals += [ |
||||
("AUTO_HIGH_BEAM_ON", "DAS_6"), |
||||
] |
||||
signals += CarState.get_cruise_signals()[0] |
||||
checks += CarState.get_cruise_signals()[1] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) |
||||
@ -0,0 +1,71 @@ |
||||
from cereal import car |
||||
from selfdrive.car.chrysler.values import RAM_CARS |
||||
|
||||
GearShifter = car.CarState.GearShifter |
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||
|
||||
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam): |
||||
# LKAS_HUD - Controls what lane-keeping icon is displayed |
||||
|
||||
# == Color == |
||||
# 0 hidden? |
||||
# 1 white |
||||
# 2 green |
||||
# 3 ldw |
||||
|
||||
# == Lines == |
||||
# 03 white Lines |
||||
# 04 grey lines |
||||
# 09 left lane close |
||||
# 0A right lane close |
||||
# 0B left Lane very close |
||||
# 0C right Lane very close |
||||
# 0D left cross cross |
||||
# 0E right lane cross |
||||
|
||||
# == Alerts == |
||||
# 7 Normal |
||||
# 6 lane departure place hands on wheel |
||||
|
||||
color = 2 if lkas_active else 1 |
||||
lines = 3 if lkas_active else 0 |
||||
alerts = 7 if lkas_active else 0 |
||||
|
||||
if hud_count < (1 * 4): # first 3 seconds, 4Hz |
||||
alerts = 1 |
||||
|
||||
if hud_alert in (VisualAlert.ldw, VisualAlert.steerRequired): |
||||
color = 4 |
||||
lines = 0 |
||||
alerts = 6 |
||||
|
||||
values = { |
||||
"LKAS_ICON_COLOR": color, |
||||
"CAR_MODEL": car_model, |
||||
"LKAS_LANE_LINES": lines, |
||||
"LKAS_ALERTS": alerts, |
||||
} |
||||
|
||||
if CP.carFingerprint in RAM_CARS: |
||||
values['AUTO_HIGH_BEAM_ON'] = auto_high_beam |
||||
|
||||
return packer.make_can_msg("DAS_6", 0, values) |
||||
|
||||
|
||||
def create_lkas_command(packer, CP, apply_steer, lkas_control_bit): |
||||
# LKAS_COMMAND Lane-keeping signal to turn the wheel |
||||
enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1 |
||||
values = { |
||||
"STEERING_TORQUE": apply_steer, |
||||
"LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0, |
||||
} |
||||
return packer.make_can_msg("LKAS_COMMAND", 0, values) |
||||
|
||||
|
||||
def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False): |
||||
values = { |
||||
"ACC_Cancel": cancel, |
||||
"ACC_Resume": resume, |
||||
"COUNTER": frame % 0x10, |
||||
} |
||||
return packer.make_can_msg("CRUISE_BUTTONS", bus, values) |
||||
@ -0,0 +1,99 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from panda import Panda |
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config |
||||
from selfdrive.car.chrysler.values import CAR, DBC, RAM_CARS |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
ret.carName = "chrysler" |
||||
|
||||
ret.radarOffCan = DBC[candidate]['radar'] is None |
||||
|
||||
param = Panda.FLAG_CHRYSLER_RAM_DT if candidate in RAM_CARS else None |
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler, param)] |
||||
|
||||
ret.steerActuatorDelay = 0.1 |
||||
ret.steerLimitTimer = 0.4 |
||||
|
||||
ret.minSteerSpeed = 3.8 # m/s |
||||
if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): |
||||
# TODO: allow 2019 cars to steer down to 13 m/s if already engaged. |
||||
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. |
||||
|
||||
# Chrysler |
||||
if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020): |
||||
ret.mass = 2242. + STD_CARGO_KG |
||||
ret.wheelbase = 3.089 |
||||
ret.steerRatio = 16.2 # 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 |
||||
|
||||
# Jeep |
||||
elif candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): |
||||
ret.mass = 1778 + STD_CARGO_KG |
||||
ret.wheelbase = 2.71 |
||||
ret.steerRatio = 16.7 |
||||
ret.steerActuatorDelay = 0.2 |
||||
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] |
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] |
||||
ret.lateralTuning.pid.kf = 0.00006 |
||||
|
||||
# Ram |
||||
elif candidate == CAR.RAM_1500: |
||||
ret.steerActuatorDelay = 0.2 |
||||
|
||||
ret.wheelbase = 3.88 |
||||
ret.steerRatio = 16.3 |
||||
ret.mass = 2493. + STD_CARGO_KG |
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) |
||||
|
||||
ret.minSteerSpeed = 14.5 |
||||
if car_fw is not None: |
||||
for fw in car_fw: |
||||
if fw.ecu == 'eps' and fw.fwVersion in (b"68312176AE", b"68312176AG", b"68273275AG"): |
||||
ret.minSteerSpeed = 0. |
||||
|
||||
else: |
||||
raise ValueError(f"Unsupported car: {candidate}") |
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
|
||||
# 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) |
||||
|
||||
ret.enableBsm = 720 in fingerprint[0] |
||||
|
||||
return ret |
||||
|
||||
def _update(self, c): |
||||
ret = self.CS.update(self.cp, self.cp_cam) |
||||
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret) |
||||
|
||||
# events |
||||
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) |
||||
events = self.dp_atl_warning(ret, events) |
||||
|
||||
# Low speed steer alert hysteresis logic |
||||
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5): |
||||
self.low_speed_alert = True |
||||
elif ret.vEgo > (self.CP.minSteerSpeed + 1.): |
||||
self.low_speed_alert = False |
||||
if self.low_speed_alert: |
||||
events.add(car.CarEvent.EventName.belowSteerSpeed) |
||||
|
||||
ret.events = events.to_msg() |
||||
|
||||
return ret |
||||
|
||||
def apply(self, c): |
||||
return self.CC.update(c, self.CS) |
||||
@ -0,0 +1,91 @@ |
||||
#!/usr/bin/env python3 |
||||
from opendbc.can.parser import CANParser |
||||
from cereal import car |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
from selfdrive.car.chrysler.values import DBC |
||||
|
||||
RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724 |
||||
RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages |
||||
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D) |
||||
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D) |
||||
|
||||
def _create_radar_can_parser(car_fingerprint): |
||||
dbc = DBC[car_fingerprint]['radar'] |
||||
if dbc is None: |
||||
return None |
||||
|
||||
msg_n = len(RADAR_MSGS_C) |
||||
# list of [(signal name, message name or number), (...)] |
||||
# [('RADAR_STATE', 1024), |
||||
# ('LONG_DIST', 1072), |
||||
# ('LONG_DIST', 1073), |
||||
# ('LONG_DIST', 1074), |
||||
# ('LONG_DIST', 1075), |
||||
|
||||
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 |
||||
|
||||
checks = list(zip(RADAR_MSGS_C + |
||||
RADAR_MSGS_D, |
||||
[20] * msg_n + # 20Hz (0.05s) |
||||
[20] * msg_n)) # 20Hz (0.05s) |
||||
|
||||
return CANParser(DBC[car_fingerprint]['radar'], 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): |
||||
super().__init__(CP) |
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint) |
||||
self.updated_messages = set() |
||||
self.trigger_msg = LAST_MSG |
||||
|
||||
def update(self, can_strings): |
||||
if self.rcp is None: |
||||
return super().update(None) |
||||
|
||||
vls = self.rcp.update_strings(can_strings) |
||||
self.updated_messages.update(vls) |
||||
|
||||
if self.trigger_msg not in self.updated_messages: |
||||
return None |
||||
|
||||
ret = car.RadarData.new_message() |
||||
errors = [] |
||||
if not self.rcp.can_valid: |
||||
errors.append("canError") |
||||
ret.errors = errors |
||||
|
||||
for ii in self.updated_messages: # ii should be the message ID as a number |
||||
cpt = self.rcp.vl[ii] |
||||
trackId = _address_to_track(ii) |
||||
|
||||
if trackId not in self.pts: |
||||
self.pts[trackId] = car.RadarData.RadarPoint.new_message() |
||||
self.pts[trackId].trackId = trackId |
||||
self.pts[trackId].aRel = float('nan') |
||||
self.pts[trackId].yvRel = float('nan') |
||||
self.pts[trackId].measured = True |
||||
|
||||
if 'LONG_DIST' in cpt: # c_* message |
||||
self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car |
||||
# our lat_dist is positive to the right in car's frame. |
||||
# TODO what does yRel want? |
||||
self.pts[trackId].yRel = cpt['LAT_DIST'] # in car frame's y axis, left is positive |
||||
else: # d_* message |
||||
self.pts[trackId].vRel = cpt['REL_SPEED'] |
||||
|
||||
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid. |
||||
ret.points = [x for x in self.pts.values() if x.dRel != 0] |
||||
|
||||
self.updated_messages.clear() |
||||
return ret |
||||
@ -0,0 +1,191 @@ |
||||
from dataclasses import dataclass |
||||
from enum import Enum |
||||
from typing import Dict, List, Optional, Union |
||||
|
||||
from cereal import car |
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarInfo, Harness |
||||
Ecu = car.CarParams.Ecu |
||||
|
||||
|
||||
class CAR: |
||||
# Chrysler |
||||
PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" |
||||
PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" |
||||
PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" |
||||
PACIFICA_2018 = "CHRYSLER PACIFICA 2018" |
||||
PACIFICA_2020 = "CHRYSLER PACIFICA 2020" |
||||
|
||||
# Jeep |
||||
JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk |
||||
JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk |
||||
|
||||
# Ram |
||||
RAM_1500 = "RAM 1500 5TH GEN" |
||||
|
||||
|
||||
class CarControllerParams: |
||||
def __init__(self, CP): |
||||
self.STEER_MAX = 261 # higher than this faults the EPS on Chrysler/Jeep. Ram DT allows more |
||||
self.STEER_ERROR_MAX = 80 |
||||
|
||||
if CP.carFingerprint in RAM_CARS: |
||||
self.STEER_DELTA_UP = 6 |
||||
self.STEER_DELTA_DOWN = 6 |
||||
else: |
||||
self.STEER_DELTA_UP = 3 |
||||
self.STEER_DELTA_DOWN = 3 |
||||
|
||||
STEER_THRESHOLD = 120 |
||||
|
||||
RAM_CARS = {CAR.RAM_1500, } |
||||
|
||||
@dataclass |
||||
class ChryslerCarInfo(CarInfo): |
||||
package: str = "Adaptive Cruise Control" |
||||
harness: Enum = Harness.fca |
||||
|
||||
CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { |
||||
CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"), |
||||
CAR.PACIFICA_2018_HYBRID: None, # same platforms |
||||
CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-22"), |
||||
CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), |
||||
CAR.PACIFICA_2020: [ |
||||
ChryslerCarInfo("Chrysler Pacifica 2019-20"), |
||||
ChryslerCarInfo("Chrysler Pacifica 2021", package="All"), |
||||
], |
||||
CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), |
||||
CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), |
||||
CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-22", harness=Harness.ram), |
||||
} |
||||
|
||||
# Unique CAN messages: |
||||
# Only the hybrids have 270: 8 |
||||
# Only the gas have 55: 8, 416: 7 |
||||
# For 564, All 2017 have length 4, whereas 2018-19 have length 8. |
||||
# For 924, Pacifica 2017 has length 3, whereas all 2018-19 have length 8. |
||||
# For 560, All 2019 have length 8, whereas all 2017-18 have length 4. |
||||
|
||||
# Jeep Grand Cherokee unique messages: |
||||
# 2017 Trailhawk: 618: 8 |
||||
# For 924, Trailhawk 2017 has length 3, whereas 2018 V6 has length 8. |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.PACIFICA_2017_HYBRID: [{ |
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 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_2020: [{ |
||||
55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8 |
||||
}], |
||||
CAR.PACIFICA_2018_HYBRID: [{ |
||||
68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8 |
||||
}, |
||||
# 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 |
||||
}, |
||||
# Based on "8190c7275a24557b|2020-02-24--09-57-23" |
||||
{ |
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1536: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 |
||||
}], |
||||
CAR.JEEP_CHEROKEE: [{ |
||||
55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 |
||||
}, |
||||
# Based on c88f65eeaee4003a|2022-08-04--15-37-16 |
||||
{ |
||||
257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 678: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1235: 8, 1242: 8, 1252: 8, 1792: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1892: 8, 1927: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 |
||||
}], |
||||
CAR.JEEP_CHEROKEE_2019: [{ |
||||
# Jeep Grand Cherokee 2019, including most 2020 models |
||||
55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 |
||||
}], |
||||
} |
||||
|
||||
FW_VERSIONS = { |
||||
CAR.RAM_1500: { |
||||
(Ecu.combinationMeter, 0x742, None): [ |
||||
b'68294063AH', |
||||
b'68294063AG', |
||||
b'68434860AC', |
||||
b'68527375AD', |
||||
b'68453503AC', |
||||
], |
||||
(Ecu.srs, 0x744, None): [ |
||||
b'68441329AB', |
||||
b'68490898AA', |
||||
b'68428609AB', |
||||
b'68500728AA', |
||||
], |
||||
(Ecu.esp, 0x747, None): [ |
||||
b'68432418AD', |
||||
b'68432418AB', |
||||
b'68436004AE', |
||||
b'68438454AD', |
||||
b'68436004AD', |
||||
b'68535469AB', |
||||
b'68438454AC', |
||||
], |
||||
(Ecu.fwdRadar, 0x753, None): [ |
||||
b'68320950AL', |
||||
b'68320950AJ', |
||||
b'68454268AB', |
||||
b'68475160AG', |
||||
b'04672892AB', |
||||
b'68475160AE', |
||||
], |
||||
(Ecu.eps, 0x75A, None): [ |
||||
b'68273275AG', |
||||
b'68469901AA', |
||||
b'68552788AA', |
||||
], |
||||
(Ecu.engine, 0x7e0, None): [ |
||||
b'68448163AJ', |
||||
b'68500630AD', |
||||
b'68539650AD', |
||||
], |
||||
(Ecu.transmission, 0x7e1, None): [ |
||||
b'68360078AL', |
||||
b'68384328AD', |
||||
b'68360085AL', |
||||
b'68360081AM', |
||||
b'68502994AD', |
||||
b'68445533AB', |
||||
b'68540431AB', |
||||
b'68484467AC', |
||||
], |
||||
(Ecu.gateway, 0x18DACBF1, None): [ |
||||
b'68402660AB', |
||||
b'68445283AB', |
||||
b'68533631AB', |
||||
b'68500483AB', |
||||
], |
||||
}, |
||||
} |
||||
|
||||
DBC = { |
||||
CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.JEEP_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.JEEP_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), |
||||
CAR.RAM_1500: dbc_dict('chrysler_ram_dt_generated', None), |
||||
} |
||||
@ -0,0 +1,89 @@ |
||||
from cereal import car |
||||
from common.numpy_fast import clip, interp |
||||
from opendbc.can.packer import CANPacker |
||||
from selfdrive.car.ford import fordcan |
||||
from selfdrive.car.ford.values import CarControllerParams |
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||
|
||||
|
||||
def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo): |
||||
# rate limit |
||||
steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last) |
||||
rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN |
||||
max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) |
||||
apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff)) |
||||
|
||||
return apply_steer |
||||
|
||||
|
||||
class CarController: |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.CP = CP |
||||
self.VM = VM |
||||
self.packer = CANPacker(dbc_name) |
||||
self.frame = 0 |
||||
|
||||
self.apply_steer_last = 0 |
||||
self.main_on_last = False |
||||
self.lkas_enabled_last = False |
||||
self.steer_alert_last = False |
||||
|
||||
def update(self, CC, CS): |
||||
can_sends = [] |
||||
|
||||
actuators = CC.actuators |
||||
hud_control = CC.hudControl |
||||
|
||||
main_on = CS.out.cruiseState.available |
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) |
||||
|
||||
if CC.cruiseControl.cancel: |
||||
# cancel stock ACC |
||||
can_sends.append(fordcan.spam_cancel_button(self.packer)) |
||||
|
||||
# apply rate limits |
||||
new_steer = actuators.steeringAngleDeg |
||||
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) |
||||
|
||||
# send steering commands at 20Hz |
||||
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: |
||||
lca_rq = 1 if CC.latActive else 0 |
||||
|
||||
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented |
||||
path_angle = apply_steer |
||||
|
||||
# convert steer angle to curvature |
||||
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) |
||||
|
||||
# TODO: get other actuators |
||||
curvature_rate = 0 |
||||
path_offset = 0 |
||||
|
||||
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately |
||||
precision = 0 # 0=Comfortable, 1=Precise |
||||
|
||||
self.apply_steer_last = apply_steer |
||||
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature)) |
||||
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, |
||||
path_offset, path_angle, curvature_rate, curvature)) |
||||
|
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) |
||||
|
||||
# send lkas ui command at 1Hz or if ui state changes |
||||
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: |
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) |
||||
|
||||
# send acc ui command at 20Hz or if ui state changes |
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: |
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) |
||||
|
||||
self.main_on_last = main_on |
||||
self.lkas_enabled_last = CC.latActive |
||||
self.steer_alert_last = steer_alert |
||||
|
||||
new_actuators = actuators.copy() |
||||
new_actuators.steeringAngleDeg = apply_steer |
||||
|
||||
self.frame += 1 |
||||
return new_actuators, can_sends |
||||
@ -0,0 +1,215 @@ |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from opendbc.can.can_define import CANDefine |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from selfdrive.car.ford.values import CANBUS, DBC |
||||
|
||||
GearShifter = car.CarState.GearShifter |
||||
TransmissionType = car.CarParams.TransmissionType |
||||
|
||||
|
||||
class CarState(CarStateBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) |
||||
if CP.transmissionType == TransmissionType.automatic: |
||||
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"] |
||||
|
||||
def update(self, cp, cp_cam): |
||||
ret = car.CarState.new_message() |
||||
|
||||
# car speed |
||||
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS |
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG |
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 |
||||
|
||||
# gas pedal |
||||
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. |
||||
ret.gasPressed = ret.gas > 1e-6 |
||||
|
||||
# brake pedal |
||||
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm |
||||
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2 |
||||
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2) |
||||
|
||||
# steering wheel |
||||
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] |
||||
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] |
||||
ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0 |
||||
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 |
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) |
||||
# ret.espDisabled = False # TODO: find traction control signal |
||||
|
||||
# cruise state |
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS |
||||
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) |
||||
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) |
||||
|
||||
# gear |
||||
if self.CP.transmissionType == TransmissionType.automatic: |
||||
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"], None) |
||||
ret.gearShifter = self.parse_gear_shifter(gear) |
||||
elif self.CP.transmissionType == TransmissionType.manual: |
||||
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0 |
||||
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]): |
||||
ret.gearShifter = GearShifter.reverse |
||||
else: |
||||
ret.gearShifter = GearShifter.drive |
||||
|
||||
# safety |
||||
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) |
||||
ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled |
||||
|
||||
# button presses |
||||
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 |
||||
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 |
||||
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) |
||||
|
||||
# lock info |
||||
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"], |
||||
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]]) |
||||
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2 |
||||
|
||||
# blindspot sensors |
||||
if self.CP.enableBsm: |
||||
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 |
||||
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 |
||||
|
||||
# Stock values from IPMA so that we can retain some stock functionality |
||||
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] |
||||
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) |
||||
("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) |
||||
("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped |
||||
("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status |
||||
("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct) |
||||
("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm) |
||||
("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed |
||||
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) |
||||
# The units might change with IPC settings? |
||||
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status |
||||
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) |
||||
# Calculates steering angle (and offset) from pinion |
||||
# angle and driving measurements. |
||||
# StePinRelInit_An_Sns is the pinion angle, initialised |
||||
# to zero at the beginning of the drive. |
||||
("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm) |
||||
("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status |
||||
("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel |
||||
("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch |
||||
("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle |
||||
("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver |
||||
("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger |
||||
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left |
||||
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right |
||||
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("EngVehicleSpThrottle2", 50), |
||||
("Yaw_Data_FD1", 100), |
||||
("DesiredTorqBrk", 50), |
||||
("EngVehicleSpThrottle", 100), |
||||
("BrakeSnData_4", 50), |
||||
("EngBrakeData", 10), |
||||
("SteeringPinion_Data", 100), |
||||
("EPAS_INFO", 50), |
||||
("Lane_Assist_Data3_FD1", 33), |
||||
("Steering_Data_FD1", 10), |
||||
("BodyInfo_3_FD1", 2), |
||||
("RCMStatusMessage2_FD1", 10), |
||||
] |
||||
|
||||
if CP.transmissionType == TransmissionType.automatic: |
||||
signals += [ |
||||
("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position |
||||
] |
||||
checks += [ |
||||
("Gear_Shift_by_Wire_FD1", 10), |
||||
] |
||||
elif CP.transmissionType == TransmissionType.manual: |
||||
signals += [ |
||||
("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct) |
||||
("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"), # BCM reverse light |
||||
] |
||||
checks += [ |
||||
("Engine_Clutch_Data", 33), |
||||
("BCM_Lamp_Stat_FD1", 1), |
||||
] |
||||
|
||||
if CP.enableBsm: |
||||
signals += [ |
||||
("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left |
||||
("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right |
||||
] |
||||
checks += [ |
||||
("Side_Detect_L_Stat", 5), |
||||
("Side_Detect_R_Stat", 5), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main) |
||||
|
||||
@staticmethod |
||||
def get_cam_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("HaDsply_No_Cs", "ACCDATA_3"), |
||||
("HaDsply_No_Cnt", "ACCDATA_3"), |
||||
("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message |
||||
("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance |
||||
("AccStopRes_B_Dsply", "ACCDATA_3"), |
||||
("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning |
||||
("Tja_D_Stat", "ACCDATA_3"), # TJA status |
||||
("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text |
||||
("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon |
||||
("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text |
||||
("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled |
||||
("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting |
||||
("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting |
||||
("CadsAlignIncplt_B_Actl", "ACCDATA_3"), |
||||
("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting |
||||
("CadsRadrBlck_B_Actl", "ACCDATA_3"), |
||||
("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status |
||||
("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting |
||||
("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting |
||||
("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text |
||||
("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning |
||||
("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert |
||||
("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert |
||||
("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap |
||||
("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting |
||||
("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting |
||||
|
||||
("FeatConfigIpmaActl", "IPMA_Data"), |
||||
("FeatNoIpmaActl", "IPMA_Data"), |
||||
("PersIndexIpma_D_Actl", "IPMA_Data"), |
||||
("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping |
||||
("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines) |
||||
("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error |
||||
("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime |
||||
("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater? |
||||
("CamraStats_D_Dsply", "IPMA_Data"), # Camera status |
||||
("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level |
||||
("DasStats_D_Dsply", "IPMA_Data"), # DAS status |
||||
("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning |
||||
("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status |
||||
("Set_Me_X1", "IPMA_Data"), |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("ACCDATA_3", 5), |
||||
("IPMA_Data", 1), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera) |
||||
@ -0,0 +1,144 @@ |
||||
from common.numpy_fast import clip |
||||
|
||||
|
||||
def create_lkas_command(packer, angle_deg: float, curvature: float): |
||||
""" |
||||
Creates a CAN message for the Ford LKAS Command. |
||||
|
||||
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the |
||||
PSCM lockout. |
||||
|
||||
Frequency is 20Hz. |
||||
""" |
||||
|
||||
values = { |
||||
"LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] |
||||
"LkaActvStats_D2_Req": 0, # action [0|7] |
||||
"LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees |
||||
"LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick |
||||
"LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter |
||||
"LdwActvStats_D_Req": 0, # LDW status [0|7] |
||||
"LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength |
||||
} |
||||
return packer.make_can_msg("Lane_Assist_Data1", 0, values) |
||||
|
||||
|
||||
def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): |
||||
""" |
||||
Creates a CAN message for the Ford TJA/LCA Command. |
||||
|
||||
This command can apply "Lane Centering" manoeuvres: continuous lane centering |
||||
for traffic jam assist and highway driving. It is not subject to the PSCM |
||||
lockout. |
||||
|
||||
The PSCM should be configured to accept TJA/LCA commands before these |
||||
commands will be processed. This can be done using tools such as Forscan. |
||||
|
||||
Frequency is 20Hz. |
||||
""" |
||||
|
||||
values = { |
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter |
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] |
||||
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] |
||||
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] |
||||
"LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] |
||||
"LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11), # Path offset [-5.12|5.11] meter |
||||
"LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians |
||||
"LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2 |
||||
"LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094), # Curvature [-0.02|0.02094] 1/meter |
||||
} |
||||
return packer.make_can_msg("LateralMotionControl", 0, values) |
||||
|
||||
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values): |
||||
""" |
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status. |
||||
|
||||
Show the LKAS status with the "driver assist" lines in the IPC. |
||||
|
||||
Stock functionality is maintained by passing through unmodified signals. |
||||
|
||||
Frequency is 1Hz. |
||||
""" |
||||
|
||||
# LaActvStats_D_Dsply |
||||
# TODO: get LDW state from OP |
||||
if enabled: |
||||
lines = 6 |
||||
elif main_on: |
||||
lines = 0 |
||||
else: |
||||
lines = 30 |
||||
|
||||
values = { |
||||
"FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535] |
||||
"FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535] |
||||
"PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7] |
||||
"AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3] |
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] |
||||
"LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1] |
||||
"LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed |
||||
"CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1] |
||||
"CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3] |
||||
"DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7] |
||||
"DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3] |
||||
"DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3] |
||||
"AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3] |
||||
"Set_Me_X1": stock_values["Set_Me_X1"], # [0|15] |
||||
} |
||||
return packer.make_can_msg("IPMA_Data", 0, values) |
||||
|
||||
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values): |
||||
""" |
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision |
||||
warning and traffic jam assist status. |
||||
|
||||
Stock functionality is maintained by passing through unmodified signals. |
||||
|
||||
Frequency is 20Hz. |
||||
""" |
||||
|
||||
values = { |
||||
"HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255] |
||||
"HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15] |
||||
"AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3] |
||||
"AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15] |
||||
"AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1] |
||||
"TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7] |
||||
"Tja_D_Stat": 2 if enabled else (1 if main_on else 0), # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7] |
||||
"TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7] |
||||
"IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3] |
||||
"AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15] |
||||
"FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1] |
||||
"FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1] |
||||
"AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1] |
||||
"CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1] |
||||
"AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1] |
||||
"CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1] |
||||
"CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1] |
||||
"AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1] |
||||
"FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3] |
||||
"FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7] |
||||
"AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3] |
||||
"FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1] |
||||
"FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1] |
||||
"AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7] |
||||
"AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1] |
||||
"FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1] |
||||
} |
||||
return packer.make_can_msg("ACCDATA_3", 0, values) |
||||
|
||||
|
||||
def spam_cancel_button(packer, cancel=1): |
||||
""" |
||||
Creates a CAN message for the Ford SCCM buttons/switches. |
||||
|
||||
Includes cruise control buttons, turn lights and more. |
||||
""" |
||||
|
||||
values = { |
||||
"CcAslButtnCnclPress": cancel, # CC cancel button |
||||
} |
||||
return packer.make_can_msg("Steering_Data_FD1", 0, values) |
||||
@ -0,0 +1,81 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint |
||||
from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
|
||||
EventName = car.CarEvent.EventName |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
|
||||
ret.carName = "ford" |
||||
#ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] |
||||
ret.dashcamOnly = True |
||||
|
||||
# Angle-based steering |
||||
# TODO: use curvature control when ready |
||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||
ret.steerActuatorDelay = 0.1 |
||||
ret.steerLimitTimer = 1.0 |
||||
|
||||
# TODO: detect stop-and-go vehicles |
||||
stop_and_go = False |
||||
|
||||
if candidate == CAR.ESCAPE_MK4: |
||||
ret.wheelbase = 2.71 |
||||
ret.steerRatio = 14.3 # Copied from Focus |
||||
tire_stiffness_factor = 0.5328 # Copied from Focus |
||||
ret.mass = 1750 + STD_CARGO_KG |
||||
|
||||
elif candidate == CAR.FOCUS_MK4: |
||||
ret.wheelbase = 2.7 |
||||
ret.steerRatio = 14.3 |
||||
tire_stiffness_factor = 0.5328 |
||||
ret.mass = 1350 + STD_CARGO_KG |
||||
|
||||
else: |
||||
raise ValueError(f"Unsupported car: ${candidate}") |
||||
|
||||
# Auto Transmission: Gear_Shift_by_Wire_FD1 |
||||
# TODO: detect transmission in car_fw? |
||||
if 0x5A in fingerprint[0]: |
||||
ret.transmissionType = TransmissionType.automatic |
||||
else: |
||||
ret.transmissionType = TransmissionType.manual |
||||
|
||||
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat |
||||
# TODO: detect bsm in car_fw? |
||||
ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] |
||||
|
||||
# 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) else 20. * CV.MPH_TO_MS |
||||
# LCA can steer down to zero |
||||
ret.minSteerSpeed = 0. |
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) |
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, |
||||
tire_stiffness_factor=tire_stiffness_factor) |
||||
|
||||
return ret |
||||
|
||||
def _update(self, c): |
||||
ret = self.CS.update(self.cp, self.cp_cam) |
||||
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret) |
||||
|
||||
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) |
||||
events = self.dp_atl_warning(ret, events) |
||||
ret.events = events.to_msg() |
||||
|
||||
return ret |
||||
|
||||
def apply(self, c): |
||||
return self.CC.update(c, self.CS) |
||||
@ -0,0 +1,78 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.ford.values import CANBUS, DBC |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
|
||||
RADAR_MSGS = list(range(0x500, 0x540)) |
||||
|
||||
|
||||
def _create_radar_can_parser(car_fingerprint): |
||||
if DBC[car_fingerprint]['radar'] is None: |
||||
return None |
||||
|
||||
msg_n = len(RADAR_MSGS) |
||||
signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, |
||||
RADAR_MSGS * 3)) |
||||
checks = list(zip(RADAR_MSGS, [20] * msg_n)) |
||||
|
||||
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CANBUS.radar) |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
self.validCnt = {key: 0 for key in RADAR_MSGS} |
||||
self.track_id = 0 |
||||
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint) |
||||
self.trigger_msg = 0x53f |
||||
self.updated_messages = set() |
||||
|
||||
def update(self, can_strings): |
||||
if self.rcp is None: |
||||
return super().update(None) |
||||
|
||||
vls = self.rcp.update_strings(can_strings) |
||||
self.updated_messages.update(vls) |
||||
|
||||
if self.trigger_msg not in self.updated_messages: |
||||
return None |
||||
|
||||
ret = car.RadarData.new_message() |
||||
errors = [] |
||||
if not self.rcp.can_valid: |
||||
errors.append("canError") |
||||
ret.errors = errors |
||||
|
||||
for ii in sorted(self.updated_messages): |
||||
cpt = self.rcp.vl[ii] |
||||
|
||||
if cpt['X_Rel'] > 0.00001: |
||||
self.validCnt[ii] = 0 # reset counter |
||||
|
||||
if cpt['X_Rel'] > 0.00001: |
||||
self.validCnt[ii] += 1 |
||||
else: |
||||
self.validCnt[ii] = max(self.validCnt[ii] - 1, 0) |
||||
#print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle'] |
||||
|
||||
# radar point only valid if there have been enough valid measurements |
||||
if self.validCnt[ii] > 0: |
||||
if ii not in self.pts: |
||||
self.pts[ii] = car.RadarData.RadarPoint.new_message() |
||||
self.pts[ii].trackId = self.track_id |
||||
self.track_id += 1 |
||||
self.pts[ii].dRel = cpt['X_Rel'] # from front of car |
||||
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive |
||||
self.pts[ii].vRel = cpt['V_Rel'] |
||||
self.pts[ii].aRel = float('nan') |
||||
self.pts[ii].yvRel = float('nan') |
||||
self.pts[ii].measured = True |
||||
else: |
||||
if ii in self.pts: |
||||
del self.pts[ii] |
||||
|
||||
ret.points = list(self.pts.values()) |
||||
self.updated_messages.clear() |
||||
return ret |
||||
@ -0,0 +1,85 @@ |
||||
from collections import namedtuple |
||||
from typing import Dict, List, Union |
||||
|
||||
from cereal import car |
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarInfo |
||||
|
||||
Ecu = car.CarParams.Ecu |
||||
TransmissionType = car.CarParams.TransmissionType |
||||
GearShifter = car.CarState.GearShifter |
||||
|
||||
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) |
||||
|
||||
|
||||
class CarControllerParams: |
||||
# Messages: Lane_Assist_Data1, LateralMotionControl |
||||
LKAS_STEER_STEP = 5 |
||||
# Message: IPMA_Data |
||||
LKAS_UI_STEP = 100 |
||||
# Message: ACCDATA_3 |
||||
ACC_UI_STEP = 5 |
||||
|
||||
STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) |
||||
STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) |
||||
|
||||
|
||||
class CANBUS: |
||||
main = 0 |
||||
radar = 1 |
||||
camera = 2 |
||||
|
||||
|
||||
class CAR: |
||||
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" |
||||
FOCUS_MK4 = "FORD FOCUS 4TH GEN" |
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { |
||||
CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"), |
||||
CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"), |
||||
} |
||||
|
||||
|
||||
FW_VERSIONS = { |
||||
CAR.ESCAPE_MK4: { |
||||
(Ecu.eps, 0x730, None): [ |
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.esp, 0x760, None): [ |
||||
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdRadar, 0x764, None): [ |
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdCamera, 0x706, None): [ |
||||
b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.engine, 0x7E0, None): [ |
||||
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
}, |
||||
CAR.FOCUS_MK4: { |
||||
(Ecu.eps, 0x730, None): [ |
||||
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.esp, 0x760, None): [ |
||||
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdRadar, 0x764, None): [ |
||||
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdCamera, 0x706, None): [ |
||||
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.engine, 0x7E0, None): [ |
||||
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
}, |
||||
} |
||||
|
||||
|
||||
DBC = { |
||||
CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', None), |
||||
CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', None), |
||||
} |
||||
@ -0,0 +1,128 @@ |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from common.numpy_fast import interp |
||||
from common.realtime import DT_CTRL |
||||
from opendbc.can.packer import CANPacker |
||||
from selfdrive.car import apply_std_steer_torque_limits |
||||
from selfdrive.car.gm import gmcan |
||||
from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, EV_CAR |
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||
NetworkLocation = car.CarParams.NetworkLocation |
||||
|
||||
|
||||
class CarController: |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.CP = CP |
||||
self.start_time = 0. |
||||
self.apply_steer_last = 0 |
||||
self.apply_gas = 0 |
||||
self.apply_brake = 0 |
||||
self.frame = 0 |
||||
|
||||
self.lka_steering_cmd_counter_last = -1 |
||||
self.lka_icon_status_last = (False, False) |
||||
|
||||
self.params = CarControllerParams() |
||||
|
||||
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt']) |
||||
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) |
||||
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) |
||||
|
||||
def update(self, CC, CS): |
||||
actuators = CC.actuators |
||||
hud_control = CC.hudControl |
||||
hud_alert = hud_control.visualAlert |
||||
hud_v_cruise = hud_control.setSpeed |
||||
if hud_v_cruise > 70: |
||||
hud_v_cruise = 0 |
||||
|
||||
# Send CAN commands. |
||||
can_sends = [] |
||||
|
||||
# Steering (50Hz) |
||||
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the |
||||
# next Panda loopback confirmation in the current CS frame. |
||||
if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: |
||||
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter |
||||
elif (self.frame % self.params.STEER_STEP) == 0: |
||||
lkas_enabled = CC.latActive and CS.out.vEgo > self.params.MIN_STEER_SPEED |
||||
if lkas_enabled: |
||||
new_steer = int(round(actuators.steer * self.params.STEER_MAX)) |
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) |
||||
else: |
||||
apply_steer = 0 |
||||
|
||||
self.apply_steer_last = apply_steer |
||||
# GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the |
||||
# moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. |
||||
idx = (CS.lka_steering_cmd_counter + 1) % 4 |
||||
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) |
||||
|
||||
if self.CP.openpilotLongitudinalControl: |
||||
# Gas/regen, brakes, and UI commands - all at 25Hz |
||||
if self.frame % 4 == 0: |
||||
if not CC.longActive: |
||||
# Stock ECU sends max regen when not enabled |
||||
self.apply_gas = self.params.MAX_ACC_REGEN |
||||
self.apply_brake = 0 |
||||
else: |
||||
if self.CP.carFingerprint in EV_CAR: |
||||
self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) |
||||
self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) |
||||
else: |
||||
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) |
||||
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) |
||||
|
||||
idx = (self.frame // 4) % 4 |
||||
|
||||
at_full_stop = CC.longActive and CS.out.standstill |
||||
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) |
||||
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation |
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) |
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) |
||||
|
||||
# Send dashboard UI commands (ACC status) |
||||
send_fcw = hud_alert == VisualAlert.fcw |
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, |
||||
hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) |
||||
|
||||
# Radar needs to know current speed and yaw rate (50hz), |
||||
# and that ADAS is alive (10hz) |
||||
if not self.CP.radarOffCan: |
||||
tt = self.frame * DT_CTRL |
||||
time_and_headlights_step = 10 |
||||
if self.frame % time_and_headlights_step == 0: |
||||
idx = (self.frame // time_and_headlights_step) % 4 |
||||
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) |
||||
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) |
||||
|
||||
speed_and_accelerometer_step = 2 |
||||
if self.frame % speed_and_accelerometer_step == 0: |
||||
idx = (self.frame // speed_and_accelerometer_step) % 4 |
||||
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) |
||||
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) |
||||
|
||||
if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: |
||||
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) |
||||
|
||||
# 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 self.CP.networkLocation != NetworkLocation.fwdCamera and (self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last): |
||||
steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) |
||||
can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) |
||||
self.lka_icon_status_last = lka_icon_status |
||||
|
||||
new_actuators = actuators.copy() |
||||
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX |
||||
new_actuators.gas = self.apply_gas |
||||
new_actuators.brake = self.apply_brake |
||||
|
||||
self.frame += 1 |
||||
return new_actuators, can_sends |
||||
@ -0,0 +1,148 @@ |
||||
from cereal import car |
||||
from common.numpy_fast import mean |
||||
from opendbc.can.can_define import CANDefine |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD |
||||
|
||||
TransmissionType = car.CarParams.TransmissionType |
||||
|
||||
|
||||
class CarState(CarStateBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) |
||||
self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] |
||||
self.lka_steering_cmd_counter = 0 |
||||
|
||||
def update(self, pt_cp, loopback_cp): |
||||
ret = car.CarState.new_message() |
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons |
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] |
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds( |
||||
pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], |
||||
pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"], |
||||
pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"], |
||||
pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"], |
||||
) |
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) |
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.standstill = ret.vEgoRaw < 0.01 |
||||
|
||||
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: |
||||
ret.gearShifter = self.parse_gear_shifter("T") |
||||
else: |
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) |
||||
|
||||
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. |
||||
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 |
||||
ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 |
||||
|
||||
# Regen braking is braking |
||||
if self.CP.transmissionType == TransmissionType.direct: |
||||
ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 |
||||
|
||||
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. |
||||
ret.gasPressed = ret.gas > 1e-5 |
||||
|
||||
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] |
||||
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"] |
||||
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"] |
||||
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"] |
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD |
||||
self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] |
||||
|
||||
# 0 inactive, 1 active, 2 temporarily limited, 3 failed |
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"] |
||||
ret.steerFaultTemporary = self.lkas_status == 2 |
||||
ret.steerFaultPermanent = self.lkas_status == 3 |
||||
|
||||
# 1 - open, 0 - closed |
||||
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or |
||||
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or |
||||
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or |
||||
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1) |
||||
|
||||
# 1 - latched |
||||
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0 |
||||
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 |
||||
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 |
||||
|
||||
ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1 |
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 |
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 |
||||
ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED |
||||
|
||||
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF |
||||
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("BrakePedalPosition", "EBCMBrakePedalPosition"), |
||||
("FrontLeftDoor", "BCMDoorBeltStatus"), |
||||
("FrontRightDoor", "BCMDoorBeltStatus"), |
||||
("RearLeftDoor", "BCMDoorBeltStatus"), |
||||
("RearRightDoor", "BCMDoorBeltStatus"), |
||||
("LeftSeatBelt", "BCMDoorBeltStatus"), |
||||
("RightSeatBelt", "BCMDoorBeltStatus"), |
||||
("TurnSignals", "BCMTurnSignals"), |
||||
("AcceleratorPedal2", "AcceleratorPedal2"), |
||||
("CruiseState", "AcceleratorPedal2"), |
||||
("ACCButtons", "ASCMSteeringButton"), |
||||
("SteeringWheelAngle", "PSCMSteeringAngle"), |
||||
("SteeringWheelRate", "PSCMSteeringAngle"), |
||||
("FLWheelSpd", "EBCMWheelSpdFront"), |
||||
("FRWheelSpd", "EBCMWheelSpdFront"), |
||||
("RLWheelSpd", "EBCMWheelSpdRear"), |
||||
("RRWheelSpd", "EBCMWheelSpdRear"), |
||||
("PRNDL2", "ECMPRDNL2"), |
||||
("ManualMode", "ECMPRDNL2"), |
||||
("LKADriverAppldTrq", "PSCMStatus"), |
||||
("LKATorqueDelivered", "PSCMStatus"), |
||||
("LKATorqueDeliveredStatus", "PSCMStatus"), |
||||
("TractionControlOn", "ESPStatus"), |
||||
("ParkBrake", "VehicleIgnitionAlt"), |
||||
("CruiseMainOn", "ECMEngineStatus"), |
||||
] |
||||
|
||||
checks = [ |
||||
("BCMTurnSignals", 1), |
||||
("ECMPRDNL2", 10), |
||||
("PSCMStatus", 10), |
||||
("ESPStatus", 10), |
||||
("BCMDoorBeltStatus", 10), |
||||
("VehicleIgnitionAlt", 10), |
||||
("EBCMWheelSpdFront", 20), |
||||
("EBCMWheelSpdRear", 20), |
||||
("AcceleratorPedal2", 33), |
||||
("ASCMSteeringButton", 33), |
||||
("ECMEngineStatus", 100), |
||||
("PSCMSteeringAngle", 100), |
||||
("EBCMBrakePedalPosition", 100), |
||||
] |
||||
|
||||
if CP.transmissionType == TransmissionType.direct: |
||||
signals.append(("RegenPaddle", "EBCMRegenPaddle")) |
||||
checks.append(("EBCMRegenPaddle", 50)) |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN) |
||||
|
||||
@staticmethod |
||||
def get_loopback_can_parser(CP): |
||||
signals = [ |
||||
("RollingCounter", "ASCMLKASteeringCmd"), |
||||
] |
||||
|
||||
checks = [ |
||||
("ASCMLKASteeringCmd", 10), # 10 Hz is the stock inactive rate (every 100ms). |
||||
# While active 50 Hz (every 20 ms) is normal |
||||
# EPS will tolerate around 200ms when active before faulting |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK) |
||||
@ -0,0 +1,125 @@ |
||||
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_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): |
||||
mode = 0x1 |
||||
if apply_brake > 0: |
||||
mode = 0xa |
||||
if at_full_stop: |
||||
mode = 0xd |
||||
|
||||
# TODO: this is to have GM bringing the car to complete stop, |
||||
# but currently it conflicts with OP controls, so turned off. |
||||
#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, fcw): |
||||
# 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, |
||||
"FCWAlert": 0x3 if fcw else 0 |
||||
} |
||||
|
||||
return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values) |
||||
|
||||
def create_adas_time_status(bus, tt, idx): |
||||
dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff, |
||||
((tt & 0xf) << 4) + (idx << 2)] |
||||
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] |
||||
chksum = chksum & 0xfff |
||||
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] |
||||
return make_can_msg(0xa1, bytes(dat), bus) |
||||
|
||||
def create_adas_steering_status(bus, idx): |
||||
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] |
||||
chksum = 0x60 + sum(dat) |
||||
dat += [chksum >> 8, chksum & 0xff] |
||||
return make_can_msg(0x306, bytes(dat), bus) |
||||
|
||||
def create_adas_accelerometer_speed_status(bus, speed_ms, idx): |
||||
spd = int(speed_ms * 16) & 0xfff |
||||
accel = 0 & 0xfff |
||||
# 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L |
||||
#stick = 0x08 |
||||
near_range_cutoff = 0x27 |
||||
near_range_mode = 1 if spd <= near_range_cutoff else 0 |
||||
far_range_mode = 1 - near_range_mode |
||||
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] |
||||
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] |
||||
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] |
||||
return make_can_msg(0x308, bytes(dat), bus) |
||||
|
||||
def create_adas_headlights_status(packer, bus): |
||||
values = { |
||||
"Always42": 0x42, |
||||
"Always4": 0x4, |
||||
} |
||||
return packer.make_can_msg("ASCMHeadlight", bus, values) |
||||
|
||||
def create_lka_icon_command(bus, active, critical, steer): |
||||
if active and steer == 1: |
||||
if critical: |
||||
dat = b"\x50\xc0\x14" |
||||
else: |
||||
dat = b"\x50\x40\x18" |
||||
elif active: |
||||
if critical: |
||||
dat = b"\x40\xc0\x14" |
||||
else: |
||||
dat = b"\x40\x40\x18" |
||||
else: |
||||
dat = b"\x00\x00\x00" |
||||
return make_can_msg(0x104c006c, dat, bus) |
||||
@ -0,0 +1,191 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from math import fabs |
||||
|
||||
from common.conversions import Conversions as CV |
||||
from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config |
||||
from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type |
||||
EventName = car.CarEvent.EventName |
||||
GearShifter = car.CarState.GearShifter |
||||
TransmissionType = car.CarParams.TransmissionType |
||||
NetworkLocation = car.CarParams.NetworkLocation |
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, |
||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
@staticmethod |
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed): |
||||
params = CarControllerParams() |
||||
return params.ACCEL_MIN, params.ACCEL_MAX |
||||
|
||||
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. |
||||
@staticmethod |
||||
def get_steer_feedforward_volt(desired_angle, v_ego): |
||||
desired_angle *= 0.02904609 |
||||
sigmoid = desired_angle / (1 + fabs(desired_angle)) |
||||
return 0.10006696 * sigmoid * (v_ego + 3.12485927) |
||||
|
||||
@staticmethod |
||||
def get_steer_feedforward_acadia(desired_angle, v_ego): |
||||
desired_angle *= 0.09760208 |
||||
sigmoid = desired_angle / (1 + fabs(desired_angle)) |
||||
return 0.04689655 * sigmoid * (v_ego + 10.028217) |
||||
|
||||
def get_steer_feedforward_function(self): |
||||
if self.CP.carFingerprint == CAR.VOLT: |
||||
return self.get_steer_feedforward_volt |
||||
elif self.CP.carFingerprint == CAR.ACADIA: |
||||
return self.get_steer_feedforward_acadia |
||||
else: |
||||
return CarInterfaceBase.get_steer_feedforward_default |
||||
|
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
ret.carName = "gm" |
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] |
||||
ret.pcmCruise = False # For ASCM, stock non-adaptive cruise control is kept off |
||||
ret.radarOffCan = False # For ASCM, radar exists |
||||
ret.transmissionType = TransmissionType.automatic |
||||
# NetworkLocation.gateway: OBD-II harness (typically ASCM), NetworkLocation.fwdCamera: non-ASCM |
||||
ret.networkLocation = NetworkLocation.gateway |
||||
|
||||
# These cars have been put into dashcam only due to both a lack of users and test coverage. |
||||
# These cars likely still work fine. Once a user confirms each car works and a test route is |
||||
# added to selfdrive/car/tests/routes.py, we can remove it from this list. |
||||
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} |
||||
|
||||
# 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.openpilotLongitudinalControl = True |
||||
tire_stiffness_factor = 0.444 # not optimized yet |
||||
|
||||
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. |
||||
ret.minSteerSpeed = 7 * CV.MPH_TO_MS |
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] |
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] |
||||
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 |
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet |
||||
|
||||
ret.longitudinalTuning.kpBP = [5., 35.] |
||||
ret.longitudinalTuning.kpV = [2.4, 1.5] |
||||
ret.longitudinalTuning.kiBP = [0.] |
||||
ret.longitudinalTuning.kiV = [0.36] |
||||
|
||||
ret.steerLimitTimer = 0.4 |
||||
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz |
||||
|
||||
# supports stop and go, but initial engage must (conservatively) be above 18mph |
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS |
||||
|
||||
if candidate == CAR.VOLT: |
||||
ret.transmissionType = TransmissionType.direct |
||||
ret.mass = 1607. + STD_CARGO_KG |
||||
ret.wheelbase = 2.69 |
||||
ret.steerRatio = 17.7 # Stock 15.7, LiveParameters |
||||
tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters |
||||
ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh |
||||
|
||||
ret.lateralTuning.pid.kpBP = [0., 40.] |
||||
ret.lateralTuning.pid.kpV = [0., 0.17] |
||||
ret.lateralTuning.pid.kiBP = [0.] |
||||
ret.lateralTuning.pid.kiV = [0.] |
||||
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() |
||||
ret.steerActuatorDelay = 0.2 |
||||
|
||||
elif candidate == CAR.MALIBU: |
||||
ret.mass = 1496. + STD_CARGO_KG |
||||
ret.wheelbase = 2.83 |
||||
ret.steerRatio = 15.8 |
||||
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.steerRatio = 15.7 |
||||
|
||||
elif candidate == CAR.ACADIA: |
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm |
||||
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG |
||||
ret.wheelbase = 2.86 |
||||
ret.steerRatio = 14.4 # end to end is 13.46 |
||||
ret.centerToFront = ret.wheelbase * 0.4 |
||||
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() |
||||
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking |
||||
|
||||
elif candidate == CAR.BUICK_REGAL: |
||||
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 |
||||
ret.wheelbase = 2.83 # 111.4 inches in meters |
||||
ret.steerRatio = 14.4 # guess for tourx |
||||
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx |
||||
|
||||
elif candidate == CAR.CADILLAC_ATS: |
||||
ret.mass = 1601. + STD_CARGO_KG |
||||
ret.wheelbase = 2.78 |
||||
ret.steerRatio = 15.3 |
||||
ret.centerToFront = ret.wheelbase * 0.49 |
||||
|
||||
elif candidate == CAR.ESCALADE_ESV: |
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm |
||||
ret.mass = 2739. + STD_CARGO_KG |
||||
ret.wheelbase = 3.302 |
||||
ret.steerRatio = 17.3 |
||||
ret.centerToFront = ret.wheelbase * 0.49 |
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] |
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] |
||||
ret.lateralTuning.pid.kf = 0.000045 |
||||
tire_stiffness_factor = 1.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): |
||||
ret = self.CS.update(self.cp, self.cp_loopback) |
||||
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret) |
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: |
||||
be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) |
||||
|
||||
# Suppress resume button if we're resuming from stop so we don't adjust speed. |
||||
if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): |
||||
be.type = ButtonType.unknown |
||||
|
||||
ret.buttonEvents = [be] |
||||
|
||||
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, |
||||
GearShifter.eco, GearShifter.manumatic], |
||||
pcm_enable=self.CP.pcmCruise) |
||||
events = self.dp_atl_warning(ret, events) |
||||
|
||||
if ret.vEgo < self.CP.minEnableSpeed: |
||||
events.add(EventName.belowEngageSpeed) |
||||
if ret.cruiseState.standstill: |
||||
events.add(EventName.resumeRequired) |
||||
if ret.vEgo < self.CP.minSteerSpeed: |
||||
events.add(car.CarEvent.EventName.belowSteerSpeed) |
||||
|
||||
# handle button presses |
||||
events.events.extend(create_button_enable_events(ret.buttonEvents, pcm_cruise=self.CP.pcmCruise)) |
||||
|
||||
ret.events = events.to_msg() |
||||
|
||||
return ret |
||||
|
||||
def apply(self, c): |
||||
return self.CC.update(c, self.CS) |
||||
@ -0,0 +1,103 @@ |
||||
#!/usr/bin/env python3 |
||||
import math |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.gm.values import DBC, CAR, CanBus |
||||
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(car_fingerprint): |
||||
if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV): |
||||
return None |
||||
|
||||
# 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)) |
||||
|
||||
checks = list({(s[1], 14) for s in signals}) |
||||
|
||||
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE) |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
|
||||
self.rcp = create_radar_can_parser(CP.carFingerprint) |
||||
|
||||
self.trigger_msg = LAST_RADAR_MSG |
||||
self.updated_messages = set() |
||||
self.radar_ts = CP.radarTimeStep |
||||
|
||||
def update(self, can_strings): |
||||
if self.rcp is None: |
||||
return super().update(None) |
||||
|
||||
vls = self.rcp.update_strings(can_strings) |
||||
self.updated_messages.update(vls) |
||||
|
||||
if self.trigger_msg not in self.updated_messages: |
||||
return None |
||||
|
||||
ret = car.RadarData.new_message() |
||||
header = self.rcp.vl[RADAR_HEADER_MSG] |
||||
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \ |
||||
header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \ |
||||
header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt'] |
||||
errors = [] |
||||
if not self.rcp.can_valid: |
||||
errors.append("canError") |
||||
if fault: |
||||
errors.append("fault") |
||||
ret.errors = errors |
||||
|
||||
currentTargets = set() |
||||
num_targets = header['FLRRNumValidTargets'] |
||||
|
||||
# Not all radar messages describe targets, |
||||
# no need to monitor all of the self.rcp.msgs_upd |
||||
for ii in self.updated_messages: |
||||
if ii == RADAR_HEADER_MSG: |
||||
continue |
||||
|
||||
if num_targets == 0: |
||||
break |
||||
|
||||
cpt = self.rcp.vl[ii] |
||||
# Zero distance means it's an empty target slot |
||||
if cpt['TrkRange'] > 0.0: |
||||
targetId = cpt['TrkObjectID'] |
||||
currentTargets.add(targetId) |
||||
if targetId not in self.pts: |
||||
self.pts[targetId] = car.RadarData.RadarPoint.new_message() |
||||
self.pts[targetId].trackId = targetId |
||||
distance = cpt['TrkRange'] |
||||
self.pts[targetId].dRel = distance # from front of car |
||||
# From driver's pov, left is positive |
||||
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance |
||||
self.pts[targetId].vRel = cpt['TrkRangeRate'] |
||||
self.pts[targetId].aRel = float('nan') |
||||
self.pts[targetId].yvRel = float('nan') |
||||
|
||||
for oldTarget in list(self.pts.keys()): |
||||
if oldTarget not in currentTargets: |
||||
del self.pts[oldTarget] |
||||
|
||||
ret.points = list(self.pts.values()) |
||||
self.updated_messages.clear() |
||||
return ret |
||||
@ -0,0 +1,160 @@ |
||||
from collections import defaultdict |
||||
from dataclasses import dataclass, field |
||||
from enum import Enum |
||||
from typing import Dict, List, Union |
||||
|
||||
from cereal import car |
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness |
||||
Ecu = car.CarParams.Ecu |
||||
|
||||
|
||||
class CarControllerParams: |
||||
STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output |
||||
STEER_STEP = 2 # Control frames per command (50hz) |
||||
STEER_DELTA_UP = 7 # Delta rates require review due to observed EPS weakness |
||||
STEER_DELTA_DOWN = 17 |
||||
MIN_STEER_SPEED = 3. # m/s |
||||
STEER_DRIVER_ALLOWANCE = 50 |
||||
STEER_DRIVER_MULTIPLIER = 4 |
||||
STEER_DRIVER_FACTOR = 100 |
||||
NEAR_STOP_BRAKE_PHASE = 0.5 # m/s |
||||
|
||||
# Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera" |
||||
ADAS_KEEPALIVE_STEP = 100 |
||||
CAMERA_KEEPALIVE_STEP = 100 |
||||
|
||||
# Volt gasbrake lookups |
||||
# TODO: These values should be confirmed on non-Volt vehicles |
||||
MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. |
||||
ZERO_GAS = 2048 # Coasting |
||||
MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen |
||||
MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen |
||||
|
||||
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we |
||||
# perform the closed loop control, and might need some |
||||
# to apply some more braking if we're on a downhill slope. |
||||
# Our controller should still keep the 2 second average above |
||||
# -3.5 m/s^2 as per planner limits |
||||
ACCEL_MAX = 2. # m/s^2 |
||||
ACCEL_MIN = -4. # m/s^2 |
||||
|
||||
EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX] |
||||
EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.] |
||||
|
||||
# ICE has much less engine braking force compared to regen in EVs, |
||||
# lower threshold removes some braking deadzone |
||||
GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX] |
||||
BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] |
||||
|
||||
GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] |
||||
BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] |
||||
|
||||
|
||||
class CAR: |
||||
HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" |
||||
VOLT = "CHEVROLET VOLT PREMIER 2017" |
||||
CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" |
||||
MALIBU = "CHEVROLET MALIBU PREMIER 2017" |
||||
ACADIA = "GMC ACADIA DENALI 2018" |
||||
BUICK_REGAL = "BUICK REGAL ESSENCE 2018" |
||||
ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" |
||||
|
||||
|
||||
EV_CAR = {CAR.VOLT} |
||||
STEER_THRESHOLD = 1.0 |
||||
|
||||
|
||||
class Footnote(Enum): |
||||
OBD_II = CarFootnote( |
||||
'Requires a <a href="https://github.com/commaai/openpilot/wiki/GM#hardware">community built ASCM harness</a>. ' + |
||||
'<b><i>NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).</i></b>', |
||||
Column.MODEL) |
||||
|
||||
|
||||
@dataclass |
||||
class GMCarInfo(CarInfo): |
||||
package: str = "Adaptive Cruise Control" |
||||
harness: Enum = Harness.obd_ii |
||||
footnotes: List[Enum] = field(default_factory=lambda: [Footnote.OBD_II]) |
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { |
||||
CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"), |
||||
CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0), |
||||
CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"), |
||||
CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"), |
||||
CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), |
||||
CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), |
||||
CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), |
||||
} |
||||
|
||||
|
||||
class CruiseButtons: |
||||
INIT = 0 |
||||
UNPRESS = 1 |
||||
RES_ACCEL = 2 |
||||
DECEL_SET = 3 |
||||
MAIN = 5 |
||||
CANCEL = 6 |
||||
|
||||
class AccState: |
||||
OFF = 0 |
||||
ACTIVE = 1 |
||||
FAULTED = 3 |
||||
STANDSTILL = 4 |
||||
|
||||
class CanBus: |
||||
POWERTRAIN = 0 |
||||
OBSTACLE = 1 |
||||
CHASSIS = 2 |
||||
SW_GMLAN = 3 |
||||
LOOPBACK = 128 |
||||
DROPPED = 192 |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.HOLDEN_ASTRA: [ |
||||
# Astra BK MY17, ASCM unplugged |
||||
{ |
||||
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.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, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7 |
||||
}, |
||||
# 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 |
||||
}], |
||||
CAR.ESCALADE_ESV: [ |
||||
{ |
||||
309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8 |
||||
}], |
||||
} |
||||
|
||||
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) |
||||
@ -0,0 +1,89 @@ |
||||
from cereal import car |
||||
from common.numpy_fast import clip, interp |
||||
from opendbc.can.packer import CANPacker |
||||
from selfdrive.car.nissan import nissancan |
||||
from selfdrive.car.nissan.values import CAR, CarControllerParams |
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||
|
||||
|
||||
class CarController: |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.CP = CP |
||||
self.car_fingerprint = CP.carFingerprint |
||||
self.frame = 0 |
||||
|
||||
self.lkas_max_torque = 0 |
||||
self.last_angle = 0 |
||||
|
||||
self.packer = CANPacker(dbc_name) |
||||
|
||||
def update(self, CC, CS): |
||||
actuators = CC.actuators |
||||
hud_control = CC.hudControl |
||||
pcm_cancel_cmd = CC.cruiseControl.cancel |
||||
|
||||
can_sends = [] |
||||
|
||||
### STEER ### |
||||
lkas_hud_msg = CS.lkas_hud_msg |
||||
lkas_hud_info_msg = CS.lkas_hud_info_msg |
||||
apply_angle = actuators.steeringAngleDeg |
||||
|
||||
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 |
||||
|
||||
if CC.latActive: |
||||
# windup slower |
||||
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): |
||||
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) |
||||
else: |
||||
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU) |
||||
|
||||
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) |
||||
|
||||
# Max torque from driver before EPS will give up and not apply torque |
||||
if not bool(CS.out.steeringPressed): |
||||
self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE |
||||
else: |
||||
# Scale max torque based on how much torque the driver is applying to the wheel |
||||
self.lkas_max_torque = max( |
||||
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum |
||||
CarControllerParams.LKAS_MAX_TORQUE * 0.5, |
||||
# Start scaling torque at STEER_THRESHOLD |
||||
CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD) |
||||
) |
||||
|
||||
else: |
||||
apply_angle = CS.out.steeringAngleDeg |
||||
self.lkas_max_torque = 0 |
||||
|
||||
self.last_angle = apply_angle |
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: |
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg)) |
||||
|
||||
# TODO: Find better way to cancel! |
||||
# For some reason spamming the cancel button is unreliable on the Leaf |
||||
# We now cancel by making propilot think the seatbelt is unlatched, |
||||
# this generates a beep and a warning message every time you disengage |
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0: |
||||
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) |
||||
|
||||
can_sends.append(nissancan.create_steering_control( |
||||
self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) |
||||
|
||||
if lkas_hud_msg and lkas_hud_info_msg: |
||||
if self.frame % 2 == 0: |
||||
can_sends.append(nissancan.create_lkas_hud_msg( |
||||
self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) |
||||
|
||||
if self.frame % 50 == 0: |
||||
can_sends.append(nissancan.create_lkas_hud_info_msg( |
||||
self.packer, lkas_hud_info_msg, steer_hud_alert |
||||
)) |
||||
|
||||
new_actuators = actuators.copy() |
||||
new_actuators.steeringAngleDeg = apply_angle |
||||
|
||||
self.frame += 1 |
||||
return new_actuators, can_sends |
||||
@ -0,0 +1,350 @@ |
||||
import copy |
||||
from collections import deque |
||||
from cereal import car |
||||
from opendbc.can.can_define import CANDefine |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from common.conversions import Conversions as CV |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams |
||||
|
||||
TORQUE_SAMPLES = 12 |
||||
|
||||
class CarState(CarStateBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) |
||||
|
||||
self.lkas_hud_msg = None |
||||
self.lkas_hud_info_msg = None |
||||
|
||||
self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) |
||||
self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] |
||||
|
||||
def update(self, cp, cp_adas, cp_cam): |
||||
ret = car.CarState.new_message() |
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): |
||||
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] |
||||
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): |
||||
ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] |
||||
|
||||
ret.gasPressed = bool(ret.gas > 3) |
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): |
||||
ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) |
||||
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): |
||||
ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"]) |
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds( |
||||
cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"], |
||||
cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"], |
||||
cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"], |
||||
cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"], |
||||
) |
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. |
||||
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.standstill = ret.vEgoRaw < 0.01 |
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA: |
||||
ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) |
||||
else: |
||||
ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) |
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): |
||||
ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 |
||||
ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) |
||||
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): |
||||
if self.CP.carFingerprint == CAR.LEAF: |
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 |
||||
elif self.CP.carFingerprint == CAR.LEAF_IC: |
||||
ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1 |
||||
ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) |
||||
elif self.CP.carFingerprint == CAR.ALTIMA: |
||||
ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 |
||||
ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"]) |
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA: |
||||
speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"] |
||||
else: |
||||
speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] |
||||
|
||||
if speed != 255: |
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): |
||||
conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS |
||||
else: |
||||
conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS |
||||
ret.cruiseState.speed = speed * conversion |
||||
ret.cruiseState.speedCluster = (speed - 1) * conversion # Speed on HUD is always 1 lower than actually sent on can bus |
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA: |
||||
ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] |
||||
else: |
||||
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] |
||||
|
||||
self.steeringTorqueSamples.append(ret.steeringTorque) |
||||
# Filtering driver torque to prevent steeringPressed false positives |
||||
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD) |
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] |
||||
|
||||
ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) |
||||
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) |
||||
|
||||
ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"], |
||||
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"], |
||||
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FR"], |
||||
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FL"]]) |
||||
|
||||
ret.espDisabled = bool(cp.vl["ESP"]["ESP_DISABLED"]) |
||||
|
||||
can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) |
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) |
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA: |
||||
self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) |
||||
else: |
||||
self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) |
||||
|
||||
self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) |
||||
|
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): |
||||
self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) |
||||
|
||||
if self.CP.carFingerprint != CAR.ALTIMA: |
||||
self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) |
||||
self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"), |
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"), |
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"), |
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"), |
||||
|
||||
("STEER_ANGLE", "STEER_ANGLE_SENSOR"), |
||||
|
||||
("DOOR_OPEN_FR", "DOORS_LIGHTS"), |
||||
("DOOR_OPEN_FL", "DOORS_LIGHTS"), |
||||
("DOOR_OPEN_RR", "DOORS_LIGHTS"), |
||||
("DOOR_OPEN_RL", "DOORS_LIGHTS"), |
||||
|
||||
("RIGHT_BLINKER", "LIGHTS"), |
||||
("LEFT_BLINKER", "LIGHTS"), |
||||
|
||||
("ESP_DISABLED", "ESP"), |
||||
|
||||
("GEAR_SHIFTER", "GEARBOX"), |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("STEER_ANGLE_SENSOR", 100), |
||||
("WHEEL_SPEEDS_REAR", 50), |
||||
("WHEEL_SPEEDS_FRONT", 50), |
||||
("ESP", 25), |
||||
("GEARBOX", 25), |
||||
("DOORS_LIGHTS", 10), |
||||
("LIGHTS", 10), |
||||
] |
||||
|
||||
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): |
||||
signals += [ |
||||
("USER_BRAKE_PRESSED", "DOORS_LIGHTS"), |
||||
|
||||
("GAS_PEDAL", "GAS_PEDAL"), |
||||
("SEATBELT_DRIVER_LATCHED", "HUD"), |
||||
("SPEED_MPH", "HUD"), |
||||
|
||||
("PROPILOT_BUTTON", "CRUISE_THROTTLE"), |
||||
("CANCEL_BUTTON", "CRUISE_THROTTLE"), |
||||
("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"), |
||||
("SET_BUTTON", "CRUISE_THROTTLE"), |
||||
("RES_BUTTON", "CRUISE_THROTTLE"), |
||||
("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"), |
||||
("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"), |
||||
("GAS_PEDAL", "CRUISE_THROTTLE"), |
||||
("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), |
||||
("NEW_SIGNAL_2", "CRUISE_THROTTLE"), |
||||
("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"), |
||||
("unsure1", "CRUISE_THROTTLE"), |
||||
("unsure2", "CRUISE_THROTTLE"), |
||||
("unsure3", "CRUISE_THROTTLE"), |
||||
] |
||||
|
||||
checks += [ |
||||
("GAS_PEDAL", 100), |
||||
("CRUISE_THROTTLE", 50), |
||||
("HUD", 25), |
||||
] |
||||
|
||||
elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): |
||||
signals += [ |
||||
("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), |
||||
("GAS_PEDAL", "CRUISE_THROTTLE"), |
||||
("CRUISE_AVAILABLE", "CRUISE_THROTTLE"), |
||||
("SPEED_MPH", "HUD_SETTINGS"), |
||||
("SEATBELT_DRIVER_LATCHED", "SEATBELT"), |
||||
|
||||
# Copy other values, we use this to cancel |
||||
("CANCEL_SEATBELT", "CANCEL_MSG"), |
||||
("NEW_SIGNAL_1", "CANCEL_MSG"), |
||||
("NEW_SIGNAL_2", "CANCEL_MSG"), |
||||
("NEW_SIGNAL_3", "CANCEL_MSG"), |
||||
] |
||||
checks += [ |
||||
("BRAKE_PEDAL", 100), |
||||
("CRUISE_THROTTLE", 50), |
||||
("CANCEL_MSG", 50), |
||||
("HUD_SETTINGS", 25), |
||||
("SEATBELT", 10), |
||||
] |
||||
|
||||
if CP.carFingerprint == CAR.ALTIMA: |
||||
signals += [ |
||||
("LKAS_ENABLED", "LKAS_SETTINGS"), |
||||
("CRUISE_ENABLED", "CRUISE_STATE"), |
||||
("SET_SPEED", "PROPILOT_HUD"), |
||||
] |
||||
checks += [ |
||||
("CRUISE_STATE", 10), |
||||
("LKAS_SETTINGS", 10), |
||||
("PROPILOT_HUD", 50), |
||||
] |
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) |
||||
|
||||
signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) |
||||
checks.append(("STEER_TORQUE_SENSOR", 100)) |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) |
||||
|
||||
@staticmethod |
||||
def get_adas_can_parser(CP): |
||||
# this function generates lists for signal, messages and initial values |
||||
|
||||
if CP.carFingerprint == CAR.ALTIMA: |
||||
signals = [ |
||||
("DESIRED_ANGLE", "LKAS"), |
||||
("SET_0x80_2", "LKAS"), |
||||
("MAX_TORQUE", "LKAS"), |
||||
("SET_0x80", "LKAS"), |
||||
("COUNTER", "LKAS"), |
||||
("LKA_ACTIVE", "LKAS"), |
||||
|
||||
("CRUISE_ON", "PRO_PILOT"), |
||||
] |
||||
checks = [ |
||||
("LKAS", 100), |
||||
("PRO_PILOT", 100), |
||||
] |
||||
else: |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("LKAS_ENABLED", "LKAS_SETTINGS"), |
||||
|
||||
("CRUISE_ENABLED", "CRUISE_STATE"), |
||||
|
||||
("DESIRED_ANGLE", "LKAS"), |
||||
("SET_0x80_2", "LKAS"), |
||||
("MAX_TORQUE", "LKAS"), |
||||
("SET_0x80", "LKAS"), |
||||
("COUNTER", "LKAS"), |
||||
("LKA_ACTIVE", "LKAS"), |
||||
|
||||
# Below are the HUD messages. We copy the stock message and modify |
||||
("LARGE_WARNING_FLASHING", "PROPILOT_HUD"), |
||||
("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"), |
||||
("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"), |
||||
("LEAD_CAR", "PROPILOT_HUD"), |
||||
("LEAD_CAR_ERROR", "PROPILOT_HUD"), |
||||
("FRONT_RADAR_ERROR", "PROPILOT_HUD"), |
||||
("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"), |
||||
("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"), |
||||
("LKAS_ERROR_FLASHING", "PROPILOT_HUD"), |
||||
("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"), |
||||
("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"), |
||||
("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"), |
||||
("FOLLOW_DISTANCE", "PROPILOT_HUD"), |
||||
("AUDIBLE_TONE", "PROPILOT_HUD"), |
||||
("SPEED_SET_ICON", "PROPILOT_HUD"), |
||||
("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"), |
||||
("unknown59", "PROPILOT_HUD"), |
||||
("unknown55", "PROPILOT_HUD"), |
||||
("unknown26", "PROPILOT_HUD"), |
||||
("unknown28", "PROPILOT_HUD"), |
||||
("unknown31", "PROPILOT_HUD"), |
||||
("SET_SPEED", "PROPILOT_HUD"), |
||||
("unknown43", "PROPILOT_HUD"), |
||||
("unknown08", "PROPILOT_HUD"), |
||||
("unknown05", "PROPILOT_HUD"), |
||||
("unknown02", "PROPILOT_HUD"), |
||||
|
||||
("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"), |
||||
("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"), |
||||
("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"), |
||||
("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"), |
||||
("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), |
||||
("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), |
||||
("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), |
||||
("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"), |
||||
("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"), |
||||
("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"), |
||||
("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"), |
||||
("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"), |
||||
("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), |
||||
("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), |
||||
("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), |
||||
("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown07", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown10", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown15", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown23", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown19", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown31", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown32", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown46", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown61", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown55", "PROPILOT_HUD_INFO_MSG"), |
||||
("unknown50", "PROPILOT_HUD_INFO_MSG"), |
||||
] |
||||
|
||||
checks = [ |
||||
("PROPILOT_HUD_INFO_MSG", 2), |
||||
("LKAS_SETTINGS", 10), |
||||
("CRUISE_STATE", 50), |
||||
("PROPILOT_HUD", 50), |
||||
("LKAS", 100), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) |
||||
|
||||
@staticmethod |
||||
def get_cam_can_parser(CP): |
||||
signals = [] |
||||
checks = [] |
||||
|
||||
if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): |
||||
signals.append(("CRUISE_ON", "PRO_PILOT")) |
||||
checks.append(("PRO_PILOT", 100)) |
||||
elif CP.carFingerprint == CAR.ALTIMA: |
||||
signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) |
||||
checks.append(("STEER_TORQUE_SENSOR", 100)) |
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) |
||||
@ -0,0 +1,73 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
from selfdrive.car.nissan.values import CAR |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
|
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
ret.carName = "nissan" |
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] |
||||
|
||||
ret.steerLimitTimer = 1.0 |
||||
|
||||
ret.steerActuatorDelay = 0.1 |
||||
|
||||
if candidate in (CAR.ROGUE, CAR.XTRAIL): |
||||
ret.mass = 1610 + STD_CARGO_KG |
||||
ret.wheelbase = 2.705 |
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
ret.steerRatio = 17 |
||||
elif candidate in (CAR.LEAF, CAR.LEAF_IC): |
||||
ret.mass = 1610 + STD_CARGO_KG |
||||
ret.wheelbase = 2.705 |
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
ret.steerRatio = 17 |
||||
elif candidate == CAR.ALTIMA: |
||||
# Altima has EPS on C-CAN unlike the others that have it on V-CAN |
||||
ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus |
||||
ret.mass = 1492 + STD_CARGO_KG |
||||
ret.wheelbase = 2.824 |
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
ret.steerRatio = 17 |
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||
ret.radarOffCan = True |
||||
|
||||
# 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): |
||||
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) |
||||
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret) |
||||
|
||||
buttonEvents = [] |
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = car.CarState.ButtonEvent.Type.accelCruise |
||||
buttonEvents.append(be) |
||||
|
||||
events = self.create_common_events(ret) |
||||
events = self.dp_atl_warning(ret, events) |
||||
|
||||
if self.CS.lkas_enabled: |
||||
events.add(car.CarEvent.EventName.invalidLkasSetting) |
||||
|
||||
ret.events = events.to_msg() |
||||
|
||||
return ret |
||||
|
||||
def apply(self, c): |
||||
return self.CC.update(c, self.CS) |
||||
@ -0,0 +1,70 @@ |
||||
import copy |
||||
import crcmod |
||||
from selfdrive.car.nissan.values import CAR |
||||
|
||||
# TODO: add this checksum to the CANPacker |
||||
nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) |
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque): |
||||
values = { |
||||
"COUNTER": frame % 0x10, |
||||
"DESIRED_ANGLE": apply_steer, |
||||
"SET_0x80_2": 0x80, |
||||
"SET_0x80": 0x80, |
||||
"MAX_TORQUE": lkas_max_torque if steer_on else 0, |
||||
"LKA_ACTIVE": steer_on, |
||||
} |
||||
|
||||
dat = packer.make_can_msg("LKAS", 0, values)[2] |
||||
|
||||
values["CHECKSUM"] = nissan_checksum(dat[:7]) |
||||
return packer.make_can_msg("LKAS", 0, values) |
||||
|
||||
|
||||
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg): |
||||
values = copy.copy(cruise_throttle_msg) |
||||
can_bus = 2 |
||||
|
||||
if car_fingerprint == CAR.ALTIMA: |
||||
can_bus = 1 |
||||
|
||||
values["CANCEL_BUTTON"] = 1 |
||||
values["NO_BUTTON_PRESSED"] = 0 |
||||
values["PROPILOT_BUTTON"] = 0 |
||||
values["SET_BUTTON"] = 0 |
||||
values["RES_BUTTON"] = 0 |
||||
values["FOLLOW_DISTANCE_BUTTON"] = 0 |
||||
|
||||
return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values) |
||||
|
||||
|
||||
def create_cancel_msg(packer, cancel_msg, cruise_cancel): |
||||
values = copy.copy(cancel_msg) |
||||
|
||||
if cruise_cancel: |
||||
values["CANCEL_SEATBELT"] = 1 |
||||
|
||||
return packer.make_can_msg("CANCEL_MSG", 2, values) |
||||
|
||||
|
||||
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart): |
||||
values = lkas_hud_msg |
||||
|
||||
values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0 |
||||
values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0 |
||||
|
||||
values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0 |
||||
values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0 |
||||
values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0 |
||||
|
||||
return packer.make_can_msg("PROPILOT_HUD", 0, values) |
||||
|
||||
|
||||
def create_lkas_hud_info_msg(packer, lkas_hud_info_msg, steer_hud_alert): |
||||
values = lkas_hud_info_msg |
||||
|
||||
if steer_hud_alert: |
||||
values["HANDS_ON_WHEEL_WARNING"] = 1 |
||||
|
||||
return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values) |
||||
@ -0,0 +1,5 @@ |
||||
#!/usr/bin/env python3 |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
pass |
||||
@ -0,0 +1,141 @@ |
||||
from dataclasses import dataclass |
||||
from typing import Dict, List, Optional, Union |
||||
from enum import Enum |
||||
|
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarInfo, Harness |
||||
from cereal import car |
||||
Ecu = car.CarParams.Ecu |
||||
|
||||
|
||||
class CarControllerParams: |
||||
ANGLE_DELTA_BP = [0., 5., 15.] |
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit |
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit |
||||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower |
||||
STEER_THRESHOLD = 1.0 |
||||
|
||||
|
||||
class CAR: |
||||
XTRAIL = "NISSAN X-TRAIL 2017" |
||||
LEAF = "NISSAN LEAF 2018" |
||||
# Leaf with ADAS ECU found behind instrument cluster instead of glovebox |
||||
# Currently the only known difference between them is the inverted seatbelt signal. |
||||
LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster" |
||||
ROGUE = "NISSAN ROGUE 2019" |
||||
ALTIMA = "NISSAN ALTIMA 2020" |
||||
|
||||
|
||||
@dataclass |
||||
class NissanCarInfo(CarInfo): |
||||
package: str = "ProPILOT Assist" |
||||
harness: Enum = Harness.nissan_a |
||||
|
||||
|
||||
CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { |
||||
CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), |
||||
CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22"), |
||||
CAR.LEAF_IC: None, # same platforms |
||||
CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), |
||||
CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b), |
||||
} |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.XTRAIL: [ |
||||
{ |
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837: 8, 2015: 8, 2016: 8, 2024: 8 |
||||
}, |
||||
{ |
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3, 1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8 |
||||
}, |
||||
], |
||||
CAR.LEAF: [ |
||||
{ |
||||
2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8 |
||||
}, |
||||
# 2020 Leaf SV Plus |
||||
{ |
||||
2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8 |
||||
}, |
||||
], |
||||
CAR.LEAF_IC: [ |
||||
{ |
||||
2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8 |
||||
}, |
||||
], |
||||
CAR.ROGUE: [ |
||||
{ |
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 |
||||
}, |
||||
], |
||||
CAR.ALTIMA: [ |
||||
{ |
||||
2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 438: 8, 451: 8, 517: 8, 520: 2, 522: 8, 523: 6, 539: 8, 541: 7, 542: 8, 543: 8, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 570: 8, 576: 8, 577: 8, 582: 8, 583: 8, 584: 8, 586: 8, 587: 8, 588: 8, 589: 8, 590: 8, 591: 8, 592: 8, 600: 8, 601: 8, 610: 8, 611: 8, 612: 8, 614: 8, 615: 8, 616: 8, 617: 8, 622: 8, 623: 8, 634: 7, 638: 8, 645: 8, 648: 5, 654: 6, 658: 8, 659: 8, 660: 8, 661: 8, 665: 8, 666: 8, 674: 2, 675: 8, 676: 8, 682: 8, 683: 8, 684: 8, 685: 8, 686: 8, 687: 8, 689: 8, 690: 8, 703: 8, 708: 7, 709: 7, 711: 7, 712: 7, 713: 7, 714: 8, 715: 8, 716: 8, 717: 7, 718: 7, 719: 7, 720: 7, 723: 8, 726: 7, 727: 7, 728: 7, 735: 8, 746: 8, 748: 6, 749: 6, 750: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 779: 7, 781: 7, 782: 7, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1144: 7, 1145: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1258: 8, 1259: 8, 1266: 8, 1273: 7, 1306: 1, 1314: 8, 1323: 8, 1324: 8, 1342: 1, 1376: 8, 1401: 8, 1454: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 |
||||
}, |
||||
] |
||||
} |
||||
|
||||
FW_VERSIONS = { |
||||
CAR.ALTIMA: { |
||||
(Ecu.fwdCamera, 0x707, None): [ |
||||
b'284N86CA1D', |
||||
], |
||||
(Ecu.eps, 0x742, None): [ |
||||
b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80', |
||||
], |
||||
(Ecu.engine, 0x7e0, None): [ |
||||
b'237109HE2B', |
||||
], |
||||
(Ecu.gateway, 0x18dad0f1, None): [ |
||||
b'284U29HE0A', |
||||
], |
||||
}, |
||||
CAR.LEAF_IC: { |
||||
(Ecu.fwdCamera, 0x707, None): [ |
||||
b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', |
||||
b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80', |
||||
], |
||||
(Ecu.esp, 0x740, None): [ |
||||
b'476605SH1D', |
||||
b'476605SK2A', |
||||
], |
||||
(Ecu.eps, 0x742, None): [ |
||||
b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80', |
||||
b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80', |
||||
], |
||||
(Ecu.gateway, 0x18dad0f1, None): [ |
||||
b'284U25SH3A', |
||||
b'284U25SK2D', |
||||
], |
||||
}, |
||||
CAR.XTRAIL: { |
||||
(Ecu.fwdCamera, 0x707, None): [ |
||||
b'284N86FR2A', |
||||
], |
||||
(Ecu.esp, 0x740, None): [ |
||||
b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80', |
||||
b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80', |
||||
], |
||||
(Ecu.eps, 0x742, None): [ |
||||
b'6FP2A\x99A\x05\x02N123F\x18\x02\x00\x00\x00\x00\x00\x00\x00\x80', |
||||
], |
||||
(Ecu.combinationMeter, 0x743, None): [ |
||||
b'6FR2A\x18B\x05\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', |
||||
], |
||||
(Ecu.engine, 0x7e0, None): [ |
||||
b'6FU9B\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', |
||||
b'6FR9A\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', |
||||
], |
||||
(Ecu.gateway, 0x18dad0f1, None): [ |
||||
b'284U26FR0E', |
||||
], |
||||
}, |
||||
} |
||||
|
||||
DBC = { |
||||
CAR.XTRAIL: dbc_dict('nissan_x_trail_2017', None), |
||||
CAR.LEAF: dbc_dict('nissan_leaf_2018', None), |
||||
CAR.LEAF_IC: dbc_dict('nissan_leaf_2018', None), |
||||
CAR.ROGUE: dbc_dict('nissan_x_trail_2017', None), |
||||
CAR.ALTIMA: dbc_dict('nissan_x_trail_2017', None), |
||||
} |
||||
@ -0,0 +1,69 @@ |
||||
from common.numpy_fast import clip, interp |
||||
from opendbc.can.packer import CANPacker |
||||
from selfdrive.car.tesla.teslacan import TeslaCAN |
||||
from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams |
||||
|
||||
|
||||
class CarController: |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.CP = CP |
||||
self.frame = 0 |
||||
self.last_angle = 0 |
||||
self.packer = CANPacker(dbc_name) |
||||
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) |
||||
self.tesla_can = TeslaCAN(self.packer, self.pt_packer) |
||||
|
||||
def update(self, CC, CS): |
||||
actuators = CC.actuators |
||||
pcm_cancel_cmd = CC.cruiseControl.cancel |
||||
|
||||
can_sends = [] |
||||
|
||||
# Temp disable steering on a hands_on_fault, and allow for user override |
||||
hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3 |
||||
lkas_enabled = CC.latActive and not hands_on_fault |
||||
|
||||
if lkas_enabled: |
||||
apply_angle = actuators.steeringAngleDeg |
||||
|
||||
# Angular rate limit based on speed |
||||
steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) |
||||
rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN |
||||
max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) |
||||
apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff) |
||||
|
||||
# To not fault the EPS |
||||
apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) |
||||
else: |
||||
apply_angle = CS.out.steeringAngleDeg |
||||
|
||||
self.last_angle = apply_angle |
||||
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) |
||||
|
||||
# Longitudinal control (in sync with stock message, about 40Hz) |
||||
if self.CP.openpilotLongitudinalControl: |
||||
target_accel = actuators.accel |
||||
target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0) |
||||
max_accel = 0 if target_accel < 0 else target_accel |
||||
min_accel = 0 if target_accel > 0 else target_accel |
||||
|
||||
while len(CS.das_control_counters) > 0: |
||||
can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft())) |
||||
|
||||
# Cancel on user steering override, since there is no steering torque blending |
||||
if hands_on_fault: |
||||
pcm_cancel_cmd = True |
||||
|
||||
if self.frame % 10 == 0 and pcm_cancel_cmd: |
||||
# Spam every possible counter value, otherwise it might not be accepted |
||||
for counter in range(16): |
||||
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter)) |
||||
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter)) |
||||
|
||||
# TODO: HUD control |
||||
|
||||
new_actuators = actuators.copy() |
||||
new_actuators.steeringAngleDeg = apply_angle |
||||
|
||||
self.frame += 1 |
||||
return new_actuators, can_sends |
||||
@ -0,0 +1,193 @@ |
||||
import copy |
||||
from collections import deque |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from opendbc.can.parser import CANParser |
||||
from opendbc.can.can_define import CANDefine |
||||
|
||||
class CarState(CarStateBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
self.button_states = {button.event_type: False for button in BUTTONS} |
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis']) |
||||
|
||||
# Needed by carcontroller |
||||
self.msg_stw_actn_req = None |
||||
self.hands_on_level = 0 |
||||
self.steer_warning = None |
||||
self.acc_state = 0 |
||||
self.das_control_counters = deque(maxlen=32) |
||||
|
||||
def update(self, cp, cp_cam): |
||||
ret = car.CarState.new_message() |
||||
|
||||
# Vehicle speed |
||||
ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS |
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.standstill = (ret.vEgo < 0.1) |
||||
|
||||
# Gas pedal |
||||
ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0 |
||||
ret.gasPressed = (ret.gas > 0) |
||||
|
||||
# Brake pedal |
||||
ret.brake = 0 |
||||
ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) |
||||
|
||||
# Steering wheel |
||||
self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"] |
||||
self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None) |
||||
steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None) |
||||
|
||||
ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"] |
||||
ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate |
||||
ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"] |
||||
ret.steeringPressed = (self.hands_on_level > 0) |
||||
ret.steerFaultPermanent = steer_status == "EAC_FAULT" |
||||
ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON")) |
||||
|
||||
# Cruise state |
||||
cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None) |
||||
speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None) |
||||
|
||||
acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")) |
||||
|
||||
ret.cruiseState.enabled = acc_enabled |
||||
if speed_units == "KPH": |
||||
ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS |
||||
elif speed_units == "MPH": |
||||
ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS |
||||
ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled) |
||||
ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special |
||||
|
||||
# Gear |
||||
ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")] |
||||
|
||||
# Buttons |
||||
buttonEvents = [] |
||||
for button in BUTTONS: |
||||
state = (cp.vl[button.can_addr][button.can_msg] in button.values) |
||||
if self.button_states[button.event_type] != state: |
||||
event = car.CarState.ButtonEvent.new_message() |
||||
event.type = button.event_type |
||||
event.pressed = state |
||||
buttonEvents.append(event) |
||||
self.button_states[button.event_type] = state |
||||
ret.buttonEvents = buttonEvents |
||||
|
||||
# Doors |
||||
ret.doorOpen = any([(self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS]) |
||||
|
||||
# Blinkers |
||||
ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1) |
||||
ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) |
||||
|
||||
# Seatbelt |
||||
ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) |
||||
|
||||
# TODO: blindspot |
||||
|
||||
# AEB |
||||
ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1) |
||||
|
||||
# Messages needed by carcontroller |
||||
self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"]) |
||||
self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"] |
||||
self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"]) |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("ESP_vehicleSpeed", "ESP_B"), |
||||
("DI_pedalPos", "DI_torque1"), |
||||
("DI_brakePedal", "DI_torque2"), |
||||
("StW_AnglHP", "STW_ANGLHP_STAT"), |
||||
("StW_AnglHP_Spd", "STW_ANGLHP_STAT"), |
||||
("EPAS_handsOnLevel", "EPAS_sysStatus"), |
||||
("EPAS_torsionBarTorque", "EPAS_sysStatus"), |
||||
("EPAS_internalSAS", "EPAS_sysStatus"), |
||||
("EPAS_eacStatus", "EPAS_sysStatus"), |
||||
("EPAS_eacErrorCode", "EPAS_sysStatus"), |
||||
("DI_cruiseState", "DI_state"), |
||||
("DI_digitalSpeed", "DI_state"), |
||||
("DI_speedUnits", "DI_state"), |
||||
("DI_gear", "DI_torque2"), |
||||
("DOOR_STATE_FL", "GTW_carState"), |
||||
("DOOR_STATE_FR", "GTW_carState"), |
||||
("DOOR_STATE_RL", "GTW_carState"), |
||||
("DOOR_STATE_RR", "GTW_carState"), |
||||
("DOOR_STATE_FrontTrunk", "GTW_carState"), |
||||
("BOOT_STATE", "GTW_carState"), |
||||
("BC_indicatorLStatus", "GTW_carState"), |
||||
("BC_indicatorRStatus", "GTW_carState"), |
||||
("SDM_bcklDrivStatus", "SDM1"), |
||||
("driverBrakeStatus", "BrakeMessage"), |
||||
|
||||
# We copy this whole message when spamming cancel |
||||
("SpdCtrlLvr_Stat", "STW_ACTN_RQ"), |
||||
("VSL_Enbl_Rq", "STW_ACTN_RQ"), |
||||
("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"), |
||||
("DTR_Dist_Rq", "STW_ACTN_RQ"), |
||||
("TurnIndLvr_Stat", "STW_ACTN_RQ"), |
||||
("HiBmLvr_Stat", "STW_ACTN_RQ"), |
||||
("WprWashSw_Psd", "STW_ACTN_RQ"), |
||||
("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"), |
||||
("StW_Lvr_Stat", "STW_ACTN_RQ"), |
||||
("StW_Cond_Flt", "STW_ACTN_RQ"), |
||||
("StW_Cond_Psd", "STW_ACTN_RQ"), |
||||
("HrnSw_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw00_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw01_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw02_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw03_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw04_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw05_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw06_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw07_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw08_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw09_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw10_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw11_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw12_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw13_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw14_Psd", "STW_ACTN_RQ"), |
||||
("StW_Sw15_Psd", "STW_ACTN_RQ"), |
||||
("WprSw6Posn", "STW_ACTN_RQ"), |
||||
("MC_STW_ACTN_RQ", "STW_ACTN_RQ"), |
||||
("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"), |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("ESP_B", 50), |
||||
("DI_torque1", 100), |
||||
("DI_torque2", 100), |
||||
("STW_ANGLHP_STAT", 100), |
||||
("EPAS_sysStatus", 25), |
||||
("DI_state", 10), |
||||
("STW_ACTN_RQ", 10), |
||||
("GTW_carState", 10), |
||||
("SDM1", 10), |
||||
("BrakeMessage", 50), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.chassis) |
||||
|
||||
@staticmethod |
||||
def get_cam_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("DAS_accState", "DAS_control"), |
||||
("DAS_aebEvent", "DAS_control"), |
||||
("DAS_controlCounter", "DAS_control"), |
||||
] |
||||
checks = [ |
||||
# sig_address, frequency |
||||
("DAS_control", 40), |
||||
] |
||||
return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot_chassis) |
||||
@ -0,0 +1,67 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from panda import Panda |
||||
from selfdrive.car.tesla.values import CANBUS, CAR |
||||
from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
ret.carName = "tesla" |
||||
|
||||
# There is no safe way to do steer blending with user torque, |
||||
# so the steering behaves like autopilot. This is not |
||||
# how openpilot should be, hence dashcamOnly |
||||
ret.dashcamOnly = True |
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||
|
||||
# Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command |
||||
ret.longitudinalTuning.kpBP = [0] |
||||
ret.longitudinalTuning.kpV = [0] |
||||
ret.longitudinalTuning.kiBP = [0] |
||||
ret.longitudinalTuning.kiV = [0] |
||||
ret.stopAccel = 0.0 |
||||
ret.longitudinalActuatorDelayUpperBound = 0.5 # s |
||||
ret.radarTimeStep = (1.0 / 8) # 8Hz |
||||
|
||||
# Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus |
||||
# If so, we assume that it is connected to the longitudinal harness. |
||||
if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()): |
||||
ret.openpilotLongitudinalControl = True |
||||
ret.safetyConfigs = [ |
||||
get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL), |
||||
get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN), |
||||
] |
||||
else: |
||||
ret.openpilotLongitudinalControl = False |
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] |
||||
|
||||
ret.steerLimitTimer = 1.0 |
||||
ret.steerActuatorDelay = 0.25 |
||||
|
||||
if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): |
||||
ret.mass = 2100. + STD_CARGO_KG |
||||
ret.wheelbase = 2.959 |
||||
ret.centerToFront = ret.wheelbase * 0.5 |
||||
ret.steerRatio = 15.0 |
||||
else: |
||||
raise ValueError(f"Unsupported car: {candidate}") |
||||
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) |
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) |
||||
|
||||
return ret |
||||
|
||||
def _update(self, c): |
||||
ret = self.CS.update(self.cp, self.cp_cam) |
||||
|
||||
ret.events = self.create_common_events(ret).to_msg() |
||||
|
||||
return ret |
||||
|
||||
def apply(self, c): |
||||
return self.CC.update(c, self.CS) |
||||
@ -0,0 +1,111 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.tesla.values import DBC, CANBUS |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
|
||||
RADAR_MSGS_A = list(range(0x310, 0x36E, 3)) |
||||
RADAR_MSGS_B = list(range(0x311, 0x36F, 3)) |
||||
NUM_POINTS = len(RADAR_MSGS_A) |
||||
|
||||
def get_radar_can_parser(CP): |
||||
# Status messages |
||||
signals = [ |
||||
('RADC_HWFail', 'TeslaRadarSguInfo'), |
||||
('RADC_SGUFail', 'TeslaRadarSguInfo'), |
||||
('RADC_SensorDirty', 'TeslaRadarSguInfo'), |
||||
] |
||||
|
||||
checks = [ |
||||
('TeslaRadarSguInfo', 10), |
||||
] |
||||
|
||||
# Radar tracks. There are also raw point clouds available, |
||||
# we don't use those. |
||||
for i in range(NUM_POINTS): |
||||
msg_id_a = RADAR_MSGS_A[i] |
||||
msg_id_b = RADAR_MSGS_B[i] |
||||
|
||||
# There is a bunch more info in the messages, |
||||
# but these are the only things actually used in openpilot |
||||
signals.extend([ |
||||
('LongDist', msg_id_a), |
||||
('LongSpeed', msg_id_a), |
||||
('LatDist', msg_id_a), |
||||
('LongAccel', msg_id_a), |
||||
('Meas', msg_id_a), |
||||
('Tracked', msg_id_a), |
||||
('Index', msg_id_a), |
||||
|
||||
('LatSpeed', msg_id_b), |
||||
('Index2', msg_id_b), |
||||
]) |
||||
|
||||
checks.extend([ |
||||
(msg_id_a, 8), |
||||
(msg_id_b, 8), |
||||
]) |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, CANBUS.radar) |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
self.rcp = get_radar_can_parser(CP) |
||||
self.updated_messages = set() |
||||
self.track_id = 0 |
||||
self.trigger_msg = RADAR_MSGS_B[-1] |
||||
|
||||
def update(self, can_strings): |
||||
if self.rcp is None: |
||||
return super().update(None) |
||||
|
||||
values = self.rcp.update_strings(can_strings) |
||||
self.updated_messages.update(values) |
||||
|
||||
if self.trigger_msg not in self.updated_messages: |
||||
return None |
||||
|
||||
ret = car.RadarData.new_message() |
||||
|
||||
# Errors |
||||
errors = [] |
||||
sgu_info = self.rcp.vl['TeslaRadarSguInfo'] |
||||
if not self.rcp.can_valid: |
||||
errors.append('canError') |
||||
if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']: |
||||
errors.append('fault') |
||||
ret.errors = errors |
||||
|
||||
# Radar tracks |
||||
for i in range(NUM_POINTS): |
||||
msg_a = self.rcp.vl[RADAR_MSGS_A[i]] |
||||
msg_b = self.rcp.vl[RADAR_MSGS_B[i]] |
||||
|
||||
# Make sure msg A and B are together |
||||
if msg_a['Index'] != msg_b['Index2']: |
||||
continue |
||||
|
||||
# Check if it's a valid track |
||||
if not msg_a['Tracked']: |
||||
if i in self.pts: |
||||
del self.pts[i] |
||||
continue |
||||
|
||||
# New track! |
||||
if i not in self.pts: |
||||
self.pts[i] = car.RadarData.RadarPoint.new_message() |
||||
self.pts[i].trackId = self.track_id |
||||
self.track_id += 1 |
||||
|
||||
# Parse track data |
||||
self.pts[i].dRel = msg_a['LongDist'] |
||||
self.pts[i].yRel = msg_a['LatDist'] |
||||
self.pts[i].vRel = msg_a['LongSpeed'] |
||||
self.pts[i].aRel = msg_a['LongAccel'] |
||||
self.pts[i].yvRel = msg_b['LatSpeed'] |
||||
self.pts[i].measured = bool(msg_a['Meas']) |
||||
|
||||
ret.points = list(self.pts.values()) |
||||
self.updated_messages.clear() |
||||
return ret |
||||
@ -0,0 +1,62 @@ |
||||
import copy |
||||
import crcmod |
||||
|
||||
from common.conversions import Conversions as CV |
||||
from selfdrive.car.tesla.values import CANBUS, CarControllerParams |
||||
|
||||
|
||||
class TeslaCAN: |
||||
def __init__(self, packer, pt_packer): |
||||
self.packer = packer |
||||
self.pt_packer = pt_packer |
||||
self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) |
||||
|
||||
@staticmethod |
||||
def checksum(msg_id, dat): |
||||
# TODO: get message ID from name instead |
||||
ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) |
||||
ret += sum(dat) |
||||
return ret & 0xFF |
||||
|
||||
def create_steering_control(self, angle, enabled, frame): |
||||
values = { |
||||
"DAS_steeringAngleRequest": -angle, |
||||
"DAS_steeringHapticRequest": 0, |
||||
"DAS_steeringControlType": 1 if enabled else 0, |
||||
"DAS_steeringControlCounter": (frame % 16), |
||||
} |
||||
|
||||
data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2] |
||||
values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3]) |
||||
return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values) |
||||
|
||||
def create_action_request(self, msg_stw_actn_req, cancel, bus, counter): |
||||
values = copy.copy(msg_stw_actn_req) |
||||
|
||||
if cancel: |
||||
values["SpdCtrlLvr_Stat"] = 1 |
||||
values["MC_STW_ACTN_RQ"] = counter |
||||
|
||||
data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2] |
||||
values["CRC_STW_ACTN_RQ"] = self.crc(data[:7]) |
||||
return self.packer.make_can_msg("STW_ACTN_RQ", bus, values) |
||||
|
||||
def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt): |
||||
messages = [] |
||||
values = { |
||||
"DAS_setSpeed": speed * CV.MS_TO_KPH, |
||||
"DAS_accState": acc_state, |
||||
"DAS_aebEvent": 0, |
||||
"DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN, |
||||
"DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX, |
||||
"DAS_accelMin": min_accel, |
||||
"DAS_accelMax": max_accel, |
||||
"DAS_controlCounter": cnt, |
||||
"DAS_controlChecksum": 0, |
||||
} |
||||
|
||||
for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]: |
||||
data = packer.make_can_msg("DAS_control", bus, values)[2] |
||||
values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7]) |
||||
messages.append(packer.make_can_msg("DAS_control", bus, values)) |
||||
return messages |
||||
@ -0,0 +1,77 @@ |
||||
from collections import namedtuple |
||||
from typing import Dict, List, Union |
||||
|
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarInfo |
||||
from cereal import car |
||||
|
||||
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) |
||||
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) |
||||
|
||||
|
||||
class CAR: |
||||
AP1_MODELS = 'TESLA AP1 MODEL S' |
||||
AP2_MODELS = 'TESLA AP2 MODEL S' |
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { |
||||
CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"), |
||||
CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"), |
||||
} |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.AP2_MODELS: [ |
||||
{ |
||||
1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2015: 8, 2043: 5, 2045: 4 |
||||
}, |
||||
], |
||||
CAR.AP1_MODELS: [ |
||||
{ |
||||
1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4 |
||||
}, |
||||
], |
||||
} |
||||
|
||||
DBC = { |
||||
CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), |
||||
CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), |
||||
} |
||||
|
||||
class CANBUS: |
||||
# Lateral harness |
||||
chassis = 0 |
||||
radar = 1 |
||||
autopilot_chassis = 2 |
||||
|
||||
# Longitudinal harness |
||||
powertrain = 4 |
||||
private = 5 |
||||
autopilot_powertrain = 6 |
||||
|
||||
GEAR_MAP = { |
||||
"DI_GEAR_INVALID": car.CarState.GearShifter.unknown, |
||||
"DI_GEAR_P": car.CarState.GearShifter.park, |
||||
"DI_GEAR_R": car.CarState.GearShifter.reverse, |
||||
"DI_GEAR_N": car.CarState.GearShifter.neutral, |
||||
"DI_GEAR_D": car.CarState.GearShifter.drive, |
||||
"DI_GEAR_SNA": car.CarState.GearShifter.unknown, |
||||
} |
||||
|
||||
DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"] |
||||
|
||||
# Make sure the message and addr is also in the CAN parser! |
||||
BUTTONS = [ |
||||
Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]), |
||||
Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]), |
||||
Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]), |
||||
Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]), |
||||
Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]), |
||||
Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), |
||||
] |
||||
|
||||
class CarControllerParams: |
||||
RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) |
||||
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) |
||||
JERK_LIMIT_MAX = 8 |
||||
JERK_LIMIT_MIN = -8 |
||||
ACCEL_TO_SPEED_MULTIPLIER = 3 |
||||
@ -0,0 +1,75 @@ |
||||
#!/usr/bin/env python3 |
||||
import math |
||||
import unittest |
||||
import importlib |
||||
from parameterized import parameterized |
||||
|
||||
from cereal import car |
||||
from selfdrive.car.fingerprints import all_known_cars |
||||
from selfdrive.car.car_helpers import interfaces |
||||
from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS |
||||
|
||||
class TestCarInterfaces(unittest.TestCase): |
||||
|
||||
@parameterized.expand([(car,) for car in all_known_cars()]) |
||||
def test_car_interfaces(self, car_name): |
||||
if car_name in FINGERPRINTS: |
||||
fingerprint = FINGERPRINTS[car_name][0] |
||||
else: |
||||
fingerprint = {} |
||||
|
||||
CarInterface, CarController, CarState = interfaces[car_name] |
||||
fingerprints = { |
||||
0: fingerprint, |
||||
1: fingerprint, |
||||
2: fingerprint, |
||||
} |
||||
|
||||
car_fw = [] |
||||
|
||||
car_params = CarInterface.get_params(car_name, fingerprints, car_fw) |
||||
car_interface = CarInterface(car_params, CarController, CarState) |
||||
assert car_params |
||||
assert car_interface |
||||
|
||||
self.assertGreater(car_params.mass, 1) |
||||
self.assertGreater(car_params.maxLateralAccel, 0) |
||||
|
||||
if car_params.steerControlType != car.CarParams.SteerControlType.angle: |
||||
tuning = car_params.lateralTuning.which() |
||||
if tuning == 'pid': |
||||
self.assertTrue(len(car_params.lateralTuning.pid.kpV)) |
||||
elif tuning == 'torque': |
||||
kf = car_params.lateralTuning.torque.kf |
||||
self.assertTrue(not math.isnan(kf) and kf > 0) |
||||
self.assertTrue(not math.isnan(car_params.lateralTuning.torque.friction)) |
||||
elif tuning == 'indi': |
||||
self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) |
||||
|
||||
# Run car interface |
||||
CC = car.CarControl.new_message() |
||||
for _ in range(10): |
||||
car_interface.update(CC, []) |
||||
car_interface.apply(CC) |
||||
car_interface.apply(CC) |
||||
|
||||
CC = car.CarControl.new_message() |
||||
CC.enabled = True |
||||
for _ in range(10): |
||||
car_interface.update(CC, []) |
||||
car_interface.apply(CC) |
||||
car_interface.apply(CC) |
||||
|
||||
# Test radar interface |
||||
RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface |
||||
radar_interface = RadarInterface(car_params) |
||||
assert radar_interface |
||||
|
||||
# Run radar interface once |
||||
radar_interface.update([]) |
||||
if not car_params.radarOffCan and radar_interface.rcp is not None and \ |
||||
hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): |
||||
radar_interface._update([radar_interface.trigger_msg]) |
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue