1
1
const int CADILLAC_STEER_MAX = 150 ; // 1s
2
2
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 ;
3
11
4
12
int cadillac_ign = 0 ;
5
13
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 ;
6
17
uint32_t cadillac_ts_last = 0 ;
7
18
19
+ struct sample_t cadillac_torque_driver ; // last 3 driver torques measured
20
+
8
21
static void cadillac_rx_hook (CAN_FIFOMailBox_TypeDef * to_push ) {
9
22
int bus_number = (to_push -> RDTR >> 4 ) & 0xFF ;
10
23
uint32_t addr = to_push -> RIR >> 21 ;
11
24
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
+
12
33
// this message isn't all zeros when ignition is on
13
34
if ((addr == 0x160 ) && (bus_number == 0 ) && to_push -> RDLR ) {
14
35
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
16
37
}
17
38
18
39
// 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) {
32
53
33
54
// block steering cmd above 150
34
55
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
+
43
129
}
44
130
return true;
45
131
}
@@ -50,14 +136,13 @@ static void cadillac_init(int16_t param) {
50
136
51
137
static int cadillac_ign_hook () {
52
138
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 );
54
140
if (ts_elapsed > CADILLAC_IGNITION_TIMEOUT ) {
55
141
cadillac_ign = 0 ;
56
142
}
57
143
return cadillac_ign ;
58
144
}
59
145
60
- // Placeholder file, actual safety is TODO.
61
146
const safety_hooks cadillac_hooks = {
62
147
.init = cadillac_init ,
63
148
.rx = cadillac_rx_hook ,
0 commit comments