From b2ffaae60ee73581f58b0cab757d5ac09db9dd5c Mon Sep 17 00:00:00 2001 From: rbiasini Date: Wed, 19 Feb 2020 23:46:38 -0800 Subject: [PATCH] Chrysler: disengage on gas press (#442) * add gas cancellation for chrysler * added regr test too * also add bus checks to chrysler. SHould have been there already --- board/safety/safety_chrysler.h | 25 ++++++++++++++++++++++--- tests/safety/test.c | 2 ++ tests/safety/test_chrysler.py | 22 ++++++++++++++++++++++ 3 files changed, 46 insertions(+), 3 deletions(-) diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 293839b7f62d98..dd37f05a3dbd67 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -4,18 +4,23 @@ const uint32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks const int CHRYSLER_MAX_RATE_UP = 3; const int CHRYSLER_MAX_RATE_DOWN = 3; const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor +const int CHRYSLER_GAS_THRSLD = 30; // 7% more than 2m/s const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}}; // TODO: do checksum and counter checks AddrCheckStruct chrysler_rx_checks[] = { {.addr = {544}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {514}, .bus = 0, .expected_timestep = 10000U}, {.addr = {500}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {308}, .bus = 0, .expected_timestep = 20000U}, // verify ts }; const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]); int chrysler_rt_torque_last = 0; int chrysler_desired_torque_last = 0; int chrysler_cruise_engaged_last = 0; +bool chrysler_gas_prev = false; +int chrysler_speed = 0; uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured @@ -29,7 +34,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); // Measured eps torque - if (addr == 544) { + if ((addr == 544) && (bus == 0)) { int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U; // update array of samples @@ -37,7 +42,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1F4) { + if ((addr == 500) && (bus == 0)) { int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7; if (cruise_engaged && !chrysler_cruise_engaged_last) { controls_allowed = 1; @@ -48,7 +53,21 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { chrysler_cruise_engaged_last = cruise_engaged; } - // TODO: add gas pressed check + // update speed + if ((addr == 514) && (bus == 0)) { + int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4); + int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4); + chrysler_speed = (speed_l + speed_r) / 2; + } + + // exit controls on rising edge of gas press + if ((addr == 308) && (bus == 0)) { + bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0; + if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { + controls_allowed = 0; + } + chrysler_gas_prev = gas; + } // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) { diff --git a/tests/safety/test.c b/tests/safety/test.c index 8de48f676b0b38..85b87ff78ba071 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -312,6 +312,8 @@ void init_tests_hyundai(void){ void init_tests_chrysler(void){ init_tests(); + chrysler_gas_prev = false; + chrysler_speed = 0; chrysler_torque_meas.min = 0; chrysler_torque_meas.max = 0; chrysler_desired_torque_last = 0; diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 2c472706fe1c99..09720bfd01a212 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -28,6 +28,18 @@ def _button_msg(self, buttons): to_send[0].RDLR = buttons return to_send + def _speed_msg(self, speed): + speed = int(speed / 0.071028) + to_send = make_msg(0, 514, 4) + to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \ + ((speed & 0xFF0) << 12) + ((speed & 0xF) << 28) + return to_send + + def _gas_msg(self, gas): + to_send = make_msg(0, 308) + to_send[0].RDHR = (gas & 0x7F) << 8 + return to_send + def _set_prev_torque(self, t): self.safety.set_chrysler_desired_torque_last(t) self.safety.set_chrysler_rt_torque_last(t) @@ -80,6 +92,16 @@ def test_disable_control_allowed_from_cruise(self): self.safety.safety_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) + def test_gas_disable(self): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._speed_msg(2.2)) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.safety_rx_hook(self._speed_msg(2.3)) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + def test_non_realtime_limit_up(self): self.safety.set_controls_allowed(True)