Chrysler: safety now based on motor torque
parent
039d1834b6
commit
be0061dccb
|
@ -1,17 +1,16 @@
|
|||
const int CHRYSLER_MAX_STEER = 261;
|
||||
const int CHRYSLER_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
||||
const int32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int CHRYSLER_MAX_RATE_UP = 4;
|
||||
const int CHRYSLER_MAX_RATE_DOWN = 8;
|
||||
const int CHRYSLER_DRIVER_TORQUE_ALLOWANCE = 0; // TODO
|
||||
const int CHRYSLER_DRIVER_TORQUE_FACTOR = 0; // TODO
|
||||
const int CHRYSLER_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
||||
const int32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int CHRYSLER_MAX_RATE_UP = 3;
|
||||
const int CHRYSLER_MAX_RATE_DOWN = 3;
|
||||
const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor
|
||||
|
||||
int chrysler_camera_detected = 0;
|
||||
int chrysler_rt_torque_last = 0;
|
||||
int chrysler_desired_torque_last = 0;
|
||||
int chrysler_cruise_engaged_last = 0;
|
||||
uint32_t chrysler_ts_last = 0;
|
||||
struct sample_t chrysler_torque_driver; // last few driver torques measured
|
||||
struct sample_t chrysler_torque_meas; // last few torques measured
|
||||
|
||||
static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int bus = (to_push->RDTR >> 4) & 0xFF;
|
||||
|
@ -26,11 +25,13 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
addr = to_push->RIR >> 21;
|
||||
}
|
||||
|
||||
// TODO
|
||||
// Measured eps torque
|
||||
if (addr == 544) {
|
||||
int torque_driver_new = 0;
|
||||
int rdhr = to_push->RDHR;
|
||||
int torque_meas_new = ((rdhr & 0x7) << 8) + ((rdhr & 0xFF00) >> 8) - 1024;
|
||||
|
||||
// update array of samples
|
||||
update_sample(&chrysler_torque_driver, torque_driver_new);
|
||||
update_sample(&chrysler_torque_meas, torque_meas_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
|
@ -67,12 +68,8 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
addr = to_send->RIR >> 21;
|
||||
}
|
||||
|
||||
// LKA STEER: safety check
|
||||
|
||||
|
||||
|
||||
// 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.
|
||||
// LKA STEER
|
||||
if (addr == 0x292) {
|
||||
int rdlr = to_send->RDLR;
|
||||
int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8) - 1024;
|
||||
|
@ -85,9 +82,8 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, CHRYSLER_MAX_STEER, -CHRYSLER_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, chrysler_desired_torque_last, &chrysler_torque_driver,
|
||||
CHRYSLER_MAX_STEER, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN,
|
||||
CHRYSLER_DRIVER_TORQUE_ALLOWANCE, CHRYSLER_DRIVER_TORQUE_FACTOR);
|
||||
violation |= dist_to_meas_check(desired_torque, chrysler_desired_torque_last,
|
||||
&chrysler_torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR);
|
||||
|
||||
// used next time
|
||||
chrysler_desired_torque_last = desired_torque;
|
||||
|
|
|
@ -42,11 +42,13 @@ void set_toyota_torque_meas(int min, int max);
|
|||
void set_cadillac_torque_driver(int min, int max);
|
||||
void set_gm_torque_driver(int min, int max);
|
||||
void set_hyundai_torque_driver(int min, int max);
|
||||
void set_chrysler_torque_driver(int min, int max);
|
||||
void set_chrysler_torque_meas(int min, int max);
|
||||
void set_toyota_rt_torque_last(int t);
|
||||
void set_toyota_desired_torque_last(int t);
|
||||
int get_toyota_torque_meas_min(void);
|
||||
int get_toyota_torque_meas_max(void);
|
||||
int get_chrysler_torque_meas_min(void);
|
||||
int get_chrysler_torque_meas_max(void);
|
||||
|
||||
void init_tests_honda(void);
|
||||
int get_ego_speed(void);
|
||||
|
|
|
@ -82,9 +82,17 @@ void set_hyundai_torque_driver(int min, int max){
|
|||
hyundai_torque_driver.max = max;
|
||||
}
|
||||
|
||||
void set_chrysler_torque_driver(int min, int max){
|
||||
chrysler_torque_driver.min = min;
|
||||
chrysler_torque_driver.max = max;
|
||||
void set_chrysler_torque_meas(int min, int max){
|
||||
chrysler_torque_meas.min = min;
|
||||
chrysler_torque_meas.max = max;
|
||||
}
|
||||
|
||||
int get_chrysler_torque_meas_min(void){
|
||||
return chrysler_torque_meas.min;
|
||||
}
|
||||
|
||||
int get_chrysler_torque_meas_max(void){
|
||||
return chrysler_torque_meas.max;
|
||||
}
|
||||
|
||||
int get_toyota_torque_meas_min(void){
|
||||
|
|
|
@ -3,15 +3,14 @@ import unittest
|
|||
import numpy as np
|
||||
import libpandasafety_py
|
||||
|
||||
MAX_RATE_UP = 4
|
||||
MAX_RATE_DOWN = 8
|
||||
MAX_RATE_UP = 3
|
||||
MAX_RATE_DOWN = 3
|
||||
MAX_STEER = 261
|
||||
|
||||
MAX_RT_DELTA = 112
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 50;
|
||||
DRIVER_TORQUE_FACTOR = 2;
|
||||
MAX_TORQUE_ERROR = 80
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
|
@ -41,11 +40,12 @@ class TestChryslerSafety(unittest.TestCase):
|
|||
def _set_prev_torque(self, t):
|
||||
self.safety.set_chrysler_desired_torque_last(t)
|
||||
self.safety.set_chrysler_rt_torque_last(t)
|
||||
self.safety.set_chrysler_torque_meas(t, t)
|
||||
|
||||
def _torque_driver_msg(self, torque):
|
||||
def _torque_meas_msg(self, torque):
|
||||
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||
to_send[0].RIR = 544 << 21
|
||||
to_send[0].RDLR = (torque + 2048) << 11
|
||||
to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8)
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
|
@ -59,7 +59,7 @@ class TestChryslerSafety(unittest.TestCase):
|
|||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-0x200, 0x200):
|
||||
for t in range(-MAX_STEER*2, MAX_STEER*2):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
self._set_prev_torque(t)
|
||||
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0):
|
||||
|
@ -91,95 +91,80 @@ class TestChryslerSafety(unittest.TestCase):
|
|||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_chrysler_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(MAX_RATE_UP)))
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(-MAX_RATE_UP)))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_chrysler_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
#def test_against_torque_driver(self):
|
||||
# self.safety.set_controls_allowed(True)
|
||||
self.safety.set_chrysler_rt_torque_last(MAX_STEER)
|
||||
torque_meas = MAX_STEER - MAX_TORQUE_ERROR - 20
|
||||
self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
|
||||
self.safety.set_chrysler_desired_torque_last(MAX_STEER)
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN)))
|
||||
|
||||
# for sign in [-1, 1]:
|
||||
# for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||
# t *= -sign
|
||||
# self.safety.set_chrysler_torque_driver(t, t)
|
||||
# self._set_prev_torque(MAX_STEER * sign)
|
||||
# self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
self.safety.set_chrysler_rt_torque_last(MAX_STEER)
|
||||
self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
|
||||
self.safety.set_chrysler_desired_torque_last(MAX_STEER)
|
||||
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1)))
|
||||
|
||||
# self.safety.set_chrysler_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
# self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
def test_exceed_torque_sensor(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
# # 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_chrysler_torque_driver(-driver_torque, -driver_torque)
|
||||
# self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(torque_desired)))
|
||||
# self._set_prev_torque(torque_desired + delta)
|
||||
# self.safety.set_chrysler_torque_driver(-driver_torque, -driver_torque)
|
||||
# self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
for sign in [-1, 1]:
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_TORQUE_ERROR + 2, 2): # step needs to be smaller than MAX_TORQUE_ERROR
|
||||
t *= sign
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
|
||||
|
||||
# self._set_prev_torque(MAX_STEER * sign)
|
||||
# self.safety.set_chrysler_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
# self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
# self._set_prev_torque(MAX_STEER * sign)
|
||||
# self.safety.set_chrysler_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
# self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(0)))
|
||||
# self._set_prev_torque(MAX_STEER * sign)
|
||||
# self.safety.set_chrysler_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
# self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
self.assertFalse(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 2))))
|
||||
|
||||
|
||||
def test_realtime_limits(self):
|
||||
def test_realtime_limit_up(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_chrysler()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_chrysler_torque_driver(0, 0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
for t in np.arange(0, MAX_RT_DELTA+1, 1):
|
||||
t *= sign
|
||||
self.safety.set_chrysler_torque_meas(t, t)
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
|
||||
self.assertFalse(self.safety.chrysler_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):
|
||||
for t in np.arange(0, MAX_RT_DELTA+1, 1):
|
||||
t *= sign
|
||||
self.safety.set_chrysler_torque_meas(t, t)
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(t)))
|
||||
|
||||
# Increase timer to update rt_torque_last
|
||||
self.safety.set_timer(RT_INTERVAL + 1)
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(sign * MAX_RT_DELTA)))
|
||||
self.assertTrue(self.safety.chrysler_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
def test_torque_measurements(self):
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(50))
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(-50))
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
|
||||
|
||||
#def test_spam_cancel_safety_check(self):
|
||||
# RESUME_BTN = 1
|
||||
# SET_BTN = 2
|
||||
# CANCEL_BTN = 4
|
||||
# BUTTON_MSG = 1265
|
||||
# self.safety.set_controls_allowed(0)
|
||||
# self.assertTrue(self.safety.chrysler_tx_hook(self._button_msg(CANCEL_BTN)))
|
||||
# self.assertFalse(self.safety.chrysler_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
# self.assertFalse(self.safety.chrysler_tx_hook(self._button_msg(SET_BTN)))
|
||||
# # do not block resume if we are engaged already
|
||||
# self.safety.set_controls_allowed(1)
|
||||
# self.assertTrue(self.safety.chrysler_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min())
|
||||
self.assertEqual(50, self.safety.get_chrysler_torque_meas_max())
|
||||
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
|
||||
self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
|
||||
self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min())
|
||||
|
||||
self.safety.chrysler_rx_hook(self._torque_meas_msg(0))
|
||||
self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
|
||||
self.assertEqual(0, self.safety.get_chrysler_torque_meas_min())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
Loading…
Reference in New Issue