Vehicle Researcher
7 years ago
parent
8f6e36f426
commit
285c52eb69
32 changed files with 972 additions and 157 deletions
@ -1,4 +1,26 @@ |
|||||||
# functions common among cars |
# functions common among cars |
||||||
|
from common.numpy_fast import clip |
||||||
|
|
||||||
|
|
||||||
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): |
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): |
||||||
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} |
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" |
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue