Branch data MC/DC data Line data Source code
1 : : : /**
2 : : : * @file aeb_controller.c
3 : : : * @brief Implements the main logic for the Autonomous Emergency Braking (AEB) controller system.
4 : : : *
5 : : : * This file contains the core implementation of the AEB controller, including the main control loop,
6 : : : * message handling, state management, and CAN message translation logic. It interfaces with sensors
7 : : : * via message queues, processes data (e.g., velocity, obstacles, pedal inputs), and makes
8 : : : * decisions based on TTC (Time to Collision) to control actuators accordingly.
9 : : : *
10 : : : * The controller supports multiple operating states such as standby, active, alarm, and brake, and
11 : : : * ensures safe and timely actuation decisions based on vehicle and environmental data.
12 : : : *
13 : : : */
14 : : :
15 : : : // Necessary libraries
16 : : : #include <stdio.h>
17 : : : #include <stdlib.h>
18 : : : #include <unistd.h>
19 : : : #include <pthread.h>
20 : : : #include <stdbool.h>
21 : : : #include <time.h>
22 : : : #include "constants.h"
23 : : : #include "mq_utils.h"
24 : : : #include "sensors_input.h"
25 : : : #include "dbc.h"
26 : : : #include "actuators.h"
27 : : : #include "ttc_control.h"
28 : : :
29 : : : #define LOOP_EMPTY_ITERATIONS_MAX 11
30 : : :
31 : : : /**
32 : : : * @enum aeb_controller_state
33 : : : * @brief Enum defining the possible states of the AEB system.
34 : : : * Abstraction according to [SwR-12] (@ref SwR-12)
35 : : : */
36 : : : typedef enum
37 : : : {
38 : : : AEB_STATE_ACTIVE, /**< AEB system is active and performing actions */
39 : : : AEB_STATE_ALARM, /**< AEB system is in alarm state, but not yet braking */
40 : : : AEB_STATE_BRAKE, /**< AEB system is actively braking the vehicle */
41 : : : AEB_STATE_STANDBY /**< AEB system is in standby, waiting for data or conditions */
42 : : : } aeb_controller_state;
43 : : :
44 : : : // Function prototypes
45 : : : void *mainWorkingLoop(void *arg);
46 : : : void print_info();
47 : : : void translateAndCallCanMsg(can_msg captured_frame);
48 : : : void updateInternalPedalsState(can_msg captured_frame);
49 : : : void updateInternalSpeedState(can_msg captured_frame);
50 : : : void updateInternalObstacleState(can_msg captured_frame);
51 : : : void updateInternalCarCState(can_msg captured_frame);
52 : : : can_msg updateCanMsgOutput(aeb_controller_state state);
53 : : : aeb_controller_state getAEBState(sensors_input_data aeb_internal_state, double ttc);
54 : : :
55 : : : // Global variables for message queues and internal state
56 : : : mqd_t sensors_mq, actuators_mq; /**< Message queues for sensors and actuators */
57 : : : pthread_t aeb_controller_id; /**< Thread ID for the AEB controller */
58 : : :
59 : : : sensors_input_data aeb_internal_state = {
60 : : : .relative_velocity = 0.0,
61 : : : .has_obstacle = false,
62 : : : .obstacle_distance = 0.0,
63 : : : .brake_pedal = false,
64 : : : .accelerator_pedal = false,
65 : : : .aeb_system_enabled = true,
66 : : : .reverse_enabled = false,
67 : : : .relative_acceleration = 0.0};
68 : : :
69 : : : can_msg captured_can_frame = {
70 : : : .identifier = ID_CAR_C,
71 : : : .dataFrame = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
72 : : :
73 : : : can_msg out_can_frame = {
74 : : : .identifier = ID_AEB_S,
75 : : : .dataFrame = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
76 : : :
77 : : : //! [SwR-5] (@ref SwR-5)
78 : : : can_msg empty_msg = { // [SwR-5]
79 : : : .identifier = ID_EMPTY,
80 : : : .dataFrame = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
81 : : :
82 : : : /**
83 : : : * @brief The main entry point of the AEB (Autonomous Emergency Braking) controller system.
84 : : : *
85 : : : * This function initializes message queues for sensors and actuators, then starts the
86 : : : * AEB controller thread to begin processing. It is designed for use in a production environment
87 : : : * where the AEB controller operates continuously.
88 : : : *
89 : : : * @return 0 on successful execution. If an error occurs while creating the AEB controller thread,
90 : : : * the function will terminate with an error code.
91 : : : *
92 : : : * @note This function is only included in production builds, as it is enclosed in
93 : : : * a preprocessor check (`#ifndef TEST_MODE_CONTROLLER`).
94 : : : */
95 : : : #ifndef TEST_MODE // Main for the AEB controller process in production
96 : : : int main()
97 : : : {
98 : : : // Open message queues for communication with sensors and actuators
99 : : : sensors_mq = open_mq(SENSORS_MQ);
100 : : : actuators_mq = open_mq(ACTUATORS_MQ);
101 : : :
102 : : : // Create the AEB controller thread
103 : : : int controller_thread = pthread_create(&aeb_controller_id, NULL, mainWorkingLoop, NULL);
104 : : : if (controller_thread != 0)
105 : : : {
106 : : : // If thread creation fails, print error and exit
107 : : : perror("AEB Controller: It wasn't possible to create the associated thread\n");
108 : : : exit(53);
109 : : : }
110 : : :
111 : : : // Wait for the controller thread to finish
112 : : : controller_thread = pthread_join(aeb_controller_id, NULL);
113 : : :
114 : : : return 0;
115 : : : }
116 : : :
117 : : : /**
118 : : : * @brief Main loop for the AEB controller that processes sensor data and makes decisions.
119 : : : *
120 : : : * This function continuously checks the message queue for new sensor data, processes it,
121 : : : * and sends commands to the actuators based on the calculated AEB state.
122 : : : *
123 : : : * Requirements [SwR-5] (@ref SwR-5), [SwR-6] (@ref SwR-6) and [SwR-9] (@ref SwR-9)
124 : : : *
125 : : : * @param arg Arguments passed to the thread (not used here).
126 : : : * @return NULL.
127 : : : */
128 : : : void *mainWorkingLoop(void *arg)
129 : : : {
130 : : : aeb_controller_state state = AEB_STATE_STANDBY;
131 : : :
132 : : : int empty_mq_counter = 0;
133 : : : while (empty_mq_counter < LOOP_EMPTY_ITERATIONS_MAX)
134 : : : {
135 : : : if (read_mq(sensors_mq, &captured_can_frame) != -1) // Reads message from sensors [SwR-9]
136 : : : {
137 : : : empty_mq_counter = 0; // Reset counter if data is received
138 : : :
139 : : : translateAndCallCanMsg(captured_can_frame); // Process the received CAN message
140 : : :
141 : : : double ttc = ttc_calc(aeb_internal_state.obstacle_distance, aeb_internal_state.relative_velocity,
142 : : : aeb_internal_state.relative_acceleration);
143 : : :
144 : : : state = getAEBState(aeb_internal_state, ttc);
145 : : :
146 : : : out_can_frame = updateCanMsgOutput(state);
147 : : :
148 : : : if (state == AEB_STATE_STANDBY) // [SwR-5]
149 : : : write_mq(actuators_mq, &empty_msg); // Send empty message when in standby state
150 : : : else
151 : : : write_mq(actuators_mq, &out_can_frame); // Send the appropriate message based on the current state
152 : : : }
153 : : : else
154 : : : empty_mq_counter++; // Increment counter if no message is received
155 : : :
156 : : : usleep(200000); // Wait for a short period before the next iteration (to be replaced later)
157 : : : }
158 : : :
159 : : : printf("AEB Controller: empty_mq_counter reached the limit, exiting\n");
160 : : : return NULL;
161 : : : }
162 : : :
163 : : : /**
164 : : : * @brief Prints debug information about the AEB system's internal state.
165 : : : *
166 : : : * This function displays the current values of key variables such as relative velocity,
167 : : : * obstacle distance, pedal states, and more.
168 : : : */
169 : : : void print_info()
170 : : : {
171 : : : printf("relative_velocity: %lf\n", aeb_internal_state.relative_velocity);
172 : : : printf("has_obstacle: %s\n", aeb_internal_state.has_obstacle ? "true" : "false");
173 : : : printf("obstacle_distance: %lf\n", aeb_internal_state.obstacle_distance);
174 : : : printf("brake_pedal: %s\n", aeb_internal_state.brake_pedal ? "true" : "false");
175 : : : printf("accelerator_pedal: %s\n", aeb_internal_state.accelerator_pedal ? "true" : "false");
176 : : : printf("aeb_system_enabled: %s\n", aeb_internal_state.aeb_system_enabled ? "true" : "false");
177 : : : printf("Is vehicle in reverse: %s\n", aeb_internal_state.reverse_enabled ? "true" : "false");
178 : : : }
179 : : : #endif
180 : : :
181 : : : /**
182 : : : * @brief Translates the received CAN message and calls the appropriate handler
183 : : : * function based on the message identifier.
184 : : : *
185 : : : * This function checks the message identifier and calls the appropriate function
186 : : : * to handle the CAN message data.
187 : : : *
188 : : : * @param captured_frame The received CAN message to be processed.
189 : : : */
190 : : 8 : void translateAndCallCanMsg(can_msg captured_frame)
191 : : : {
192 [ + + + + : : 8 : switch (captured_frame.identifier)
+ ]
193 : : : {
194 : : 2 : case ID_PEDALS:
195 : : 2 : updateInternalPedalsState(captured_frame);
196 : : 2 : break;
197 : : 2 : case ID_SPEED_S:
198 : : 2 : updateInternalSpeedState(captured_frame);
199 : : 2 : break;
200 : : 2 : case ID_OBSTACLE_S:
201 : : 2 : updateInternalObstacleState(captured_frame);
202 : : 2 : break;
203 : : 1 : case ID_CAR_C:
204 : : 1 : updateInternalCarCState(captured_frame);
205 : : 1 : break;
206 : : 1 : default:
207 : : 1 : printf("CAN Identifier unknown\n");
208 : : 1 : break;
209 : : : }
210 : : 8 : }
211 : : :
212 : : : /**
213 : : : * @brief Updates the internal state for the brake and accelerator pedals from the received CAN message.
214 : : : *
215 : : : * This function updates the brake and accelerator pedals' states based on the received CAN message.
216 : : : *
217 : : : * @param captured_frame The captured CAN message containing pedal data.
218 : : : */
219 : : 8 : void updateInternalPedalsState(can_msg captured_frame)
220 : : : {
221 [ + + ]: [ T F ]: 8 : if (captured_frame.dataFrame[0] == 0x00)
222 : : : {
223 : : 3 : aeb_internal_state.accelerator_pedal = false;
224 : : : }
225 [ + + ]: [ T F ]: 5 : else if (captured_frame.dataFrame[0] == 0x01)
226 : : : {
227 : : 4 : aeb_internal_state.accelerator_pedal = true;
228 : : : }
229 : : :
230 [ + + ]: [ T F ]: 8 : if (captured_frame.dataFrame[1] == 0x00)
231 : : : {
232 : : 3 : aeb_internal_state.brake_pedal = false;
233 : : : }
234 [ + + ]: [ T F ]: 5 : else if (captured_frame.dataFrame[1] == 0x01)
235 : : : {
236 : : 4 : aeb_internal_state.brake_pedal = true;
237 : : : }
238 : : 8 : }
239 : : :
240 : : : /**
241 : : : * @brief Updates the internal speed state based on the received CAN message.
242 : : : *
243 : : : * This function updates the internal speed (relative velocity) from the received CAN message.
244 : : : * [SwR-10] (@req SwR-10)
245 : : : *
246 : : : * @param captured_frame The captured CAN message containing speed data.
247 : : : */
248 : : 14 : void updateInternalSpeedState(can_msg captured_frame)
249 : : : {
250 : : : unsigned int data_speed; // used for can frame conversion
251 : : 14 : double new_internal_speed = 0.0;
252 : : : unsigned int data_acel; // used for can frame conversion for acceleration
253 : : 14 : double new_internal_acel = 0.0;
254 : : :
255 : : : // update internal data according to the relative velocity detected by the sensor
256 [ + + + - ]:[ T F T f ]: 14 : if (captured_frame.dataFrame[0] == 0xFE && captured_frame.dataFrame[1] == 0xFF)
257 : : : { // DBC: Clear Data
258 : : 1 : new_internal_speed = 0.0;
259 : : : }
260 [ + + + + ]:[ T F T F ]: 13 : else if (captured_frame.dataFrame[0] == 0xFF && captured_frame.dataFrame[1] == 0xFF)
261 : : : { // DBC: Do nothing
262 : : : ;
263 : : : }
264 : : : else
265 : : : {
266 : : : // Conversion from CAN data frame, according to dbc in the requirement file
267 : : : // [SwR-10]
268 : : 12 : data_speed = captured_frame.dataFrame[0] + (captured_frame.dataFrame[1] << 8);
269 : : 12 : new_internal_speed = data_speed * RES_SPEED_S;
270 : : : }
271 : : :
272 [ + + ]: [ T F ]: 14 : if (new_internal_speed > 251.0)
273 : : : { // DBC: Max value constraint
274 : : 1 : new_internal_speed = 251.0;
275 : : : }
276 : : :
277 : : 14 : aeb_internal_state.relative_velocity = new_internal_speed;
278 : : :
279 : : : // update internal data according to the movement direction reported by the sensor
280 [ + + ]: [ T F ]: 14 : if (captured_frame.dataFrame[2] == 0x01)
281 : : : {
282 : : 2 : aeb_internal_state.reverse_enabled = true;
283 : : : }
284 : : : else
285 : : : {
286 : : 12 : aeb_internal_state.reverse_enabled = false;
287 : : : }
288 : : :
289 : : : // update internal data according to the relative acceleration detected by the sensor
290 [ + + + - ]:[ T F T f ]: 14 : if (captured_frame.dataFrame[3] == 0xFE && captured_frame.dataFrame[4] == 0xFF)
291 : : : { // DBC: Clear Data
292 : : 1 : new_internal_acel = 0.0;
293 : : : }
294 [ + + + - ]:[ T F T f ]: 13 : else if (captured_frame.dataFrame[3] == 0xFF && captured_frame.dataFrame[4] == 0xFF)
295 : : : { // DBC: Do nothing
296 : : : ;
297 : : : }
298 : : : else
299 : : : {
300 : : : // Conversion from CAN data frame, according to dbc in the requirement file
301 : : : // [SwR-10]
302 : : 12 : data_acel = captured_frame.dataFrame[3] + (captured_frame.dataFrame[4] << 8);
303 : : 12 : new_internal_acel = (data_acel + OFFSET_ACCELERATION_S) * RES_ACCELERATION_S;
304 : : : }
305 : : :
306 [ + + ]: [ T F ]: 14 : if (captured_frame.dataFrame[5] == 0x01)
307 : : : {
308 : : 2 : new_internal_acel *= -1;
309 : : : }
310 : : : else
311 : : : {
312 : : : ;
313 : : : }
314 : : :
315 [ + + ]: [ T F ]: 14 : if (new_internal_acel > MAX_ACCELERATION_S)
316 : : : { // DBC: Max value constraint
317 : : 9 : new_internal_acel = MAX_ACCELERATION_S;
318 : : : }
319 [ + + ]: [ T F ]: 5 : else if (new_internal_acel < MIN_ACCELERATION_S)
320 : : : {
321 : : : // DBC: Min value constraint
322 : : 1 : new_internal_acel = MIN_ACCELERATION_S;
323 : : : }
324 : : :
325 : : 14 : aeb_internal_state.relative_acceleration = new_internal_acel;
326 : : :
327 : : : // printf("acel is: %.3lf\n", aeb_internal_state.relative_acceleration);
328 : : 14 : }
329 : : :
330 : : : /**
331 : : : * @brief Updates the internal obstacle state based on the received CAN message.
332 : : : *
333 : : : * This function updates whether an obstacle is detected and the distance to it based on the received CAN message.
334 : : : *
335 : : : * @param captured_frame The captured CAN message containing obstacle data.
336 : : : */
337 : : 10 : void updateInternalObstacleState(can_msg captured_frame)
338 : : : {
339 : : : unsigned int data_distance; // used for can frame conversion
340 : : 10 : double new_internal_distance = 0.0;
341 : : :
342 : : : // Check if there is an obstacle
343 [ + + ]: [ T F ]: 10 : if (captured_frame.dataFrame[2] == 0x00)
344 : : : {
345 : : 4 : aeb_internal_state.has_obstacle = false;
346 : : 4 : aeb_internal_state.obstacle_distance = 300.0; // Set max distance when no obstacle is detected
347 : : 4 : return;
348 : : : }
349 [ + + ]: [ T F ]: 6 : else if (captured_frame.dataFrame[2] == 0x01)
350 : : : {
351 : : 5 : aeb_internal_state.has_obstacle = true;
352 : : : }
353 : : :
354 : : : // Handle special cases for clearing data or doing nothing
355 [ + + + + ]:[ T F T F ]: 6 : if (captured_frame.dataFrame[1] == 0xFF && captured_frame.dataFrame[0] == 0xFE)
356 : : : { // DBC: Clear Data
357 : : 1 : new_internal_distance = 300.0; // Set to max distance
358 : : : }
359 [ + + + + ]:[ T F T F ]: 5 : else if (captured_frame.dataFrame[1] == 0xFF && captured_frame.dataFrame[0] == 0xFF)
360 : : : { // DBC: Do nothing
361 : : : ;
362 : : : }
363 : : : else
364 : : : {
365 : : : // Conversion from CAN data frame, according to dbc in the requirement file
366 : : 4 : data_distance = captured_frame.dataFrame[0] + (captured_frame.dataFrame[1] << 8);
367 : : 4 : new_internal_distance = data_distance * RES_OBSTACLE_S;
368 : : : }
369 : : :
370 : : : // Apply the max distance constraint
371 [ + + ]: [ T F ]: 6 : if (new_internal_distance > 300.0)
372 : : : { // DBC: Max value constraint
373 : : 1 : new_internal_distance = 300.0;
374 : : : }
375 : : :
376 : : : // Update internal state with calculated or reset distance
377 : : 6 : aeb_internal_state.obstacle_distance = new_internal_distance;
378 : : : }
379 : : :
380 : : : /**
381 : : : * @brief Updates the internal car state based on the received CAN message.
382 : : : *
383 : : : * This function updates the status of the AEB system (on/off) based on the data in the CAN message.
384 : : : *
385 : : : * @param captured_frame The captured CAN message containing car state data.
386 : : : */
387 : : 5 : void updateInternalCarCState(can_msg captured_frame)
388 : : : {
389 [ + + ]: [ T F ]: 5 : if (captured_frame.dataFrame[0] == 0x01)
390 : : : {
391 : : 3 : aeb_internal_state.aeb_system_enabled = true;
392 : : : }
393 : : : else
394 : : : {
395 : : 2 : aeb_internal_state.aeb_system_enabled = false; // off state or invalid state
396 : : : }
397 : : 5 : }
398 : : :
399 : : : /**
400 : : : * @brief Generates the appropriate CAN message for the AEB system state.
401 : : : *
402 : : : * This function creates a CAN message to be sent to the actuators based on the current
403 : : : * state of the AEB system (e.g., brake, alarm, etc.).
404 : : : *
405 : : : * Requirements [SwR-2] (@ref SwR-2), [SwR-14] (@ref SwR-14) and [SwR-15] (@ref SwR-15)
406 : : : *
407 : : : * @param state The current state of the AEB system.
408 : : : * @return The generated CAN message.
409 : : : */
410 : : 5 : can_msg updateCanMsgOutput(aeb_controller_state state)
411 : : : {
412 : : 5 : can_msg aux = {.identifier = ID_AEB_S, .dataFrame = BASE_DATA_FRAME};
413 : : :
414 [ + + + + : : 5 : switch (state)
+ ]
415 : : : {
416 : : 1 : case AEB_STATE_BRAKE:
417 : : 1 : aux.dataFrame[0] = 0x01; // activate warning system
418 : : 1 : aux.dataFrame[1] = 0x01; // activate braking system
419 : : 1 : break;
420 : : 1 : case AEB_STATE_ALARM:
421 : : 1 : aux.dataFrame[0] = 0x01; // activate warning system
422 : : 1 : aux.dataFrame[1] = 0x00; // don't activate braking system
423 : : 1 : break;
424 : : 1 : case AEB_STATE_ACTIVE:
425 : : 1 : aux.dataFrame[0] = 0x00; // don't activate warning system
426 : : 1 : aux.dataFrame[1] = 0x00; // don't activate braking system
427 : : 1 : break;
428 : : 1 : case AEB_STATE_STANDBY:
429 : : 1 : aux.dataFrame[0] = 0x00; // don't activate warning system
430 : : 1 : aux.dataFrame[1] = 0x00; // don't activate braking system
431 : : 1 : break;
432 : : 1 : default:
433 : : 1 : break;
434 : : : }
435 : : :
436 : : 5 : return aux;
437 : : : }
438 : : :
439 : : : /**
440 : : : * @brief Determines the current state of the AEB system based on sensor input and TTC.
441 : : : *
442 : : : * This function evaluates the current AEB state based on multiple sensor
443 : : : * parameters, such as relative velocity, obstacle presence, and TTC (Time to Collision).
444 : : : *
445 : : : * Requirements [SwR-7] (@ref SwR-7), [SwR-8] (@ref SwR-8), [SwR-12] (@ref SwR-12) and [SwR-16] (@ref SwR-16)
446 : : : *
447 : : : * @param aeb_internal_state The current sensor data for the AEB system.
448 : : : * @param ttc The time-to-collision value, calculated based on obstacle distance and vehicle speed.
449 : : : * @return The current AEB state.
450 : : : */
451 : : 14 : aeb_controller_state getAEBState(sensors_input_data aeb_internal_state, double ttc)
452 : : : {
453 : : 14 : aeb_controller_state my_new_state = AEB_STATE_ACTIVE;
454 : : :
455 [ + + ]: [ T F ]: 14 : if (aeb_internal_state.aeb_system_enabled == false)
456 : : 1 : return AEB_STATE_STANDBY;
457 : : :
458 [ + + + + ]:[ T F T F : 13 : if (aeb_internal_state.brake_pedal == false && aeb_internal_state.accelerator_pedal == false &&
T F T F ]
459 [ + + + + ]: : 9 : aeb_internal_state.relative_velocity >= MIN_SPD_ENABLED && aeb_internal_state.relative_velocity <= MAX_SPD_ENABLED)
460 : : : {
461 [ + + ]: [ T F ]: 5 : if (ttc < THRESHOLD_BRAKING)
462 : : 2 : return AEB_STATE_BRAKE;
463 [ + + ]: [ T F ]: 3 : if (ttc < THRESHOLD_ALARM)
464 : : 2 : return AEB_STATE_ALARM;
465 : : : }
466 : : :
467 [ + + ]: [ T F ]: 9 : if (ttc < THRESHOLD_ALARM)
468 : : 8 : my_new_state = AEB_STATE_ALARM;
469 : : :
470 : : 9 : return my_new_state;
471 : : : }
|
