GCC Code Coverage Report

Directory: ./
File: safety/safety_volkswagen_pq.h
Date: 2025-02-27 09:09:49
Exec Total Coverage
Lines: 98 98 100.0%
Functions: 7 7 100.0%
Branches: 81 85 95.3%
Line Branch Exec Source
1 #pragma once
2
3 #include "safety_declarations.h"
4 #include "safety_volkswagen_common.h"
5
6 #define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque
7 #define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque
8 #define MSG_BREMSE_1 0x1A0 // RX from ABS, for ego speed
9 #define MSG_MOTOR_2 0x288 // RX from ECU, for CC state and brake switch state
10 #define MSG_ACC_SYSTEM 0x368 // TX by OP, longitudinal acceleration controls
11 #define MSG_MOTOR_3 0x380 // RX from ECU, for driver throttle input
12 #define MSG_GRA_NEU 0x38A // TX by OP, ACC control buttons for cancel/resume
13 #define MSG_MOTOR_5 0x480 // RX from ECU, for ACC main switch state
14 #define MSG_ACC_GRA_ANZEIGE 0x56A // TX by OP, ACC HUD
15 #define MSG_LDW_1 0x5BE // TX by OP, Lane line recognition and text alerts
16
17 4042 static uint32_t volkswagen_pq_get_checksum(const CANPacket_t *to_push) {
18 4042 int addr = GET_ADDR(to_push);
19
20
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 4033 times.
 4042 return (uint32_t)GET_BYTE(to_push, (addr == MSG_MOTOR_5) ? 7 : 0);
21 }
22
23 4033 static uint8_t volkswagen_pq_get_counter(const CANPacket_t *to_push) {
24 4033 int addr = GET_ADDR(to_push);
25 4033 uint8_t counter = 0U;
26
27
2/2
✓ Branch 0 taken 4024 times.
✓ Branch 1 taken 9 times.
 4033 if (addr == MSG_LENKHILFE_3) {
28 4024 counter = (uint8_t)(GET_BYTE(to_push, 1) & 0xF0U) >> 4;
29
1/2
✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
 9 } else if (addr == MSG_GRA_NEU) {
30 9 counter = (uint8_t)(GET_BYTE(to_push, 2) & 0xF0U) >> 4;
31 } else {
32 }
33
34 4033 return counter;
35 }
36
37 4042 static uint32_t volkswagen_pq_compute_checksum(const CANPacket_t *to_push) {
38 4042 int addr = GET_ADDR(to_push);
39 4042 int len = GET_LEN(to_push);
40 4042 uint8_t checksum = 0U;
41
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 4033 times.
 4042 int checksum_byte = (addr == MSG_MOTOR_5) ? 7 : 0;
42
43 // Simple XOR over the payload, except for the byte where the checksum lives.
44
2/2
✓ Branch 0 taken 24252 times.
✓ Branch 1 taken 4042 times.
 28294 for (int i = 0; i < len; i++) {
45
2/2
✓ Branch 0 taken 20210 times.
✓ Branch 1 taken 4042 times.
 24252 if (i != checksum_byte) {
46 20210 checksum ^= (uint8_t)GET_BYTE(to_push, i);
47 }
48 }
49
50 4042 return checksum;
51 }
52
53 67 static safety_config volkswagen_pq_init(uint16_t param) {
54 // Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
55 static const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8},
56 {MSG_GRA_NEU, 0, 4}, {MSG_GRA_NEU, 2, 4}};
57
58 static const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8},
59 {MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZEIGE, 0, 8}};
60
61 static RxCheck volkswagen_pq_rx_checks[] = {
62 {.msg = {{MSG_LENKHILFE_3, 0, 6, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
63 {.msg = {{MSG_BREMSE_1, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
64 {.msg = {{MSG_MOTOR_2, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 50U}, { 0 }, { 0 }}},
65 {.msg = {{MSG_MOTOR_3, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
66 {.msg = {{MSG_MOTOR_5, 0, 8, .check_checksum = true, .max_counter = 0U, .frequency = 50U}, { 0 }, { 0 }}},
67 {.msg = {{MSG_GRA_NEU, 0, 4, .check_checksum = true, .max_counter = 15U, .frequency = 30U}, { 0 }, { 0 }}},
68 };
69
70 UNUSED(param);
71
72 67 volkswagen_set_button_prev = false;
73 67 volkswagen_resume_button_prev = false;
74
75 #ifdef ALLOW_DEBUG
76 67 volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
77 #endif
78
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 31 times.
 67 return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_LONG_TX_MSGS) : \
79 BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
80 }
81
82 21001 static void volkswagen_pq_rx_hook(const CANPacket_t *to_push) {
83
2/2
✓ Branch 0 taken 9737 times.
✓ Branch 1 taken 11264 times.
 21001 if (GET_BUS(to_push) == 0U) {
84 9737 int addr = GET_ADDR(to_push);
85
86 // Update in-motion state from speed value.
87 // Signal: Bremse_1.Geschwindigkeit_neu__Bremse_1_
88
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 9721 times.
 9737 if (addr == MSG_BREMSE_1) {
89 16 int speed = ((GET_BYTE(to_push, 2) & 0xFEU) >> 1) | (GET_BYTE(to_push, 3) << 7);
90 16 vehicle_moving = speed > 0;
91 }
92
93 // Update driver input torque samples
94 // Signal: Lenkhilfe_3.LH3_LM (absolute torque)
95 // Signal: Lenkhilfe_3.LH3_LMSign (direction)
96
2/2
✓ Branch 0 taken 4026 times.
✓ Branch 1 taken 5711 times.
 9737 if (addr == MSG_LENKHILFE_3) {
97 4026 int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3U) << 8);
98 4026 int sign = (GET_BYTE(to_push, 3) & 0x4U) >> 2;
99
2/2
✓ Branch 0 taken 1976 times.
✓ Branch 1 taken 2050 times.
 4026 if (sign == 1) {
100 1976 torque_driver_new *= -1;
101 }
102 4026 update_sample(&torque_driver, torque_driver_new);
103 }
104
105
2/2
✓ Branch 0 taken 4873 times.
✓ Branch 1 taken 4864 times.
 9737 if (volkswagen_longitudinal) {
106
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4865 times.
 4873 if (addr == MSG_MOTOR_5) {
107 // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off
108 // Signal: Motor_5.GRA_Hauptschalter
109 8 acc_main_on = GET_BIT(to_push, 50U);
110
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 if (!acc_main_on) {
111 4 controls_allowed = false;
112 }
113 }
114
115
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 4863 times.
 4873 if (addr == MSG_GRA_NEU) {
116 // If ACC main switch is on, enter controls on falling edge of Set or Resume
117 // Signal: GRA_Neu.GRA_Neu_Setzen
118 // Signal: GRA_Neu.GRA_Neu_Recall
119 10 bool set_button = GET_BIT(to_push, 16U);
120 10 bool resume_button = GET_BIT(to_push, 17U);
121
6/8
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 10 if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
122 4 controls_allowed = acc_main_on;
123 }
124 10 volkswagen_set_button_prev = set_button;
125 10 volkswagen_resume_button_prev = resume_button;
126 // Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously
127 // Signal: GRA_ACC_01.GRA_Abbrechen
128
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9 times.
 10 if (GET_BIT(to_push, 9U)) {
129 1 controls_allowed = false;
130 }
131 }
132 } else {
133
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 4844 times.
 4864 if (addr == MSG_MOTOR_2) {
134 // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
135 // Signal: Motor_2.GRA_Status
136 20 int acc_status = (GET_BYTE(to_push, 2) & 0xC0U) >> 6;
137
3/4
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 20 bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
138 20 pcm_cruise_check(cruise_engaged);
139 }
140 }
141
142 // Signal: Motor_3.Fahrpedal_Rohsignal
143
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 9715 times.
 9737 if (addr == MSG_MOTOR_3) {
144 22 gas_pressed = (GET_BYTE(to_push, 2));
145 }
146
147 // Signal: Motor_2.Bremslichtschalter
148
2/2
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 9704 times.
 9737 if (addr == MSG_MOTOR_2) {
149 33 brake_pressed = (GET_BYTE(to_push, 2) & 0x1U);
150 }
151
152 9737 generic_rx_checks((addr == MSG_HCA_1));
153 }
154 21001 }
155
156 5844 static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) {
157 // lateral limits
158 const SteeringLimits VOLKSWAGEN_PQ_STEERING_LIMITS = {
159 .max_steer = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
160 .max_rt_delta = 113, // 6 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 75 ; 125 * 1.5 for safety pad = 113
161 .max_rt_interval = 250000, // 250ms between real time checks
162 .max_rate_up = 6, // 3.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
163 .max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
164 .driver_torque_multiplier = 3,
165 .driver_torque_allowance = 80,
166 .type = TorqueDriverLimited,
167 };
168
169 // longitudinal limits
170 // acceleration in m/s2 * 1000 to avoid floating point math
171 const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = {
172 .max_accel = 2000,
173 .min_accel = -3500,
174 .inactive_accel = 3010, // VW sends one increment above the max range when inactive
175 };
176
177 5844 int addr = GET_ADDR(to_send);
178 5844 bool tx = true;
179
180 // Safety check for HCA_1 Heading Control Assist torque
181 // Signal: HCA_1.LM_Offset (absolute torque)
182 // Signal: HCA_1.LM_Offsign (direction)
183
2/2
✓ Branch 0 taken 5228 times.
✓ Branch 1 taken 616 times.
 5844 if (addr == MSG_HCA_1) {
184 5228 int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x7FU) << 8);
185 5228 desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm
186 5228 int sign = (GET_BYTE(to_send, 3) & 0x80U) >> 7;
187
2/2
✓ Branch 0 taken 2586 times.
✓ Branch 1 taken 2642 times.
 5228 if (sign == 1) {
188 2586 desired_torque *= -1;
189 }
190
191 5228 uint32_t hca_status = ((GET_BYTE(to_send, 1) >> 4) & 0xFU);
192
4/4
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 5225 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 2 times.
 5228 bool steer_req = ((hca_status == 5U) || (hca_status == 7U));
193
194
2/2
✓ Branch 1 taken 2750 times.
✓ Branch 2 taken 2478 times.
 5228 if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_PQ_STEERING_LIMITS)) {
195 2750 tx = false;
196 }
197 }
198
199 // Safety check for acceleration commands
200 // To avoid floating point math, scale upward and compare to pre-scaled safety m/s2 boundaries
201
2/2
✓ Branch 0 taken 609 times.
✓ Branch 1 taken 5235 times.
 5844 if (addr == MSG_ACC_SYSTEM) {
202 // Signal: ACC_System.ACS_Sollbeschl (acceleration in m/s2, scale 0.005, offset -7.22)
203 609 int desired_accel = ((((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) * 5U) - 7220U;
204
205
2/2
✓ Branch 1 taken 381 times.
✓ Branch 2 taken 228 times.
 609 if (longitudinal_accel_checks(desired_accel, VOLKSWAGEN_PQ_LONG_LIMITS)) {
206 381 tx = false;
207 }
208 }
209
210 // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
211 // This avoids unintended engagements while still allowing resume spam
212
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5840 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 1 times.
 5844 if ((addr == MSG_GRA_NEU) && !controls_allowed) {
213 // Signal: GRA_Neu.GRA_Neu_Setzen
214 // Signal: GRA_Neu.GRA_Neu_Recall
215
4/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
 3 if (GET_BIT(to_send, 16U) || GET_BIT(to_send, 17U)) {
216 2 tx = false;
217 }
218 }
219
220 5844 return tx;
221 }
222
223 16896 static int volkswagen_pq_fwd_hook(int bus_num, int addr) {
224 16896 int bus_fwd = -1;
225
226
3/3
✓ Branch 0 taken 5632 times.
✓ Branch 1 taken 5632 times.
✓ Branch 2 taken 5632 times.
 16896 switch (bus_num) {
227 5632 case 0:
228 // Forward all traffic from the Extended CAN onward
229 5632 bus_fwd = 2;
230 5632 break;
231 5632 case 2:
232
4/4
✓ Branch 0 taken 5630 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 5628 times.
 5632 if ((addr == MSG_HCA_1) || (addr == MSG_LDW_1)) {
233 // openpilot takes over LKAS steering control and related HUD messages from the camera
234 4 bus_fwd = -1;
235
6/6
✓ Branch 0 taken 2814 times.
✓ Branch 1 taken 2814 times.
✓ Branch 2 taken 2813 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 2812 times.
 5628 } else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZEIGE))) {
236 // openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar
237 } else {
238 // Forward all remaining traffic from Extended CAN devices to J533 gateway
239 5626 bus_fwd = 0;
240 }
241 5632 break;
242 5632 default:
243 // No other buses should be in use; fallback to do-not-forward
244 5632 bus_fwd = -1;
245 5632 break;
246 }
247
248 16896 return bus_fwd;
249 }
250
251 const safety_hooks volkswagen_pq_hooks = {
252 .init = volkswagen_pq_init,
253 .rx = volkswagen_pq_rx_hook,
254 .tx = volkswagen_pq_tx_hook,
255 .fwd = volkswagen_pq_fwd_hook,
256 .get_counter = volkswagen_pq_get_counter,
257 .get_checksum = volkswagen_pq_get_checksum,
258 .compute_checksum = volkswagen_pq_compute_checksum,
259 };
260