Vehicle Researcher
7 years ago
parent
4173219960
commit
e07d32c790
32 changed files with 978 additions and 163 deletions
@ -1,4 +1,26 @@ |
||||
# functions common among cars |
||||
from common.numpy_fast import clip |
||||
|
||||
|
||||
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): |
||||
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} |
||||
|
||||
|
||||
def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): |
||||
|
||||
# limits due to driver torque |
||||
driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER |
||||
driver_min_torque = -LIMITS.STEER_MAX + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER |
||||
max_steer_allowed = max(min(LIMITS.STEER_MAX, driver_max_torque), 0) |
||||
min_steer_allowed = min(max(-LIMITS.STEER_MAX, driver_min_torque), 0) |
||||
apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed) |
||||
|
||||
# slow rate if steer torque increases in magnitude |
||||
if apply_torque_last > 0: |
||||
apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP), |
||||
apply_torque_last + LIMITS.STEER_DELTA_UP) |
||||
else: |
||||
apply_torque = clip(apply_torque, apply_torque_last - LIMITS.STEER_DELTA_UP, |
||||
min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) |
||||
|
||||
return int(round(apply_torque)) |
||||
|
||||
@ -0,0 +1,76 @@ |
||||
from selfdrive.car import apply_std_steer_torque_limits |
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp |
||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \ |
||||
create_1191, create_1156, \ |
||||
create_clu11 |
||||
from selfdrive.car.hyundai.values import Buttons |
||||
from selfdrive.can.packer import CANPacker |
||||
|
||||
|
||||
# Steer torque limits |
||||
|
||||
class SteerLimitParams: |
||||
STEER_MAX = 250 # 409 is the max |
||||
STEER_DELTA_UP = 3 |
||||
STEER_DELTA_DOWN = 7 |
||||
STEER_DRIVER_ALLOWANCE = 50 |
||||
STEER_DRIVER_MULTIPLIER = 2 |
||||
STEER_DRIVER_FACTOR = 1 |
||||
|
||||
class CarController(object): |
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera): |
||||
self.apply_steer_last = 0 |
||||
self.car_fingerprint = car_fingerprint |
||||
self.lkas11_cnt = 0 |
||||
self.cnt = 0 |
||||
self.last_resume_cnt = 0 |
||||
self.enable_camera = enable_camera |
||||
# True when giraffe switch 2 is low and we need to replace all the camera messages |
||||
# otherwise we forward the camera msgs and we just replace the lkas cmd signals |
||||
self.camera_disconnected = False |
||||
|
||||
self.packer = CANPacker(dbc_name) |
||||
|
||||
def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): |
||||
|
||||
if not self.enable_camera: |
||||
return |
||||
|
||||
### Steering Torque |
||||
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX |
||||
|
||||
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) |
||||
|
||||
if not enabled: |
||||
apply_steer = 0 |
||||
|
||||
steer_req = 1 if enabled else 0 |
||||
|
||||
self.apply_steer_last = apply_steer |
||||
|
||||
can_sends = [] |
||||
|
||||
self.lkas11_cnt = self.cnt % 0x10 |
||||
self.clu11_cnt = self.cnt % 0x10 |
||||
|
||||
if self.camera_disconnected: |
||||
if (self.cnt % 10) == 0: |
||||
can_sends.append(create_lkas12()) |
||||
if (self.cnt % 50) == 0: |
||||
can_sends.append(create_1191()) |
||||
if (self.cnt % 7) == 0: |
||||
can_sends.append(create_1156()) |
||||
|
||||
can_sends.append(create_lkas11(self.packer, apply_steer, steer_req, self.lkas11_cnt, |
||||
enabled, CS.lkas11, hud_alert, keep_stock=(not self.camera_disconnected))) |
||||
|
||||
if pcm_cancel_cmd: |
||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) |
||||
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: |
||||
self.last_resume_cnt = self.cnt |
||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) |
||||
|
||||
### Send messages to canbus |
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) |
||||
|
||||
self.cnt += 1 |
||||
@ -0,0 +1,220 @@ |
||||
from selfdrive.car.hyundai.values import DBC |
||||
from selfdrive.can.parser import CANParser |
||||
from selfdrive.config import Conversions as CV |
||||
from common.kalman.simple_kalman import KF1D |
||||
import numpy as np |
||||
|
||||
|
||||
def get_can_parser(CP): |
||||
|
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("WHL_SPD_FL", "WHL_SPD11", 0), |
||||
("WHL_SPD_FR", "WHL_SPD11", 0), |
||||
("WHL_SPD_RL", "WHL_SPD11", 0), |
||||
("WHL_SPD_RR", "WHL_SPD11", 0), |
||||
|
||||
("YAW_RATE", "ESP12", 0), |
||||
|
||||
("CF_Gway_DrvSeatBeltInd", "CGW4", 1), |
||||
("CF_Gway_DrvSeatBeltSw", "CGW1", 0), |
||||
("CF_Gway_TSigLHSw", "CGW1", 0), |
||||
("CF_Gway_TurnSigLh", "CGW1", 0), |
||||
("CF_Gway_TSigRHSw", "CGW1", 0), |
||||
("CF_Gway_TurnSigRh", "CGW1", 0), |
||||
|
||||
("BRAKE_ACT", "EMS12", 0), |
||||
("PV_AV_CAN", "EMS12", 0), |
||||
("TPS", "EMS12", 0), |
||||
|
||||
("CYL_PRES", "ESP12", 0), |
||||
|
||||
("CF_Clu_CruiseSwState", "CLU11", 0), |
||||
("CF_Clu_CruiseSwMain" , "CLU11", 0), |
||||
("CF_Clu_SldMainSW", "CLU11", 0), |
||||
("CF_Clu_ParityBit1", "CLU11", 0), |
||||
("CF_Clu_VanzDecimal" , "CLU11", 0), |
||||
("CF_Clu_Vanz", "CLU11", 0), |
||||
("CF_Clu_SPEED_UNIT", "CLU11", 0), |
||||
("CF_Clu_DetentOut", "CLU11", 0), |
||||
("CF_Clu_RheostatLevel", "CLU11", 0), |
||||
("CF_Clu_CluInfo", "CLU11", 0), |
||||
("CF_Clu_AmpInfo", "CLU11", 0), |
||||
("CF_Clu_AliveCnt1", "CLU11", 0), |
||||
|
||||
("CF_Lvr_Gear","LVR12",0), |
||||
|
||||
("ACCEnable", "TCS13", 0), |
||||
("ACC_REQ", "TCS13", 0), |
||||
("DriverBraking", "TCS13", 0), |
||||
("DriverOverride", "TCS13", 0), |
||||
|
||||
("ESC_Off_Step", "TCS15", 0), |
||||
|
||||
("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) |
||||
|
||||
("CR_Mdps_DrvTq", "MDPS11", 0), |
||||
|
||||
("CR_Mdps_StrColTq", "MDPS12", 0), |
||||
("CF_Mdps_ToiActive", "MDPS12", 0), |
||||
("CF_Mdps_ToiUnavail", "MDPS12", 0), |
||||
("CF_Mdps_FailStat", "MDPS12", 0), |
||||
("CR_Mdps_OutTq", "MDPS12", 0), |
||||
|
||||
("VSetDis", "SCC11", 0), |
||||
("SCCInfoDisplay", "SCC11", 0), |
||||
("ACCMode", "SCC12", 1), |
||||
|
||||
("SAS_Angle", "SAS11", 0), |
||||
("SAS_Speed", "SAS11", 0), |
||||
] |
||||
|
||||
checks = [ |
||||
# address, frequency |
||||
("MDPS12", 50), |
||||
("MDPS11", 100), |
||||
("TCS15", 10), |
||||
("TCS13", 50), |
||||
("CLU11", 50), |
||||
("ESP12", 100), |
||||
("EMS12", 100), |
||||
("CGW1", 10), |
||||
("CGW4", 5), |
||||
("WHL_SPD11", 50), |
||||
("SCC11", 50), |
||||
("SCC12", 50), |
||||
("SAS11", 100) |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) |
||||
|
||||
def get_camera_parser(CP): |
||||
|
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("CF_Lkas_LdwsSysState", "LKAS11", 0), |
||||
("CF_Lkas_SysWarning", "LKAS11", 0), |
||||
("CF_Lkas_LdwsLHWarning", "LKAS11", 0), |
||||
("CF_Lkas_LdwsRHWarning", "LKAS11", 0), |
||||
("CF_Lkas_HbaLamp", "LKAS11", 0), |
||||
("CF_Lkas_FcwBasReq", "LKAS11", 0), |
||||
("CF_Lkas_ToiFlt", "LKAS11", 0), |
||||
("CF_Lkas_HbaSysState", "LKAS11", 0), |
||||
("CF_Lkas_FcwOpt", "LKAS11", 0), |
||||
("CF_Lkas_HbaOpt", "LKAS11", 0), |
||||
("CF_Lkas_FcwSysState", "LKAS11", 0), |
||||
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), |
||||
("CF_Lkas_FusionState", "LKAS11", 0), |
||||
("CF_Lkas_FcwOpt_USM", "LKAS11", 0), |
||||
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0) |
||||
] |
||||
|
||||
checks = [] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) |
||||
|
||||
class CarState(object): |
||||
def __init__(self, CP): |
||||
|
||||
self.CP = CP |
||||
|
||||
# initialize can parser |
||||
self.car_fingerprint = CP.carFingerprint |
||||
|
||||
# vEgo kalman filter |
||||
dt = 0.01 |
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) |
||||
# R = 1e3 |
||||
self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), |
||||
A=np.matrix([[1.0, dt], [0.0, 1.0]]), |
||||
C=np.matrix([1.0, 0.0]), |
||||
K=np.matrix([[0.12287673], [0.29666309]])) |
||||
self.v_ego = 0.0 |
||||
self.left_blinker_on = 0 |
||||
self.left_blinker_flash = 0 |
||||
self.right_blinker_on = 0 |
||||
self.right_blinker_flash = 0 |
||||
|
||||
def update(self, cp, cp_cam): |
||||
# copy can_valid |
||||
self.can_valid = cp.can_valid |
||||
|
||||
# update prevs, update must run once per Loop |
||||
self.prev_left_blinker_on = self.left_blinker_on |
||||
self.prev_right_blinker_on = self.right_blinker_on |
||||
|
||||
self.door_all_closed = True |
||||
self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] |
||||
|
||||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] |
||||
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step'] |
||||
|
||||
self.park_brake = False |
||||
self.main_on = True |
||||
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 |
||||
self.pcm_acc_status = int(self.acc_active) |
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners |
||||
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS |
||||
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS |
||||
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS |
||||
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS |
||||
self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. |
||||
|
||||
self.low_speed_lockout = self.v_wheel < 1.0 |
||||
|
||||
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default |
||||
if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed |
||||
self.v_ego_x = np.matrix([[self.v_wheel], [0.0]]) |
||||
|
||||
self.v_ego_raw = self.v_wheel |
||||
v_ego_x = self.v_ego_kf.update(self.v_wheel) |
||||
self.v_ego = float(v_ego_x[0]) |
||||
self.a_ego = float(v_ego_x[1]) |
||||
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) |
||||
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS |
||||
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv |
||||
self.standstill = not self.v_wheel > 0.1 |
||||
|
||||
self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] |
||||
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] |
||||
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] |
||||
self.main_on = True |
||||
self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] |
||||
self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh'] |
||||
self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] |
||||
self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh'] |
||||
self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > 100. |
||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE |
||||
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] |
||||
self.brake_error = 0 |
||||
self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] |
||||
self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq'] |
||||
self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. |
||||
|
||||
self.user_brake = 0 |
||||
|
||||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] |
||||
self.brake_lights = bool(self.brake_pressed) |
||||
if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1): |
||||
self.pedal_gas = 0 |
||||
else: |
||||
self.pedal_gas = cp.vl["EMS12"]['TPS'] |
||||
self.car_gas = cp.vl["EMS12"]['TPS'] |
||||
|
||||
# Gear Selecton - This should be compatible with all Kia/Hyundai with Auto's |
||||
gear = cp.vl["LVR12"]["CF_Lvr_Gear"] |
||||
if gear == 5: |
||||
self.gear_shifter = "drive" |
||||
elif gear == 6: |
||||
self.gear_shifter = "neutral" |
||||
elif gear == 0: |
||||
self.gear_shifter = "park" |
||||
elif gear == 7: |
||||
self.gear_shifter = "reverse" |
||||
else: |
||||
self.gear_shifter = "unknown" |
||||
|
||||
# save the entire LKAS11 and CLU11 |
||||
self.lkas11 = cp_cam.vl["LKAS11"] |
||||
self.clu11 = cp.vl["CLU11"] |
||||
@ -0,0 +1,67 @@ |
||||
import crcmod |
||||
|
||||
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) |
||||
|
||||
def make_can_msg(addr, dat, alt): |
||||
return [addr, 0, dat, alt] |
||||
|
||||
def create_lkas11(packer, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False): |
||||
values = { |
||||
"CF_Lkas_Icon": 3 if enabled else 0, |
||||
"CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"] if keep_stock else 1, |
||||
"CF_Lkas_SysWarning": hud_alert, |
||||
"CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0, |
||||
"CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"] if keep_stock else 0, |
||||
"CF_Lkas_HbaLamp": lkas11["CF_Lkas_HbaLamp"] if keep_stock else 0, |
||||
"CF_Lkas_FcwBasReq": lkas11["CF_Lkas_FcwBasReq"] if keep_stock else 0, |
||||
"CR_Lkas_StrToqReq": apply_steer, |
||||
"CF_Lkas_ActToi": steer_req, |
||||
"CF_Lkas_ToiFlt": lkas11["CF_Lkas_ToiFlt"] if keep_stock else 0, |
||||
"CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"] if keep_stock else 1, |
||||
"CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"] if keep_stock else 0, |
||||
"CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"] if keep_stock else 3, |
||||
"CF_Lkas_MsgCount": cnt, |
||||
"CF_Lkas_FcwSysState": lkas11["CF_Lkas_FcwSysState"] if keep_stock else 0, |
||||
"CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"] if keep_stock else 0, |
||||
"CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"] if keep_stock else 0, |
||||
"CF_Lkas_Chksum": 0, |
||||
"CF_Lkas_FcwOpt_USM": 2 if enabled else 1, |
||||
"CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"] if keep_stock else 3, |
||||
} |
||||
|
||||
dat = packer.make_can_msg("LKAS11", 0, values)[2] |
||||
dat = dat[:6] + dat[7] |
||||
checksum = hyundai_checksum(dat) |
||||
|
||||
values["CF_Lkas_Chksum"] = checksum |
||||
|
||||
return packer.make_can_msg("LKAS11", 0, values) |
||||
|
||||
def create_lkas12(): |
||||
return make_can_msg(1342, "\x00\x00\x00\x00\x60\x05", 0) |
||||
|
||||
|
||||
def create_1191(): |
||||
return make_can_msg(1191, "\x01\x00", 0) |
||||
|
||||
|
||||
def create_1156(): |
||||
return make_can_msg(1156, "\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) |
||||
|
||||
def create_clu11(packer, clu11, button): |
||||
values = { |
||||
"CF_Clu_CruiseSwState": button, |
||||
"CF_Clu_CruiseSwMain": clu11["CF_Clu_CruiseSwMain"], |
||||
"CF_Clu_SldMainSW": clu11["CF_Clu_SldMainSW"], |
||||
"CF_Clu_ParityBit1": clu11["CF_Clu_ParityBit1"], |
||||
"CF_Clu_VanzDecimal": clu11["CF_Clu_VanzDecimal"], |
||||
"CF_Clu_Vanz": clu11["CF_Clu_Vanz"], |
||||
"CF_Clu_SPEED_UNIT": clu11["CF_Clu_SPEED_UNIT"], |
||||
"CF_Clu_DetentOut": clu11["CF_Clu_DetentOut"], |
||||
"CF_Clu_RheostatLevel": clu11["CF_Clu_RheostatLevel"], |
||||
"CF_Clu_CluInfo": clu11["CF_Clu_CluInfo"], |
||||
"CF_Clu_AmpInfo": clu11["CF_Clu_AmpInfo"], |
||||
"CF_Clu_AliveCnt1": 0, |
||||
} |
||||
|
||||
return packer.make_can_msg("CLU11", 0, values) |
||||
@ -0,0 +1,259 @@ |
||||
#!/usr/bin/env python |
||||
from cereal import car, log |
||||
from common.realtime import sec_since_boot |
||||
from selfdrive.config import Conversions as CV |
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
||||
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser |
||||
from selfdrive.car.hyundai.values import CAMERA_MSGS, get_hud_alerts |
||||
|
||||
try: |
||||
from selfdrive.car.hyundai.carcontroller import CarController |
||||
except ImportError: |
||||
CarController = None |
||||
|
||||
|
||||
class CarInterface(object): |
||||
def __init__(self, CP, sendcan=None): |
||||
self.CP = CP |
||||
self.VM = VehicleModel(CP) |
||||
self.idx = 0 |
||||
self.lanes = 0 |
||||
self.lkas_request = 0 |
||||
|
||||
self.gas_pressed_prev = False |
||||
self.brake_pressed_prev = False |
||||
self.can_invalid_count = 0 |
||||
self.cruise_enabled_prev = False |
||||
|
||||
# *** init the major players *** |
||||
self.CS = CarState(CP) |
||||
self.cp = get_can_parser(CP) |
||||
self.cp_cam = get_camera_parser(CP) |
||||
|
||||
# sending if read only is False |
||||
if sendcan is not None: |
||||
self.sendcan = sendcan |
||||
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera) |
||||
|
||||
@staticmethod |
||||
def compute_gb(accel, speed): |
||||
return float(accel) / 3.0 |
||||
|
||||
@staticmethod |
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target): |
||||
return 1.0 |
||||
|
||||
@staticmethod |
||||
def get_params(candidate, fingerprint): |
||||
|
||||
# kg of standard extra cargo to count for drive, gas, etc... |
||||
std_cargo = 136 |
||||
|
||||
ret = car.CarParams.new_message() |
||||
|
||||
ret.carName = "hyundai" |
||||
ret.carFingerprint = candidate |
||||
ret.radarOffCan = True |
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModels.hyundai |
||||
|
||||
ret.enableCruise = True # stock acc |
||||
|
||||
# FIXME: hardcoding honda civic 2016 touring params so they can be used to |
||||
# scale unknown params for other cars |
||||
mass_civic = 2923 * CV.LB_TO_KG + std_cargo |
||||
wheelbase_civic = 2.70 |
||||
centerToFront_civic = wheelbase_civic * 0.4 |
||||
centerToRear_civic = wheelbase_civic - centerToFront_civic |
||||
rotationalInertia_civic = 2500 |
||||
tireStiffnessFront_civic = 192150 |
||||
tireStiffnessRear_civic = 202500 |
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, Prius has larger delay |
||||
|
||||
#borrowing a lot from corolla, given similar car size |
||||
ret.steerKf = 0.00005 # full torque for 20 deg at 80mph means 0.00007818594 |
||||
ret.steerRateCost = 0.5 |
||||
ret.mass = 3982 * CV.LB_TO_KG + std_cargo |
||||
ret.wheelbase = 2.766 |
||||
ret.steerRatio = 13.8 * 1.15 # 15% higher at the center seems reasonable |
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] |
||||
ret.steerKpV, ret.steerKiV = [[0.37], [0.1]] |
||||
ret.longitudinalKpBP = [0.] |
||||
ret.longitudinalKpV = [0.] |
||||
ret.longitudinalKiBP = [0.] |
||||
ret.longitudinalKiV = [0.] |
||||
tire_stiffness_factor = 1. |
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.4 |
||||
|
||||
# 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. |
||||
|
||||
centerToRear = ret.wheelbase - ret.centerToFront |
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for |
||||
# civic and scaling by mass and wheelbase |
||||
ret.rotationalInertia = rotationalInertia_civic * \ |
||||
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) |
||||
|
||||
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by |
||||
# mass and CG position, so all cars will have approximately similar dyn behaviors |
||||
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ |
||||
ret.mass / mass_civic * \ |
||||
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) |
||||
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ |
||||
ret.mass / mass_civic * \ |
||||
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) |
||||
|
||||
|
||||
# no rear steering, at least on the listed cars above |
||||
ret.steerRatioRear = 0. |
||||
ret.steerControlType = car.CarParams.SteerControlType.torque |
||||
|
||||
# steer, gas, brake limitations VS speed |
||||
ret.steerMaxBP = [0.] |
||||
ret.steerMaxV = [1.0] |
||||
ret.gasMaxBP = [0.] |
||||
ret.gasMaxV = [1.] |
||||
ret.brakeMaxBP = [0.] |
||||
ret.brakeMaxV = [1.] |
||||
ret.longPidDeadzoneBP = [0.] |
||||
ret.longPidDeadzoneV = [0.] |
||||
|
||||
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) |
||||
|
||||
ret.steerLimitAlert = False |
||||
ret.stoppingControl = False |
||||
ret.startAccel = 0.0 |
||||
|
||||
return ret |
||||
|
||||
# returns a car.CarState |
||||
def update(self, c): |
||||
# ******************* do can recv ******************* |
||||
canMonoTimes = [] |
||||
self.cp.update(int(sec_since_boot() * 1e9), False) |
||||
self.cp_cam.update(int(sec_since_boot() * 1e9), False) |
||||
self.CS.update(self.cp, self.cp_cam) |
||||
# create message |
||||
ret = car.CarState.new_message() |
||||
# speeds |
||||
ret.vEgo = self.CS.v_ego |
||||
ret.vEgoRaw = self.CS.v_ego_raw |
||||
ret.aEgo = self.CS.a_ego |
||||
ret.yawRate = self.CS.yaw_rate |
||||
ret.standstill = self.CS.standstill |
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl |
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr |
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl |
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr |
||||
|
||||
# gear shifter |
||||
ret.gearShifter = self.CS.gear_shifter |
||||
|
||||
# gas pedal |
||||
ret.gas = self.CS.car_gas |
||||
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading |
||||
|
||||
# brake pedal |
||||
ret.brake = self.CS.user_brake |
||||
ret.brakePressed = self.CS.brake_pressed != 0 |
||||
ret.brakeLights = self.CS.brake_lights |
||||
|
||||
# steering wheel |
||||
ret.steeringAngle = self.CS.angle_steers |
||||
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned |
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver |
||||
ret.steeringPressed = self.CS.steer_override |
||||
|
||||
# cruise state |
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 |
||||
if self.CS.pcm_acc_status != 0: |
||||
ret.cruiseState.speed = self.CS.cruise_set_speed |
||||
else: |
||||
ret.cruiseState.speed = 0 |
||||
ret.cruiseState.available = bool(self.CS.main_on) |
||||
ret.cruiseState.standstill = False |
||||
|
||||
# TODO: button presses |
||||
buttonEvents = [] |
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: |
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = 'leftBlinker' |
||||
be.pressed = self.CS.left_blinker_on != 0 |
||||
buttonEvents.append(be) |
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: |
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = 'rightBlinker' |
||||
be.pressed = self.CS.right_blinker_on != 0 |
||||
buttonEvents.append(be) |
||||
|
||||
ret.buttonEvents = buttonEvents |
||||
ret.leftBlinker = bool(self.CS.left_blinker_on) |
||||
ret.rightBlinker = bool(self.CS.right_blinker_on) |
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed |
||||
ret.seatbeltUnlatched = not self.CS.seatbelt |
||||
|
||||
#ret.genericToggle = self.CS.generic_toggle |
||||
|
||||
# events |
||||
events = [] |
||||
if not self.CS.can_valid: |
||||
self.can_invalid_count += 1 |
||||
if self.can_invalid_count >= 5: |
||||
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) |
||||
else: |
||||
self.can_invalid_count = 0 |
||||
if not ret.gearShifter == 'drive': |
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if ret.doorOpen: |
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if ret.seatbeltUnlatched: |
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if self.CS.esp_disabled: |
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if not self.CS.main_on: |
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) |
||||
if ret.gearShifter == 'reverse': |
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) |
||||
if self.CS.steer_error: |
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) |
||||
|
||||
# enable request in prius is simple, as we activate when Toyota is active (rising edge) |
||||
if ret.cruiseState.enabled and not self.cruise_enabled_prev: |
||||
events.append(create_event('pcmEnable', [ET.ENABLE])) |
||||
elif not ret.cruiseState.enabled: |
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE])) |
||||
|
||||
# disable on pedals rising edge or when brake is pressed and speed isn't zero |
||||
if (ret.gasPressed and not self.gas_pressed_prev) or \ |
||||
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)): |
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) |
||||
|
||||
if ret.gasPressed: |
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) |
||||
|
||||
ret.events = events |
||||
ret.canMonoTimes = canMonoTimes |
||||
|
||||
self.gas_pressed_prev = ret.gasPressed |
||||
self.brake_pressed_prev = ret.brakePressed |
||||
self.cruise_enabled_prev = ret.cruiseState.enabled |
||||
|
||||
return ret.as_reader() |
||||
|
||||
def apply(self, c, perception_state=log.Live20Data.new_message()): |
||||
|
||||
hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert) |
||||
|
||||
self.CC.update(self.sendcan, c.enabled, self.CS, c.actuators, |
||||
c.cruiseControl.cancel, hud_alert) |
||||
|
||||
return False |
||||
@ -0,0 +1,24 @@ |
||||
#!/usr/bin/env python |
||||
from cereal import car |
||||
import time |
||||
|
||||
|
||||
class RadarInterface(object): |
||||
def __init__(self, CP): |
||||
# radar |
||||
self.pts = {} |
||||
self.delay = 0.1 |
||||
|
||||
def update(self): |
||||
|
||||
ret = car.RadarState.new_message() |
||||
time.sleep(0.05) # radard runs on RI updates |
||||
|
||||
return ret |
||||
|
||||
if __name__ == "__main__": |
||||
RI = RadarInterface(None) |
||||
while 1: |
||||
ret = RI.update() |
||||
print(chr(27) + "[2J") |
||||
print ret |
||||
@ -0,0 +1,29 @@ |
||||
from selfdrive.car import dbc_dict |
||||
|
||||
def get_hud_alerts(visual_alert, audble_alert): |
||||
if visual_alert == "steerRequired": |
||||
return 4 if audble_alert != "none" else 5 |
||||
else: |
||||
return 0 |
||||
|
||||
class CAR: |
||||
SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" |
||||
|
||||
class Buttons: |
||||
NONE = 0 |
||||
RES_ACCEL = 1 |
||||
SET_DECEL = 2 |
||||
CANCEL = 4 |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.SANTA_FE: [{ |
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 |
||||
}], |
||||
} |
||||
|
||||
CAMERA_MSGS = [832, 1156, 1191, 1342] # msgs sent by the camera |
||||
|
||||
DBC = { |
||||
CAR.SANTA_FE: dbc_dict('hyundai_santa_fe_2019_ccan', None), |
||||
} |
||||
|
||||
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.5.2-release" |
||||
#define COMMA_VERSION "0.5.3-release" |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:53a9238e3f5c8906cc0d4bbff573040d65a28010a6fc3ac0273030836ec123e7 |
||||
size 1626840 |
||||
oid sha256:c53f1d6f255068830c7d5a71388f5ccaa1c3d9e44ea5b0ad647f6a91b1c59bc3 |
||||
size 1630952 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:7fbdea9a1cf92975aa4b7506e93dde1e9b8d87f6c2426f98aa7d8831726fb94e |
||||
oid sha256:1a568f1797a6bbf71e6d5d3173a0d502805960d0d82ce36ed34465460b2640d8 |
||||
size 1171544 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:35db92ee0ee547a9bdf8260893abed58cd079ca43d3978a18a16def2a6b66aab |
||||
oid sha256:057df31401bf1f324fbdf6aa4cfd06a3b3129df01f59fc342739555d8a53d737 |
||||
size 1159016 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:5f1db707acfb0ef06a15433692d8214695b97ec66ef36a63f74d6b16095a8e35 |
||||
size 8775896 |
||||
oid sha256:a05fa759a09e211f2dddafb7172b0b0e988ffeaa34e66c0a5bbf17d58e2e01c8 |
||||
size 8800616 |
||||
|
||||
Loading…
Reference in new issue