Skip to content

Commit

Permalink
Toyota safety: vars and consts need 'toyota_' prefix
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbiasini
rbiasini committed Jul 18, 2018
1 parent 5c905b7 commit ba8762d
Show file tree
Hide file tree
Showing 5 changed files with 84 additions and 83 deletions.
75 changes: 38 additions & 37 deletions board/safety/safety_toyota.h
View file
Original file line number Diff line number Diff line change
@@ -1,32 +1,32 @@
struct sample_t torque_meas; // last 3 motor torques produced by the eps
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps

// global torque limit
const int MAX_TORQUE = 1500; // max torque cmd allowed ever
const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever

// rate based torque limit + stay within actually applied
// packet is sent at 100hz, so this limit is 1000/sec
const int MAX_RATE_UP = 10; // ramp up slow
const int MAX_RATE_DOWN = 25; // ramp down fast
const int MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor
const int TOYOTA_MAX_RATE_UP = 10; // ramp up slow
const int TOYOTA_MAX_RATE_DOWN = 25; // ramp down fast
const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor

// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int RT_INTERVAL = 250000; // 250ms between real time checks
const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real time checks
const int TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks

// longitudinal limits
const int MAX_ACCEL = 1500; // 1.5 m/s2
const int MIN_ACCEL = -3000; // 3.0 m/s2
const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2
const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2

// global actuation limit state
int actuation_limits = 1; // by default steer limits are imposed
int dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
int toyota_actuation_limits = 1; // by default steer limits are imposed
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file

// state of torque limits
int desired_torque_last = 0; // last desired steer torque
int rt_torque_last = 0; // last desired torque for real time check
uint32_t ts_last = 0;
int cruise_engaged_last = 0; // cruise state
int toyota_desired_torque_last = 0; // last desired steer torque
int toyota_rt_torque_last = 0; // last desired torque for real time check
uint32_t toyota_ts_last = 0;
int toyota_cruise_engaged_last = 0; // cruise state


static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
Expand All @@ -36,25 +36,25 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
torque_meas_new = to_signed(torque_meas_new, 16);

// scale by dbc_factor
torque_meas_new = (torque_meas_new * dbc_eps_torque_factor) / 100;
torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100;

// increase torque_meas by 1 to be conservative on rounding
torque_meas_new += (torque_meas_new > 0 ? 1 : -1);

// update array of sample
update_sample(&torque_meas, torque_meas_new);
update_sample(&toyota_torque_meas, torque_meas_new);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((to_push->RIR>>21) == 0x1D2) {
// 4 bits: 55-52
int cruise_engaged = to_push->RDHR & 0xF00000;
if (cruise_engaged && !cruise_engaged_last) {
if (cruise_engaged && !toyota_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
controls_allowed = 0;
}
cruise_engaged_last = cruise_engaged;
toyota_cruise_engaged_last = cruise_engaged;
}
}

Expand All @@ -70,8 +70,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if ((to_send->RIR>>21) == 0x343) {
int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
desired_accel = to_signed(desired_accel, 16);
if (controls_allowed && actuation_limits) {
int violation = max_limit_check(desired_accel, MAX_ACCEL, MIN_ACCEL);
if (controls_allowed && toyota_actuation_limits) {
int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) return 0;
} else if (!controls_allowed && (desired_accel != 0)) {
return 0;
Expand All @@ -87,25 +87,26 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t ts = TIM2->CNT;

// only check if controls are allowed and actuation_limits are imposed
if (controls_allowed && actuation_limits) {
if (controls_allowed && toyota_actuation_limits) {

// *** global torque limit check ***
violation |= max_limit_check(desired_torque, MAX_TORQUE, -MAX_TORQUE);
violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE);

// *** torque rate limit check ***
violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, MAX_RATE_UP, MAX_RATE_DOWN, MAX_TORQUE_ERROR);
violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last,
&toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR);

// used next time
desired_torque_last = desired_torque;
toyota_desired_torque_last = desired_torque;

// *** torque real time rate limit check ***
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, MAX_RT_DELTA);
violation |= rt_rate_limit_check(desired_torque, toyota_rt_torque_last, TOYOTA_MAX_RT_DELTA);

// every RT_INTERVAL set the new limits
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
if (ts_elapsed > RT_INTERVAL) {
rt_torque_last = desired_torque;
ts_last = ts;
uint32_t ts_elapsed = get_ts_elapsed(ts, toyota_ts_last);
if (ts_elapsed > TOYOTA_RT_INTERVAL) {
toyota_rt_torque_last = desired_torque;
toyota_ts_last = ts;
}
}

Expand All @@ -116,9 +117,9 @@ static int toyota_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) {
desired_torque_last = 0;
rt_torque_last = 0;
ts_last = ts;
toyota_desired_torque_last = 0;
toyota_rt_torque_last = 0;
toyota_ts_last = ts;
}

if (violation) {
Expand All @@ -138,8 +139,8 @@ static int toyota_tx_lin_hook(int lin_num, uint8_t *data, int len) {

static void toyota_init(int16_t param) {
controls_allowed = 0;
actuation_limits = 1;
dbc_eps_torque_factor = param;
toyota_actuation_limits = 1;
toyota_dbc_eps_torque_factor = param;
}

static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
Expand All @@ -157,8 +158,8 @@ const safety_hooks toyota_hooks = {

static void toyota_nolimits_init(int16_t param) {
controls_allowed = 0;
actuation_limits = 0;
dbc_eps_torque_factor = param;
toyota_actuation_limits = 0;
toyota_dbc_eps_torque_factor = param;
}

const safety_hooks toyota_nolimits_hooks = {
Expand Down
8 changes: 4 additions & 4 deletions board/safety/safety_toyota_ipas.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
// TODO: refactor to repeat less code

// IPAS override
const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value

struct lookup_t {
float x[3];
Expand Down Expand Up @@ -92,7 +92,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// 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 > RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) {
if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) {
rt_angle_last = angle_meas_new;
ts_angle_last = ts;
}
Expand All @@ -118,8 +118,8 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// exit controls on high steering override
if (angle_control && ((torque_driver.min > IPAS_OVERRIDE_THRESHOLD) ||
(torque_driver.max < -IPAS_OVERRIDE_THRESHOLD) ||
if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
(torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) ||
(ipas_state==5))) {
controls_allowed = 0;
}
Expand Down
10 changes: 5 additions & 5 deletions tests/safety/libpandasafety_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,13 @@
int get_controls_allowed(void);
void init_tests_toyota(void);
void set_timer(int t);
void set_torque_meas(int min, int max);
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_rt_torque_last(int t);
void set_desired_torque_last(int t);
int get_torque_meas_min(void);
int get_torque_meas_max(void);
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);
void init_tests_honda(void);
int get_ego_speed(void);
Expand Down
34 changes: 17 additions & 17 deletions tests/safety/test.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ typedef struct
uint32_t CNT;
} TIM_TypeDef;

struct sample_t torque_meas;
struct sample_t toyota_torque_meas;
struct sample_t cadillac_torque_driver;
struct sample_t gm_torque_driver;

Expand Down Expand Up @@ -60,9 +60,9 @@ void set_timer(int t){
timer.CNT = t;
}

void set_torque_meas(int min, int max){
torque_meas.min = min;
torque_meas.max = max;
void set_toyota_torque_meas(int min, int max){
toyota_torque_meas.min = min;
toyota_torque_meas.max = max;
}

void set_cadillac_torque_driver(int min, int max){
Expand All @@ -75,16 +75,16 @@ void set_gm_torque_driver(int min, int max){
gm_torque_driver.max = max;
}

int get_torque_meas_min(void){
return torque_meas.min;
int get_toyota_torque_meas_min(void){
return toyota_torque_meas.min;
}

int get_torque_meas_max(void){
return torque_meas.max;
int get_toyota_torque_meas_max(void){
return toyota_torque_meas.max;
}

void set_rt_torque_last(int t){
rt_torque_last = t;
void set_toyota_rt_torque_last(int t){
toyota_rt_torque_last = t;
}

void set_cadillac_rt_torque_last(int t){
Expand All @@ -95,8 +95,8 @@ void set_gm_rt_torque_last(int t){
gm_rt_torque_last = t;
}

void set_desired_torque_last(int t){
desired_torque_last = t;
void set_toyota_desired_torque_last(int t){
toyota_desired_torque_last = t;
}

void set_cadillac_desired_torque_last(int t){
Expand Down Expand Up @@ -129,11 +129,11 @@ void set_bosch_hardware(bool c){
}

void init_tests_toyota(void){
torque_meas.min = 0;
torque_meas.max = 0;
desired_torque_last = 0;
rt_torque_last = 0;
ts_last = 0;
toyota_torque_meas.min = 0;
toyota_torque_meas.max = 0;
toyota_desired_torque_last = 0;
toyota_rt_torque_last = 0;
toyota_ts_last = 0;
set_timer(0);
}

Expand Down
40 changes: 20 additions & 20 deletions tests/safety/test_toyota.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,9 @@ def setUp(cls):
cls.safety.init_tests_toyota()

def _set_prev_torque(self, t):
self.safety.set_desired_torque_last(t)
self.safety.set_rt_torque_last(t)
self.safety.set_torque_meas(t, t)
self.safety.set_toyota_desired_torque_last(t)
self.safety.set_toyota_rt_torque_last(t)
self.safety.set_toyota_torque_meas(t, t)

def _torque_meas_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
Expand Down Expand Up @@ -158,9 +158,9 @@ def test_torque_absolute_limits(self):
for controls_allowed in [True, False]:
for torque in np.arange(-MAX_TORQUE - 1000, MAX_TORQUE + 1000, MAX_RATE_UP):
self.safety.set_controls_allowed(controls_allowed)
self.safety.set_rt_torque_last(torque)
self.safety.set_torque_meas(torque, torque)
self.safety.set_desired_torque_last(torque - MAX_RATE_UP)
self.safety.set_toyota_rt_torque_last(torque)
self.safety.set_toyota_torque_meas(torque, torque)
self.safety.set_toyota_desired_torque_last(torque - MAX_RATE_UP)

if controls_allowed:
send = (-MAX_TORQUE <= torque <= MAX_TORQUE)
Expand All @@ -181,14 +181,14 @@ def test_non_realtime_limit_up(self):
def test_non_realtime_limit_down(self):
self.safety.set_controls_allowed(True)

self.safety.set_rt_torque_last(1000)
self.safety.set_torque_meas(500, 500)
self.safety.set_desired_torque_last(1000)
self.safety.set_toyota_rt_torque_last(1000)
self.safety.set_toyota_torque_meas(500, 500)
self.safety.set_toyota_desired_torque_last(1000)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN)))

self.safety.set_rt_torque_last(1000)
self.safety.set_torque_meas(500, 500)
self.safety.set_desired_torque_last(1000)
self.safety.set_toyota_rt_torque_last(1000)
self.safety.set_toyota_torque_meas(500, 500)
self.safety.set_toyota_desired_torque_last(1000)
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1)))

def test_exceed_torque_sensor(self):
Expand All @@ -210,14 +210,14 @@ def test_realtime_limit_up(self):
self._set_prev_torque(0)
for t in np.arange(0, 380, 10):
t *= sign
self.safety.set_torque_meas(t, t)
self.safety.set_toyota_torque_meas(t, t)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))
self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * 380)))

self._set_prev_torque(0)
for t in np.arange(0, 370, 10):
t *= sign
self.safety.set_torque_meas(t, t)
self.safety.set_toyota_torque_meas(t, t)
self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t)))

# Increase timer to update rt_torque_last
Expand All @@ -233,16 +233,16 @@ def test_torque_measurements(self):
self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.safety.toyota_rx_hook(self._torque_meas_msg(0))

self.assertEqual(-51, self.safety.get_torque_meas_min())
self.assertEqual(51, self.safety.get_torque_meas_max())
self.assertEqual(-51, self.safety.get_toyota_torque_meas_min())
self.assertEqual(51, self.safety.get_toyota_torque_meas_max())

self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.assertEqual(-1, self.safety.get_torque_meas_max())
self.assertEqual(-51, self.safety.get_torque_meas_min())
self.assertEqual(-1, self.safety.get_toyota_torque_meas_max())
self.assertEqual(-51, self.safety.get_toyota_torque_meas_min())

self.safety.toyota_rx_hook(self._torque_meas_msg(0))
self.assertEqual(-1, self.safety.get_torque_meas_max())
self.assertEqual(-1, self.safety.get_torque_meas_min())
self.assertEqual(-1, self.safety.get_toyota_torque_meas_max())
self.assertEqual(-1, self.safety.get_toyota_torque_meas_min())

def test_ipas_override(self):

Expand Down

0 comments on commit ba8762d

Please sign in to comment.