Hyundai longitudinal (#711)

* Hyundai longitudinal * return right addr checks * add flag to pythong * fix define * check for stock ecu * add rx check for buttons * Block FCA11 actuation * misra * review 1 * comment about scaling * clean up buttons * use define for flag * more extensive button checking * check for AEB in scc12 * unsigned * add knockout tests * more unsigned * cleaner
2026-02-18 17:23:52 +08:00 · 2021-09-13 20:41:10 -07:00
parent 1befaad8b0
commit dd22fafc3c
4 changed files with 229 additions and 27 deletions
--- a/board/safety/safety_hyundai.h
+++ b/board/safety/safety_hyundai.h
@@ -6,15 +6,28 @@ const int HYUNDAI_MAX_RATE_DOWN = 7;
 const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
 const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
 const int HYUNDAI_STANDSTILL_THRSLD = 30;  // ~1kph
+
+const int HYUNDAI_MAX_ACCEL = 200;  // 1/100 m/s2
+const int HYUNDAI_MIN_ACCEL = -350; // 1/100 m/s2
+
 const CanMsg HYUNDAI_TX_MSGS[] = {
  {832, 0, 8},  // LKAS11 Bus 0
  {1265, 0, 4}, // CLU11 Bus 0
  {1157, 0, 4}, // LFAHDA_MFC Bus 0
-  // {1056, 0, 8}, //   SCC11,  Bus 0
-  // {1057, 0, 8}, //   SCC12,  Bus 0
-  // {1290, 0, 8}, //   SCC13,  Bus 0
-  // {905, 0, 8},  //   SCC14,  Bus 0
-  // {1186, 0, 8}  //   4a2SCC, Bus 0
+ };
+
+const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
+  {832, 0, 8},  // LKAS11 Bus 0
+  {1265, 0, 4}, // CLU11 Bus 0
+  {1157, 0, 4}, // LFAHDA_MFC Bus 0
+  {1056, 0, 8}, // SCC11 Bus 0
+  {1057, 0, 8}, // SCC12 Bus 0
+  {1290, 0, 8}, // SCC13 Bus 0
+  {905, 0, 8},  // SCC14 Bus 0
+  {1186, 0, 2}, // FRT_RADAR11 Bus 0
+  {909, 0, 8},  // FCA11 Bus 0
+  {1155, 0, 8}, // FCA12 Bus 0
+  {2000, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable)
 };

 AddrCheckStruct hyundai_addr_checks[] = {
@@ -26,6 +39,15 @@ AddrCheckStruct hyundai_addr_checks[] = {
 };
 #define HYUNDAI_ADDR_CHECK_LEN (sizeof(hyundai_addr_checks) / sizeof(hyundai_addr_checks[0]))

+AddrCheckStruct hyundai_long_addr_checks[] = {
+  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
+           {881, 0, 8, .expected_timestep = 10000U}, { 0 }}},
+  {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{1265, 0, 4, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+};
+#define HYUNDAI_LONG_ADDR_CHECK_LEN (sizeof(hyundai_long_addr_checks) / sizeof(hyundai_long_addr_checks[0]))
+
 // older hyundai models have less checks due to missing counters and checksums
 AddrCheckStruct hyundai_legacy_addr_checks[] = {
  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
@@ -38,10 +60,12 @@ AddrCheckStruct hyundai_legacy_addr_checks[] = {

 const int HYUNDAI_PARAM_EV_GAS = 1;
 const int HYUNDAI_PARAM_HYBRID_GAS = 2;
+const int HYUNDAI_PARAM_LONGITUDINAL = 4;

 bool hyundai_legacy = false;
 bool hyundai_ev_gas_signal = false;
 bool hyundai_hybrid_gas_signal = false;
+bool hyundai_longitudinal = false;

 addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};

@@ -57,6 +81,8 @@ static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
    cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7;
  } else if (addr == 1057) {
    cnt = GET_BYTE(to_push, 7) & 0xF;
+  } else if (addr == 1265) {
+    cnt = (GET_BYTE(to_push, 3) >> 4) & 0xF;
  } else {
    cnt = 0;
  }
@@ -132,17 +158,35 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
      update_sample(&torque_driver, torque_driver_new);
    }

-    // enter controls on rising edge of ACC, exit controls on ACC off
-    if (addr == 1057) {
-      // 2 bits: 13-14
-      int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
-      if (cruise_engaged && !cruise_engaged_prev) {
-        controls_allowed = 1;
+    if (hyundai_longitudinal) {
+      // ACC steering wheel buttons
+      if (addr == 1265) {
+        int button = GET_BYTE(to_push, 0) & 0x7;
+        switch (button) {
+          case 1:  // resume
+          case 2:  // set
+            controls_allowed = 1;
+            break;
+          case 4:  // cancel
+            controls_allowed = 0;
+            break;
+          default:
+            break;  // any other button is irrelevant
+        }
      }
-      if (!cruise_engaged) {
-        controls_allowed = 0;
+    } else {
+      // enter controls on rising edge of ACC, exit controls on ACC off
+      if (addr == 1057) {
+        // 2 bits: 13-14
+        int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
+        if (cruise_engaged && !cruise_engaged_prev) {
+          controls_allowed = 1;
+        }
+        if (!cruise_engaged) {
+          controls_allowed = 0;
+        }
+        cruise_engaged_prev = cruise_engaged;
      }
-      cruise_engaged_prev = cruise_engaged;
    }

    // read gas pressed signal
@@ -167,7 +211,14 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
      brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
    }

-    generic_rx_checks((addr == 832));
+    bool stock_ecu_detected = (addr == 832);
+
+    // If openpilot is controlling longitudinal we need to ensure the radar is turned off
+    // Enforce by checking we don't see SCC12
+    if (hyundai_longitudinal && (addr == 1057)) {
+      stock_ecu_detected = true;
+    }
+    generic_rx_checks(stock_ecu_detected);
  }
  return valid;
 }
@@ -177,14 +228,53 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
  int tx = 1;
  int addr = GET_ADDR(to_send);

-  if (!msg_allowed(to_send, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]))) {
-    tx = 0;
+  if (hyundai_longitudinal) {
+    tx = msg_allowed(to_send, HYUNDAI_LONG_TX_MSGS, sizeof(HYUNDAI_LONG_TX_MSGS)/sizeof(HYUNDAI_LONG_TX_MSGS[0]));
+  } else {
+    tx = msg_allowed(to_send, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]));
  }

  if (relay_malfunction) {
    tx = 0;
  }

+  // FCA11: Block any potential actuation
+  if (addr == 909) {
+    int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
+    int FCA_CmdAct = (GET_BYTE(to_send, 2) >> 5) & 1;
+    int CF_VSM_DecCmdAct = (GET_BYTE(to_send, 3) >> 7) & 1;
+
+    if ((CR_VSM_DecCmd != 0) || (FCA_CmdAct != 0) || (CF_VSM_DecCmdAct != 0)) {
+      tx = 0;
+    }
+  }
+
+  // ACCEL: safety check
+  if (addr == 1057) {
+    int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7) << 8) | GET_BYTE(to_send, 3)) - 1023;
+    int desired_accel_val = ((GET_BYTE(to_send, 5) << 3) | (GET_BYTE(to_send, 4) >> 5)) - 1023;
+
+    int aeb_decel_cmd = GET_BYTE(to_send, 2);
+    int aeb_req = (GET_BYTE(to_send, 6) >> 6) & 1;
+
+    bool violation = 0;
+
+    if (!controls_allowed) {
+      if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
+        violation = 1;
+      }
+    }
+    violation |= max_limit_check(desired_accel_raw, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
+    violation |= max_limit_check(desired_accel_val, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
+
+    violation |= (aeb_decel_cmd != 0);
+    violation |= (aeb_req != 0);
+
+    if (violation) {
+      tx = 0;
+    }
+  }
+
  // LKA STEER: safety check
  if (addr == 832) {
    int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024;
@@ -232,6 +322,13 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
    }
  }

+  // UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
+  if (addr == 2000) {
+    if ((GET_BYTES_04(to_send) != 0x00803E02) || (GET_BYTES_48(to_send) != 0x0)) {
+      tx = 0;
+    }
+  }
+
  // 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
@@ -266,9 +363,15 @@ static const addr_checks* hyundai_init(int16_t param) {
  relay_malfunction_reset();

  hyundai_legacy = false;
+  hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL);
  hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
  hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
-  hyundai_rx_checks = (addr_checks){hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
+
+  if (hyundai_longitudinal) {
+    hyundai_rx_checks = (addr_checks){hyundai_long_addr_checks, HYUNDAI_LONG_ADDR_CHECK_LEN};
+  } else {
+    hyundai_rx_checks = (addr_checks){hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
+  }
  return &hyundai_rx_checks;
 }

@@ -277,6 +380,7 @@ static const addr_checks* hyundai_legacy_init(int16_t param) {
  relay_malfunction_reset();

  hyundai_legacy = true;
+  hyundai_longitudinal = false;
  hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
  hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
  hyundai_rx_checks = (addr_checks){hyundai_legacy_addr_checks, HYUNDAI_LEGACY_ADDR_CHECK_LEN};
--- a/python/init.py
+++ b/python/init.py
@@ -150,6 +150,7 @@ class Panda(object):

  FLAG_HONDA_ALT_BRAKE = 1
  FLAG_HONDA_BOSCH_LONG = 2
+  FLAG_HYUNDAI_LONG = 4

  def __init__(self, serial=None, claim=True):
    self._serial = serial
--- a/tests/safety/test_honda.py
+++ b/tests/safety/test_honda.py
@@ -387,7 +387,8 @@ class TestHondaBoschLongSafety(TestHondaBoschSafety):

  def test_brake_safety_check(self):
    for controls_allowed in [True, False]:
-      for accel in np.arange(0, self.MAX_BRAKE - 1, -0.1):
+      for accel in np.arange(0, self.MAX_BRAKE - 1, -0.01):
+        accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
        self.safety.set_controls_allowed(controls_allowed)
        send = self.MAX_BRAKE <= accel <= 0 if controls_allowed else accel == 0
        self.assertEqual(send, self._tx(self._send_gas_brake_msg(self.NO_GAS, accel)), (controls_allowed, accel))
--- a/tests/safety/test_hyundai.py
+++ b/tests/safety/test_hyundai.py
@@ -4,7 +4,7 @@ import numpy as np
 from panda import Panda
 from panda.tests.safety import libpandasafety_py
 import panda.tests.safety.common as common
-from panda.tests.safety.common import CANPackerPanda
+from panda.tests.safety.common import CANPackerPanda, make_msg

 MAX_RATE_UP = 3
 MAX_RATE_DOWN = 7
@@ -16,6 +16,14 @@ RT_INTERVAL = 250000
 DRIVER_TORQUE_ALLOWANCE = 50
 DRIVER_TORQUE_FACTOR = 2

+MAX_ACCEL = 2.0
+MIN_ACCEL = -3.5
+
+class Buttons:
+  RESUME = 1
+  SET = 2
+  CANCEL = 4
+
 # 4 bit checkusm used in some hyundai messages
 # lives outside the can packer because we never send this msg
 def checksum(msg):
@@ -196,16 +204,13 @@ class TestHyundaiSafety(common.PandaSafetyTest):
      self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1))))

  def test_spam_cancel_safety_check(self):
-    RESUME_BTN = 1
-    SET_BTN = 2
-    CANCEL_BTN = 4
    self.safety.set_controls_allowed(0)
-    self.assertTrue(self._tx(self._button_msg(CANCEL_BTN)))
-    self.assertFalse(self._tx(self._button_msg(RESUME_BTN)))
-    self.assertFalse(self._tx(self._button_msg(SET_BTN)))
+    self.assertTrue(self._tx(self._button_msg(Buttons.CANCEL)))
+    self.assertFalse(self._tx(self._button_msg(Buttons.RESUME)))
+    self.assertFalse(self._tx(self._button_msg(Buttons.SET)))
    # do not block resume if we are engaged already
    self.safety.set_controls_allowed(1)
-    self.assertTrue(self._tx(self._button_msg(RESUME_BTN)))
+    self.assertTrue(self._tx(self._button_msg(Buttons.RESUME)))


 class TestHyundaiLegacySafety(TestHyundaiSafety):
@@ -239,6 +244,97 @@ class TestHyundaiLegacySafetyHEV(TestHyundaiSafety):
    values = {"CR_Vcu_AccPedDep_Pos": gas}
    return self.packer.make_can_msg_panda("E_EMS11", 0, values, fix_checksum=checksum)

+class TestHyundaiLongitudinalSafety(TestHyundaiSafety):
+  TX_MSGS = [[832, 0], [1265, 0], [1157, 0], [1056, 0], [1057, 0], [1290, 0], [905, 0], [1186, 0], [909, 0], [1155, 0], [2000, 0]]
+  cnt_button = 0
+
+  def setUp(self):
+    self.packer = CANPackerPanda("hyundai_kia_generic")
+    self.safety = libpandasafety_py.libpandasafety
+    self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, Panda.FLAG_HYUNDAI_LONG)
+    self.safety.init_tests()
+
+  # override these tests from PandaSafetyTest, hyundai longitudinal uses button enable
+  def test_disable_control_allowed_from_cruise(self):
+    pass
+
+  def test_enable_control_allowed_from_cruise(self):
+    pass
+
+  def test_cruise_engaged_prev(self):
+    pass
+
+  def _pcm_status_msg(self, enable):
+    raise NotImplementedError
+
+  def _button_msg(self, buttons):
+    values = {"CF_Clu_CruiseSwState": buttons, "CF_Clu_AliveCnt1": self.cnt_button}
+    self.__class__.cnt_button += 1
+    return self.packer.make_can_msg_panda("CLU11", 0, values)
+
+  def _send_accel_msg(self, accel, aeb_req=False, aeb_decel=0):
+    values = {
+      "aReqRaw": accel,
+      "aReqValue": accel,
+      "AEB_CmdAct": int(aeb_req),
+      "CR_VSM_DecCmd": aeb_decel,
+    }
+    return self.packer.make_can_msg_panda("SCC12", 0, values)
+
+  def _send_fca11_msg(self, idx=0, aeb_req=False, aeb_decel=0):
+    values = {
+      "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1,
+      "Supplemental_Counter": idx % 0xF,
+      "FCA_Status": 2,
+      "CR_VSM_DecCmd": aeb_decel,
+      "CF_VSM_DecCmdAct": int(aeb_req),
+      "FCA_CmdAct": int(aeb_req),
+    }
+    return self.packer.make_can_msg_panda("FCA11", 0, values)
+
+  def test_no_aeb_fca11(self):
+    self.assertTrue(self._tx(self._send_fca11_msg()))
+    self.assertFalse(self._tx(self._send_fca11_msg(aeb_req=True)))
+    self.assertFalse(self._tx(self._send_fca11_msg(aeb_decel=1.0)))
+
+  def test_no_aeb_scc12(self):
+    self.assertTrue(self._tx(self._send_accel_msg(0)))
+    self.assertFalse(self._tx(self._send_accel_msg(0, aeb_req=True)))
+    self.assertFalse(self._tx(self._send_accel_msg(0, aeb_decel=1.0)))
+
+  def test_set_resume_buttons(self):
+    for btn in range(8):
+      self.safety.set_controls_allowed(0)
+      self._rx(self._button_msg(btn))
+      self.assertEqual(btn in [Buttons.RESUME, Buttons.SET], self.safety.get_controls_allowed(), msg=f"btn {btn}")
+
+  def test_cancel_button(self):
+    self.safety.set_controls_allowed(1)
+    self._rx(self._button_msg(Buttons.CANCEL))
+    self.assertFalse(self.safety.get_controls_allowed())
+
+  def test_accel_safety_check(self):
+    for controls_allowed in [True, False]:
+      for accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.01):
+        accel = round(accel, 2) # floats might not hit exact boundary conditions without rounding
+        self.safety.set_controls_allowed(controls_allowed)
+        send = MIN_ACCEL <= accel <= MAX_ACCEL if controls_allowed else accel == 0
+        self.assertEqual(send, self._tx(self._send_accel_msg(accel)), (controls_allowed, accel))
+
+  def test_diagnostics(self):
+    tester_present = common.package_can_msg((0x7d0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0))
+    self.assertTrue(self.safety.safety_tx_hook(tester_present))
+
+    not_tester_present = common.package_can_msg((0x7d0, 0, b"\x03\xAA\xAA\x00\x00\x00\x00\x00", 0))
+    self.assertFalse(self.safety.safety_tx_hook(not_tester_present))
+
+  def test_radar_alive(self):
+    # If the radar knockout failed, make sure the relay malfunction is shown
+    self.assertFalse(self.safety.get_relay_malfunction())
+    self._rx(make_msg(0, 1057, 8))
+    self.assertTrue(self.safety.get_relay_malfunction())
+
+

 if __name__ == "__main__":
  unittest.main()