| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #pragma once | ||
| 2 | |||
| 3 | #include "safety_declarations.h" | ||
| 4 | |||
| 5 | static bool tesla_longitudinal = false; | ||
| 6 | static bool tesla_stock_aeb = false; | ||
| 7 | |||
| 8 | 51614 | static void tesla_rx_hook(const CANPacket_t *to_push) { | |
| 9 | 51614 | int bus = GET_BUS(to_push); | |
| 10 | 51614 | int addr = GET_ADDR(to_push); | |
| 11 | |||
| 12 |
2/2✓ Branch 0 taken 40348 times.
✓ Branch 1 taken 11266 times.
|
51614 | if (bus == 0) { |
| 13 | // Steering angle: (0.1 * val) - 819.2 in deg. | ||
| 14 |
2/2✓ Branch 0 taken 24026 times.
✓ Branch 1 taken 16322 times.
|
40348 | if (addr == 0x370) { |
| 15 | // Store it 1/10 deg to match steering request | ||
| 16 | 24026 | int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U; | |
| 17 | 24026 | update_sample(&angle_meas, angle_meas_new); | |
| 18 | } | ||
| 19 | |||
| 20 | // Vehicle speed | ||
| 21 |
2/2✓ Branch 0 taken 10622 times.
✓ Branch 1 taken 29726 times.
|
40348 | if (addr == 0x257) { |
| 22 | // Vehicle speed: ((val * 0.08) - 40) / MS_TO_KPH | ||
| 23 | 10622 | float speed = ((((GET_BYTE(to_push, 2) << 4) | (GET_BYTE(to_push, 1) >> 4)) * 0.08) - 40) / 3.6; | |
| 24 | 10622 | UPDATE_VEHICLE_SPEED(speed); | |
| 25 | } | ||
| 26 | |||
| 27 | // Gas pressed | ||
| 28 |
2/2✓ Branch 0 taken 22 times.
✓ Branch 1 taken 40326 times.
|
40348 | if (addr == 0x118) { |
| 29 | 22 | gas_pressed = (GET_BYTE(to_push, 4) != 0U); | |
| 30 | } | ||
| 31 | |||
| 32 | // Brake pressed | ||
| 33 |
2/2✓ Branch 0 taken 26 times.
✓ Branch 1 taken 40322 times.
|
40348 | if (addr == 0x39d) { |
| 34 | 26 | brake_pressed = (GET_BYTE(to_push, 2) & 0x03U) == 2U; | |
| 35 | } | ||
| 36 | |||
| 37 | // Cruise state | ||
| 38 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 40318 times.
|
40348 | if (addr == 0x286) { |
| 39 | 30 | int cruise_state = (GET_BYTE(to_push, 1) >> 4) & 0x07U; | |
| 40 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 10 times.
|
20 | bool cruise_engaged = (cruise_state == 2) || // ENABLED |
| 41 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | (cruise_state == 3) || // STANDSTILL |
| 42 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | (cruise_state == 4) || // OVERRIDE |
| 43 |
3/4✓ Branch 0 taken 20 times.
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
|
50 | (cruise_state == 6) || // PRE_FAULT |
| 44 | (cruise_state == 7); // PRE_CANCEL | ||
| 45 | |||
| 46 | 30 | vehicle_moving = cruise_state != 3; // STANDSTILL | |
| 47 | 30 | pcm_cruise_check(cruise_engaged); | |
| 48 | } | ||
| 49 | } | ||
| 50 | |||
| 51 |
2/2✓ Branch 0 taken 5634 times.
✓ Branch 1 taken 45980 times.
|
51614 | if (bus == 2) { |
| 52 |
4/4✓ Branch 0 taken 2818 times.
✓ Branch 1 taken 2816 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2815 times.
|
5634 | if (tesla_longitudinal && (addr == 0x2b9)) { |
| 53 | // "AEB_ACTIVE" | ||
| 54 | 3 | tesla_stock_aeb = (GET_BYTE(to_push, 2) & 0x03U) == 1U; | |
| 55 | } | ||
| 56 | } | ||
| 57 | |||
| 58 |
4/4✓ Branch 0 taken 6 times.
✓ Branch 1 taken 51608 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 4 times.
|
51614 | generic_rx_checks((addr == 0x488) && (bus == 0)); // DAS_steeringControl |
| 59 |
4/4✓ Branch 0 taken 6 times.
✓ Branch 1 taken 51608 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 4 times.
|
51614 | generic_rx_checks((addr == 0x27d) && (bus == 0)); // APS_eacMonitor |
| 60 | |||
| 61 |
2/2✓ Branch 0 taken 25808 times.
✓ Branch 1 taken 25806 times.
|
51614 | if (tesla_longitudinal) { |
| 62 |
4/4✓ Branch 0 taken 5 times.
✓ Branch 1 taken 25803 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 4 times.
|
25808 | generic_rx_checks((addr == 0x2b9) && (bus == 0)); |
| 63 | } | ||
| 64 | 51614 | } | |
| 65 | |||
| 66 | |||
| 67 | 14322 | static bool tesla_tx_hook(const CANPacket_t *to_send) { | |
| 68 | const SteeringLimits TESLA_STEERING_LIMITS = { | ||
| 69 | .angle_deg_to_can = 10, | ||
| 70 | .angle_rate_up_lookup = { | ||
| 71 | {0., 5., 25.}, | ||
| 72 | {2.5, 1.5, 0.2} | ||
| 73 | }, | ||
| 74 | .angle_rate_down_lookup = { | ||
| 75 | {0., 5., 25.}, | ||
| 76 | {5., 2.0, 0.3} | ||
| 77 | }, | ||
| 78 | }; | ||
| 79 | |||
| 80 | const LongitudinalLimits TESLA_LONG_LIMITS = { | ||
| 81 | .max_accel = 425, // 2 m/s^2 | ||
| 82 | .min_accel = 288, // -3.48 m/s^2 | ||
| 83 | .inactive_accel = 375, // 0. m/s^2 | ||
| 84 | }; | ||
| 85 | |||
| 86 | 14322 | bool tx = true; | |
| 87 | 14322 | int addr = GET_ADDR(to_send); | |
| 88 | 14322 | bool violation = false; | |
| 89 | |||
| 90 | // Steering control: (0.1 * val) - 1638.35 in deg. | ||
| 91 |
2/2✓ Branch 0 taken 13052 times.
✓ Branch 1 taken 1270 times.
|
14322 | if (addr == 0x488) { |
| 92 | // We use 1/10 deg as a unit here | ||
| 93 | 13052 | int raw_angle_can = ((GET_BYTE(to_send, 0) & 0x7FU) << 8) | GET_BYTE(to_send, 1); | |
| 94 | 13052 | int desired_angle = raw_angle_can - 16384; | |
| 95 | 13052 | int steer_control_type = GET_BYTE(to_send, 2) >> 6; | |
| 96 |
3/4✓ Branch 0 taken 10156 times.
✓ Branch 1 taken 2896 times.
✓ Branch 2 taken 10156 times.
✗ Branch 3 not taken.
|
13052 | bool steer_control_enabled = (steer_control_type != 0) && // NONE |
| 97 | (steer_control_type != 3); // DISABLED | ||
| 98 | |||
| 99 |
2/2✓ Branch 1 taken 4994 times.
✓ Branch 2 taken 8058 times.
|
13052 | if (steer_angle_cmd_checks(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS)) { |
| 100 | 4994 | violation = true; | |
| 101 | } | ||
| 102 | } | ||
| 103 | |||
| 104 | // DAS_control: longitudinal control message | ||
| 105 |
2/2✓ Branch 0 taken 1270 times.
✓ Branch 1 taken 13052 times.
|
14322 | if (addr == 0x2b9) { |
| 106 | // No AEB events may be sent by openpilot | ||
| 107 | 1270 | int aeb_event = GET_BYTE(to_send, 2) & 0x03U; | |
| 108 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1264 times.
|
1270 | if (aeb_event != 0) { |
| 109 | 6 | violation = true; | |
| 110 | } | ||
| 111 | |||
| 112 | 1270 | int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4); | |
| 113 | 1270 | int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3); | |
| 114 | 1270 | int acc_state = GET_BYTE(to_send, 1) >> 4; | |
| 115 | |||
| 116 |
2/2✓ Branch 0 taken 625 times.
✓ Branch 1 taken 645 times.
|
1270 | if (tesla_longitudinal) { |
| 117 | // Don't send messages when the stock AEB system is active | ||
| 118 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 624 times.
|
625 | if (tesla_stock_aeb) { |
| 119 | 1 | violation = true; | |
| 120 | } | ||
| 121 | |||
| 122 | // Prevent both acceleration from being negative, as this could cause the car to reverse after coming to standstill | ||
| 123 |
4/4✓ Branch 0 taken 6 times.
✓ Branch 1 taken 619 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 2 times.
|
625 | if ((raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) && (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel)) { |
| 124 | 4 | violation = true; | |
| 125 | } | ||
| 126 | |||
| 127 | // Don't allow any acceleration limits above the safety limits | ||
| 128 | 625 | violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS); | |
| 129 | 625 | violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS); | |
| 130 | } else { | ||
| 131 | // does allowing cancel here disrupt stock AEB? TODO: find out and add safety or remove comment | ||
| 132 | // Can only send cancel longitudinal messages when not controlling longitudinal | ||
| 133 |
2/2✓ Branch 0 taken 639 times.
✓ Branch 1 taken 6 times.
|
645 | if (acc_state != 13) { // ACC_CANCEL_GENERIC_SILENT |
| 134 | 639 | violation = true; | |
| 135 | } | ||
| 136 | |||
| 137 | // No actuation is allowed when not controlling longitudinal | ||
| 138 |
4/4✓ Branch 0 taken 388 times.
✓ Branch 1 taken 257 times.
✓ Branch 2 taken 360 times.
✓ Branch 3 taken 28 times.
|
645 | if ((raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) || (raw_accel_min != TESLA_LONG_LIMITS.inactive_accel)) { |
| 139 | 617 | violation = true; | |
| 140 | } | ||
| 141 | } | ||
| 142 | } | ||
| 143 | |||
| 144 |
2/2✓ Branch 0 taken 6025 times.
✓ Branch 1 taken 8297 times.
|
14322 | if (violation) { |
| 145 | 6025 | tx = false; | |
| 146 | } | ||
| 147 | |||
| 148 | 14322 | return tx; | |
| 149 | } | ||
| 150 | |||
| 151 | 16898 | static int tesla_fwd_hook(int bus_num, int addr) { | |
| 152 | 16898 | int bus_fwd = -1; | |
| 153 | |||
| 154 |
2/2✓ Branch 0 taken 5632 times.
✓ Branch 1 taken 11266 times.
|
16898 | if (bus_num == 0) { |
| 155 | // Party to autopilot | ||
| 156 | 5632 | bus_fwd = 2; | |
| 157 | } | ||
| 158 | |||
| 159 |
2/2✓ Branch 0 taken 5634 times.
✓ Branch 1 taken 11264 times.
|
16898 | if (bus_num == 2) { |
| 160 | 5634 | bool block_msg = false; | |
| 161 | // DAS_steeringControl, APS_eacMonitor | ||
| 162 |
4/4✓ Branch 0 taken 5632 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 5630 times.
|
5634 | if ((addr == 0x488) || (addr == 0x27d)) { |
| 163 | 4 | block_msg = true; | |
| 164 | } | ||
| 165 | |||
| 166 | // DAS_control | ||
| 167 |
6/6✓ Branch 0 taken 2818 times.
✓ Branch 1 taken 2816 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2815 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
|
5634 | if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) { |
| 168 | 2 | block_msg = true; | |
| 169 | } | ||
| 170 | |||
| 171 |
2/2✓ Branch 0 taken 5628 times.
✓ Branch 1 taken 6 times.
|
5634 | if (!block_msg) { |
| 172 | 5628 | bus_fwd = 0; | |
| 173 | } | ||
| 174 | } | ||
| 175 | |||
| 176 | 16898 | return bus_fwd; | |
| 177 | } | ||
| 178 | |||
| 179 | 2779 | static safety_config tesla_init(uint16_t param) { | |
| 180 | |||
| 181 | static const CanMsg TESLA_M3_Y_TX_MSGS[] = { | ||
| 182 | {0x488, 0, 4}, // DAS_steeringControl | ||
| 183 | {0x2b9, 0, 8}, // DAS_control | ||
| 184 | {0x27D, 0, 3}, // APS_eacMonitor | ||
| 185 | }; | ||
| 186 | |||
| 187 | UNUSED(param); | ||
| 188 | #ifdef ALLOW_DEBUG | ||
| 189 | 2779 | const int TESLA_FLAG_LONGITUDINAL_CONTROL = 1; | |
| 190 | 2779 | tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL); | |
| 191 | #endif | ||
| 192 | |||
| 193 | 2779 | tesla_stock_aeb = false; | |
| 194 | |||
| 195 | static RxCheck tesla_model3_y_rx_checks[] = { | ||
| 196 | {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}}, // DAS_control | ||
| 197 | {.msg = {{0x257, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // DI_speed (speed in kph) | ||
| 198 | {.msg = {{0x370, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // EPAS3S_internalSAS (steering angle) | ||
| 199 | {.msg = {{0x118, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_systemStatus (gas pedal) | ||
| 200 | {.msg = {{0x39d, 0, 5, .frequency = 25U}, { 0 }, { 0 }}}, // IBST_status (brakes) | ||
| 201 | {.msg = {{0x286, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // DI_state (acc state) | ||
| 202 | {.msg = {{0x311, 0, 7, .frequency = 10U}, { 0 }, { 0 }}}, // UI_warning (blinkers, buckle switch & doors) | ||
| 203 | }; | ||
| 204 | |||
| 205 | 2779 | return BUILD_SAFETY_CFG(tesla_model3_y_rx_checks, TESLA_M3_Y_TX_MSGS); | |
| 206 | } | ||
| 207 | |||
| 208 | const safety_hooks tesla_hooks = { | ||
| 209 | .init = tesla_init, | ||
| 210 | .rx = tesla_rx_hook, | ||
| 211 | .tx = tesla_tx_hook, | ||
| 212 | .fwd = tesla_fwd_hook, | ||
| 213 | }; | ||
| 214 |