From dd22fafc3c9f36c9d96dffee5437fb9f56d7ff6d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Sep 2021 20:41:10 -0700 Subject: [PATCH] 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 --- board/safety/safety_hyundai.h | 140 +++++++++++++++++++++++++++++----- python/__init__.py | 1 + tests/safety/test_honda.py | 3 +- tests/safety/test_hyundai.py | 112 +++++++++++++++++++++++++-- 4 files changed, 229 insertions(+), 27 deletions(-) diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 286724c3..4a7591a1 100644 --- 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}; diff --git a/python/__init__.py b/python/__init__.py index 5d0d1518..8fe515cc 100644 --- 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 diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index 173dbcc7..276021ca 100755 --- 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)) diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 6ba8c82a..559f388b 100644 --- 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()