From 89989abca5def32c73b8c9e66da804acb12be357 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Wed, 3 Aug 2022 03:54:41 -0400 Subject: [PATCH] Hyundai: support camera-based SCC cars (#965) * Hyundai: Car Port for 2022 Kona Electric Camera SCC * add missing flag * set camera_scc in other safety modes * Add safety test * this will be a stock longitudinal port for now * common cruise check make this check common * Fix comment typo * Camera SCC HKG send CLU11 on bus 2 * Update CLU11 comment * fix button test when txing * just add bus 2 to existing addr checks * actually no, this will never be on bus 2 * CLU11 is always on bus 0 line * consistent param ordering * fix test fix test * this is fine * Mistook SCC for CLU, fix * try to make this a little more clear * add comment * fix that Co-authored-by: Shane Smiskol --- board/safety/safety_hyundai.h | 58 ++++++++++++++++++++----------- python/__init__.py | 1 + tests/safety/test_hyundai.py | 31 ++++++++++++----- tests/safety/test_hyundai_hda2.py | 2 +- 4 files changed, 62 insertions(+), 30 deletions(-) diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 8b072463..4a3cff9f 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -14,7 +14,7 @@ const CanMsg HYUNDAI_TX_MSGS[] = { {832, 0, 8}, // LKAS11 Bus 0 {1265, 0, 4}, // CLU11 Bus 0 {1157, 0, 4}, // LFAHDA_MFC Bus 0 - }; +}; const CanMsg HYUNDAI_LONG_TX_MSGS[] = { {832, 0, 8}, // LKAS11 Bus 0 @@ -28,14 +28,21 @@ const CanMsg HYUNDAI_LONG_TX_MSGS[] = { {909, 0, 8}, // FCA11 Bus 0 {1155, 0, 8}, // FCA12 Bus 0 {2000, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable) - }; +}; + +const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = { + {832, 0, 8}, // LKAS11 Bus 0 + {1265, 2, 4}, // CLU11 Bus 2 + {1157, 0, 4}, // LFAHDA_MFC Bus 0 +}; AddrCheckStruct hyundai_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 = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {1057, 2, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }}}, }; #define HYUNDAI_ADDR_CHECK_LEN (sizeof(hyundai_addr_checks) / sizeof(hyundai_addr_checks[0])) @@ -61,6 +68,7 @@ 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; +const int HYUNDAI_PARAM_CAMERA_SCC = 8; enum { HYUNDAI_BTN_NONE = 0, @@ -77,6 +85,7 @@ uint8_t hyundai_last_button_interaction; // button messages since the user pres bool hyundai_legacy = false; bool hyundai_ev_gas_signal = false; bool hyundai_hybrid_gas_signal = false; +bool hyundai_camera_scc = false; bool hyundai_longitudinal = false; addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN}; @@ -161,9 +170,27 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { hyundai_get_checksum, hyundai_compute_checksum, hyundai_get_counter); - if (valid && (GET_BUS(to_push) == 0U)) { - int addr = GET_ADDR(to_push); + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + // SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others + if (valid && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) { + // enter controls on rising edge of ACC and user button press, exit controls when ACC off + if (!hyundai_longitudinal && (addr == 1057)) { + // 2 bits: 13-14 + int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3U; + if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { + controls_allowed = 1; + } + + if (!cruise_engaged) { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; + } + } + + if (valid && (bus == 0)) { if (addr == 593) { int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ffU) * 0.79) - 808; // scale down new driver torque signal to match previous one // update array of samples @@ -198,20 +225,6 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { } } - // enter controls on rising edge of ACC and user button press, exit controls when ACC off - if (!hyundai_longitudinal && (addr == 1057)) { - // 2 bits: 13-14 - int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3U; - if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { - controls_allowed = 1; - } - - if (!cruise_engaged) { - controls_allowed = 0; - } - cruise_engaged_prev = cruise_engaged; - } - // read gas pressed signal if ((addr == 881) && hyundai_ev_gas_signal) { gas_pressed = (((GET_BYTE(to_push, 4) & 0x7FU) << 1) | GET_BYTE(to_push, 3) >> 7) != 0U; @@ -252,6 +265,8 @@ static int hyundai_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { if (hyundai_longitudinal) { tx = msg_allowed(to_send, HYUNDAI_LONG_TX_MSGS, sizeof(HYUNDAI_LONG_TX_MSGS)/sizeof(HYUNDAI_LONG_TX_MSGS[0])); + } else if (hyundai_camera_scc) { + tx = msg_allowed(to_send, HYUNDAI_CAMERA_SCC_TX_MSGS, sizeof(HYUNDAI_CAMERA_SCC_TX_MSGS)/sizeof(HYUNDAI_CAMERA_SCC_TX_MSGS[0])); } else { tx = msg_allowed(to_send, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0])); } @@ -382,10 +397,12 @@ static const addr_checks* hyundai_init(uint16_t param) { hyundai_legacy = 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_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC); hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; #ifdef ALLOW_DEBUG - hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL); + // TODO: add longitudinal support for camera-based SCC platform + hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL) && !hyundai_camera_scc; #endif if (hyundai_longitudinal) { @@ -399,6 +416,7 @@ static const addr_checks* hyundai_init(uint16_t param) { static const addr_checks* hyundai_legacy_init(uint16_t param) { hyundai_legacy = true; hyundai_longitudinal = false; + hyundai_camera_scc = 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_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; diff --git a/python/__init__.py b/python/__init__.py index cee39f4d..f6b8bee2 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -195,6 +195,7 @@ class Panda: FLAG_HYUNDAI_EV_GAS = 1 FLAG_HYUNDAI_HYBRID_GAS = 2 FLAG_HYUNDAI_LONG = 4 + FLAG_HYUNDAI_CAMERA_SCC = 8 FLAG_TESLA_POWERTRAIN = 1 FLAG_TESLA_LONG_CONTROL = 2 diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 64d0eafd..195c14ab 100755 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -58,6 +58,8 @@ def checksum(msg): class HyundaiButtonBase: #(common.PandaSafetyTest): # pylint: disable=no-member,abstract-method + BUTTONS_BUS = 0 # tx on this bus, rx on 0. added to all `self._tx(self._button_msg(...))` + SCC_BUS = 0 # rx on this bus def test_buttons(self): """ @@ -66,16 +68,16 @@ class HyundaiButtonBase: #(common.PandaSafetyTest): - CANCEL allowed while cruise is enabled """ self.safety.set_controls_allowed(0) - self.assertFalse(self._tx(self._button_msg(Buttons.RESUME))) - self.assertFalse(self._tx(self._button_msg(Buttons.SET))) + self.assertFalse(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_BUS))) + self.assertFalse(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_BUS))) self.safety.set_controls_allowed(1) - self.assertTrue(self._tx(self._button_msg(Buttons.RESUME))) - self.assertFalse(self._tx(self._button_msg(Buttons.SET))) + self.assertTrue(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_BUS))) + self.assertFalse(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_BUS))) for enabled in (True, False): self._rx(self._pcm_status_msg(enabled)) - self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL))) + self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL, bus=self.BUTTONS_BUS))) def test_enable_control_allowed_from_cruise(self): """ @@ -139,10 +141,10 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaSafetyTest, common.Driver self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0) self.safety.init_tests() - def _button_msg(self, buttons, main_button=0): + def _button_msg(self, buttons, main_button=0, bus=0): values = {"CF_Clu_CruiseSwState": buttons, "CF_Clu_CruiseSwMain": main_button, "CF_Clu_AliveCnt1": self.cnt_button} self.__class__.cnt_button += 1 - return self.packer.make_can_msg_panda("CLU11", 0, values) + return self.packer.make_can_msg_panda("CLU11", bus, values) def _user_gas_msg(self, gas): values = {"CF_Ems_AclAct": gas, "AliveCounter": self.cnt_gas % 4} @@ -165,7 +167,7 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaSafetyTest, common.Driver def _pcm_status_msg(self, enable): values = {"ACCMode": enable, "CR_VSM_Alive": self.cnt_cruise % 16} self.__class__.cnt_cruise += 1 - return self.packer.make_can_msg_panda("SCC12", 0, values, fix_checksum=checksum) + return self.packer.make_can_msg_panda("SCC12", self.SCC_BUS, values, fix_checksum=checksum) # TODO: this is unused def _torque_driver_msg(self, torque): @@ -193,6 +195,17 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaSafetyTest, common.Driver self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, steer_req=1))) +class TestHyundaiSafetyCameraSCC(TestHyundaiSafety): + BUTTONS_BUS = 2 # tx on 2, rx on 0 + SCC_BUS = 2 # rx on 2 + + 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_CAMERA_SCC) + self.safety.init_tests() + + class TestHyundaiLegacySafety(TestHyundaiSafety): def setUp(self): self.packer = CANPackerPanda("hyundai_kia_generic") @@ -259,7 +272,7 @@ class TestHyundaiLongitudinalSafety(TestHyundaiSafety): "AEB_CmdAct": int(aeb_req), "CR_VSM_DecCmd": aeb_decel, } - return self.packer.make_can_msg_panda("SCC12", 0, values) + return self.packer.make_can_msg_panda("SCC12", self.SCC_BUS, values) def _send_fca11_msg(self, idx=0, vsm_aeb_req=False, fca_aeb_req=False, aeb_decel=0): values = { diff --git a/tests/safety/test_hyundai_hda2.py b/tests/safety/test_hyundai_hda2.py index 51386946..87203955 100755 --- a/tests/safety/test_hyundai_hda2.py +++ b/tests/safety/test_hyundai_hda2.py @@ -55,7 +55,7 @@ class TestHyundaiHDA2(HyundaiButtonBase, common.PandaSafetyTest, common.DriverTo values = {"CRUISE_ACTIVE": enable} return self.packer.make_can_msg_panda("SCC1", 1, values) - def _button_msg(self, buttons, main_button=0): + def _button_msg(self, buttons, main_button=0, bus=1): values = { "CRUISE_BUTTONS": buttons, "ADAPTIVE_CRUISE_MAIN_BTN": main_button,