diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index 8ee63849ccde1a..77910c6c05be46 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -1,10 +1,12 @@ + // CAN msgs we care about -#define MAZDA_LKAS 0x243 -#define MAZDA_LANEINFO 0x440 -#define MAZDA_CRZ_CTRL 0x21c -#define MAZDA_WHEEL_SPEED 0x215 -#define MAZDA_STEER_TORQUE 0x240 +#define MAZDA_LKAS 0x243 +#define MAZDA_CRZ_CTRL 0x21c +#define MAZDA_CRZ_BTNS 0x09d +#define MAZDA_STEER_TORQUE 0x240 +#define MAZDA_ENGINE_DATA 0x202 +#define MAZDA_PEDALS 0x165 // CAN bus numbers #define MAZDA_MAIN 0 @@ -21,43 +23,102 @@ #define MAZDA_MAX_RATE_DOWN 25 #define MAZDA_DRIVER_TORQUE_ALLOWANCE 15 #define MAZDA_DRIVER_TORQUE_FACTOR 1 +#define MAZDA_MAX_TORQUE_ERROR 350 + +// lkas enable speed 52kph, disable at 45kph +#define MAZDA_LKAS_ENABLE_SPEED 5200 +#define MAZDA_LKAS_DISABLE_SPEED 4500 -int mazda_cruise_engaged_last = 0; -int mazda_rt_torque_last = 0; -int mazda_desired_torque_last = 0; -uint32_t mazda_ts_last = 0; -struct sample_t mazda_torque_driver; // last few driver torques measured +const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}}; +bool mazda_lkas_allowed = false; + +AddrCheckStruct mazda_rx_checks[] = { + {.msg = {{MAZDA_CRZ_CTRL, 0, 8, .expected_timestep = 20000U}}}, + {.msg = {{MAZDA_CRZ_BTNS, 0, 8, .expected_timestep = 100000U}}}, + {.msg = {{MAZDA_STEER_TORQUE, 0, 8, .expected_timestep = 12000U}}}, + {.msg = {{MAZDA_ENGINE_DATA, 0, 8, .expected_timestep = 10000U}}}, + {.msg = {{MAZDA_PEDALS, 0, 8, .expected_timestep = 20000U}}}, +}; +const int MAZDA_RX_CHECKS_LEN = sizeof(mazda_rx_checks) / sizeof(mazda_rx_checks[0]); // track msgs coming from OP so that we know what CAM msgs to drop and what to forward static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); + bool valid; - if ((addr == MAZDA_STEER_TORQUE) && (bus == MAZDA_MAIN)) { - int torque_driver_new = GET_BYTE(to_push, 0) - 127; - // update array of samples - update_sample(&mazda_torque_driver, torque_driver_new); - } + valid = addr_safety_check(to_push, mazda_rx_checks, MAZDA_RX_CHECKS_LEN, + NULL, NULL, NULL); + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + if (bus == MAZDA_MAIN) { + + if (addr == MAZDA_ENGINE_DATA) { + // sample speed: scale by 0.01 to get kph + int speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3); + + vehicle_moving = speed > 10; // moving when speed > 0.1 kph - // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == MAZDA_CRZ_CTRL) && (bus == MAZDA_MAIN)) { - int cruise_engaged = GET_BYTE(to_push, 0) & 8; - if (cruise_engaged != 0) { - if (!mazda_cruise_engaged_last) { - controls_allowed = 1; + // Enable LKAS at 52kph going up, disable at 45kph going down + if (speed > MAZDA_LKAS_ENABLE_SPEED) { + mazda_lkas_allowed = true; + } else if (speed < MAZDA_LKAS_DISABLE_SPEED) { + mazda_lkas_allowed = false; + } else { + // Misra-able appeasment block! + } + } + + if (addr == MAZDA_STEER_TORQUE) { + int torque_driver_new = GET_BYTE(to_push, 0) - 127; + // update array of samples + update_sample(&torque_driver, torque_driver_new); } - } - else { - controls_allowed = 0; - } - mazda_cruise_engaged_last = cruise_engaged; - } - // if we see wheel speed msgs on MAZDA_CAM bus then relay is closed - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == MAZDA_CAM) && (addr == MAZDA_WHEEL_SPEED)) { - relay_malfunction_set(); + // enter controls on rising edge of ACC, exit controls on ACC off + if (addr == MAZDA_CRZ_CTRL) { + bool cruise_engaged = GET_BYTE(to_push, 0) & 8; + if (cruise_engaged) { + if (!cruise_engaged_prev) { + // do not engage until we hit the speed at which lkas is on + if (mazda_lkas_allowed) { + controls_allowed = 1; + } else { + controls_allowed = 0; + cruise_engaged = false; + } + } + } else { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; + } + + // Exit controls on rising edge of gas press + if (addr == MAZDA_ENGINE_DATA) { + bool gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0)); + if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } + + // Exit controls on rising edge of brake press + if (addr == MAZDA_PEDALS) { + bool brake_pressed = (GET_BYTE(to_push, 0) & 0x10); + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + + // if we see lkas msg on MAZDA_MAIN bus then relay is closed + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MAZDA_LKAS)) { + relay_malfunction_set(); + } + } } - return 1; + return valid; } static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -65,12 +126,17 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); + if (!msg_allowed(to_send, MAZDA_TX_MSGS, sizeof(MAZDA_TX_MSGS)/sizeof(MAZDA_TX_MSGS[0]))) { + tx = 0; + } + if (relay_malfunction) { tx = 0; } // Check if msg is sent on the main BUS if (bus == MAZDA_MAIN) { + // steer cmd checks if (addr == MAZDA_LKAS) { int desired_torque = (((GET_BYTE(to_send, 0) & 0x0f) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER; @@ -83,20 +149,21 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER); // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, mazda_desired_torque_last, &mazda_torque_driver, + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN, MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR); + // used next time - mazda_desired_torque_last = desired_torque; + desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, mazda_rt_torque_last, MAZDA_MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, rt_torque_last, MAZDA_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, mazda_ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > ((uint32_t) MAZDA_RT_INTERVAL)) { - mazda_rt_torque_last = desired_torque; - mazda_ts_last = ts; + rt_torque_last = desired_torque; + ts_last = ts; } } @@ -107,9 +174,9 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - mazda_desired_torque_last = 0; - mazda_rt_torque_last = 0; - mazda_ts_last = ts; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; } if (violation) { @@ -126,8 +193,7 @@ static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = GET_ADDR(to_fwd); if (bus == MAZDA_MAIN) { bus_fwd = MAZDA_CAM; - } - else if (bus == MAZDA_CAM) { + } else if (bus == MAZDA_CAM) { if (!(addr == MAZDA_LKAS)) { bus_fwd = MAZDA_MAIN; } @@ -145,5 +211,6 @@ const safety_hooks mazda_hooks = { .tx = mazda_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = mazda_fwd_hook, - // TODO: add addr safety checks + .addr_check = mazda_rx_checks, + .addr_check_len = sizeof(mazda_rx_checks) / sizeof(mazda_rx_checks[0]), }; diff --git a/tests/safety/test_mazda.py b/tests/safety/test_mazda.py new file mode 100755 index 00000000000000..f82816c1aa94da --- /dev/null +++ b/tests/safety/test_mazda.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +import unittest +from panda import Panda +from panda.tests.safety import libpandasafety_py +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda + +MAX_RATE_UP = 10 +MAX_RATE_DOWN = 25 +MAX_STEER = 2047 + +MAX_RT_DELTA = 940 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 15 +DRIVER_TORQUE_FACTOR = 1 + + +class TestMazdaSafety(common.PandaSafetyTest): + + TX_MSGS = [[0x243, 0], [0x09d, 0]] + STANDSTILL_THRESHOLD = .1 + RELAY_MALFUNCTION_ADDR = 0x243 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [0x243]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + LKAS_ENABLE_SPEED = 52 + LKAS_DISABLE_SPEED = 45 + + def setUp(self): + self.packer = CANPackerPanda("mazda_cx5_gt_2017") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_MAZDA, 0) + self.safety.init_tests() + + def _torque_meas_msg(self, torque): + values = {"STEER_TORQUE_MOTOR": torque} + return self.packer.make_can_msg_panda("STEER_TORQUE", 0, values) + +# def _torque_driver_msg(self, torque): +# values = {"STEER_TORQUE_DRIVER": torque} +# return self.packer.make_can_msg_panda("STEER_TORQUE", 0, values) + + def _torque_msg(self, torque): + values = {"LKAS_REQUEST": torque} + return self.packer.make_can_msg_panda("CAM_LKAS", 0, values) + + def _speed_msg(self, s): + values = {"SPEED": s} + return self.packer.make_can_msg_panda("ENGINE_DATA", 0, values) + + def _brake_msg(self, pressed): + values = {"BRAKE_ON": pressed} + return self.packer.make_can_msg_panda("PEDALS", 0, values) + + def _gas_msg(self, pressed): + values = {"PEDAL_GAS": pressed} + return self.packer.make_can_msg_panda("ENGINE_DATA", 0, values) + + def _pcm_status_msg(self, cruise_on): + values = {"CRZ_ACTIVE": cruise_on} + return self.packer.make_can_msg_panda("CRZ_CTRL", 0, values) + + def test_enable_control_allowed_from_cruise(self): + self._rx(self._pcm_status_msg(False)) + self.assertFalse(self.safety.get_controls_allowed()) + + self._rx(self._speed_msg(self.LKAS_DISABLE_SPEED - 1)) + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED - 1)) + self._rx(self._pcm_status_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + self._rx(self._pcm_status_msg(False)) + + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED + 1)) + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED - 1)) + self._rx(self._pcm_status_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED + 1)) + self._rx(self._pcm_status_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED - 1)) + self.assertTrue(self.safety.get_controls_allowed()) + + # Enabled going down + self._rx(self._speed_msg(self.LKAS_DISABLE_SPEED - 1)) + self.assertTrue(self.safety.get_controls_allowed()) + + self._rx(self._pcm_status_msg(False)) + + # Disabled going up + self._rx(self._speed_msg(self.LKAS_DISABLE_SPEED + 1)) + self._rx(self._pcm_status_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_cruise_engaged_prev(self): + self._rx(self._pcm_status_msg(False)) + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED - 1)) + self._rx(self._pcm_status_msg(True)) + self.assertFalse(self.safety.get_cruise_engaged_prev()) + + self._rx(self._speed_msg(self.LKAS_ENABLE_SPEED + 1)) + + for engaged in [True, False]: + self._rx(self._pcm_status_msg(engaged)) + self.assertEqual(engaged, self.safety.get_cruise_engaged_prev()) + self._rx(self._pcm_status_msg(not engaged)) + self.assertEqual(not engaged, self.safety.get_cruise_engaged_prev()) + +if __name__ == "__main__": + unittest.main()