Kia Sorento and future Multi-Car Support

panda/board/drivers/can.h

@@ -84,7 +84,7 @@ int can_err_cnt = 0;
   CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3};
   uint8_t bus_lookup[] = {0,1,2};
   uint8_t can_num_lookup[] = {0,1,2,-1};
-  int8_t can_forwarding[] = {-1,-1,-1,-1};
+  int8_t can_forwarding[] = {1,-1,-1,-1};
   uint32_t can_speed[] = {5000, 5000, 5000, 333};
   bool can_autobaud_enabled[] = {false, false, false, false};
   #define CAN_MAX 3

panda/board/safety/safety_hyundai.h

@@ -18,9 +18,9 @@ static void hyundai_init(int16_t param) {
 static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   // forward camera to car and viceversa, excpet for lkas11 and mdps12
-  if ((bus_num == 0 || bus_num == 2) && !hyundai_giraffe_switch_1) {
+  if ((bus_num == 0 || bus_num == 1)) { // && !hyundai_giraffe_switch_1) {
     int addr = to_fwd->RIR>>21;
-    bool is_lkas_msg = (addr == 832 && bus_num == 2) || (addr == 593 && bus_num == 0);
+    bool is_lkas_msg = (addr == 832 && bus_num == 1) || (addr == 593 && bus_num == 0);
     return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2);
   }
   return -1;

selfdrive/boardd/boardd.cc

@@ -39,6 +39,7 @@
 #define SAFETY_HONDA_BOSCH 4
 #define SAFETY_FORD 5
 #define SAFETY_CADILLAC 6
+#define SAFETY_HYUNDAI 7
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
 
@@ -113,6 +114,9 @@ void *safety_setter_thread(void *s) {
   case (int)cereal::CarParams::SafetyModels::CADILLAC:
     safety_setting = SAFETY_CADILLAC;
     break;
+  case (int)cereal::CarParams::SafetyModels::HYUNDAI:
+    safety_setting = SAFETY_HYUNDAI;
+    break;
   default:
     LOGE("unknown safety model: %d", safety_model);
   }
@@ -584,7 +588,7 @@ void *pigeon_thread(void *crap) {
       //printf("got %d\n", len);
       alen += len;
     }
-    if (alen > 0) { 
+    if (alen > 0) {
       if (dat[0] == (char)0x00){
         LOGW("received invalid ublox message, resetting pigeon");
         pigeon_init();

selfdrive/car/__init__.py

@@ -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))

selfdrive/car/hyundai/carcontroller.py

@@ -0,0 +1,77 @@
+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, CHECKSUM
+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_lkas12())
+    can_sends.append(create_lkas11(self.packer, apply_steer, steer_req, self.lkas11_cnt,
+                                   enabled, CS.lkas11, hud_alert, CHECKSUM[self.car_fingerprint], 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

selfdrive/car/hyundai/carstate.py

@@ -0,0 +1,221 @@
+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),
+    ("CF_Gway_ParkBrakeSw", "CGW1", 0),
+
+    ("BRAKE_ACT", "EMS12", 0),
+    ("PV_AV_CAN", "EMS12", 0),
+    ("TPS", "EMS12", 0),
+
+    ("CYL_PRES", "ESP12", 0),
+
+    ("CF_Clu_CruiseSwState", "CLU11", 0),
+    ("CF_Clu_CruiseSwMain" , "CLU11", 0),
+    ("CF_Clu_SldMainSW", "CLU11", 0),
+    ("CF_Clu_ParityBit1", "CLU11", 0),
+    ("CF_Clu_VanzDecimal" , "CLU11", 0),
+    ("CF_Clu_Vanz", "CLU11", 0),
+    ("CF_Clu_SPEED_UNIT", "CLU11", 0),
+    ("CF_Clu_DetentOut", "CLU11", 0),
+    ("CF_Clu_RheostatLevel", "CLU11", 0),
+    ("CF_Clu_CluInfo", "CLU11", 0),
+    ("CF_Clu_AmpInfo", "CLU11", 0),
+    ("CF_Clu_AliveCnt1", "CLU11", 0),
+
+    ("CF_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, 1)
+
+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 = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
+    self.main_on = True
+    self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
+    self.pcm_acc_status = int(self.acc_active)
+
+    # calc best v_ego estimate, by averaging two opposite corners
+    self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
+    self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
+    self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
+    self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
+    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"]