From 2f9e07628983596fa641cdc515aeb0197a900a32 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Wed, 9 Oct 2019 16:54:22 -0400 Subject: [PATCH] =?UTF-8?q?Panda=20safety=20code=20for=20Volkswagen,=20Aud?= =?UTF-8?q?i,=20SEAT,=20and=20=C5=A0koda=20(#293)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Panda safety code for Volkswagen, Audi, SEAT, and Škoda. * First pass at MISRA compliance. * Second pass at MISRA compliance. * Fix scope of violation boolean. * MISRA doesn't care for implicit cast of unsigned int to boolean. * Remove superfluous newline. * Remove unused VW ignition hook code, preserve commentary. * Add 50% padding to max delta check. * Add 50% padding to max delta check. * Add clarity around the origin of our safety constants. * Update test RT delta to match safety. --- board/safety.h | 3 + board/safety/safety_volkswagen.h | 162 +++++++++++++++++++++++++ python/__init__.py | 1 + tests/safety/libpandasafety_py.py | 4 + tests/safety/test.c | 24 ++++ tests/safety/test_volkswagen.py | 193 ++++++++++++++++++++++++++++++ 6 files changed, 387 insertions(+) create mode 100644 board/safety/safety_volkswagen.h create mode 100644 tests/safety/test_volkswagen.py diff --git a/board/safety.h b/board/safety.h index ffdfebb4..65322193 100644 --- a/board/safety.h +++ b/board/safety.h @@ -14,6 +14,7 @@ #include "safety/safety_chrysler.h" #include "safety/safety_subaru.h" #include "safety/safety_mazda.h" +#include "safety/safety_volkswagen.h" #include "safety/safety_elm327.h" // from cereal.car.CarParams.SafetyModel @@ -31,6 +32,7 @@ #define SAFETY_SUBARU 11U #define SAFETY_GM_PASSIVE 12U #define SAFETY_MAZDA 13U +#define SAFETY_VOLKSWAGEN 15U #define SAFETY_TOYOTA_IPAS 16U #define SAFETY_ALLOUTPUT 17U #define SAFETY_GM_ASCM 18U @@ -80,6 +82,7 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_SUBARU, &subaru_hooks}, {SAFETY_GM_PASSIVE, &gm_passive_hooks}, {SAFETY_MAZDA, &mazda_hooks}, + {SAFETY_VOLKSWAGEN, &volkswagen_hooks}, {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, {SAFETY_GM_ASCM, &gm_ascm_hooks}, diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h new file mode 100644 index 00000000..ff38a3f4 --- /dev/null +++ b/board/safety/safety_volkswagen.h @@ -0,0 +1,162 @@ +const int VW_MAX_STEER = 300; // 3.0 nm +const int VW_MAX_RT_DELTA = 188; // 10 max rate * 50Hz send rate * 250000 RT interval / 1000000 = 125 ; 125 * 1.5 for safety pad = 187.5 +const uint32_t VW_RT_INTERVAL = 250000; // 250ms between real time checks +const int VW_MAX_RATE_UP = 10; // 5.0 nm/s available rate of change from the steering rack +const int VW_MAX_RATE_DOWN = 10; // 5.0 nm/s available rate of change from the steering rack +const int VW_DRIVER_TORQUE_ALLOWANCE = 100; +const int VW_DRIVER_TORQUE_FACTOR = 4; + +int vw_ignition_started = 0; +struct sample_t vw_torque_driver; // last few driver torques measured +int vw_rt_torque_last = 0; +int vw_desired_torque_last = 0; +uint32_t vw_ts_last = 0; + +// Safety-relevant CAN messages for the Volkswagen MQB platform. +#define MSG_EPS_01 0x09F +#define MSG_ACC_06 0x122 +#define MSG_HCA_01 0x126 +#define MSG_GRA_ACC_01 0x12B +#define MSG_LDW_02 0x397 +#define MSG_KLEMMEN_STATUS_01 0x3C0 + +static void volkswagen_init(int16_t param) { + UNUSED(param); // May use param in the future to indicate MQB vs PQ35/PQ46/NMS vs MLB, or wiring configuration. + controls_allowed = 0; + vw_ignition_started = 0; +} +static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // Monitor Klemmen_Status_01.ZAS_Kl_15 for Terminal 15 (ignition-on) status, but we make no use of it at the moment. + if ((bus == 0) && (addr == MSG_KLEMMEN_STATUS_01)) { + vw_ignition_started = (GET_BYTE(to_push, 2) & 0x2) >> 1; + } + + // Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ + // for the direction. + if ((bus == 0) && (addr == MSG_EPS_01)) { + int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8); + int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7; + if (sign == 1) { + torque_driver_new *= -1; + } + + update_sample(&vw_torque_driver, torque_driver_new); + } + + // Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control + // allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in + // order to accommodate future camera-side integrations if needed. + if (addr == MSG_ACC_06) { + int acc_status = (GET_BYTE(to_push,7) & 0x70) >> 4; + controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0; + } +} + +static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + int addr = GET_ADDR(to_send); + int tx = 1; + + // Safety check for HCA_01 Heading Control Assist torque. + if (addr == MSG_HCA_01) { + bool violation = false; + + int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8); + int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7; + if (sign == 1) { + desired_torque *= -1; + } + + uint32_t ts = TIM2->CNT; + + if (controls_allowed) { + + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, VW_MAX_STEER, -VW_MAX_STEER); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, vw_desired_torque_last, &vw_torque_driver, + VW_MAX_STEER, VW_MAX_RATE_UP, VW_MAX_RATE_DOWN, + VW_DRIVER_TORQUE_ALLOWANCE, VW_DRIVER_TORQUE_FACTOR); + vw_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, vw_rt_torque_last, VW_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, vw_ts_last); + if (ts_elapsed > VW_RT_INTERVAL) { + vw_rt_torque_last = desired_torque; + vw_ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = true; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { + vw_desired_torque_last = 0; + vw_rt_torque_last = 0; + vw_ts_last = ts; + } + + if (violation) { + tx = 0; + } + } + + // 1 allows the message through + return tx; +} + +static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + int addr = GET_ADDR(to_fwd); + int bus_fwd = -1; + + // NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN + + switch(bus_num) { + case 0: + if(addr == MSG_GRA_ACC_01) { + // OP intercepts, filters, and updates the cruise-control button messages before they reach the ACC radar. + bus_fwd = -1; + } else { + // Forward all remaining traffic from J533 gateway to Extended CAN devices. + bus_fwd = 2; + } + break; + case 2: + if((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) { + // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera. + bus_fwd = -1; + } else { + // Forward all remaining traffic from Extended CAN devices to J533 gateway. + bus_fwd = 0; + } + break; + default: + // No other buses should be in use; fallback to do-not-forward. + bus_fwd = -1; + break; + } + + return bus_fwd; +} + +// While we do monitor VW Terminal 15 (ignition-on) state, we are not currently acting on it. Instead we use the +// default GPIO ignition hook. We may do so in the future for harness integrations at the camera (where we only have +// T30 unswitched power) instead of the gateway (where we have both T30 and T15 ignition-switched power). + +const safety_hooks volkswagen_hooks = { + .init = volkswagen_init, + .rx = volkswagen_rx_hook, + .tx = volkswagen_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .ignition = default_ign_hook, + .fwd = volkswagen_fwd_hook, +}; diff --git a/python/__init__.py b/python/__init__.py index 56577091..deb49c39 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -124,6 +124,7 @@ class Panda(object): SAFETY_SUBARU = 11 SAFETY_GM_PASSIVE = 12 SAFETY_MAZDA = 13 + SAFETY_VOLKSWAGEN = 15 SAFETY_TOYOTA_IPAS = 16 SAFETY_ALLOUTPUT = 17 SAFETY_GM_ASCM = 18 diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index bb332e52..9edec08a 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -94,6 +94,10 @@ void set_subaru_desired_torque_last(int t); void set_subaru_rt_torque_last(int t); void set_subaru_torque_driver(int min, int max); +void init_tests_volkswagen(void); +void set_volkswagen_desired_torque_last(int t); +void set_volkswagen_rt_torque_last(int t); +void set_volkswagen_torque_driver(int min, int max); """) diff --git a/tests/safety/test.c b/tests/safety/test.c index d92eb868..8b874a2f 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -29,6 +29,7 @@ struct sample_t gm_torque_driver; struct sample_t hyundai_torque_driver; struct sample_t chrysler_torque_meas; struct sample_t subaru_torque_driver; +struct sample_t volkswagen_torque_driver; TIM_TypeDef timer; TIM_TypeDef *TIM2 = &timer; @@ -155,6 +156,11 @@ void set_subaru_torque_driver(int min, int max){ subaru_torque_driver.max = max; } +void set_volkswagen_torque_driver(int min, int max){ + vw_torque_driver.min = min; + vw_torque_driver.max = max; +} + int get_chrysler_torque_meas_min(void){ return chrysler_torque_meas.min; } @@ -199,6 +205,10 @@ void set_subaru_rt_torque_last(int t){ subaru_rt_torque_last = t; } +void set_volkswagen_rt_torque_last(int t){ + vw_rt_torque_last = t; +} + void set_toyota_desired_torque_last(int t){ toyota_desired_torque_last = t; } @@ -223,6 +233,10 @@ void set_subaru_desired_torque_last(int t){ subaru_desired_torque_last = t; } +void set_volkswagen_desired_torque_last(int t){ + vw_desired_torque_last = t; +} + bool get_honda_moving(void){ return honda_moving; } @@ -316,6 +330,16 @@ void init_tests_subaru(void){ set_timer(0); } +void init_tests_volkswagen(void){ + init_tests(); + vw_torque_driver.min = 0; + vw_torque_driver.max = 0; + vw_desired_torque_last = 0; + vw_rt_torque_last = 0; + vw_ts_last = 0; + set_timer(0); +} + void init_tests_honda(void){ init_tests(); honda_moving = false; diff --git a/tests/safety/test_volkswagen.py b/tests/safety/test_volkswagen.py new file mode 100644 index 00000000..b7cd5b4f --- /dev/null +++ b/tests/safety/test_volkswagen.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +import libpandasafety_py + +MAX_RATE_UP = 10 +MAX_RATE_DOWN = 10 +MAX_STEER = 300 + +MAX_RT_DELTA = 188 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 100 +DRIVER_TORQUE_FACTOR = 4 + +def sign(a): + if a > 0: + return 1 + else: + return -1 + +class TestVolkswagenSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.safety_set_mode(15, 0) + cls.safety.init_tests_volkswagen() + + def _send_msg(self, bus, addr, length): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = addr << 21 + to_send[0].RDTR = length + to_send[0].RDTR = bus << 4 + return to_send + + def _set_prev_torque(self, t): + self.safety.set_volkswagen_desired_torque_last(t) + self.safety.set_volkswagen_rt_torque_last(t) + + def _torque_driver_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x9F << 21 + + t = abs(torque) + to_send[0].RDHR = ((t & 0x1FFF) << 8) + if torque < 0: + to_send[0].RDHR |= 0x1 << 23 + return to_send + + def _torque_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x126 << 21 + + t = abs(torque) + to_send[0].RDLR = (t & 0xFFF) << 16 + if torque < 0: + to_send[0].RDLR |= 0x1 << 31 + return to_send + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_enable_control_allowed_from_cruise(self): + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x122 << 21 + to_push[0].RDHR = 0x30000000 + + self.safety.safety_rx_hook(to_push) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disable_control_allowed_from_cruise(self): + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x122 << 21 + to_push[0].RDHR = 0 + + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(to_push) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_steer_safety_check(self): + for enabled in [0, 1]: + for t in range(-500, 500): + self.safety.set_controls_allowed(enabled) + self._set_prev_torque(t) + if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t))) + else: + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + + def test_manually_enable_controls_allowed(self): + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(0) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_non_realtime_limit_up(self): + self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self._set_prev_torque(0) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + self.safety.set_controls_allowed(True) + self._set_prev_torque(0) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + def test_against_torque_driver(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): + t *= -sign + self.safety.set_volkswagen_torque_driver(t, t) + self._set_prev_torque(MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign))) + + self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) + + # spot check some individual cases + for sign in [-1, 1]: + driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign + torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign + delta = 1 * sign + self._set_prev_torque(torque_desired) + self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) + self._set_prev_torque(torque_desired + delta) + self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) + + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + + + def test_realtime_limits(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self.safety.init_tests_volkswagen() + self._set_prev_torque(0) + self.safety.set_volkswagen_torque_driver(0, 0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + + self._set_prev_torque(0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(RT_INTERVAL + 1) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) + + + def test_fwd_hook(self): + buss = list(range(0x0, 0x2)) + msgs = list(range(0x1, 0x800)) + blocked_msgs_0to2 = [0x12B] + blocked_msgs_2to0 = [0x122, 0x397] + for b in buss: + for m in msgs: + if b == 0: + fwd_bus = -1 if m in blocked_msgs_0to2 else 2 + elif b == 1: + fwd_bus = -1 + elif b == 2: + fwd_bus = -1 if m in blocked_msgs_2to0 else 0 + + # assume len 8 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8))) + + +if __name__ == "__main__": + unittest.main()