From 799c33868daac7409affe86eb87ec22c199147d4 Mon Sep 17 00:00:00 2001 From: Drew Hintz Date: Tue, 6 Nov 2018 12:28:33 -0800 Subject: [PATCH] Chrysler safety controls (#130) * Chrysler safety model and tests. * fix comments * when ACC is canceled, disallow controls * update SAFETY_CHRYSLER value in Python API and add other SAFETY values --- board/safety.h | 3 + board/safety/safety_chrysler.h | 136 ++++++++++++++++++++++++++++++ python/__init__.py | 7 ++ tests/safety/libpandasafety_py.py | 6 ++ tests/safety/test.c | 8 ++ tests/safety/test_chrysler.py | 88 +++++++++++++++++++ 6 files changed, 248 insertions(+) create mode 100644 board/safety/safety_chrysler.h create mode 100755 tests/safety/test_chrysler.py diff --git a/board/safety.h b/board/safety.h index 51a2bb8..4d2b468 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 0000000..eadc8ea --- /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/python/__init__.py b/python/__init__.py index 86b7628..c49ee83 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 dfd3960..c6df9d3 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -81,6 +81,12 @@ void set_hyundai_rt_torque_last(int t); 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 c1555db..af0db3c 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 0000000..9b8b177 --- /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()