Mazda: safety tests add missing safety checks (#525)
* Mazda: add safety test Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Update mazda safety to capture more events * gas press * brake press * speed - lkas on/off Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Customize safety tests Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Add RX checks Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Format Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Set lkas enable speed at 52 kph Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Test case when speed<enable_lkas_speed Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Be consistent with names in other places Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Add test case for lkas on when speed dips below lkas enable speed Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Make sure lkas disabled Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Use car's speed signal instead of wheel speed Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * Allow acc cancel msg When gas is pressed OP disenages stock cruise doesn't get disengaged. panda needs to know when OP engages again. OP re-enageges on cruise button presses. This commit uses the same re-enage logic in panda. Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * rebase Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * allow acc_cancel in safety tests Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com> * rename variable, fix comment Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>master
parent
bdec1398e5
commit
89658d0bde
|
@ -1,10 +1,12 @@
|
||||||
|
|
||||||
|
|
||||||
// CAN msgs we care about
|
// CAN msgs we care about
|
||||||
#define MAZDA_LKAS 0x243
|
#define MAZDA_LKAS 0x243
|
||||||
#define MAZDA_LANEINFO 0x440
|
#define MAZDA_CRZ_CTRL 0x21c
|
||||||
#define MAZDA_CRZ_CTRL 0x21c
|
#define MAZDA_CRZ_BTNS 0x09d
|
||||||
#define MAZDA_WHEEL_SPEED 0x215
|
#define MAZDA_STEER_TORQUE 0x240
|
||||||
#define MAZDA_STEER_TORQUE 0x240
|
#define MAZDA_ENGINE_DATA 0x202
|
||||||
|
#define MAZDA_PEDALS 0x165
|
||||||
|
|
||||||
// CAN bus numbers
|
// CAN bus numbers
|
||||||
#define MAZDA_MAIN 0
|
#define MAZDA_MAIN 0
|
||||||
|
@ -21,43 +23,102 @@
|
||||||
#define MAZDA_MAX_RATE_DOWN 25
|
#define MAZDA_MAX_RATE_DOWN 25
|
||||||
#define MAZDA_DRIVER_TORQUE_ALLOWANCE 15
|
#define MAZDA_DRIVER_TORQUE_ALLOWANCE 15
|
||||||
#define MAZDA_DRIVER_TORQUE_FACTOR 1
|
#define MAZDA_DRIVER_TORQUE_FACTOR 1
|
||||||
|
#define MAZDA_MAX_TORQUE_ERROR 350
|
||||||
|
|
||||||
int mazda_cruise_engaged_last = 0;
|
// lkas enable speed 52kph, disable at 45kph
|
||||||
int mazda_rt_torque_last = 0;
|
#define MAZDA_LKAS_ENABLE_SPEED 5200
|
||||||
int mazda_desired_torque_last = 0;
|
#define MAZDA_LKAS_DISABLE_SPEED 4500
|
||||||
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
|
// 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) {
|
static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
int bus = GET_BUS(to_push);
|
bool valid;
|
||||||
int addr = GET_ADDR(to_push);
|
|
||||||
|
|
||||||
if ((addr == MAZDA_STEER_TORQUE) && (bus == MAZDA_MAIN)) {
|
valid = addr_safety_check(to_push, mazda_rx_checks, MAZDA_RX_CHECKS_LEN,
|
||||||
int torque_driver_new = GET_BYTE(to_push, 0) - 127;
|
NULL, NULL, NULL);
|
||||||
// update array of samples
|
if (valid) {
|
||||||
update_sample(&mazda_torque_driver, torque_driver_new);
|
int bus = GET_BUS(to_push);
|
||||||
}
|
int addr = GET_ADDR(to_push);
|
||||||
|
|
||||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
if (bus == MAZDA_MAIN) {
|
||||||
if ((addr == MAZDA_CRZ_CTRL) && (bus == MAZDA_MAIN)) {
|
|
||||||
int cruise_engaged = GET_BYTE(to_push, 0) & 8;
|
if (addr == MAZDA_ENGINE_DATA) {
|
||||||
if (cruise_engaged != 0) {
|
// sample speed: scale by 0.01 to get kph
|
||||||
if (!mazda_cruise_engaged_last) {
|
int speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3);
|
||||||
controls_allowed = 1;
|
|
||||||
|
vehicle_moving = speed > 10; // moving when speed > 0.1 kph
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
|
||||||
controls_allowed = 0;
|
|
||||||
}
|
|
||||||
mazda_cruise_engaged_last = cruise_engaged;
|
|
||||||
}
|
}
|
||||||
|
return valid;
|
||||||
// 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();
|
|
||||||
}
|
|
||||||
return 1;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
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 addr = GET_ADDR(to_send);
|
||||||
int bus = GET_BUS(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) {
|
if (relay_malfunction) {
|
||||||
tx = 0;
|
tx = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if msg is sent on the main BUS
|
// Check if msg is sent on the main BUS
|
||||||
if (bus == MAZDA_MAIN) {
|
if (bus == MAZDA_MAIN) {
|
||||||
|
|
||||||
// steer cmd checks
|
// steer cmd checks
|
||||||
if (addr == MAZDA_LKAS) {
|
if (addr == MAZDA_LKAS) {
|
||||||
int desired_torque = (((GET_BYTE(to_send, 0) & 0x0f) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER;
|
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);
|
violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER);
|
||||||
|
|
||||||
// *** torque rate limit check ***
|
// *** 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_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN,
|
||||||
MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR);
|
MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR);
|
||||||
|
|
||||||
// used next time
|
// used next time
|
||||||
mazda_desired_torque_last = desired_torque;
|
desired_torque_last = desired_torque;
|
||||||
|
|
||||||
// *** torque real time rate limit check ***
|
// *** 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
|
// 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)) {
|
if (ts_elapsed > ((uint32_t) MAZDA_RT_INTERVAL)) {
|
||||||
mazda_rt_torque_last = desired_torque;
|
rt_torque_last = desired_torque;
|
||||||
mazda_ts_last = ts;
|
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
|
// reset to 0 if either controls is not allowed or there's a violation
|
||||||
if (violation || !controls_allowed) {
|
if (violation || !controls_allowed) {
|
||||||
mazda_desired_torque_last = 0;
|
desired_torque_last = 0;
|
||||||
mazda_rt_torque_last = 0;
|
rt_torque_last = 0;
|
||||||
mazda_ts_last = ts;
|
ts_last = ts;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (violation) {
|
if (violation) {
|
||||||
|
@ -126,8 +193,7 @@ static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||||
int addr = GET_ADDR(to_fwd);
|
int addr = GET_ADDR(to_fwd);
|
||||||
if (bus == MAZDA_MAIN) {
|
if (bus == MAZDA_MAIN) {
|
||||||
bus_fwd = MAZDA_CAM;
|
bus_fwd = MAZDA_CAM;
|
||||||
}
|
} else if (bus == MAZDA_CAM) {
|
||||||
else if (bus == MAZDA_CAM) {
|
|
||||||
if (!(addr == MAZDA_LKAS)) {
|
if (!(addr == MAZDA_LKAS)) {
|
||||||
bus_fwd = MAZDA_MAIN;
|
bus_fwd = MAZDA_MAIN;
|
||||||
}
|
}
|
||||||
|
@ -145,5 +211,6 @@ const safety_hooks mazda_hooks = {
|
||||||
.tx = mazda_tx_hook,
|
.tx = mazda_tx_hook,
|
||||||
.tx_lin = nooutput_tx_lin_hook,
|
.tx_lin = nooutput_tx_lin_hook,
|
||||||
.fwd = mazda_fwd_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]),
|
||||||
};
|
};
|
||||||
|
|
|
@ -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()
|
Loading…
Reference in New Issue