remove toyota ipas safety code and tests (#460)
rbiasini
GitHub
parent
a379faf2b0
commit
b2dbb504dc
|
|
@ -4,7 +4,6 @@
|
|||
#include "safety/safety_defaults.h"
|
||||
#include "safety/safety_honda.h"
|
||||
#include "safety/safety_toyota.h"
|
||||
#include "safety/safety_toyota_ipas.h"
|
||||
#include "safety/safety_tesla.h"
|
||||
#include "safety/safety_gm_ascm.h"
|
||||
#include "safety/safety_gm.h"
|
||||
|
|
@ -34,7 +33,6 @@
|
|||
#define SAFETY_MAZDA 13U
|
||||
#define SAFETY_NISSAN 14U
|
||||
#define SAFETY_VOLKSWAGEN_MQB 15U
|
||||
#define SAFETY_TOYOTA_IPAS 16U
|
||||
#define SAFETY_ALLOUTPUT 17U
|
||||
#define SAFETY_GM_ASCM 18U
|
||||
#define SAFETY_NOOUTPUT 19U
|
||||
|
|
@ -208,7 +206,6 @@ const safety_hook_config safety_hook_registry[] = {
|
|||
{SAFETY_NOOUTPUT, &nooutput_hooks},
|
||||
#ifdef ALLOW_DEBUG
|
||||
{SAFETY_CADILLAC, &cadillac_hooks},
|
||||
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
|
||||
{SAFETY_TESLA, &tesla_hooks},
|
||||
{SAFETY_NISSAN, &nissan_hooks},
|
||||
{SAFETY_ALLOUTPUT, &alloutput_hooks},
|
||||
|
|
|
|||
|
|
@ -1,169 +0,0 @@
|
|||
// uses tons from safety_toyota
|
||||
// TODO: refactor to repeat less code
|
||||
|
||||
// IPAS override
|
||||
const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
|
||||
|
||||
// 2m/s are added to be less restrictive
|
||||
const struct lookup_t LOOKUP_ANGLE_RATE_UP = {
|
||||
{2., 7., 17.},
|
||||
{5., .8, .15}};
|
||||
|
||||
const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = {
|
||||
{2., 7., 17.},
|
||||
{5., 3.5, .4}};
|
||||
|
||||
const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change
|
||||
const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees
|
||||
|
||||
int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override
|
||||
int angle_control = 0; // 1 if direct angle control packets are seen
|
||||
float speed = 0.;
|
||||
|
||||
struct sample_t angle_meas; // last 3 steer angles
|
||||
struct sample_t torque_driver; // last 3 driver steering torque
|
||||
|
||||
// state of angle limits
|
||||
int16_t desired_angle_last = 0; // last desired steer angle
|
||||
int16_t rt_angle_last = 0; // last desired torque for real time check
|
||||
uint32_t ts_angle_last = 0;
|
||||
|
||||
int controls_allowed_last = 0;
|
||||
|
||||
|
||||
static int toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// check standard toyota stuff as well
|
||||
bool valid = toyota_rx_hook(to_push);
|
||||
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
if (addr == 0x260) {
|
||||
// get driver steering torque
|
||||
int16_t torque_driver_new = (GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 2);
|
||||
|
||||
// update array of samples
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// get steer angle
|
||||
if (addr == 0x25) {
|
||||
int angle_meas_new = ((GET_BYTE(to_push, 0) & 0xF) << 8) | GET_BYTE(to_push, 1);
|
||||
uint32_t ts = TIM2->CNT;
|
||||
|
||||
angle_meas_new = to_signed(angle_meas_new, 12);
|
||||
|
||||
// update array of samples
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
|
||||
// *** angle real time check
|
||||
// add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz
|
||||
int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG) + 1.)));
|
||||
int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG) + 1.)));
|
||||
int highest_rt_angle = rt_angle_last + ((rt_angle_last > 0) ? rt_delta_angle_up : rt_delta_angle_down);
|
||||
int lowest_rt_angle = rt_angle_last - ((rt_angle_last > 0) ? rt_delta_angle_down : rt_delta_angle_up);
|
||||
|
||||
// every RT_INTERVAL or when controls are turned on, set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last);
|
||||
if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) {
|
||||
rt_angle_last = angle_meas_new;
|
||||
ts_angle_last = ts;
|
||||
}
|
||||
|
||||
// check for violation
|
||||
if (angle_control &&
|
||||
((angle_meas_new < lowest_rt_angle) ||
|
||||
(angle_meas_new > highest_rt_angle))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
|
||||
controls_allowed_last = controls_allowed;
|
||||
}
|
||||
|
||||
// get speed
|
||||
if (addr == 0xb4) {
|
||||
speed = ((float)((GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6))) * 0.01 / 3.6;
|
||||
}
|
||||
|
||||
// get ipas state
|
||||
if (addr == 0x262) {
|
||||
ipas_state = GET_BYTE(to_push, 0) & 0xf;
|
||||
}
|
||||
|
||||
// exit controls on high steering override
|
||||
if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
|
||||
(torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
|
||||
(ipas_state==5))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
|
||||
int tx = 1;
|
||||
int bypass_standard_tx_hook = 0;
|
||||
int bus = GET_BUS(to_send);
|
||||
int addr = GET_ADDR(to_send);
|
||||
|
||||
// Check if msg is sent on BUS 0
|
||||
if (bus == 0) {
|
||||
|
||||
// STEER ANGLE
|
||||
if ((addr == 0x266) || (addr == 0x167)) {
|
||||
|
||||
angle_control = 1; // we are in angle control mode
|
||||
int desired_angle = ((GET_BYTE(to_send, 0) & 0xF) << 8) | GET_BYTE(to_send, 1);
|
||||
int ipas_state_cmd = GET_BYTE(to_send, 0) >> 4;
|
||||
bool violation = 0;
|
||||
|
||||
desired_angle = to_signed(desired_angle, 12);
|
||||
|
||||
if (controls_allowed) {
|
||||
// add 1 to not false trigger the violation
|
||||
float delta_angle_float;
|
||||
delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG) + 1.;
|
||||
int delta_angle_up = (int) (delta_angle_float);
|
||||
delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG) + 1.;
|
||||
int delta_angle_down = (int) (delta_angle_float);
|
||||
|
||||
int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
|
||||
int lowest_desired_angle = desired_angle_last - ((desired_angle_last > 0) ? delta_angle_down : delta_angle_up);
|
||||
if ((desired_angle > highest_desired_angle) ||
|
||||
(desired_angle < lowest_desired_angle)){
|
||||
violation = 1;
|
||||
controls_allowed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// desired steer angle should be the same as steer angle measured when controls are off
|
||||
if ((!controls_allowed) &&
|
||||
((desired_angle < (angle_meas.min - 1)) ||
|
||||
(desired_angle > (angle_meas.max + 1)) ||
|
||||
(ipas_state_cmd != 1))) {
|
||||
violation = 1;
|
||||
}
|
||||
|
||||
desired_angle_last = desired_angle;
|
||||
|
||||
if (violation) {
|
||||
tx = 0;
|
||||
}
|
||||
bypass_standard_tx_hook = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// check standard toyota stuff as well if addr isn't IPAS related
|
||||
if (!bypass_standard_tx_hook) {
|
||||
tx &= toyota_tx_hook(to_send);
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_ipas_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_ipas_rx_hook,
|
||||
.tx = toyota_ipas_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
};
|
||||
|
|
@ -125,7 +125,6 @@ class Panda(object):
|
|||
SAFETY_MAZDA = 13
|
||||
SAFETY_NISSAN = 14
|
||||
SAFETY_VOLKSWAGEN_MQB = 15
|
||||
SAFETY_TOYOTA_IPAS = 16
|
||||
SAFETY_ALLOUTPUT = 17
|
||||
SAFETY_GM_ASCM = 18
|
||||
SAFETY_NOOUTPUT = 19
|
||||
|
|
|
|||
|
|
@ -39,7 +39,6 @@ bool get_gas_interceptor_detetcted(void);
|
|||
int get_gas_interceptor_prev(void);
|
||||
int get_hw_type(void);
|
||||
void set_timer(uint32_t t);
|
||||
void reset_angle_control(void);
|
||||
|
||||
int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
||||
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
||||
|
|
|
|||
|
|
@ -85,10 +85,6 @@ void set_gas_interceptor_detected(bool c){
|
|||
gas_interceptor_detected = c;
|
||||
}
|
||||
|
||||
void reset_angle_control(void){
|
||||
angle_control = 0;
|
||||
}
|
||||
|
||||
bool get_controls_allowed(void){
|
||||
return controls_allowed;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,243 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
from panda.tests.safety.common import make_msg
|
||||
from panda.tests.safety.test_toyota import toyota_checksum
|
||||
|
||||
IPAS_OVERRIDE_THRESHOLD = 200
|
||||
|
||||
ANGLE_DELTA_BP = [0., 5., 15.]
|
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
|
||||
def sign(a):
|
||||
if a > 0:
|
||||
return 1
|
||||
else:
|
||||
return -1
|
||||
|
||||
|
||||
class TestToyotaSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA_IPAS, 66)
|
||||
cls.safety.init_tests_toyota()
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
to_send = make_msg(0, 0x260)
|
||||
t = twos_comp(torque, 16)
|
||||
to_send[0].RDLR = t | ((t & 0xFF) << 16)
|
||||
to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24)
|
||||
return to_send
|
||||
|
||||
def _torque_driver_msg_array(self, torque):
|
||||
for i in range(6):
|
||||
self.safety.safety_rx_hook(self._torque_driver_msg(torque))
|
||||
|
||||
def _angle_meas_msg(self, angle):
|
||||
to_send = make_msg(0, 0x25)
|
||||
t = twos_comp(angle, 12)
|
||||
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
|
||||
return to_send
|
||||
|
||||
def _angle_meas_msg_array(self, angle):
|
||||
for i in range(6):
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
||||
|
||||
def _ipas_state_msg(self, state):
|
||||
to_send = make_msg(0, 0x262)
|
||||
to_send[0].RDLR = state & 0xF
|
||||
return to_send
|
||||
|
||||
def _ipas_control_msg(self, angle, state):
|
||||
# note: we command 2/3 of the angle due to CAN conversion
|
||||
to_send = make_msg(0, 0x266)
|
||||
t = twos_comp(angle, 12)
|
||||
to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
|
||||
to_send[0].RDLR |= ((state & 0xf) << 4)
|
||||
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
to_send = make_msg(0, 0xB4)
|
||||
speed = int(speed * 100 * 3.6)
|
||||
|
||||
to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00)
|
||||
return to_send
|
||||
|
||||
def test_ipas_override(self):
|
||||
|
||||
## angle control is not active
|
||||
self.safety.set_controls_allowed(1)
|
||||
|
||||
# 3 consecutive msgs where driver exceeds threshold but angle_control isn't active
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# ipas state is override
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(5))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
## now angle control is active
|
||||
self.safety.safety_tx_hook(self._ipas_control_msg(0, 0))
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(0))
|
||||
|
||||
# 3 consecutive msgs where driver does exceed threshold
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# ipas state is override and torque isn't overriding any more
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._torque_driver_msg_array(0)
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(5))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# 3 consecutive msgs where driver does not exceed threshold and
|
||||
# ipas state is not override
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._ipas_state_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_when_disabled(self):
|
||||
|
||||
self.safety.set_controls_allowed(0)
|
||||
|
||||
# test angle cmd too far from actual
|
||||
angle_refs = [-10, 10]
|
||||
deltas = list(range(-2, 3))
|
||||
expected_results = [False, True, True, True, False]
|
||||
|
||||
for a in angle_refs:
|
||||
self._angle_meas_msg_array(a)
|
||||
for i, d in enumerate(deltas):
|
||||
self.assertEqual(expected_results[i], self.safety.safety_tx_hook(self._ipas_control_msg(a + d, 1)))
|
||||
|
||||
# test ipas state cmd enabled
|
||||
self._angle_meas_msg_array(0)
|
||||
self.assertEqual(0, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3)))
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_when_enabled(self):
|
||||
|
||||
# ipas angle cmd should pass through when controls are enabled
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._angle_meas_msg_array(0)
|
||||
self.safety.safety_rx_hook(self._speed_msg(0.1))
|
||||
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(4, 1)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-4, 3)))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-8, 3)))
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_rate_when_disabled(self):
|
||||
|
||||
# as long as the command is close to the measured, no rate limit is enforced when
|
||||
# controls are disabled
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(0))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1)))
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(100))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(100, 1)))
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(-100))
|
||||
self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-100, 1)))
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_cmd_rate_when_enabled(self):
|
||||
|
||||
# when controls are allowed, angle cmd rate limit is enforced
|
||||
# test 1: no limitations if we stay within limits
|
||||
speeds = [0., 1., 5., 10., 15., 100.]
|
||||
angles = [-300, -100, -10, 0, 10, 100, 300]
|
||||
for a in angles:
|
||||
for s in speeds:
|
||||
|
||||
# first test against false positives
|
||||
self._angle_meas_msg_array(a)
|
||||
self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(s))
|
||||
max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
|
||||
max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# now inject too high rates
|
||||
self.assertEqual(False, self.safety.safety_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)
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) *
|
||||
(max_delta_down + 1), 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
def test_angle_measured_rate(self):
|
||||
|
||||
speeds = [0., 1., 5., 10., 15., 100.]
|
||||
angles = [-300, -100, -10, 0, 10, 100, 300]
|
||||
angles = [10]
|
||||
for a in angles:
|
||||
for s in speeds:
|
||||
self._angle_meas_msg_array(a)
|
||||
self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(s))
|
||||
#max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.)
|
||||
#max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.)
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(a))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(a + 150))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# reset no angle control at the end of the test
|
||||
self.safety.reset_angle_control()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue