From d68356b9246f37a73c91a1b1216c3f4f4ac49896 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Fri, 30 Aug 2019 20:15:03 -0700 Subject: [PATCH] Honda Nidec: fwd stock AEB (#257) * Honda Nidec: fwd stock AEB * bug * correct aeb bit * typo * Revert "typo" This reverts commit d424198b1fbc5a783c39b5bdd7d034865ed61a83. * typo * bug fix * don't have tx hook blocking aeb messages from stock system * typo * fwd stock brake even if they are the same * only stop forwarding aeb when aeb flag clears * Fix safety tests * Added fwd safety tests * tiny bit more readable --- board/safety/safety_honda.h | 47 ++++++++++++++++------ tests/safety/libpandasafety_py.py | 3 +- tests/safety/test.c | 11 +++-- tests/safety/test_honda.py | 67 ++++++++++++++++++------------- 4 files changed, 84 insertions(+), 44 deletions(-) diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 2004f1b5a3a009..ebaa15642afbb3 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -8,16 +8,19 @@ // brake > 0mph const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file -int honda_brake_prev = 0; +int honda_brake = 0; int honda_gas_prev = 0; +bool honda_brake_pressed_prev = false; bool honda_moving = false; bool honda_bosch_hardware = false; bool honda_alt_brake_msg = false; +bool honda_fwd_brake = false; static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); + int bus = GET_BUS(to_push); // sample speed if (addr == 0x158) { @@ -51,11 +54,11 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of brake press or on brake press when speed > 0 bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C); if (is_user_brake_msg) { - int brake = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); - if (brake && (!(honda_brake_prev) || honda_moving)) { + bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); + if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) { controls_allowed = 0; } - honda_brake_prev = brake; + honda_brake_pressed_prev = brake_pressed; } // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) @@ -81,6 +84,20 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { honda_gas_prev = gas; } } + if ((bus == 2) && (addr == 0x1FA)) { + bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; + int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); + + // Forward AEB when stock braking is higher than openpilot braking + // only stop forwarding when AEB event is over + if (!honda_stock_aeb) { + honda_fwd_brake = false; + } else if (honda_stock_brake >= honda_brake) { + honda_fwd_brake = true; + } else { + // Leave honda forward brake as is + } + } } // all commands: gas, brake and steering @@ -98,18 +115,21 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || - (honda_brake_prev && honda_moving); + (honda_brake_pressed_prev && honda_moving); bool current_controls_allowed = controls_allowed && !(pedal_pressed); // BRAKE: safety check - if (addr == 0x1FA) { - int brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3); + if ((addr == 0x1FA) && (bus == 0)) { + honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3); if (!current_controls_allowed || !long_controls_allowed) { - if (brake != 0) { + if (honda_brake != 0) { tx = 0; } } - if (brake > 255) { + if (honda_brake > 255) { + tx = 0; + } + if (honda_fwd_brake) { tx = 0; } } @@ -175,9 +195,12 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 2) { // block stock lkas messages and stock acc messages (if OP is doing ACC) int addr = GET_ADDR(to_fwd); - int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D); - int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F); - int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed); + bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D); + bool is_acc_hud_msg = (addr == 0x30C) || (addr == 0x39F); + bool is_brake_msg = addr == 0x1FA; + bool block_fwd = is_lkas_msg || + (is_acc_hud_msg && long_controls_allowed) || + (is_brake_msg && long_controls_allowed && !honda_fwd_brake); if (!block_fwd) { bus_fwd = 0; } diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 888bd36e948758..bb332e52b6b8e0 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -57,8 +57,9 @@ void init_tests_honda(void); bool get_honda_moving(void); -int get_honda_brake_prev(void); +bool get_honda_brake_pressed_prev(void); int get_honda_gas_prev(void); +void set_honda_fwd_brake(bool); void set_honda_alt_brake_msg(bool); void set_honda_bosch_hardware(bool); int get_honda_bosch_hardware(void); diff --git a/tests/safety/test.c b/tests/safety/test.c index 7cd9b86d86f9a0..d92eb8680f6ecd 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -227,8 +227,8 @@ bool get_honda_moving(void){ return honda_moving; } -int get_honda_brake_prev(void){ - return honda_brake_prev; +bool get_honda_brake_pressed_prev(void){ + return honda_brake_pressed_prev; } int get_honda_gas_prev(void){ @@ -247,6 +247,10 @@ int get_honda_bosch_hardware(void) { return honda_bosch_hardware; } +void set_honda_fwd_brake(bool c){ + honda_fwd_brake = c; +} + void init_tests(void){ // get HW_TYPE from env variable set in test.sh hw_type = atoi(getenv("HW_TYPE")); @@ -315,8 +319,9 @@ void init_tests_subaru(void){ void init_tests_honda(void){ init_tests(); honda_moving = false; - honda_brake_prev = 0; + honda_brake_pressed_prev = false; honda_gas_prev = 0; + honda_fwd_brake = false; } void set_gmlan_digital_output(int to_set){ diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index 8e968328ac1239..f2f59389767ec1 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -112,9 +112,9 @@ def test_sample_speed(self): self.assertEqual(1, self.safety.get_honda_moving()) def test_prev_brake(self): - self.assertFalse(self.safety.get_honda_brake_prev()) + self.assertFalse(self.safety.get_honda_brake_pressed_prev()) self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_honda_brake_prev()) + self.assertTrue(self.safety.get_honda_brake_pressed_prev()) def test_disengage_on_brake(self): self.safety.set_controls_allowed(1) @@ -204,17 +204,22 @@ def test_allow_engage_with_gas_interceptor_pressed(self): self.safety.set_gas_interceptor_detected(False) def test_brake_safety_check(self): - for long_controls_allowed in [0, 1]: - self.safety.set_long_controls_allowed(long_controls_allowed) - for brake in np.arange(0, MAX_BRAKE + 10, 1): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed and long_controls_allowed: - send = MAX_BRAKE >= brake >= 0 - else: - send = brake == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake))) + for fwd_brake in [False, True]: + self.safety.set_honda_fwd_brake(fwd_brake) + for long_controls_allowed in [0, 1]: + self.safety.set_long_controls_allowed(long_controls_allowed) + for brake in np.arange(0, MAX_BRAKE + 10, 1): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + if fwd_brake: + send = False # block openpilot brake msg when fwd'ing stock msg + elif controls_allowed and long_controls_allowed: + send = MAX_BRAKE >= brake >= 0 + else: + send = brake == 0 + self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake))) self.safety.set_long_controls_allowed(True) + self.safety.set_honda_fwd_brake(False) def test_gas_interceptor_safety_check(self): for long_controls_allowed in [0, 1]: @@ -252,27 +257,33 @@ def test_fwd_hook(self): buss = range(0x0, 0x3) msgs = range(0x1, 0x800) long_controls_allowed = [0, 1] + fwd_brake = [False, True] self.safety.set_honda_bosch_hardware(0) - for l in long_controls_allowed: - self.safety.set_long_controls_allowed(l) - blocked_msgs = [0xE4, 0x194, 0x33D] - if l: - blocked_msgs += [0x1FA ,0x30C, 0x39F] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) + for f in fwd_brake: + self.safety.set_honda_fwd_brake(f) + for l in long_controls_allowed: + self.safety.set_long_controls_allowed(l) + blocked_msgs = [0xE4, 0x194, 0x33D] + if l: + blocked_msgs += [0x30C, 0x39F] + if not f: + blocked_msgs += [0x1FA] + for b in buss: + for m in msgs: + if b == 0: + fwd_bus = 2 + elif b == 1: + fwd_bus = -1 + elif b == 2: + fwd_bus = -1 if m in blocked_msgs else 0 + + # assume len 8 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) self.safety.set_long_controls_allowed(True) + self.safety.set_honda_fwd_brake(False)