Kia Sorento and future Multi-Car Support (#346)
* hyundai WIP * steer_driver_factor is 1 * removed unnecessary file * removed unnecessary code * Update carcontroller.py bug fix * safety tuning and fixed interface stiffness * better lateral tuning, some fixes * Fix set speed * added camera state reading, autoresume from stop, cancel on accel, hud alerts * WIP * Updated for Kia Sorento *WIP* * Cleanup * clean2 * Bug Fixes * pre-merge * Add all the cars! * Panda to auto-detect Camera Bus * Move Checksum Check * Final Sorento Tuning * Make CAN3 for Cam default * Update README.md * update panda, minor aesthetic updates * few other minor changes * added steer not allowed alert * bup panda version to force panda update * fixed camera alertspull/359/head
Andrew Frahn
rbiasini
parent
d2b8c4f0cb
commit
b3654cbcc9
|
|
@ -67,6 +67,10 @@ Supported Cars
|
|||
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Hyundai<sup>6</sup>| Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Hyundai<sup>6</sup>| Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph |
|
||||
| Hyundai<sup>6</sup>| Genesis 2018 | All | Yes | Stock | 19mph | 34mph |
|
||||
| Kia<sup>6</sup> | Sorento 2018 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Kia<sup>6</sup> | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Camry 2018<sup>4</sup>| All | Yes | Stock | 0mph<sup>5</sup> | 0mph |
|
||||
|
|
@ -113,7 +117,7 @@ In Progress Cars
|
|||
- 'All-Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the GS, GSH, F, RX, RXH, LX, NX, NXH, LC, LCH, LS, LSH have this option.
|
||||
- Even though the LX have TSS-P, it does not have Steering Assist and is not supported.
|
||||
- All Hyundai with SmartSense.
|
||||
- All Kia with ACC and LKAS.
|
||||
- All Kia with SCC and LKAS.
|
||||
|
||||
How can I add support for my car?
|
||||
------
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
|||
preDriverUnresponsive @43;
|
||||
promptDriverUnresponsive @44;
|
||||
driverUnresponsive @45;
|
||||
belowSteerSpeed @46;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -284,6 +285,7 @@ struct CarParams {
|
|||
enableApgs @28 :Bool; # advanced parking guidance system
|
||||
|
||||
minEnableSpeed @17 :Float32;
|
||||
minSteerSpeed @49 :Float32;
|
||||
safetyModel @18 :Int16;
|
||||
safetyParam @41 :Int16;
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
v1.1.3
|
||||
v1.1.4
|
||||
|
|
@ -22,7 +22,7 @@ const int GM_MAX_BRAKE = 350;
|
|||
int gm_brake_prev = 0;
|
||||
int gm_gas_prev = 0;
|
||||
int gm_speed = 0;
|
||||
// silence everything if stock ECUs are still online
|
||||
// silence everything if stock car control ECUs are still online
|
||||
int gm_ascm_detected = 0;
|
||||
int gm_ignition_started = 0;
|
||||
int gm_rt_torque_last = 0;
|
||||
|
|
@ -63,8 +63,11 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
gm_speed = to_push->RDLR & 0xFFFF;
|
||||
}
|
||||
|
||||
// check if stock ASCM ECU is still online
|
||||
if (bus_number == 0 && addr == 715) {
|
||||
// Check if ASCM or LKA camera are online
|
||||
// on powertrain bus.
|
||||
// 384 = ASCMLKASteeringCmd
|
||||
// 715 = ASCMGasRegenCmd
|
||||
if (bus_number == 0 && (addr == 384 || addr == 715)) {
|
||||
gm_ascm_detected = 1;
|
||||
controls_allowed = 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@ const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
|
|||
const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
|
||||
|
||||
int hyundai_camera_detected = 0;
|
||||
int hyundai_camera_bus = 0;
|
||||
int hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high?
|
||||
int hyundai_rt_torque_last = 0;
|
||||
int hyundai_desired_torque_last = 0;
|
||||
|
|
@ -39,6 +40,11 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
controls_allowed = 0;
|
||||
}
|
||||
|
||||
// Find out which bus the camera is on
|
||||
if (addr == 832) {
|
||||
hyundai_camera_bus = bus;
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if ((to_push->RIR>>21) == 1057) {
|
||||
// 2 bits: 13-14
|
||||
|
|
@ -51,8 +57,8 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
hyundai_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// 832 is lkas cmd. If it is on bus 2, then giraffe switch 2 is high
|
||||
if ((to_push->RIR>>21) == 832 && (bus == 2)) {
|
||||
// 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high
|
||||
if ((to_push->RIR>>21) == 832 && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
|
||||
hyundai_giraffe_switch_2 = 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -123,21 +129,22 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
// FORCE CANCEL: safety check only relevant when spamming the cancel button.
|
||||
// ensuring that only the cancel button press is sent (VAL 4) when controls are off.
|
||||
// This avoids unintended engagements while still allowing resume spam
|
||||
if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
|
||||
if ((to_send->RDLR & 0x7) != 4) return 0;
|
||||
}
|
||||
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
|
||||
//if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
|
||||
// if ((to_send->RDLR & 0x7) != 4) return 0;
|
||||
//}
|
||||
|
||||
// 1 allows the message through
|
||||
return true;
|
||||
}
|
||||
|
||||
static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
|
||||
// forward cam to ccan and viceversa, except lkas cmd
|
||||
if ((bus_num == 0 || bus_num == 2) && hyundai_giraffe_switch_2) {
|
||||
int addr = to_fwd->RIR>>21;
|
||||
bool is_lkas_msg = addr == 832 && bus_num == 2;
|
||||
return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2);
|
||||
if ((bus_num == 0 || bus_num == hyundai_camera_bus) && hyundai_giraffe_switch_2) {
|
||||
|
||||
if ((to_fwd->RIR>>21) == 832 && bus_num == hyundai_camera_bus) return -1;
|
||||
if (bus_num == 0) return hyundai_camera_bus;
|
||||
if (bus_num == hyundai_camera_bus) return 0;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -168,18 +168,18 @@ class TestHyundaiSafety(unittest.TestCase):
|
|||
self.assertTrue(self.safety.hyundai_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
RESUME_BTN = 1
|
||||
SET_BTN = 2
|
||||
CANCEL_BTN = 4
|
||||
BUTTON_MSG = 1265
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(CANCEL_BTN)))
|
||||
self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(SET_BTN)))
|
||||
# do not block resume if we are engaged already
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
#def test_spam_cancel_safety_check(self):
|
||||
# RESUME_BTN = 1
|
||||
# SET_BTN = 2
|
||||
# CANCEL_BTN = 4
|
||||
# BUTTON_MSG = 1265
|
||||
# self.safety.set_controls_allowed(0)
|
||||
# self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(CANCEL_BTN)))
|
||||
# self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
# self.assertFalse(self.safety.hyundai_tx_hook(self._button_msg(SET_BTN)))
|
||||
# # do not block resume if we are engaged already
|
||||
# self.safety.set_controls_allowed(1)
|
||||
# self.assertTrue(self.safety.hyundai_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ class CarController(object):
|
|||
if (self.cnt % 7) == 0:
|
||||
can_sends.append(create_1156())
|
||||
|
||||
can_sends.append(create_lkas11(self.packer, apply_steer, steer_req, self.lkas11_cnt,
|
||||
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
|
||||
enabled, CS.lkas11, hud_alert, keep_stock=(not self.camera_disconnected)))
|
||||
|
||||
if pcm_cancel_cmd:
|
||||
|
|
|
|||
|
|
@ -17,11 +17,13 @@ def get_can_parser(CP):
|
|||
("YAW_RATE", "ESP12", 0),
|
||||
|
||||
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
|
||||
|
||||
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
|
||||
("CF_Gway_TSigLHSw", "CGW1", 0),
|
||||
("CF_Gway_TurnSigLh", "CGW1", 0),
|
||||
("CF_Gway_TSigRHSw", "CGW1", 0),
|
||||
("CF_Gway_TurnSigRh", "CGW1", 0),
|
||||
("CF_Gway_ParkBrakeSw", "CGW1", 0),
|
||||
|
||||
("BRAKE_ACT", "EMS12", 0),
|
||||
("PV_AV_CAN", "EMS12", 0),
|
||||
|
|
@ -149,7 +151,7 @@ class CarState(object):
|
|||
self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
|
||||
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
|
||||
|
||||
self.park_brake = False
|
||||
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
|
||||
self.main_on = True
|
||||
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
|
|
|
|||
|
|
@ -1,14 +1,15 @@
|
|||
import crcmod
|
||||
from selfdrive.car.hyundai.values import CHECKSUM
|
||||
|
||||
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):
|
||||
def create_lkas11(packer, car_fingerprint, 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_LdwsSysState": 3 if steer_req else 1,
|
||||
"CF_Lkas_SysWarning": hud_alert,
|
||||
"CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,
|
||||
"CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"] if keep_stock else 0,
|
||||
|
|
@ -16,7 +17,7 @@ def create_lkas11(packer, apply_steer, steer_req, cnt, enabled, lkas11, hud_aler
|
|||
"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_ToiFlt": 0,
|
||||
"CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"] if keep_stock else 1,
|
||||
"CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"] if keep_stock else 0,
|
||||
"CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"] if keep_stock else 3,
|
||||
|
|
@ -30,8 +31,19 @@ def create_lkas11(packer, apply_steer, steer_req, cnt, enabled, lkas11, hud_aler
|
|||
}
|
||||
|
||||
dat = packer.make_can_msg("LKAS11", 0, values)[2]
|
||||
dat = dat[:6] + dat[7]
|
||||
checksum = hyundai_checksum(dat)
|
||||
|
||||
if car_fingerprint in CHECKSUM["crc8"]:
|
||||
# CRC Checksum as seen on 2019 Hyundai Santa Fe
|
||||
dat = dat[:6] + dat[7]
|
||||
checksum = hyundai_checksum(dat)
|
||||
elif car_fingerprint in CHECKSUM["6B"]:
|
||||
# Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
|
||||
dat = [ord(i) for i in dat]
|
||||
checksum = sum(dat[:6]) % 256
|
||||
elif car_fingerprint in CHECKSUM["7B"]:
|
||||
# Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger
|
||||
dat = [ord(i) for i in dat]
|
||||
checksum = (sum(dat[:6]) + dat[7]) % 256
|
||||
|
||||
values["CF_Lkas_Chksum"] = checksum
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ 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
|
||||
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts
|
||||
|
||||
try:
|
||||
from selfdrive.car.hyundai.carcontroller import CarController
|
||||
|
|
@ -25,6 +25,7 @@ class CarInterface(object):
|
|||
self.brake_pressed_prev = False
|
||||
self.can_invalid_count = 0
|
||||
self.cruise_enabled_prev = False
|
||||
self.low_speed_alert = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
|
@ -55,9 +56,7 @@ class CarInterface(object):
|
|||
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
|
||||
|
|
@ -70,16 +69,55 @@ class CarInterface(object):
|
|||
tireStiffnessFront_civic = 192150
|
||||
tireStiffnessRear_civic = 202500
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, Prius has larger delay
|
||||
ret.steerActuatorDelay = 0.1 # Default 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]]
|
||||
if candidate == CAR.SANTA_FE:
|
||||
ret.steerKf = 0.00005
|
||||
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.minSteerSpeed = 0.
|
||||
elif candidate == CAR.KIA_SORENTO:
|
||||
ret.steerKf = 0.00005
|
||||
ret.steerRateCost = 0.5
|
||||
ret.mass = 1985 + std_cargo
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
|
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 0.
|
||||
elif candidate == CAR.ELANTRA:
|
||||
ret.steerKf = 0.00004
|
||||
ret.steerRateCost = 0.5
|
||||
ret.mass = 1275 + std_cargo
|
||||
ret.wheelbase = 2.7
|
||||
ret.steerRatio = 16.9
|
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.20], [0.01]]
|
||||
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.GENESIS:
|
||||
ret.steerKf = 0.00005
|
||||
ret.steerRateCost = 0.5
|
||||
ret.mass = 2060 + std_cargo
|
||||
ret.wheelbase = 3.01
|
||||
ret.steerRatio = 16.5
|
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.16], [0.01]]
|
||||
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.KIA_STINGER:
|
||||
ret.steerKf = 0.00005
|
||||
ret.steerRateCost = 0.5
|
||||
ret.mass = 1825 + std_cargo
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
|
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
||||
ret.longitudinalKpBP = [0.]
|
||||
ret.longitudinalKpV = [0.]
|
||||
ret.longitudinalKiBP = [0.]
|
||||
|
|
@ -88,10 +126,6 @@ class CarInterface(object):
|
|||
|
||||
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
|
||||
|
|
@ -201,7 +235,12 @@ class CarInterface(object):
|
|||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
#ret.genericToggle = self.CS.generic_toggle
|
||||
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
self.low_speed_alert = True
|
||||
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
|
||||
self.low_speed_alert = False
|
||||
|
||||
# events
|
||||
events = []
|
||||
|
|
@ -240,6 +279,9 @@ class CarInterface(object):
|
|||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
|
||||
if self.low_speed_alert:
|
||||
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
|
||||
|
||||
ret.events = events
|
||||
ret.canMonoTimes = canMonoTimes
|
||||
|
||||
|
|
|
|||
|
|
@ -8,6 +8,10 @@ def get_hud_alerts(visual_alert, audble_alert):
|
|||
|
||||
class CAR:
|
||||
SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019"
|
||||
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
|
||||
GENESIS = "HYUNDAI GENESIS 2018"
|
||||
KIA_SORENTO = "KIA SORENTO GT LINE 2018" # Top Trim Kia Sorento for Australian Market, AWD Diesel 8sp Auto
|
||||
KIA_STINGER = "KIA STINGER GT2 2018"
|
||||
|
||||
class Buttons:
|
||||
NONE = 0
|
||||
|
|
@ -19,11 +23,32 @@ 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
|
||||
}],
|
||||
CAR.ELANTRA: [{
|
||||
66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
}],
|
||||
CAR.GENESIS: [{
|
||||
67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
|
||||
}],
|
||||
CAR.KIA_SORENTO: [{
|
||||
67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1
|
||||
}],
|
||||
CAR.KIA_STINGER: [{
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8
|
||||
}],
|
||||
}
|
||||
|
||||
CAMERA_MSGS = [832, 1156, 1191, 1342] # msgs sent by the camera
|
||||
|
||||
DBC = {
|
||||
CAR.SANTA_FE: dbc_dict('hyundai_santa_fe_2019_ccan', None),
|
||||
CHECKSUM = {
|
||||
"crc8": [CAR.SANTA_FE],
|
||||
"6B": [CAR.KIA_SORENTO, CAR.GENESIS],
|
||||
"7B": [CAR.KIA_STINGER, CAR.ELANTRA],
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.SANTA_FE: dbc_dict('hyundai_santa_fe_2019_ccan', None),
|
||||
CAR.ELANTRA: dbc_dict('hyundai_santa_fe_2019_ccan', None),
|
||||
CAR.GENESIS: dbc_dict('hyundai_santa_fe_2019_ccan', None),
|
||||
CAR.KIA_SORENTO: dbc_dict('hyundai_santa_fe_2019_ccan', None),
|
||||
CAR.KIA_STINGER: dbc_dict('hyundai_santa_fe_2019_ccan', None),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -224,7 +224,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
|||
|
||||
|
||||
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
|
||||
driver_status, PL, LaC, LoC, VM, angle_offset, passive):
|
||||
driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric):
|
||||
# Given the state, this function returns the actuators
|
||||
|
||||
# reset actuators to zero
|
||||
|
|
@ -258,7 +258,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
|||
|
||||
# parse warnings from car specific interface
|
||||
for e in get_events(events, [ET.WARNING]):
|
||||
AM.add(e, enabled)
|
||||
extra_text = ''
|
||||
if e == "belowSteerSpeed":
|
||||
if is_metric:
|
||||
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
|
||||
else:
|
||||
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
|
||||
AM.add(e, enabled, extra_text=extra_text)
|
||||
|
||||
# *** angle offset learning ***
|
||||
|
||||
|
|
@ -410,6 +416,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
|
||||
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
|
||||
|
||||
is_metric = params.get("IsMetric") == "1"
|
||||
passive = params.get("Passive") != "0"
|
||||
if not passive:
|
||||
while 1:
|
||||
|
|
@ -506,7 +513,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
|
||||
# compute actuators
|
||||
actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
|
||||
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive)
|
||||
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric)
|
||||
prof.checkpoint("State Control")
|
||||
|
||||
# publish data
|
||||
|
|
|
|||
|
|
@ -181,6 +181,12 @@ class AlertManager(object):
|
|||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, None, None, 0., 0., .2),
|
||||
|
||||
"belowSteerSpeed": Alert(
|
||||
"TAKE CONTROL",
|
||||
"Steer Unavailable Below ",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, "steerRequired", None, 0., 0., .1),
|
||||
|
||||
"debugAlert": Alert(
|
||||
"DEBUG ALERT",
|
||||
"",
|
||||
|
|
|
|||
Loading…
Reference in New Issue