From c10a755516e07ebeddfa81b312084c23fd2b2c78 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Sat, 17 Nov 2018 01:58:34 -0800 Subject: [PATCH] Squashed 'panda/' changes from 4dd3f5a..293fa33 293fa33 Honda: fixed message forwarding addresses from can2 to can0 799c338 Chrysler safety controls (#130) c05b15b Toyota frc on (#141) 8291971 Toyota: using a more generalized CRUISE_ACTIVE bit that works on all cars 6f157aa Toyota frc on (#140) 8d2470c bumped panda version 915ee4f Honda: forwarding CAN0 to camera, so camera can stay on (#139) git-subtree-dir: panda git-subtree-split: 293fa33f830f0c7cdfad31f621c4bdc016aa7d41 --- VERSION | 2 +- board/safety.h | 3 + board/safety/safety_chrysler.h | 136 ++++++++++++++++++++++++++++++ board/safety/safety_honda.h | 34 ++++++-- board/safety/safety_toyota.h | 20 +++-- python/__init__.py | 7 ++ tests/safety/libpandasafety_py.py | 6 ++ tests/safety/test.c | 8 ++ tests/safety/test_chrysler.py | 88 +++++++++++++++++++ tests/safety/test_toyota.py | 8 +- 10 files changed, 289 insertions(+), 23 deletions(-) create mode 100644 board/safety/safety_chrysler.h create mode 100755 tests/safety/test_chrysler.py diff --git a/VERSION b/VERSION index 46882bcf3de0b4..301779b03ec401 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.1.6 \ No newline at end of file +v1.1.7 \ No newline at end of file diff --git a/board/safety.h b/board/safety.h index 51a2bb8963b3b0..4d2b46899f7969 100644 --- a/board/safety.h +++ b/board/safety.h @@ -62,6 +62,7 @@ int controls_allowed = 0; #include "safety/safety_ford.h" #include "safety/safety_cadillac.h" #include "safety/safety_hyundai.h" +#include "safety/safety_chrysler.h" #include "safety/safety_elm327.h" const safety_hooks *current_hooks = &nooutput_hooks; @@ -102,6 +103,7 @@ typedef struct { #define SAFETY_CADILLAC 6 #define SAFETY_HYUNDAI 7 #define SAFETY_TESLA 8 +#define SAFETY_CHRYSLER 9 #define SAFETY_TOYOTA_IPAS 0x1335 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_ALLOUTPUT 0x1337 @@ -116,6 +118,7 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_FORD, &ford_hooks}, {SAFETY_CADILLAC, &cadillac_hooks}, {SAFETY_HYUNDAI, &hyundai_hooks}, + {SAFETY_CHRYSLER, &chrysler_hooks}, {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, #ifdef PANDA {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h new file mode 100644 index 00000000000000..eadc8eaa7c7231 --- /dev/null +++ b/board/safety/safety_chrysler.h @@ -0,0 +1,136 @@ +// board enforces +// in-state +// ACC is active (green) +// out-state +// brake pressed +// stock LKAS ECU is online +// ACC is not active (white or disabled) + +// chrysler_: namespacing +int chrysler_speed = 0; + +// silence everything if stock ECUs are still online +int chrysler_lkas_detected = 0; +int chrysler_desired_torque_last = 0; + +static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + int bus_number = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + if (to_push->RIR & 4) { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } else { + // Normal + addr = to_push->RIR >> 21; + } + + if (addr == 0x144 && bus_number == 0) { + chrysler_speed = ((to_push->RDLR & 0xFF000000) >> 16) | (to_push->RDHR & 0xFF); + } + + // check if stock LKAS ECU is still online + if (addr == 0x292 && bus_number == 0) { + chrysler_lkas_detected = 1; + controls_allowed = 0; + } + + // ["ACC_2"]['ACC_STATUS_2'] == 7 for active (green) Adaptive Cruise Control + if (addr == 0x1f4 && bus_number == 0) { + if (((to_push->RDLR & 0x380000) >> 19) == 7) { + controls_allowed = 1; + } else { + controls_allowed = 0; + } + } + + // exit controls on brake press by human + if (addr == 0x140) { + if (to_push->RDLR & 0x4) { + controls_allowed = 0; + } + } +} + +// if controls_allowed +// allow steering up to limit +// else +// block all commands that produce actuation +static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + // There can be only one! (LKAS) + if (chrysler_lkas_detected) { + return 0; + } + + uint32_t addr; + if (to_send->RIR & 4) { + // Extended + addr = to_send->RIR >> 3; + } else { + // Normal + addr = to_send->RIR >> 21; + } + + // LKA STEER: Too large of values cause the steering actuator ECU to silently + // fault and no longer actuate the wheel until the car is rebooted. + if (addr == 0x292) { + int rdlr = to_send->RDLR; + int straight = 1024; + int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8) - straight; + int max_steer = 230; + int max_rate = 50; // ECU is fine with 100, but 3 is typical. + if (steer > max_steer) { + return false; + } + if (steer < -max_steer) { + return false; + } + if (!controls_allowed && steer != 0) { + // If controls not allowed, only allow steering to move closer to 0. + if (chrysler_desired_torque_last == 0) { + return false; + } + if ((chrysler_desired_torque_last > 0) && (steer >= chrysler_desired_torque_last)) { + return false; + } + if ((chrysler_desired_torque_last < 0) && (steer <= chrysler_desired_torque_last)) { + return false; + } + } + if (steer < (chrysler_desired_torque_last - max_rate)) { + return false; + } + if (steer > (chrysler_desired_torque_last + max_rate)) { + return false; + } + + chrysler_desired_torque_last = steer; + } + + // 1 allows the message through + return true; +} + +static int chrysler_tx_lin_hook(int lin_num, uint8_t *data, int len) { + // LIN is not used. + return false; +} + +static void chrysler_init(int16_t param) { + controls_allowed = 0; +} + +static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + return -1; +} + +const safety_hooks chrysler_hooks = { + .init = chrysler_init, + .rx = chrysler_rx_hook, + .tx = chrysler_tx_hook, + .tx_lin = chrysler_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = chrysler_fwd_hook, +}; + diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 82e6eadda24fa1..fbee6cfe861f53 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -138,15 +138,6 @@ static void honda_init(int16_t param) { honda_alt_brake_msg = false; } -const safety_hooks honda_hooks = { - .init = honda_init, - .rx = honda_rx_hook, - .tx = honda_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .ignition = default_ign_hook, - .fwd = nooutput_fwd_hook, -}; - static void honda_bosch_init(int16_t param) { controls_allowed = 0; bosch_hardware = true; @@ -154,6 +145,22 @@ static void honda_bosch_init(int16_t param) { honda_alt_brake_msg = param == 1 ? true : false; } +static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + // fwd from car to camera. also fwd certain msgs from camera to car + // 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX, + // 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud, + // 0x39f is radar hud + int addr = to_fwd->RIR>>21; + if (bus_num == 0) { + return 2; + } else if (bus_num == 2 && addr != 0xE4 && addr != 0x194 && addr != 0x1FA && + addr != 0x30C && addr != 0x33D && addr != 0x39F) { + return 0; + } + + return -1; +} + static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 1 || bus_num == 2) { int addr = to_fwd->RIR>>21; @@ -162,6 +169,15 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } +const safety_hooks honda_hooks = { + .init = honda_init, + .rx = honda_rx_hook, + .tx = honda_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = honda_fwd_hook, +}; + const safety_hooks honda_bosch_hooks = { .init = honda_bosch_init, .rx = honda_rx_hook, diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 6ddf2952d10f11..9cab1512f167cb 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -1,5 +1,5 @@ -int toyota_no_dsu_car = 0; // ch-r and camry don't have the DSU int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high? +int toyota_camera_forwarded = 0; // should we forward the camera bus? // global torque limit const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever @@ -49,8 +49,8 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // enter controls on rising edge of ACC, exit controls on ACC off if ((to_push->RIR>>21) == 0x1D2) { - // 4 bits: 55-52 - int cruise_engaged = to_push->RDHR & 0xF00000; + // 5th bit is CRUISE_ACTIVE + int cruise_engaged = to_push->RDLR & 0x20; if (cruise_engaged && !toyota_cruise_engaged_last) { controls_allowed = 1; } else if (!cruise_engaged) { @@ -60,16 +60,15 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } int bus = (to_push->RDTR >> 4) & 0xF; - // 0x680 is a radar msg only found in dsu-less cars - if ((to_push->RIR>>21) == 0x680 && (bus == 1)) { - toyota_no_dsu_car = 1; + // msgs are only on bus 2 if panda is connected to frc + if (bus == 2) { + toyota_camera_forwarded = 1; } // 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high if ((to_push->RIR>>21) == 0x2E4 && (bus == 0)) { toyota_giraffe_switch_1 = 1; } - } static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -150,13 +149,15 @@ static void toyota_init(int16_t param) { controls_allowed = 0; toyota_actuation_limits = 1; toyota_giraffe_switch_1 = 0; + toyota_camera_forwarded = 0; toyota_dbc_eps_torque_factor = param; } static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - // forward cam to radar and viceversa if car is dsu-less, except lkas cmd and hud - if ((bus_num == 0 || bus_num == 2) && toyota_no_dsu_car && !toyota_giraffe_switch_1) { + // forward cam to radar and viceversa if car, except lkas cmd and hud + // don't forward when switch 1 is high + if ((bus_num == 0 || bus_num == 2) && toyota_camera_forwarded && !toyota_giraffe_switch_1) { int addr = to_fwd->RIR>>21; bool is_lkas_msg = (addr == 0x2E4 || addr == 0x412) && bus_num == 2; return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2); @@ -177,6 +178,7 @@ static void toyota_nolimits_init(int16_t param) { controls_allowed = 0; toyota_actuation_limits = 0; toyota_giraffe_switch_1 = 0; + toyota_camera_forwarded = 0; toyota_dbc_eps_torque_factor = param; } diff --git a/python/__init__.py b/python/__init__.py index 86b76285ee0ddb..c49ee83622f523 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -105,7 +105,14 @@ class Panda(object): SAFETY_NOOUTPUT = 0 SAFETY_HONDA = 1 SAFETY_TOYOTA = 2 + SAFETY_GM = 3 SAFETY_HONDA_BOSCH = 4 + SAFETY_FORD = 5 + SAFETY_CADILLAC = 6 + SAFETY_HYUNDAI = 7 + SAFETY_TESLA = 8 + SAFETY_CHRYSLER = 9 + SAFETY_TOYOTA_IPAS = 0x1335 SAFETY_TOYOTA_NOLIMITS = 0x1336 SAFETY_ALLOUTPUT = 0x1337 SAFETY_ELM327 = 0xE327 diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index dfd396045bf316..c6df9d3340a8a6 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -81,6 +81,12 @@ void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +void init_tests_chrysler(void); +void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +void chrysler_init(int16_t param); +void set_chrysler_desired_torque_last(int t); + """) libpandasafety = ffi.dlopen(libpandasafety_fn) diff --git a/tests/safety/test.c b/tests/safety/test.c index c1555db5d57e80..af0db3c92c1132 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -121,6 +121,10 @@ void set_hyundai_desired_torque_last(int t){ hyundai_desired_torque_last = t; } +void set_chrysler_desired_torque_last(int t){ + chrysler_desired_torque_last = t; +} + int get_ego_speed(void){ return ego_speed; } @@ -184,6 +188,10 @@ void init_tests_honda(void){ gas_prev = 0; } +void init_tests_chrysler(void){ + chrysler_desired_torque_last = 0; + set_timer(0); +} void set_gmlan_digital_output(int to_set){ } diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py new file mode 100755 index 00000000000000..9b8b17720f96e9 --- /dev/null +++ b/tests/safety/test_chrysler.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python2 +import unittest +import numpy as np +import libpandasafety_py + + +class TestChryslerSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.chrysler_init(0) + cls.safety.init_tests_chrysler() + + def _acc_msg(self, active): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x1f4 << 21 + to_send[0].RDLR = 0xfe3fff1f if active else 0xfe0fff1f + return to_send + + + def _brake_msg(self, brake): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x140 << 21 + to_send[0].RDLR = 0x485 if brake else 0x480 + return to_send + + def _steer_msg(self, angle): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x292 << 21 + c_angle = (1024 + angle) + to_send[0].RDLR = 0x10 | ((c_angle & 0xf00) >> 8) | ((c_angle & 0xff) << 8) + return to_send + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_acc_enables_controls(self): + self.safety.set_controls_allowed(0) + self.safety.chrysler_rx_hook(self._acc_msg(0)) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.chrysler_rx_hook(self._acc_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.chrysler_rx_hook(self._acc_msg(0)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_disengage_on_brake(self): + self.safety.set_controls_allowed(1) + self.safety.chrysler_rx_hook(self._brake_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.chrysler_rx_hook(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_steer_calculation(self): + self.assertEqual(0x14, self._steer_msg(0)[0].RDLR) # straight, no steering + + def test_steer_tx(self): + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0))) + self.safety.set_chrysler_desired_torque_last(227) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(230))) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(231))) + self.safety.set_chrysler_desired_torque_last(-227) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(-231))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(-230))) + # verify max change + self.safety.set_chrysler_desired_torque_last(0) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(230))) + + self.safety.set_controls_allowed(0) + self.safety.set_chrysler_desired_torque_last(0) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(3))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0))) + # verify when controls not allowed we can still go back towards 0 + self.safety.set_chrysler_desired_torque_last(10) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(10))) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(11))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(7))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(4))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0))) + self.safety.set_chrysler_desired_torque_last(-10) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(-10))) + self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(-11))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(-7))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(-4))) + self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0))) + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index b4d23af65df321..40bc4bcf5d8f97 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -129,7 +129,7 @@ def test_manually_enable_controls_allowed(self): def test_enable_control_allowed_from_cruise(self): to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_push[0].RIR = 0x1D2 << 21 - to_push[0].RDHR = 0xF00000 + to_push[0].RDLR = 0x20 self.safety.toyota_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) @@ -137,7 +137,7 @@ def test_enable_control_allowed_from_cruise(self): def test_disable_control_allowed_from_cruise(self): to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') to_push[0].RIR = 0x1D2 << 21 - to_push[0].RDHR = 0 + to_push[0].RDLR = 0 self.safety.set_controls_allowed(1) self.safety.toyota_rx_hook(to_push) @@ -373,7 +373,7 @@ def test_angle_cmd_rate_when_enabled(self): self.assertTrue(self.safety.get_controls_allowed()) # now inject too high rates - self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * + self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * (max_delta_up + 1), 1))) self.assertFalse(self.safety.get_controls_allowed()) self.safety.set_controls_allowed(1) @@ -381,7 +381,7 @@ def test_angle_cmd_rate_when_enabled(self): self.assertTrue(self.safety.get_controls_allowed()) self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * + self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * (max_delta_down + 1), 1))) self.assertFalse(self.safety.get_controls_allowed())