Line data Source code
1 : #pragma once
2 :
3 : #include "safety_declarations.h"
4 : #include "can.h"
5 :
6 : // include the safety policies.
7 : #include "safety/safety_defaults.h"
8 : #include "safety/safety_honda.h"
9 : #include "safety/safety_toyota.h"
10 : #include "safety/safety_tesla.h"
11 : #include "safety/safety_gm.h"
12 : #include "safety/safety_ford.h"
13 : #include "safety/safety_hyundai.h"
14 : #include "safety/safety_chrysler.h"
15 : #include "safety/safety_rivian.h"
16 : #include "safety/safety_subaru.h"
17 : #include "safety/safety_subaru_preglobal.h"
18 : #include "safety/safety_mazda.h"
19 : #include "safety/safety_nissan.h"
20 : #include "safety/safety_volkswagen_mqb.h"
21 : #include "safety/safety_volkswagen_pq.h"
22 : #include "safety/safety_elm327.h"
23 : #include "safety/safety_body.h"
24 :
25 : // CAN-FD only safety modes
26 : #ifdef CANFD
27 : #include "safety/safety_hyundai_canfd.h"
28 : #endif
29 :
30 : // from cereal.car.CarParams.SafetyModel
31 : #define SAFETY_SILENT 0U
32 : #define SAFETY_HONDA_NIDEC 1U
33 : #define SAFETY_TOYOTA 2U
34 : #define SAFETY_ELM327 3U
35 : #define SAFETY_GM 4U
36 : #define SAFETY_HONDA_BOSCH_GIRAFFE 5U
37 : #define SAFETY_FORD 6U
38 : #define SAFETY_HYUNDAI 8U
39 : #define SAFETY_CHRYSLER 9U
40 : #define SAFETY_TESLA 10U
41 : #define SAFETY_SUBARU 11U
42 : #define SAFETY_MAZDA 13U
43 : #define SAFETY_NISSAN 14U
44 : #define SAFETY_VOLKSWAGEN_MQB 15U
45 : #define SAFETY_ALLOUTPUT 17U
46 : #define SAFETY_GM_ASCM 18U
47 : #define SAFETY_NOOUTPUT 19U
48 : #define SAFETY_HONDA_BOSCH 20U
49 : #define SAFETY_VOLKSWAGEN_PQ 21U
50 : #define SAFETY_SUBARU_PREGLOBAL 22U
51 : #define SAFETY_HYUNDAI_LEGACY 23U
52 : #define SAFETY_HYUNDAI_COMMUNITY 24U
53 : #define SAFETY_STELLANTIS 25U
54 : #define SAFETY_FAW 26U
55 : #define SAFETY_BODY 27U
56 : #define SAFETY_HYUNDAI_CANFD 28U
57 : #define SAFETY_RIVIAN 33U
58 :
59 1061905 : uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
60 1061905 : uint32_t ret = 0U;
61 4549401 : for (int i = 0; i < len; i++) {
62 3487496 : const uint32_t shift = i * 8;
63 3487496 : ret |= (((uint32_t)msg->data[start + i]) << shift);
64 : }
65 1061905 : return ret;
66 : }
67 :
68 : const int MAX_WRONG_COUNTERS = 5;
69 :
70 : // This can be set by the safety hooks
71 : bool controls_allowed = false;
72 : bool relay_malfunction = false;
73 : bool gas_pressed = false;
74 : bool gas_pressed_prev = false;
75 : bool brake_pressed = false;
76 : bool brake_pressed_prev = false;
77 : bool regen_braking = false;
78 : bool regen_braking_prev = false;
79 : bool cruise_engaged_prev = false;
80 : struct sample_t vehicle_speed;
81 : bool vehicle_moving = false;
82 : bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018
83 : int cruise_button_prev = 0;
84 : bool safety_rx_checks_invalid = false;
85 :
86 : // for safety modes with torque steering control
87 : int desired_torque_last = 0; // last desired steer torque
88 : int rt_torque_last = 0; // last desired torque for real time check
89 : int valid_steer_req_count = 0; // counter for steer request bit matching non-zero torque
90 : int invalid_steer_req_count = 0; // counter to allow multiple frames of mismatching torque request bit
91 : struct sample_t torque_meas; // last 6 motor torques produced by the eps
92 : struct sample_t torque_driver; // last 6 driver torques measured
93 : uint32_t ts_torque_check_last = 0;
94 : uint32_t ts_steer_req_mismatch_last = 0; // last timestamp steer req was mismatched with torque
95 :
96 : // state for controls_allowed timeout logic
97 : bool heartbeat_engaged = false; // openpilot enabled, passed in heartbeat USB command
98 : uint32_t heartbeat_engaged_mismatches = 0; // count of mismatches between heartbeat_engaged and controls_allowed
99 :
100 : // for safety modes with angle steering control
101 : uint32_t ts_angle_last = 0;
102 : int desired_angle_last = 0;
103 : struct sample_t angle_meas; // last 6 steer angles/curvatures
104 :
105 :
106 : int alternative_experience = 0;
107 :
108 : // time since safety mode has been changed
109 : uint32_t safety_mode_cnt = 0U;
110 :
111 : uint16_t current_safety_mode = SAFETY_SILENT;
112 : uint16_t current_safety_param = 0;
113 : static const safety_hooks *current_hooks = &nooutput_hooks;
114 : safety_config current_safety_config;
115 :
116 1696920 : static bool is_msg_valid(RxCheck addr_list[], int index) {
117 1696920 : bool valid = true;
118 1696920 : if (index != -1) {
119 1012326 : if (!addr_list[index].status.valid_checksum || !addr_list[index].status.valid_quality_flag || (addr_list[index].status.wrong_counters >= MAX_WRONG_COUNTERS)) {
120 2524 : valid = false;
121 2524 : controls_allowed = false;
122 : }
123 : }
124 1696920 : return valid;
125 : }
126 :
127 1696920 : static int get_addr_check_index(const CANPacket_t *to_push, RxCheck addr_list[], const int len) {
128 1696920 : int bus = GET_BUS(to_push);
129 1696920 : int addr = GET_ADDR(to_push);
130 1696920 : int length = GET_LEN(to_push);
131 :
132 1696920 : int index = -1;
133 6109612 : for (int i = 0; i < len; i++) {
134 : // if multiple msgs are allowed, determine which one is present on the bus
135 5425018 : if (!addr_list[i].status.msg_seen) {
136 5349250 : for (uint8_t j = 0U; (j < MAX_ADDR_CHECK_MSGS) && (addr_list[i].msg[j].addr != 0); j++) {
137 3063502 : if ((addr == addr_list[i].msg[j].addr) && (bus == addr_list[i].msg[j].bus) &&
138 9589 : (length == addr_list[i].msg[j].len)) {
139 9357 : addr_list[i].status.index = j;
140 9357 : addr_list[i].status.msg_seen = true;
141 9357 : break;
142 : }
143 : }
144 : }
145 :
146 5425018 : if (addr_list[i].status.msg_seen) {
147 3139270 : int idx = addr_list[i].status.index;
148 3139270 : if ((addr == addr_list[i].msg[idx].addr) && (bus == addr_list[i].msg[idx].bus) &&
149 1012326 : (length == addr_list[i].msg[idx].len)) {
150 1012326 : index = i;
151 1012326 : break;
152 : }
153 : }
154 : }
155 1696920 : return index;
156 : }
157 :
158 1696920 : static void update_addr_timestamp(RxCheck addr_list[], int index) {
159 1696920 : if (index != -1) {
160 1012326 : uint32_t ts = microsecond_timer_get();
161 1012326 : addr_list[index].status.last_timestamp = ts;
162 : }
163 1696920 : }
164 :
165 758560 : static void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
166 758560 : if (index != -1) {
167 758560 : uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U);
168 758560 : addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1;
169 758560 : addr_list[index].status.wrong_counters = CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
170 758560 : addr_list[index].status.last_counter = counter;
171 : }
172 758560 : }
173 :
174 1696920 : static bool rx_msg_safety_check(const CANPacket_t *to_push,
175 : const safety_config *cfg,
176 : const safety_hooks *safety_hooks) {
177 :
178 1696920 : int index = get_addr_check_index(to_push, cfg->rx_checks, cfg->rx_checks_len);
179 1696920 : update_addr_timestamp(cfg->rx_checks, index);
180 :
181 1696920 : if (index != -1) {
182 : // checksum check
183 1012326 : if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].check_checksum) {
184 876199 : uint32_t checksum = safety_hooks->get_checksum(to_push);
185 876199 : uint32_t checksum_comp = safety_hooks->compute_checksum(to_push);
186 876199 : cfg->rx_checks[index].status.valid_checksum = checksum_comp == checksum;
187 : } else {
188 136127 : cfg->rx_checks[index].status.valid_checksum = true;
189 : }
190 :
191 : // counter check (max_counter == 0 means skip check)
192 1770886 : if ((safety_hooks->get_counter != NULL) && (cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].max_counter > 0U)) {
193 758560 : uint8_t counter = safety_hooks->get_counter(to_push);
194 758560 : update_counter(cfg->rx_checks, index, counter);
195 : } else {
196 253766 : cfg->rx_checks[index].status.wrong_counters = 0U;
197 : }
198 :
199 : // quality flag check
200 1012326 : if ((safety_hooks->get_quality_flag_valid != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].quality_flag) {
201 751769 : cfg->rx_checks[index].status.valid_quality_flag = safety_hooks->get_quality_flag_valid(to_push);
202 : } else {
203 260557 : cfg->rx_checks[index].status.valid_quality_flag = true;
204 : }
205 : }
206 1696920 : return is_msg_valid(cfg->rx_checks, index);
207 : }
208 :
209 1696920 : bool safety_rx_hook(const CANPacket_t *to_push) {
210 1696920 : bool controls_allowed_prev = controls_allowed;
211 :
212 1696920 : bool valid = rx_msg_safety_check(to_push, ¤t_safety_config, current_hooks);
213 1696920 : if (valid) {
214 1694396 : current_hooks->rx(to_push);
215 : }
216 :
217 : // reset mismatches on rising edge of controls_allowed to avoid rare race condition
218 1696920 : if (controls_allowed && !controls_allowed_prev) {
219 858 : heartbeat_engaged_mismatches = 0;
220 : }
221 :
222 1696920 : return valid;
223 : }
224 :
225 3898376 : static bool tx_msg_safety_check(const CANPacket_t *to_send, const CanMsg msg_list[], int len) {
226 3898376 : int addr = GET_ADDR(to_send);
227 3898376 : int bus = GET_BUS(to_send);
228 3898376 : int length = GET_LEN(to_send);
229 :
230 3898376 : bool allowed = false;
231 17149123 : for (int i = 0; i < len; i++) {
232 15398510 : if ((addr == msg_list[i].addr) && (bus == msg_list[i].bus) && (length == msg_list[i].len)) {
233 2147763 : allowed = true;
234 2147763 : break;
235 : }
236 : }
237 3898376 : return allowed;
238 : }
239 :
240 3898376 : bool safety_tx_hook(CANPacket_t *to_send) {
241 3898376 : bool allowed = tx_msg_safety_check(to_send, current_safety_config.tx_msgs, current_safety_config.tx_msgs_len);
242 3898376 : if ((current_safety_mode == SAFETY_ALLOUTPUT) || (current_safety_mode == SAFETY_ELM327)) {
243 42247 : allowed = true;
244 : }
245 :
246 3898376 : bool safety_allowed = false;
247 : if (allowed) {
248 : }
249 3898376 : safety_allowed = current_hooks->tx(to_send);
250 :
251 3898376 : return !relay_malfunction && allowed && safety_allowed;
252 : }
253 :
254 1284098 : int safety_fwd_hook(int bus_num, int addr) {
255 1284098 : return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, addr));
256 : }
257 :
258 600818 : bool get_longitudinal_allowed(void) {
259 600818 : return controls_allowed && !gas_pressed_prev;
260 : }
261 :
262 : // Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
263 : // algorithm. Called at init time for safety modes using CRC-8.
264 67 : void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]) {
265 17219 : for (uint16_t i = 0U; i <= 0xFFU; i++) {
266 17152 : uint8_t crc = (uint8_t)i;
267 154368 : for (int j = 0; j < 8; j++) {
268 137216 : if ((crc & 0x80U) != 0U) {
269 68608 : crc = (uint8_t)((crc << 1) ^ poly);
270 : } else {
271 68608 : crc <<= 1;
272 : }
273 : }
274 17152 : crc_lut[i] = crc;
275 : }
276 67 : }
277 :
278 : #ifdef CANFD
279 756 : void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]) {
280 194292 : for (uint16_t i = 0; i < 256U; i++) {
281 193536 : uint16_t crc = i << 8U;
282 1741824 : for (uint16_t j = 0; j < 8U; j++) {
283 1548288 : if ((crc & 0x8000U) != 0U) {
284 774144 : crc = (uint16_t)((crc << 1) ^ poly);
285 : } else {
286 774144 : crc <<= 1;
287 : }
288 : }
289 193536 : crc_lut[i] = crc;
290 : }
291 756 : }
292 : #endif
293 :
294 : // 1Hz safety function called by main. Now just a check for lagging safety messages
295 72 : void safety_tick(const safety_config *cfg) {
296 72 : const uint8_t MAX_MISSED_MSGS = 10U;
297 72 : bool rx_checks_invalid = false;
298 72 : uint32_t ts = microsecond_timer_get();
299 72 : if (cfg != NULL) {
300 433 : for (int i=0; i < cfg->rx_checks_len; i++) {
301 361 : uint32_t elapsed_time = get_ts_elapsed(ts, cfg->rx_checks[i].status.last_timestamp);
302 : // lag threshold is max of: 1s and MAX_MISSED_MSGS * expected timestep.
303 : // Quite conservative to not risk false triggers.
304 : // 2s of lag is worse case, since the function is called at 1Hz
305 361 : uint32_t timestep = 1e6 / cfg->rx_checks[i].msg[cfg->rx_checks[i].status.index].frequency;
306 361 : bool lagging = elapsed_time > MAX(timestep * MAX_MISSED_MSGS, 1e6);
307 361 : cfg->rx_checks[i].status.lagging = lagging;
308 361 : if (lagging) {
309 361 : controls_allowed = false;
310 : }
311 :
312 361 : if (lagging || !is_msg_valid(cfg->rx_checks, i)) {
313 361 : rx_checks_invalid = true;
314 : }
315 : }
316 : }
317 :
318 72 : safety_rx_checks_invalid = rx_checks_invalid;
319 72 : }
320 :
321 120 : static void relay_malfunction_set(void) {
322 120 : relay_malfunction = true;
323 120 : fault_occurred(FAULT_RELAY_MALFUNCTION);
324 120 : }
325 :
326 1546148 : void generic_rx_checks(bool stock_ecu_detected) {
327 : // allow 1s of transition timeout after relay changes state before assessing malfunctioning
328 1546148 : const uint32_t RELAY_TRNS_TIMEOUT = 1U;
329 :
330 : // exit controls on rising edge of gas press
331 1546148 : if (gas_pressed && !gas_pressed_prev && !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS)) {
332 238 : controls_allowed = false;
333 : }
334 1546148 : gas_pressed_prev = gas_pressed;
335 :
336 : // exit controls on rising edge of brake press
337 1546148 : if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
338 460 : controls_allowed = false;
339 : }
340 1546148 : brake_pressed_prev = brake_pressed;
341 :
342 : // exit controls on rising edge of regen paddle
343 1546148 : if (regen_braking && (!regen_braking_prev || vehicle_moving)) {
344 18 : controls_allowed = false;
345 : }
346 1546148 : regen_braking_prev = regen_braking;
347 :
348 : // check if stock ECU is on bus broken by car harness
349 1546148 : if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) {
350 120 : relay_malfunction_set();
351 : }
352 1546148 : }
353 :
354 10344 : static void relay_malfunction_reset(void) {
355 10344 : relay_malfunction = false;
356 10344 : fault_recovered(FAULT_RELAY_MALFUNCTION);
357 10344 : }
358 :
359 : // resets values and min/max for sample_t struct
360 41376 : static void reset_sample(struct sample_t *sample) {
361 289632 : for (int i = 0; i < MAX_SAMPLE_VALS; i++) {
362 248256 : sample->values[i] = 0;
363 : }
364 41376 : update_sample(sample, 0);
365 41376 : }
366 :
367 10344 : int set_safety_hooks(uint16_t mode, uint16_t param) {
368 10344 : const safety_hook_config safety_hook_registry[] = {
369 : {SAFETY_SILENT, &nooutput_hooks},
370 : {SAFETY_HONDA_NIDEC, &honda_nidec_hooks},
371 : {SAFETY_TOYOTA, &toyota_hooks},
372 : {SAFETY_ELM327, &elm327_hooks},
373 : {SAFETY_GM, &gm_hooks},
374 : {SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
375 : {SAFETY_HYUNDAI, &hyundai_hooks},
376 : {SAFETY_CHRYSLER, &chrysler_hooks},
377 : {SAFETY_SUBARU, &subaru_hooks},
378 : {SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
379 : {SAFETY_NISSAN, &nissan_hooks},
380 : {SAFETY_NOOUTPUT, &nooutput_hooks},
381 : {SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks},
382 : {SAFETY_MAZDA, &mazda_hooks},
383 : {SAFETY_BODY, &body_hooks},
384 : {SAFETY_FORD, &ford_hooks},
385 : {SAFETY_RIVIAN, &rivian_hooks},
386 : #ifdef CANFD
387 : {SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks},
388 : #endif
389 : #ifdef ALLOW_DEBUG
390 : {SAFETY_TESLA, &tesla_hooks},
391 : {SAFETY_SUBARU_PREGLOBAL, &subaru_preglobal_hooks},
392 : {SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks},
393 : {SAFETY_ALLOUTPUT, &alloutput_hooks},
394 : #endif
395 : };
396 :
397 : // reset state set by safety mode
398 10344 : safety_mode_cnt = 0U;
399 10344 : relay_malfunction = false;
400 10344 : gas_pressed = false;
401 10344 : gas_pressed_prev = false;
402 10344 : brake_pressed = false;
403 10344 : brake_pressed_prev = false;
404 10344 : regen_braking = false;
405 10344 : regen_braking_prev = false;
406 10344 : cruise_engaged_prev = false;
407 10344 : vehicle_moving = false;
408 10344 : acc_main_on = false;
409 10344 : cruise_button_prev = 0;
410 10344 : desired_torque_last = 0;
411 10344 : rt_torque_last = 0;
412 10344 : ts_angle_last = 0;
413 10344 : desired_angle_last = 0;
414 10344 : ts_torque_check_last = 0;
415 10344 : ts_steer_req_mismatch_last = 0;
416 10344 : valid_steer_req_count = 0;
417 10344 : invalid_steer_req_count = 0;
418 :
419 : // reset samples
420 10344 : reset_sample(&vehicle_speed);
421 10344 : reset_sample(&torque_meas);
422 10344 : reset_sample(&torque_driver);
423 10344 : reset_sample(&angle_meas);
424 :
425 10344 : controls_allowed = false;
426 10344 : relay_malfunction_reset();
427 10344 : safety_rx_checks_invalid = false;
428 :
429 10344 : current_safety_config.rx_checks = NULL;
430 10344 : current_safety_config.rx_checks_len = 0;
431 10344 : current_safety_config.tx_msgs = NULL;
432 10344 : current_safety_config.tx_msgs_len = 0;
433 :
434 10344 : int set_status = -1; // not set
435 10344 : int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
436 237912 : for (int i = 0; i < hook_config_count; i++) {
437 227568 : if (safety_hook_registry[i].id == mode) {
438 10344 : current_hooks = safety_hook_registry[i].hooks;
439 10344 : current_safety_mode = mode;
440 10344 : current_safety_param = param;
441 10344 : set_status = 0; // set
442 : }
443 : }
444 10344 : if ((set_status == 0) && (current_hooks->init != NULL)) {
445 10344 : safety_config cfg = current_hooks->init(param);
446 10344 : current_safety_config.rx_checks = cfg.rx_checks;
447 10344 : current_safety_config.rx_checks_len = cfg.rx_checks_len;
448 10344 : current_safety_config.tx_msgs = cfg.tx_msgs;
449 10344 : current_safety_config.tx_msgs_len = cfg.tx_msgs_len;
450 : // reset all dynamic fields in addr struct
451 66240 : for (int j = 0; j < current_safety_config.rx_checks_len; j++) {
452 55896 : current_safety_config.rx_checks[j].status = (RxStatus){0};
453 : }
454 : }
455 10344 : return set_status;
456 : }
457 :
458 : // convert a trimmed integer to signed 32 bit int
459 648220 : int to_signed(int d, int bits) {
460 648220 : int d_signed = d;
461 648220 : int max_value = (1 << MAX((bits - 1), 0));
462 648220 : if (d >= max_value) {
463 224976 : d_signed = d - (1 << MAX(bits, 0));
464 : }
465 648220 : return d_signed;
466 : }
467 :
468 : // given a new sample, update the sample_t struct
469 1282110 : void update_sample(struct sample_t *sample, int sample_new) {
470 7692660 : for (int i = MAX_SAMPLE_VALS - 1; i > 0; i--) {
471 6410550 : sample->values[i] = sample->values[i-1];
472 : }
473 1282110 : sample->values[0] = sample_new;
474 :
475 : // get the minimum and maximum measured samples
476 1282110 : sample->min = sample->values[0];
477 1282110 : sample->max = sample->values[0];
478 7692660 : for (int i = 1; i < MAX_SAMPLE_VALS; i++) {
479 6410550 : if (sample->values[i] < sample->min) {
480 519483 : sample->min = sample->values[i];
481 : }
482 6410550 : if (sample->values[i] > sample->max) {
483 244864 : sample->max = sample->values[i];
484 : }
485 : }
486 1282110 : }
487 :
488 4235065 : static bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
489 4235065 : return (val > MAX_VAL) || (val < MIN_VAL);
490 : }
491 :
492 : // check that commanded torque value isn't too far from measured
493 44331 : static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
494 : const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
495 :
496 : // *** val rate limit check ***
497 44331 : int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
498 44331 : int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
499 :
500 : // if we've exceeded the meas val, we must start moving toward 0
501 44331 : int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, MAX(val_meas->max, 0) + MAX_ERROR));
502 44331 : int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR));
503 :
504 : // check for violation
505 44331 : return max_limit_check(val, highest_allowed, lowest_allowed);
506 : }
507 :
508 : // check that commanded value isn't fighting against driver
509 1300141 : static bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver,
510 : const int MAX_VAL, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
511 : const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {
512 :
513 : // torque delta/rate limits
514 1300141 : int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
515 1300141 : int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
516 :
517 : // driver
518 1300141 : int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
519 1300141 : int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
520 :
521 : // if we've exceeded the applied torque, we must start moving toward 0
522 1300141 : int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN,
523 : MAX(driver_max_limit, 0)));
524 1300141 : int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN,
525 : MIN(driver_min_limit, 0)));
526 :
527 : // check for violation
528 1300141 : return max_limit_check(val, highest_allowed, lowest_allowed);
529 : }
530 :
531 :
532 : // real time check, mainly used for steer torque rate limiter
533 1344472 : static bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
534 :
535 : // *** torque real time rate limit check ***
536 1344472 : int highest_val = MAX(val_last, 0) + MAX_RT_DELTA;
537 1344472 : int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA;
538 :
539 : // check for violation
540 1344472 : return max_limit_check(val, highest_val, lowest_val);
541 : }
542 :
543 :
544 : // interp function that holds extreme values
545 182060 : static float interpolate(struct lookup_t xy, float x) {
546 :
547 182060 : int size = sizeof(xy.x) / sizeof(xy.x[0]);
548 182060 : float ret = xy.y[size - 1]; // default output is last point
549 :
550 : // x is lower than the first point in the x array. Return the first point
551 182060 : if (x <= xy.x[0]) {
552 102422 : ret = xy.y[0];
553 :
554 : } else {
555 : // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
556 124950 : for (int i=0; i < (size - 1); i++) {
557 109554 : if (x < xy.x[i+1]) {
558 64242 : float x0 = xy.x[i];
559 64242 : float y0 = xy.y[i];
560 64242 : float dx = xy.x[i+1] - x0;
561 64242 : float dy = xy.y[i+1] - y0;
562 : // dx should not be zero as xy.x is supposed to be monotonic
563 64242 : dx = MAX(dx, 0.0001);
564 64242 : ret = (dy * (x - x0) / dx) + y0;
565 64242 : break;
566 : }
567 : }
568 : }
569 182060 : return ret;
570 : }
571 :
572 638454 : int ROUND(float val) {
573 638454 : return val + ((val > 0.0) ? 0.5 : -0.5);
574 : }
575 :
576 : // Safety checks for longitudinal actuation
577 25110 : bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) {
578 25110 : bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
579 25110 : bool accel_inactive = desired_accel == limits.inactive_accel;
580 25110 : return !(accel_valid || accel_inactive);
581 : }
582 :
583 102012 : bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits) {
584 102012 : return !get_longitudinal_allowed() && (desired_speed != limits.inactive_speed);
585 : }
586 :
587 32802 : bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits) {
588 32802 : bool transmission_rpm_valid = get_longitudinal_allowed() && !max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
589 32802 : bool transmission_rpm_inactive = desired_transmission_rpm == limits.inactive_transmission_rpm;
590 32802 : return !(transmission_rpm_valid || transmission_rpm_inactive);
591 : }
592 :
593 157569 : bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) {
594 157569 : bool gas_valid = get_longitudinal_allowed() && !max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
595 157569 : bool gas_inactive = desired_gas == limits.inactive_gas;
596 157569 : return !(gas_valid || gas_inactive);
597 : }
598 :
599 280727 : bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits) {
600 280727 : bool violation = false;
601 280727 : violation |= !get_longitudinal_allowed() && (desired_brake != 0);
602 280727 : violation |= desired_brake > limits.max_brake;
603 280727 : return violation;
604 : }
605 :
606 : // Safety checks for torque-based steering commands
607 1431969 : bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) {
608 1431969 : bool violation = false;
609 1431969 : uint32_t ts = microsecond_timer_get();
610 :
611 1431969 : if (controls_allowed) {
612 : // *** global torque limit check ***
613 1344472 : violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);
614 :
615 : // *** torque rate limit check ***
616 1344472 : if (limits.type == TorqueDriverLimited) {
617 1300141 : violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
618 1300141 : limits.max_steer, limits.max_rate_up, limits.max_rate_down,
619 1300141 : limits.driver_torque_allowance, limits.driver_torque_multiplier);
620 : } else {
621 44331 : violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas,
622 44331 : limits.max_rate_up, limits.max_rate_down, limits.max_torque_error);
623 : }
624 1344472 : desired_torque_last = desired_torque;
625 :
626 : // *** torque real time rate limit check ***
627 1344472 : violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta);
628 :
629 : // every RT_INTERVAL set the new limits
630 1344472 : uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last);
631 1344472 : if (ts_elapsed > limits.max_rt_interval) {
632 873 : rt_torque_last = desired_torque;
633 873 : ts_torque_check_last = ts;
634 : }
635 : }
636 :
637 : // no torque if controls is not allowed
638 1431969 : if (!controls_allowed && (desired_torque != 0)) {
639 87361 : violation = true;
640 : }
641 :
642 : // certain safety modes set their steer request bit low for one or more frame at a
643 : // predefined max frequency to avoid steering faults in certain situations
644 1431969 : bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0);
645 1431969 : if (!limits.has_steer_req_tolerance) {
646 72581 : if (steer_req_mismatch) {
647 36 : violation = true;
648 : }
649 :
650 : } else {
651 1359388 : if (steer_req_mismatch) {
652 638256 : if (invalid_steer_req_count == 0) {
653 : // disallow torque cut if not enough recent matching steer_req messages
654 632218 : if (valid_steer_req_count < limits.min_valid_request_frames) {
655 628893 : violation = true;
656 : }
657 :
658 : // or we've cut torque too recently in time
659 632218 : uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last);
660 632218 : if (ts_elapsed < limits.min_valid_request_rt_interval) {
661 626246 : violation = true;
662 : }
663 : } else {
664 : // or we're cutting more frames consecutively than allowed
665 6038 : if (invalid_steer_req_count >= limits.max_invalid_request_frames) {
666 3062 : violation = true;
667 : }
668 : }
669 :
670 638256 : valid_steer_req_count = 0;
671 638256 : ts_steer_req_mismatch_last = ts;
672 638256 : invalid_steer_req_count = MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
673 : } else {
674 721132 : valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
675 721132 : invalid_steer_req_count = 0;
676 : }
677 : }
678 :
679 : // reset to 0 if either controls is not allowed or there's a violation
680 1431969 : if (violation || !controls_allowed) {
681 763788 : valid_steer_req_count = 0;
682 763788 : invalid_steer_req_count = 0;
683 763788 : desired_torque_last = 0;
684 763788 : rt_torque_last = 0;
685 763788 : ts_torque_check_last = ts;
686 763788 : ts_steer_req_mismatch_last = ts;
687 : }
688 :
689 1431969 : return violation;
690 : }
691 :
692 : // Safety checks for angle-based steering commands
693 132585 : bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) {
694 132585 : bool violation = false;
695 :
696 132585 : if (controls_allowed && steer_control_enabled) {
697 : // convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
698 : // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
699 : // always slightly above openpilot's in case we read an updated speed in between angle commands
700 : // TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset
701 82045 : int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.;
702 82045 : int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.) * limits.angle_deg_to_can) + 1.;
703 :
704 : // allow down limits at zero since small floats will be rounded to 0
705 82045 : int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
706 82045 : int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);
707 :
708 : // check that commanded angle value isn't too far from measured, used to limit torque for some safety modes
709 : // ensure we start moving in direction of meas while respecting rate limits if error is exceeded
710 82045 : if (limits.enforce_angle_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) {
711 : // the rate limits above are liberally above openpilot's to avoid false positives.
712 : // likewise, allow a lower rate for moving towards meas when error is exceeded
713 8985 : int delta_angle_up_lower = interpolate(limits.angle_rate_up_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can;
714 8985 : int delta_angle_down_lower = interpolate(limits.angle_rate_down_lookup, (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.) * limits.angle_deg_to_can;
715 :
716 8985 : int highest_desired_angle_lower = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up_lower : delta_angle_down_lower);
717 8985 : int lowest_desired_angle_lower = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down_lower : delta_angle_up_lower);
718 :
719 8985 : lowest_desired_angle = MIN(MAX(lowest_desired_angle, angle_meas.min - limits.max_angle_error - 1), highest_desired_angle_lower);
720 8985 : highest_desired_angle = MAX(MIN(highest_desired_angle, angle_meas.max + limits.max_angle_error + 1), lowest_desired_angle_lower);
721 :
722 : // don't enforce above the max steer
723 8985 : lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_steer, limits.max_steer);
724 8985 : highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_steer, limits.max_steer);
725 : }
726 :
727 : // check for violation;
728 82045 : violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
729 : }
730 132585 : desired_angle_last = desired_angle;
731 :
732 : // Angle should either be 0 or same as current angle while not steering
733 132585 : if (!steer_control_enabled) {
734 45947 : violation |= (limits.inactive_angle_is_zero ? (desired_angle != 0) :
735 12064 : max_limit_check(desired_angle, angle_meas.max + 1, angle_meas.min - 1));
736 : }
737 :
738 : // No angle control allowed when controls are not allowed
739 132585 : violation |= !controls_allowed && steer_control_enabled;
740 :
741 132585 : return violation;
742 : }
743 :
744 371 : void pcm_cruise_check(bool cruise_engaged) {
745 : // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
746 371 : if (!cruise_engaged) {
747 208 : controls_allowed = false;
748 : }
749 371 : if (cruise_engaged && !cruise_engaged_prev) {
750 135 : controls_allowed = true;
751 : }
752 371 : cruise_engaged_prev = cruise_engaged;
753 371 : }
|
