| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 | 1049779 | uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) { | |
| 60 | 1049779 | uint32_t ret = 0U; | |
| 61 |
2/2✓ Branch 0 taken 3439128 times.
✓ Branch 1 taken 1049779 times.
|
4488907 | for (int i = 0; i < len; i++) { |
| 62 | 3439128 | const uint32_t shift = i * 8; | |
| 63 | 3439128 | ret |= (((uint32_t)msg->data[start + i]) << shift); | |
| 64 | } | ||
| 65 | 1049779 | 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 |
2/2✓ Branch 0 taken 1012326 times.
✓ Branch 1 taken 684594 times.
|
1696920 | if (index != -1) { |
| 119 |
6/6✓ Branch 0 taken 1012196 times.
✓ Branch 1 taken 130 times.
✓ Branch 2 taken 1009820 times.
✓ Branch 3 taken 2376 times.
✓ Branch 4 taken 13 times.
✓ Branch 5 taken 1009807 times.
|
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 | 2519 | valid = false; | |
| 121 | 2519 | 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 |
2/2✓ Branch 0 taken 5425018 times.
✓ Branch 1 taken 684594 times.
|
6109612 | for (int i = 0; i < len; i++) { |
| 134 | // if multiple msgs are allowed, determine which one is present on the bus | ||
| 135 |
2/2✓ Branch 0 taken 2295105 times.
✓ Branch 1 taken 3129913 times.
|
5425018 | if (!addr_list[i].status.msg_seen) { |
| 136 |
4/4✓ Branch 0 taken 5010577 times.
✓ Branch 1 taken 338673 times.
✓ Branch 2 taken 3063502 times.
✓ Branch 3 taken 1947075 times.
|
5349250 | for (uint8_t j = 0U; (j < MAX_ADDR_CHECK_MSGS) && (addr_list[i].msg[j].addr != 0); j++) { |
| 137 |
4/4✓ Branch 0 taken 11354 times.
✓ Branch 1 taken 3052148 times.
✓ Branch 2 taken 9589 times.
✓ Branch 3 taken 1765 times.
|
3063502 | if ((addr == addr_list[i].msg[j].addr) && (bus == addr_list[i].msg[j].bus) && |
| 138 |
2/2✓ Branch 0 taken 9357 times.
✓ Branch 1 taken 232 times.
|
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 |
2/2✓ Branch 0 taken 3139270 times.
✓ Branch 1 taken 2285748 times.
|
5425018 | if (addr_list[i].status.msg_seen) { |
| 147 | 3139270 | int idx = addr_list[i].status.index; | |
| 148 |
4/4✓ Branch 0 taken 1012761 times.
✓ Branch 1 taken 2126509 times.
✓ Branch 2 taken 1012326 times.
✓ Branch 3 taken 435 times.
|
3139270 | if ((addr == addr_list[i].msg[idx].addr) && (bus == addr_list[i].msg[idx].bus) && |
| 149 |
1/2✓ Branch 0 taken 1012326 times.
✗ Branch 1 not taken.
|
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 |
2/2✓ Branch 0 taken 1012326 times.
✓ Branch 1 taken 684594 times.
|
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 |
1/2✓ Branch 0 taken 758560 times.
✗ Branch 1 not taken.
|
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 |
2/2✓ Branch 0 taken 757567 times.
✓ Branch 1 taken 993 times.
|
758560 | addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1; |
| 169 |
2/2✓ Branch 0 taken 758553 times.
✓ Branch 1 taken 7 times.
|
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 |
2/2✓ Branch 0 taken 1012326 times.
✓ Branch 1 taken 684594 times.
|
1696920 | if (index != -1) { |
| 182 | // checksum check | ||
| 183 |
5/6✓ Branch 0 taken 914693 times.
✓ Branch 1 taken 97633 times.
✓ Branch 2 taken 914693 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 876199 times.
✓ Branch 5 taken 38494 times.
|
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 |
4/4✓ Branch 0 taken 783013 times.
✓ Branch 1 taken 229313 times.
✓ Branch 2 taken 758560 times.
✓ Branch 3 taken 24453 times.
|
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 |
4/4✓ Branch 0 taken 756922 times.
✓ Branch 1 taken 255404 times.
✓ Branch 2 taken 751769 times.
✓ Branch 3 taken 5153 times.
|
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 |
2/2✓ Branch 0 taken 1694401 times.
✓ Branch 1 taken 2519 times.
|
1696920 | if (valid) { |
| 214 | 1694401 | current_hooks->rx(to_push); | |
| 215 | } | ||
| 216 | |||
| 217 | // reset mismatches on rising edge of controls_allowed to avoid rare race condition | ||
| 218 |
4/4✓ Branch 0 taken 601004 times.
✓ Branch 1 taken 1095916 times.
✓ Branch 2 taken 858 times.
✓ Branch 3 taken 600146 times.
|
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 |
2/2✓ Branch 0 taken 15398510 times.
✓ Branch 1 taken 1750613 times.
|
17149123 | for (int i = 0; i < len; i++) { |
| 232 |
6/6✓ Branch 0 taken 2150344 times.
✓ Branch 1 taken 13248166 times.
✓ Branch 2 taken 2148211 times.
✓ Branch 3 taken 2133 times.
✓ Branch 4 taken 2147763 times.
✓ Branch 5 taken 448 times.
|
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 |
4/4✓ Branch 0 taken 3875848 times.
✓ Branch 1 taken 22528 times.
✓ Branch 2 taken 19719 times.
✓ Branch 3 taken 3856129 times.
|
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 |
2/2✓ Branch 0 taken 2190010 times.
✓ Branch 1 taken 1708366 times.
|
3898376 | if (allowed) { |
| 248 | 2190010 | safety_allowed = current_hooks->tx(to_send); | |
| 249 | } | ||
| 250 | |||
| 251 |
6/6✓ Branch 0 taken 3290120 times.
✓ Branch 1 taken 608256 times.
✓ Branch 2 taken 2189799 times.
✓ Branch 3 taken 1100321 times.
✓ Branch 4 taken 812187 times.
✓ Branch 5 taken 1377612 times.
|
3898376 | return !relay_malfunction && allowed && safety_allowed; |
| 252 | } | ||
| 253 | |||
| 254 | 1284098 | int safety_fwd_hook(int bus_num, int addr) { | |
| 255 |
2/2✓ Branch 0 taken 675842 times.
✓ Branch 1 taken 608256 times.
|
1284098 | return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, addr)); |
| 256 | } | ||
| 257 | |||
| 258 | 600274 | bool get_longitudinal_allowed(void) { | |
| 259 |
4/4✓ Branch 0 taken 300228 times.
✓ Branch 1 taken 300046 times.
✓ Branch 2 taken 300156 times.
✓ Branch 3 taken 72 times.
|
600274 | 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 |
2/2✓ Branch 0 taken 17152 times.
✓ Branch 1 taken 67 times.
|
17219 | for (uint16_t i = 0U; i <= 0xFFU; i++) { |
| 266 | 17152 | uint8_t crc = (uint8_t)i; | |
| 267 |
2/2✓ Branch 0 taken 137216 times.
✓ Branch 1 taken 17152 times.
|
154368 | for (int j = 0; j < 8; j++) { |
| 268 |
2/2✓ Branch 0 taken 68608 times.
✓ Branch 1 taken 68608 times.
|
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 |
2/2✓ Branch 0 taken 193536 times.
✓ Branch 1 taken 756 times.
|
194292 | for (uint16_t i = 0; i < 256U; i++) { |
| 281 | 193536 | uint16_t crc = i << 8U; | |
| 282 |
2/2✓ Branch 0 taken 1548288 times.
✓ Branch 1 taken 193536 times.
|
1741824 | for (uint16_t j = 0; j < 8U; j++) { |
| 283 |
2/2✓ Branch 0 taken 774144 times.
✓ Branch 1 taken 774144 times.
|
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 |
1/2✓ Branch 0 taken 72 times.
✗ Branch 1 not taken.
|
72 | if (cfg != NULL) { |
| 300 |
2/2✓ Branch 0 taken 361 times.
✓ Branch 1 taken 72 times.
|
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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 361 times.
|
361 | bool lagging = elapsed_time > MAX(timestep * MAX_MISSED_MSGS, 1e6); |
| 307 | 361 | cfg->rx_checks[i].status.lagging = lagging; | |
| 308 |
1/2✓ Branch 0 taken 361 times.
✗ Branch 1 not taken.
|
361 | if (lagging) { |
| 309 | 361 | controls_allowed = false; | |
| 310 | } | ||
| 311 | |||
| 312 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 361 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
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 | 1546153 | void generic_rx_checks(bool stock_ecu_detected) { | |
| 327 | // allow 1s of transition timeout after relay changes state before assessing malfunctioning | ||
| 328 | 1546153 | const uint32_t RELAY_TRNS_TIMEOUT = 1U; | |
| 329 | |||
| 330 | // exit controls on rising edge of gas press | ||
| 331 |
6/6✓ Branch 0 taken 12232 times.
✓ Branch 1 taken 1533921 times.
✓ Branch 2 taken 310 times.
✓ Branch 3 taken 11922 times.
✓ Branch 4 taken 238 times.
✓ Branch 5 taken 72 times.
|
1546153 | if (gas_pressed && !gas_pressed_prev && !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS)) { |
| 332 | 238 | controls_allowed = false; | |
| 333 | } | ||
| 334 | 1546153 | gas_pressed_prev = gas_pressed; | |
| 335 | |||
| 336 | // exit controls on rising edge of brake press | ||
| 337 |
6/6✓ Branch 0 taken 870 times.
✓ Branch 1 taken 1545283 times.
✓ Branch 2 taken 562 times.
✓ Branch 3 taken 308 times.
✓ Branch 4 taken 152 times.
✓ Branch 5 taken 410 times.
|
1546153 | if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { |
| 338 | 460 | controls_allowed = false; | |
| 339 | } | ||
| 340 | 1546153 | brake_pressed_prev = brake_pressed; | |
| 341 | |||
| 342 | // exit controls on rising edge of regen paddle | ||
| 343 |
6/6✓ Branch 0 taken 33 times.
✓ Branch 1 taken 1546120 times.
✓ Branch 2 taken 21 times.
✓ Branch 3 taken 12 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 15 times.
|
1546153 | if (regen_braking && (!regen_braking_prev || vehicle_moving)) { |
| 344 | 18 | controls_allowed = false; | |
| 345 | } | ||
| 346 | 1546153 | regen_braking_prev = regen_braking; | |
| 347 | |||
| 348 | // check if stock ECU is on bus broken by car harness | ||
| 349 |
4/4✓ Branch 0 taken 1474567 times.
✓ Branch 1 taken 71586 times.
✓ Branch 2 taken 120 times.
✓ Branch 3 taken 1474447 times.
|
1546153 | if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) { |
| 350 | 120 | relay_malfunction_set(); | |
| 351 | } | ||
| 352 | 1546153 | } | |
| 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 |
2/2✓ Branch 0 taken 248256 times.
✓ Branch 1 taken 41376 times.
|
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 |
2/2✓ Branch 0 taken 227568 times.
✓ Branch 1 taken 10344 times.
|
237912 | for (int i = 0; i < hook_config_count; i++) { |
| 437 |
2/2✓ Branch 0 taken 10344 times.
✓ Branch 1 taken 217224 times.
|
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 |
2/4✓ Branch 0 taken 10344 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 10344 times.
✗ Branch 3 not taken.
|
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 |
2/2✓ Branch 0 taken 55896 times.
✓ Branch 1 taken 10344 times.
|
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 | 648113 | int to_signed(int d, int bits) { | |
| 460 | 648113 | int d_signed = d; | |
| 461 | 648113 | int max_value = (1 << MAX((bits - 1), 0)); | |
| 462 |
2/2✓ Branch 0 taken 224976 times.
✓ Branch 1 taken 423137 times.
|
648113 | if (d >= max_value) { |
| 463 | 224976 | d_signed = d - (1 << MAX(bits, 0)); | |
| 464 | } | ||
| 465 | 648113 | 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 |
2/2✓ Branch 0 taken 6410550 times.
✓ Branch 1 taken 1282110 times.
|
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 |
2/2✓ Branch 0 taken 6410550 times.
✓ Branch 1 taken 1282110 times.
|
7692660 | for (int i = 1; i < MAX_SAMPLE_VALS; i++) { |
| 479 |
2/2✓ Branch 0 taken 519483 times.
✓ Branch 1 taken 5891067 times.
|
6410550 | if (sample->values[i] < sample->min) { |
| 480 | 519483 | sample->min = sample->values[i]; | |
| 481 | } | ||
| 482 |
2/2✓ Branch 0 taken 244864 times.
✓ Branch 1 taken 6165686 times.
|
6410550 | if (sample->values[i] > sample->max) { |
| 483 | 244864 | sample->max = sample->values[i]; | |
| 484 | } | ||
| 485 | } | ||
| 486 | 1282110 | } | |
| 487 | |||
| 488 | 4234981 | static bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) { | |
| 489 |
4/4✓ Branch 0 taken 2917698 times.
✓ Branch 1 taken 1317283 times.
✓ Branch 2 taken 40921 times.
✓ Branch 3 taken 2876777 times.
|
4234981 | 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 | 1300137 | 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 | 1300137 | int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP; | |
| 515 | 1300137 | int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP; | |
| 516 | |||
| 517 | // driver | ||
| 518 | 1300137 | int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR; | |
| 519 | 1300137 | 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 | 1300137 | int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, | |
| 523 | MAX(driver_max_limit, 0))); | ||
| 524 | 1300137 | 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 | 1300137 | return max_limit_check(val, highest_allowed, lowest_allowed); | |
| 529 | } | ||
| 530 | |||
| 531 | |||
| 532 | // real time check, mainly used for steer torque rate limiter | ||
| 533 | 1344468 | 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 | 1344468 | int highest_val = MAX(val_last, 0) + MAX_RT_DELTA; | |
| 537 | 1344468 | int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA; | |
| 538 | |||
| 539 | // check for violation | ||
| 540 | 1344468 | 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 |
2/2✓ Branch 0 taken 102422 times.
✓ Branch 1 taken 79638 times.
|
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 |
2/2✓ Branch 0 taken 109554 times.
✓ Branch 1 taken 15396 times.
|
124950 | for (int i=0; i < (size - 1); i++) { |
| 557 |
2/2✓ Branch 0 taken 64242 times.
✓ Branch 1 taken 45312 times.
|
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 |
1/2✓ Branch 0 taken 64242 times.
✗ Branch 1 not taken.
|
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 |
2/2✓ Branch 0 taken 434596 times.
✓ Branch 1 taken 203858 times.
|
638454 | return val + ((val > 0.0) ? 0.5 : -0.5); |
| 574 | } | ||
| 575 | |||
| 576 | // Safety checks for longitudinal actuation | ||
| 577 | 24757 | bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) { | |
| 578 |
4/4✓ Branch 1 taken 12371 times.
✓ Branch 2 taken 12386 times.
✓ Branch 4 taken 9085 times.
✓ Branch 5 taken 3286 times.
|
24757 | bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel); |
| 579 | 24757 | bool accel_inactive = desired_accel == limits.inactive_accel; | |
| 580 |
4/4✓ Branch 0 taken 15672 times.
✓ Branch 1 taken 9085 times.
✓ Branch 2 taken 14708 times.
✓ Branch 3 taken 964 times.
|
24757 | return !(accel_valid || accel_inactive); |
| 581 | } | ||
| 582 | |||
| 583 | 102004 | bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits) { | |
| 584 |
4/4✓ Branch 1 taken 51004 times.
✓ Branch 2 taken 51000 times.
✓ Branch 3 taken 50490 times.
✓ Branch 4 taken 514 times.
|
102004 | return !get_longitudinal_allowed() && (desired_speed != limits.inactive_speed); |
| 585 | } | ||
| 586 | |||
| 587 | 32778 | bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits) { | |
| 588 |
4/4✓ Branch 1 taken 16388 times.
✓ Branch 2 taken 16390 times.
✓ Branch 4 taken 7206 times.
✓ Branch 5 taken 9182 times.
|
32778 | bool transmission_rpm_valid = get_longitudinal_allowed() && !max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm); |
| 589 | 32778 | bool transmission_rpm_inactive = desired_transmission_rpm == limits.inactive_transmission_rpm; | |
| 590 |
4/4✓ Branch 0 taken 25572 times.
✓ Branch 1 taken 7206 times.
✓ Branch 2 taken 25564 times.
✓ Branch 3 taken 8 times.
|
32778 | return !(transmission_rpm_valid || transmission_rpm_inactive); |
| 591 | } | ||
| 592 | |||
| 593 | 157483 | bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) { | |
| 594 |
4/4✓ Branch 1 taken 78736 times.
✓ Branch 2 taken 78747 times.
✓ Branch 4 taken 48779 times.
✓ Branch 5 taken 29957 times.
|
157483 | bool gas_valid = get_longitudinal_allowed() && !max_limit_check(desired_gas, limits.max_gas, limits.min_gas); |
| 595 | 157483 | bool gas_inactive = desired_gas == limits.inactive_gas; | |
| 596 |
4/4✓ Branch 0 taken 108704 times.
✓ Branch 1 taken 48779 times.
✓ Branch 2 taken 103922 times.
✓ Branch 3 taken 4782 times.
|
157483 | return !(gas_valid || gas_inactive); |
| 597 | } | ||
| 598 | |||
| 599 | 280671 | bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits) { | |
| 600 | 280671 | bool violation = false; | |
| 601 |
4/4✓ Branch 1 taken 140337 times.
✓ Branch 2 taken 140334 times.
✓ Branch 3 taken 140316 times.
✓ Branch 4 taken 21 times.
|
280671 | violation |= !get_longitudinal_allowed() && (desired_brake != 0); |
| 602 | 280671 | violation |= desired_brake > limits.max_brake; | |
| 603 | 280671 | return violation; | |
| 604 | } | ||
| 605 | |||
| 606 | // Safety checks for torque-based steering commands | ||
| 607 | 1431700 | bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) { | |
| 608 | 1431700 | bool violation = false; | |
| 609 | 1431700 | uint32_t ts = microsecond_timer_get(); | |
| 610 | |||
| 611 |
2/2✓ Branch 0 taken 1344468 times.
✓ Branch 1 taken 87232 times.
|
1431700 | if (controls_allowed) { |
| 612 | // *** global torque limit check *** | ||
| 613 | 1344468 | violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer); | |
| 614 | |||
| 615 | // *** torque rate limit check *** | ||
| 616 |
2/2✓ Branch 0 taken 1300137 times.
✓ Branch 1 taken 44331 times.
|
1344468 | if (limits.type == TorqueDriverLimited) { |
| 617 | 1300137 | violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, | |
| 618 | 1300137 | limits.max_steer, limits.max_rate_up, limits.max_rate_down, | |
| 619 | 1300137 | 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 | 1344468 | desired_torque_last = desired_torque; | |
| 625 | |||
| 626 | // *** torque real time rate limit check *** | ||
| 627 | 1344468 | violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta); | |
| 628 | |||
| 629 | // every RT_INTERVAL set the new limits | ||
| 630 | 1344468 | uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); | |
| 631 |
2/2✓ Branch 0 taken 873 times.
✓ Branch 1 taken 1343595 times.
|
1344468 | 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 |
4/4✓ Branch 0 taken 87232 times.
✓ Branch 1 taken 1344468 times.
✓ Branch 2 taken 87169 times.
✓ Branch 3 taken 63 times.
|
1431700 | if (!controls_allowed && (desired_torque != 0)) { |
| 639 | 87169 | 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 |
4/4✓ Branch 0 taken 638109 times.
✓ Branch 1 taken 793591 times.
✓ Branch 2 taken 638100 times.
✓ Branch 3 taken 9 times.
|
1431700 | bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0); |
| 645 |
2/2✓ Branch 0 taken 72511 times.
✓ Branch 1 taken 1359189 times.
|
1431700 | if (!limits.has_steer_req_tolerance) { |
| 646 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 72491 times.
|
72511 | if (steer_req_mismatch) { |
| 647 | 20 | violation = true; | |
| 648 | } | ||
| 649 | |||
| 650 | } else { | ||
| 651 |
2/2✓ Branch 0 taken 638080 times.
✓ Branch 1 taken 721109 times.
|
1359189 | if (steer_req_mismatch) { |
| 652 |
2/2✓ Branch 0 taken 632042 times.
✓ Branch 1 taken 6038 times.
|
638080 | if (invalid_steer_req_count == 0) { |
| 653 | // disallow torque cut if not enough recent matching steer_req messages | ||
| 654 |
2/2✓ Branch 0 taken 628717 times.
✓ Branch 1 taken 3325 times.
|
632042 | if (valid_steer_req_count < limits.min_valid_request_frames) { |
| 655 | 628717 | violation = true; | |
| 656 | } | ||
| 657 | |||
| 658 | // or we've cut torque too recently in time | ||
| 659 | 632042 | uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last); | |
| 660 |
2/2✓ Branch 0 taken 626070 times.
✓ Branch 1 taken 5972 times.
|
632042 | if (ts_elapsed < limits.min_valid_request_rt_interval) { |
| 661 | 626070 | violation = true; | |
| 662 | } | ||
| 663 | } else { | ||
| 664 | // or we're cutting more frames consecutively than allowed | ||
| 665 |
2/2✓ Branch 0 taken 3062 times.
✓ Branch 1 taken 2976 times.
|
6038 | if (invalid_steer_req_count >= limits.max_invalid_request_frames) { |
| 666 | 3062 | violation = true; | |
| 667 | } | ||
| 668 | } | ||
| 669 | |||
| 670 | 638080 | valid_steer_req_count = 0; | |
| 671 | 638080 | ts_steer_req_mismatch_last = ts; | |
| 672 | 638080 | invalid_steer_req_count = MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames); | |
| 673 | } else { | ||
| 674 | 721109 | valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames); | |
| 675 | 721109 | invalid_steer_req_count = 0; | |
| 676 | } | ||
| 677 | } | ||
| 678 | |||
| 679 | // reset to 0 if either controls is not allowed or there's a violation | ||
| 680 |
4/4✓ Branch 0 taken 668240 times.
✓ Branch 1 taken 763460 times.
✓ Branch 2 taken 63 times.
✓ Branch 3 taken 668177 times.
|
1431700 | if (violation || !controls_allowed) { |
| 681 | 763523 | valid_steer_req_count = 0; | |
| 682 | 763523 | invalid_steer_req_count = 0; | |
| 683 | 763523 | desired_torque_last = 0; | |
| 684 | 763523 | rt_torque_last = 0; | |
| 685 | 763523 | ts_torque_check_last = ts; | |
| 686 | 763523 | ts_steer_req_mismatch_last = ts; | |
| 687 | } | ||
| 688 | |||
| 689 | 1431700 | return violation; | |
| 690 | } | ||
| 691 | |||
| 692 | // Safety checks for angle-based steering commands | ||
| 693 | 132522 | bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) { | |
| 694 | 132522 | bool violation = false; | |
| 695 | |||
| 696 |
4/4✓ Branch 0 taken 96902 times.
✓ Branch 1 taken 35620 times.
✓ Branch 2 taken 82045 times.
✓ Branch 3 taken 14857 times.
|
132522 | 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 |
2/2✓ Branch 0 taken 39398 times.
✓ Branch 1 taken 42647 times.
|
82045 | int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); |
| 706 |
2/2✓ Branch 0 taken 42173 times.
✓ Branch 1 taken 39872 times.
|
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 |
4/4✓ Branch 0 taken 15690 times.
✓ Branch 1 taken 66355 times.
✓ Branch 2 taken 8985 times.
✓ Branch 3 taken 6705 times.
|
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 |
2/2✓ Branch 0 taken 3948 times.
✓ Branch 1 taken 5037 times.
|
8985 | int highest_desired_angle_lower = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up_lower : delta_angle_down_lower); |
| 717 |
2/2✓ Branch 0 taken 5037 times.
✓ Branch 1 taken 3948 times.
|
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 |
1/2✓ Branch 0 taken 8985 times.
✗ Branch 1 not taken.
|
8985 | lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_steer, limits.max_steer); |
| 724 |
2/2✓ Branch 0 taken 7896 times.
✓ Branch 1 taken 1089 times.
|
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 | 132522 | desired_angle_last = desired_angle; | |
| 731 | |||
| 732 | // Angle should either be 0 or same as current angle while not steering | ||
| 733 |
2/2✓ Branch 0 taken 33820 times.
✓ Branch 1 taken 98702 times.
|
132522 | if (!steer_control_enabled) { |
| 734 |
2/2✓ Branch 0 taken 21783 times.
✓ Branch 1 taken 12037 times.
|
45857 | violation |= (limits.inactive_angle_is_zero ? (desired_angle != 0) : |
| 735 | 12037 | 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 |
4/4✓ Branch 0 taken 35620 times.
✓ Branch 1 taken 96902 times.
✓ Branch 2 taken 16657 times.
✓ Branch 3 taken 18963 times.
|
132522 | violation |= !controls_allowed && steer_control_enabled; |
| 740 | |||
| 741 | 132522 | 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 |
2/2✓ Branch 0 taken 208 times.
✓ Branch 1 taken 163 times.
|
371 | if (!cruise_engaged) { |
| 747 | 208 | controls_allowed = false; | |
| 748 | } | ||
| 749 |
4/4✓ Branch 0 taken 163 times.
✓ Branch 1 taken 208 times.
✓ Branch 2 taken 135 times.
✓ Branch 3 taken 28 times.
|
371 | if (cruise_engaged && !cruise_engaged_prev) { |
| 750 | 135 | controls_allowed = true; | |
| 751 | } | ||
| 752 | 371 | cruise_engaged_prev = cruise_engaged; | |
| 753 | 371 | } | |
| 754 |