Skip to content

Commit c7e2c2d

Browse files
authored
Cadillac (#119)
* added some steer safety to Cadillac * bug fixes * added cadillac full steering safety. To be tested * RT checks and max checks working. Need to test driver torque based limits * cadillac steer safety should be done * unneded lines
1 parent 83bcaa3 commit c7e2c2d

File tree

4 files changed

+309
-11
lines changed

4 files changed

+309
-11
lines changed

board/safety/safety_cadillac.h

+96-11
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,39 @@
11
const int CADILLAC_STEER_MAX = 150; // 1s
22
const int CADILLAC_IGNITION_TIMEOUT = 1000000; // 1s
3+
// real time torque limit to prevent controls spamming
4+
// the real time limit is 1500/sec
5+
const int CADILLAC_MAX_RT_DELTA = 75; // max delta torque allowed for real time checks
6+
const int32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks
7+
const int CADILLAC_MAX_RATE_UP = 2;
8+
const int CADILLAC_MAX_RATE_DOWN = 5;
9+
const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50;
10+
const int CADILLAC_DRIVER_TORQUE_FACTOR = 4;
311

412
int cadillac_ign = 0;
513
int cadillac_cruise_engaged_last = 0;
14+
uint32_t cadillac_ts_ign_last = 0;
15+
int cadillac_rt_torque_last = 0;
16+
int cadillac_desired_torque_last = 0;
617
uint32_t cadillac_ts_last = 0;
718

19+
struct sample_t cadillac_torque_driver; // last 3 driver torques measured
20+
821
static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
922
int bus_number = (to_push->RDTR >> 4) & 0xFF;
1023
uint32_t addr = to_push->RIR >> 21;
1124

25+
if (addr == 356) {
26+
int torque_driver_new = ((to_push->RDLR & 0x3) << 8) | ((to_push->RDLR >> 8) & 0xFF);
27+
torque_driver_new = to_signed(torque_driver_new, 11);
28+
29+
// update array of sample
30+
update_sample(&cadillac_torque_driver, torque_driver_new);
31+
}
32+
1233
// this message isn't all zeros when ignition is on
1334
if ((addr == 0x160) && (bus_number == 0) && to_push->RDLR) {
1435
cadillac_ign = 1;
15-
cadillac_ts_last = TIM2->CNT; // reset timer when ign is received
36+
cadillac_ts_ign_last = TIM2->CNT; // reset timer when ign is received
1637
}
1738

1839
// enter controls on rising edge of ACC, exit controls on ACC off
@@ -32,14 +53,79 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
3253

3354
// block steering cmd above 150
3455
if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
35-
int lkas_cmd = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
36-
lkas_cmd = to_signed(lkas_cmd, 14);
37-
// block message is controls are allowed and lkas command exceeds max, or
38-
// if controls aren't allowed and lkas cmd isn't 0
39-
if (controls_allowed &&
40-
((lkas_cmd > CADILLAC_STEER_MAX) || (lkas_cmd < -CADILLAC_STEER_MAX))) {
41-
return 0;
42-
} else if (!controls_allowed && lkas_cmd) return 0;
56+
int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
57+
int violation = 0;
58+
uint32_t ts = TIM2->CNT;
59+
desired_torque = to_signed(desired_torque, 14);
60+
61+
if (controls_allowed) {
62+
63+
// *** global torque limit check ***
64+
if ((desired_torque > CADILLAC_STEER_MAX) || (desired_torque < -CADILLAC_STEER_MAX)) {
65+
violation = 1;
66+
}
67+
68+
// *** torque rate limit check ***
69+
int highest_allowed_torque = max(cadillac_desired_torque_last, 0) + CADILLAC_MAX_RATE_UP;
70+
int lowest_allowed_torque = min(cadillac_desired_torque_last, 0) - CADILLAC_MAX_RATE_UP;
71+
72+
int driver_torque_max_limit = CADILLAC_STEER_MAX +
73+
(CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) *
74+
CADILLAC_DRIVER_TORQUE_FACTOR;
75+
int driver_torque_min_limit = -CADILLAC_STEER_MAX +
76+
(-CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) *
77+
CADILLAC_DRIVER_TORQUE_FACTOR;
78+
79+
// if we've exceeded the applied torque, we must start moving toward 0
80+
highest_allowed_torque = min(highest_allowed_torque,
81+
max(cadillac_desired_torque_last - CADILLAC_MAX_RATE_DOWN,
82+
max(driver_torque_max_limit, 0)));
83+
lowest_allowed_torque = max(lowest_allowed_torque,
84+
min(cadillac_desired_torque_last + CADILLAC_MAX_RATE_DOWN,
85+
min(driver_torque_min_limit, 0)));
86+
87+
// check for violation
88+
if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) {
89+
violation = 1;
90+
}
91+
92+
//// used next time
93+
cadillac_desired_torque_last = desired_torque;
94+
95+
// *** torque real time rate limit check ***
96+
int highest_rt_torque = max(cadillac_rt_torque_last, 0) + CADILLAC_MAX_RT_DELTA;
97+
int lowest_rt_torque = min(cadillac_rt_torque_last, 0) - CADILLAC_MAX_RT_DELTA;
98+
99+
100+
// check for violation
101+
if ((desired_torque < lowest_rt_torque) || (desired_torque > highest_rt_torque)) {
102+
violation = 1;
103+
}
104+
105+
// every RT_INTERVAL set the new limits
106+
uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
107+
if (ts_elapsed > RT_INTERVAL) {
108+
cadillac_rt_torque_last = desired_torque;
109+
cadillac_ts_last = ts;
110+
}
111+
}
112+
113+
// no torque if controls is not allowed
114+
if (!controls_allowed && (desired_torque != 0)) {
115+
violation = 1;
116+
}
117+
118+
// reset to 0 if either controls is not allowed or there's a violation
119+
if (violation || !controls_allowed) {
120+
cadillac_desired_torque_last = 0;
121+
cadillac_rt_torque_last = 0;
122+
cadillac_ts_last = ts;
123+
}
124+
125+
if (violation) {
126+
return false;
127+
}
128+
43129
}
44130
return true;
45131
}
@@ -50,14 +136,13 @@ static void cadillac_init(int16_t param) {
50136

51137
static int cadillac_ign_hook() {
52138
uint32_t ts = TIM2->CNT;
53-
uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
139+
uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_ign_last);
54140
if (ts_elapsed > CADILLAC_IGNITION_TIMEOUT) {
55141
cadillac_ign = 0;
56142
}
57143
return cadillac_ign;
58144
}
59145

60-
// Placeholder file, actual safety is TODO.
61146
const safety_hooks cadillac_hooks = {
62147
.init = cadillac_init,
63148
.rx = cadillac_rx_hook,

tests/safety/libpandasafety_py.py

+8
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
void init_tests_toyota(void);
4040
void set_timer(int t);
4141
void set_torque_meas(int min, int max);
42+
void set_cadillac_torque_driver(int min, int max);
4243
void set_rt_torque_last(int t);
4344
void set_desired_torque_last(int t);
4445
int get_torque_meas_min(void);
@@ -52,6 +53,13 @@
5253
int get_brake_prev(void);
5354
int get_gas_prev(void);
5455
56+
void init_tests_cadillac(void);
57+
void cadillac_init(int16_t param);
58+
void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
59+
int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
60+
void set_cadillac_desired_torque_last(int t);
61+
void set_cadillac_rt_torque_last(int t);
62+
5563
void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
5664
int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
5765

tests/safety/test.c

+23
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ typedef struct
2323
} TIM_TypeDef;
2424

2525
struct sample_t torque_meas;
26+
struct sample_t cadillac_torque_driver;
2627

2728
TIM_TypeDef timer;
2829
TIM_TypeDef *TIM2 = &timer;
@@ -63,6 +64,11 @@ void set_torque_meas(int min, int max){
6364
torque_meas.max = max;
6465
}
6566

67+
void set_cadillac_torque_driver(int min, int max){
68+
cadillac_torque_driver.min = min;
69+
cadillac_torque_driver.max = max;
70+
}
71+
6672
int get_torque_meas_min(void){
6773
return torque_meas.min;
6874
}
@@ -75,10 +81,18 @@ void set_rt_torque_last(int t){
7581
rt_torque_last = t;
7682
}
7783

84+
void set_cadillac_rt_torque_last(int t){
85+
cadillac_rt_torque_last = t;
86+
}
87+
7888
void set_desired_torque_last(int t){
7989
desired_torque_last = t;
8090
}
8191

92+
void set_cadillac_desired_torque_last(int t){
93+
cadillac_desired_torque_last = t;
94+
}
95+
8296
int get_ego_speed(void){
8397
return ego_speed;
8498
}
@@ -100,6 +114,15 @@ void init_tests_toyota(void){
100114
set_timer(0);
101115
}
102116

117+
void init_tests_cadillac(void){
118+
cadillac_torque_driver.min = 0;
119+
cadillac_torque_driver.max = 0;
120+
cadillac_desired_torque_last = 0;
121+
cadillac_rt_torque_last = 0;
122+
cadillac_ts_last = 0;
123+
set_timer(0);
124+
}
125+
103126
void init_tests_honda(void){
104127
ego_speed = 0;
105128
gas_interceptor_detected = 0;

0 commit comments

Comments
 (0)