Kia and Hyundai Port

cereal/car.capnp

@@ -302,6 +302,7 @@ struct CarParams {
     hondaBosch @5;
     ford @6;
     cadillac @7;
+    hyundai @8;
   }
 
   # things about the car in the manual

launch_chffrplus.sh

@@ -6,11 +6,11 @@ fi
 
 function launch {
   # apply update
-  if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
-     git reset --hard @{u} &&
-     git clean -xdf &&
-     exec "${BASH_SOURCE[0]}"
-  fi
+  #if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
+  #   git reset --hard @{u} &&
+  #   git clean -xdf &&
+  #   exec "${BASH_SOURCE[0]}"
+  #fi
 
   # no cpu rationing for now
   echo 0-3 > /dev/cpuset/background/cpus

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.h

@@ -16,7 +16,7 @@ int max_limit_check(int val, const int MAX, const int MIN);
 int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
   const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
 int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
-  const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, 
+  const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
   const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
 int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 
@@ -49,6 +49,7 @@ int controls_allowed = 0;
 #include "safety/safety_gm.h"
 #include "safety/safety_ford.h"
 #include "safety/safety_cadillac.h"
+#include "safety/safety_hyundai.h"
 #include "safety/safety_elm327.h"
 
 const safety_hooks *current_hooks = &nooutput_hooks;
@@ -87,6 +88,7 @@ typedef struct {
 #define SAFETY_HONDA_BOSCH 4
 #define SAFETY_FORD 5
 #define SAFETY_CADILLAC 6
+#define SAFETY_HYUNDAI 7
 #define SAFETY_TOYOTA_IPAS 0x1335
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
@@ -100,6 +102,7 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_GM, &gm_hooks},
   {SAFETY_FORD, &ford_hooks},
   {SAFETY_CADILLAC, &cadillac_hooks},
+  {SAFETY_HYUNDAI, &hyundai_hooks},
   {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
 #ifdef PANDA
   {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},

panda/board/safety/safety_hyundai.h

@@ -0,0 +1,32 @@
+// Kia Hyundai Safety
+
+static int hyundai_ign_hook() {
+  return true;
+}
+static void hyundai_init(int16_t param) {
+  controls_allowed = 1;
+}
+
+static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+  // Steering Value Safety Check
+
+  return true;
+}
+
+static int hyundai_tx_lin_hook(int lin_num, uint8_t *data, int len) {
+  return true;
+}
+
+static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
+const safety_hooks hyundai_hooks = {
+  .init = hyundai_init,
+  .rx = default_rx_hook,
+  .tx = hyundai_tx_hook,
+  .tx_lin = hyundai_tx_lin_hook,
+  .ignition = hyundai_ign_hook,
+  .fwd = hyundai_fwd_hook,
+};
+

panda/python/__init__.py

@@ -106,6 +106,7 @@ class Panda(object):
   SAFETY_HONDA = 1
   SAFETY_TOYOTA = 2
   SAFETY_HONDA_BOSCH = 4
+  SAFETY_HYUNDAI = 7
   SAFETY_TOYOTA_NOLIMITS = 0x1336
   SAFETY_ALLOUTPUT = 0x1337
   SAFETY_ELM327 = 0xE327
@@ -534,4 +535,3 @@ def kline_recv(self, bus=2):
     msg = self.kline_ll_recv(2, bus=bus)
     msg += self.kline_ll_recv(ord(msg[1])-2, bus=bus)
     return msg
-

run_controlsd.sh

@@ -0,0 +1,2 @@
+#/bin/sh
+PYTHONPATH=/data/openpilot PREPAREONLY=1 /data/openpilot/selfdrive/controls/controlsd.py

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);
   }

selfdrive/can/parser.cc

@@ -200,7 +200,7 @@ class CANParser {
         }
       }
       if (!msg) {
-        fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str());
+        fprintf(stderr, "CANParser: could not find message 0x%X in dbc %s\n", op.address, dbc_name.c_str());
         assert(false);
       }
 

selfdrive/car/car_helpers.py

@@ -12,6 +12,7 @@ def load_interfaces(x):
     try:
       imp = __import__('selfdrive.car.%s.interface' % interface, fromlist=['CarInterface']).CarInterface
     except ImportError:
+      cloudlog.warning("Import Error %s", ImportError)
       imp = None
     for car in x[interface]:
       ret[car] = imp
@@ -70,7 +71,7 @@ def fingerprint(logcan, timeout):
     # broadcast immediately
     if len(candidate_cars) == 1 and st is not None:
       # TODO: better way to decide to wait more if Toyota
-      time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1
+      time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0] or "HYUNDAI" in candidate_cars[0]) else 0.1
       if (ts-st) > time_fingerprint:
         break
 

selfdrive/car/hyundai/camstate.py

@@ -0,0 +1,61 @@
+from selfdrive.car.hyundai.values import CAR, DBC
+from selfdrive.can.parser import CANParser
+from selfdrive.config import Conversions as CV
+
+def get_can_parser2(CP):
+
+  signals = [
+    ("Byte0", "LKAS11", 0),
+    ("Byte1", "LKAS11", 0),
+    ("Byte2", "LKAS11", 0),
+    ("Byte3", "LKAS11", 0),
+    ("Byte4", "LKAS11", 0),
+    ("Byte5", "LKAS11", 0),
+    ("Byte6", "LKAS11", 0), # Checksum
+    ("Byte7", "LKAS11", 0),
+
+    ("Byte0", "LKAS12", 0),
+    ("Byte1", "LKAS12", 0),
+    ("Byte2", "LKAS12", 0),
+    ("Byte3", "LKAS12", 0),
+    ("Byte4", "LKAS12", 0),
+    ("Byte5", "LKAS12", 0)
+  ]
+
+  checks = [
+    ("LKAS11", 100),    # LKAS11 = 100Hz
+    ("LKAS12", 10)      # LKAS12 = 10Hz
+  ]
+
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
+
+
+class CamState(object):
+  def __init__(self, CP):
+    self.CP = CP
+
+    # initialize can parser
+    self.car_fingerprint = CP.carFingerprint
+
+
+  def update(self, cp):
+    # copy can_valid
+    self.can_valid = cp.can_valid
+
+    # LKAS11 From Camera
+    self.lkas11_b0 = int(cp.vl["LKAS11"]['Byte0'])
+    self.lkas11_b1 = int(cp.vl["LKAS11"]['Byte1'])
+    self.lkas11_b2 = int(cp.vl["LKAS11"]['Byte2'])
+    self.lkas11_b3 = int(cp.vl["LKAS11"]['Byte3'])
+    self.lkas11_b4 = int(cp.vl["LKAS11"]['Byte4'])
+    self.lkas11_b5 = int(cp.vl["LKAS11"]['Byte5'])
+    self.lkas11_b6 = int(cp.vl["LKAS11"]['Byte6'])
+    self.lkas11_b7 = int(cp.vl["LKAS11"]['Byte7'])
+
+    # LKAS10 From Camera    
+    self.lkas12_b0 = int(cp.vl["LKAS12"]['Byte0'])
+    self.lkas12_b1 = int(cp.vl["LKAS12"]['Byte1'])
+    self.lkas12_b2 = int(cp.vl["LKAS12"]['Byte2'])
+    self.lkas12_b3 = int(cp.vl["LKAS12"]['Byte3'])
+    self.lkas12_b4 = int(cp.vl["LKAS12"]['Byte4'])
+    self.lkas12_b5 = int(cp.vl["LKAS12"]['Byte5'])

selfdrive/car/hyundai/carcontroller.py

@@ -0,0 +1,174 @@
+from common.numpy_fast import clip, interp
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.hyundai.hyundaican import make_can_msg, create_lkas11, create_lkas12b
+from selfdrive.car.hyundai.values import ECU, STATIC_MSGS, CAR
+from selfdrive.can.packer import CANPacker
+
+
+# Steer torque limits
+STEER_MAX = 200   # Actual limit is about 1023, but not tested, and not needed
+STEER_DELTA = 5   # We have no Panda Safety, don't be silly here!   Good idea, YOU add Panda Safety!
+
+
+TARGET_IDS = [0x340]
+
+
+class CarController(object):
+  def __init__(self, dbc_name, car_fingerprint, enable_camera):
+    self.braking = False
+    self.controls_allowed = True
+    self.last_steer = 0
+    self.car_fingerprint = car_fingerprint
+    self.angle_control = False
+    self.idx = 0
+    self.lkas_request = 0
+    self.lanes = 0
+    self.steer_angle_enabled = False
+    self.ipas_reset_counter = 0
+    self.turning_inhibit = 0
+    self.limited_steer = 0
+    self.hide_lkas_fault = 100
+    print self.car_fingerprint
+
+    self.fake_ecus = set()
+    if enable_camera: self.fake_ecus.add(ECU.CAM)
+    self.packer = CANPacker(dbc_name)
+
+  def update(self, sendcan, enabled, CS, frame, actuators, CamS):
+    # When the driver starts inputting torque, reduce the requested torque
+    #  We do not want to remove torque because we want to know if OP is still steering
+    #  And how we do it, meh, who cares, this might do.
+    if abs(CS.steer_torque_driver) > 1:
+      self.limited_steer = actuators.steer / (abs(CS.steer_torque_driver) * 2)
+    else:
+      self.limited_steer = actuators.steer
+
+    # Steering Torque Scaling is to STEER_MAX, + 1024 for center
+    apply_steer = int(round((self.limited_steer * STEER_MAX) + 1024))
+
+    # This is redundant clipping code, kept in case it needs to be advanced
+    max_lim = 1024 + STEER_MAX
+    min_lim = 1024 - STEER_MAX
+    apply_steer = clip(apply_steer, min_lim, max_lim)
+
+    # Very basic Rate Limiting
+    # TODO: Revisit this
+    if (apply_steer - self.last_steer) > STEER_DELTA:
+      apply_steer = self.last_steer + STEER_DELTA
+    elif (self.last_steer - apply_steer) > STEER_DELTA:
+      apply_steer = self.last_steer - STEER_DELTA
+
+
+    # Inhibits *outside of* alerts
+    #    Because the Turning Indicator Status is based on Lights and not Stalk, latching is 
+    #    needed for the disable to work.
+    if CS.left_blinker_on == 1 or CS.right_blinker_on == 1:
+      self.turning_inhibit = 180  # Disable for 1.8 Seconds after blinker turned off
+
+    if self.turning_inhibit > 0:
+      self.turning_inhibit = self.turning_inhibit - 1
+
+    if not enabled or self.turning_inhibit > 0:
+      apply_steer = 1024     # 1024 is midpoint (no steer)
+      self.last_steer = 1024 # Reset Last Steer
+    else:
+      self.lanes = 3 * 4     # bit 0 = Right Lane, bit 1 = Left Lane, Offset by 2 bits in byte.
+
+    self.last_steer = apply_steer
+
+    can_sends = []
+
+    # Index is 4 bits long, this is the counter
+    self.idx = self.idx + 1
+    if self.idx >= 16:
+      self.idx = 0
+
+    # Byte 4 is used for Index and HBA
+    #   We generate the Index, but pass through HBA
+    lkas11_byte4 = self.idx * 16
+
+    # Split apply steer Word into 2 Bytes
+    apply_steer_a = apply_steer & 0xFF
+    apply_steer_b = (apply_steer >> 8) & 0xFF
+
+
+    # If Request to Steer is anything but 0 torque, turn on ActToi
+    if apply_steer != 1024:
+      apply_steer_b = apply_steer_b + 0x08
+
+
+    if enabled:
+      # When we send Torque signals that the camera does not expet, it faults.
+      #   This masks the fault for 750ms after bringing stock back on.
+      #   This does NOT mean that the factory system will be enabled, it will still be off.
+      #      This was tested at 500ms, and 1 in 10 disables, a fault was still seen.
+      self.hide_lkas_fault = 75
+
+      # Generate the 7 bytes as needed for OP Control.
+      #   Anything we don't generate, pass through from camera
+      lkas11_byte0 = int(self.lanes) + (CamS.lkas11_b0 & 0xC3)
+      lkas11_byte1 = CamS.lkas11_b1
+      lkas11_byte2 = apply_steer_a
+      lkas11_byte3 = apply_steer_b + (CamS.lkas11_b3 & 0xE0)   # ToiFlt always comes on, don't pass it
+      lkas11_byte4 = lkas11_byte4 + (CamS.lkas11_b4 & 0x0F)    # Always use our counter
+      lkas11_byte5 = CamS.lkas11_b5
+      lkas11_byte7 = CamS.lkas11_b7
+    else:
+      # Pass Through the 7 bytes so that Factory LKAS is in control
+      #   We still use our counter, because otherwise duplicates and missed messages from the camera
+      #   is possible due to the implementation method.  As such, we recreate the checksum as well
+      # Byte 0 defined below due to Fault Masking
+      lkas11_byte1 = CamS.lkas11_b1
+      lkas11_byte2 = CamS.lkas11_b2
+      # Byte 3 defined below due to Fault Masking
+      lkas11_byte4 = lkas11_byte4 + (CamS.lkas11_b4 & 0x0F)    # Always use our counter
+      lkas11_byte5 = CamS.lkas11_b5
+      lkas11_byte7 = CamS.lkas11_b7
+      # This is the Fault Masking needed in byte 0 and byte 3
+      if self.hide_lkas_fault > 0:
+        lkas11_byte0 = int(self.lanes) + (CamS.lkas11_b0 & 0xC3)
+        lkas11_byte3 = CamS.lkas11_b3 & 0xE7
+        self.hide_lkas_fault = self.hide_lkas_fault - 1
+      else:
+        lkas11_byte0 = CamS.lkas11_b0
+        lkas11_byte3 = CamS.lkas11_b3
+
+
+
+    # Create Checksum
+    #   Sorento checksum is Byte 0 to 5
+    #   Other models appear to be Byte 0 to Byte 5 as well as Byte 7
+    if CS.car_fingerprint == CAR.SORENTO:
+      # 6 Byte Checksum
+      checksum = (lkas11_byte0 + lkas11_byte1 + lkas11_byte2 + lkas11_byte3 + \
+        lkas11_byte4 + lkas11_byte5) % 256
+    else:
+      # 7 Byte Checksum
+      checksum = (lkas11_byte0 + lkas11_byte1 + lkas11_byte2 + lkas11_byte3 + \
+        lkas11_byte4 + lkas11_byte5 + lkas11_byte7) % 256
+
+
+
+    # Create LKAS11 Message at 100Hz
+    can_sends.append(create_lkas11(self.packer, lkas11_byte0, \
+      lkas11_byte1, lkas11_byte2, lkas11_byte3, lkas11_byte4, \
+      lkas11_byte5, checksum, lkas11_byte7))
+
+
+
+    # Create LKAS12 Message at 10Hz
+    if (frame % 10) == 0:
+      can_sends.append(create_lkas12b(self.packer, CamS.lkas12_b0, CamS.lkas12_b1, \
+        CamS.lkas12_b2, CamS.lkas12_b3, CamS.lkas12_b4, CamS.lkas12_b5))
+
+
+
+    # Limit Terminal Debugging to 5Hz
+    if (frame % 20) == 0:
+      print "controlsdDebug steer", actuators.steer, "strToq", CS.steer_torque_driver, "lim steer", self.limited_steer, "v_ego", \
+        CS.v_ego, "strAng", CS.angle_steers
+
+
+
+    # Send messages to canbus
+    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())