Branch data MC/DC data Line data Source code
1 : : : /**
2 : : : * @file ttc_control.c
3 : : : * @brief TTC (Time to Collision) and AEB (Autonomous Emergency Braking) control logic module.
4 : : : *
5 : : : * This module provides core logic for calculating the time to collision (TTC) between a vehicle
6 : : : * and an obstacle using a uniformly variable motion model. Based on the TTC and vehicle conditions,
7 : : : * it optionally enables the Autonomous Emergency Braking (AEB) system and other related safety features.
8 : : : *
9 : : : * The main functionalities include:
10 : : : * - Calculation of TTC based on relative speed, distance, and acceleration.
11 : : : * - Decision-making logic for enabling the AEB system, triggering alarms, locking seatbelts, and
12 : : : * unlocking doors in critical situations.
13 : : : */
14 : : :
15 : : : #include "ttc_control.h"
16 : : : #include "constants.h"
17 : : : #include <stdio.h>
18 : : :
19 : : : /**
20 : : : * @brief Calculate the time to collision (TTC) based on relative distance and speed.
21 : : : *
22 : : : * This function calculates the time to collision using the Uniformly Variable Motion (UVM) model.
23 : : : * It uses a quadratic equation to calculate the time to collision (TTC) between two objects
24 : : : * based on their relative distance and speed. If the relative acceleration is zero, the function
25 : : : * simply calculates the time by dividing the relative distance by the relative speed [SwR-1] (@ref SwR-1), [SwR-10]
26 : : : * (@ref SwR-10), [SwR-11] (@ref SwR-11).
27 : : : *
28 : : : * @param dis_rel The relative distance between the objects in meters.
29 : : : * @param spd_rel The relative speed between the objects in km/h.
30 : : : * @param rel_acel The relative acceleration between the objects in m/s2.
31 : : : *
32 : : : * @return The time to collision in seconds. If no real solution is found (i.e., negative discriminant),
33 : : : * the function returns -1.0. If there is no relative acceleration, the time to collision is calculated
34 : : : * as distance divided by speed.
35 : : : *
36 : : : * \anchor ttc_calc
37 : : : *
38 : : : */
39 : : 19 : double ttc_calc(double dis_rel, double spd_rel, double rel_acel) {
40 : : : double a, b, c, ttc, delta;
41 : : :
42 : : 19 : a = rel_acel;
43 : : 19 : b = spd_rel / 3.6;
44 : : 19 : c = dis_rel;
45 : : :
46 [ + + ]: [ T F ]: 19 : if (a == 0) return ttc = c / b;
47 : : :
48 : : 17 : delta = b * b + 2 * a * c;
49 : : :
50 [ + + ]: [ T F ]: 17 : if (delta < 0) return 99; // Case: no collision possible ahead
51 : : :
52 [ + + ]: [ T F ]: 14 : else if (delta == 0) return -b / a;
53 : : :
54 : : : else {
55 : : 12 : ttc = (-b + sqrt(delta)) / a;
56 : : 12 : return ttc;
57 : : : }
58 : : : }
59 : : :
60 : : : // Useful but unused function
61 : : : #ifndef aeb_decision
62 : : : /**
63 : : : * @brief Controls the Autonomous Emergency Braking (AEB) system.
64 : : : *
65 : : : * This function calculates the Time to Collision (TTC) based on the relative distance
66 : : : * and speed of the vehicle. If the TTC is below a certain threshold, the AEB system
67 : : : * triggers the alarm and may engage the braking system. It also prepares other safety
68 : : : * features like seatbelt locking and door unlocking in critical conditions.
69 : : : *
70 : : : * The decision is made based on predefined TTC thresholds:
71 : : : * - If the TTC is below the `THRESHOLD_ALARM` (e.g., 2 seconds), an alarm is triggered.
72 : : : * - If the TTC is below the `THRESHOLD_BRAKING` (e.g., 1 second), the braking system is activated.
73 : : : * - If the TTC is less than half of the braking threshold, the braking system is prepared for a collision.
74 : : : *
75 : : : * The AEB system and the alarm are only triggered if the `enable_aeb` flag is set to true.
76 : : : *
77 : : : * @param enable_aeb A pointer to a boolean flag that enables or disables the AEB system. [SwR-5] (@ref SwR-5)
78 : : : * @param alarm_cluster A pointer to a boolean flag that triggers the alarm in the cluster. [SwR-2] (@ref SwR-2)
79 : : : * @param enable_breaking A pointer to a boolean flag that enables or disables the braking system. [SwR-3] (@ref SwR-3)
80 : : : * @param lk_seatbelt A pointer to a boolean flag that locks the seatbelt in case of emergency. [Sys-F-14]
81 : : : * @param lk_doors A pointer to a boolean flag that locks or unlocks the doors in an emergency. [Sys-F-14]
82 : : : * @param spd A pointer to the current speed of the vehicle (in km/h).
83 : : : * @param dist A pointer to the current distance to the obstacle (in meters).
84 : : : *
85 : : : * @note The function complies with the requirements ([SwR-2] (@ref SwR-2), [SwR-3](@ref SwR-3), [SwR-5] (@ref SwR-5),
86 : : : * SwR-11 (@ref SwR-11), SwR-14 (@ref SwR-14), SwR-15 (@ref SwR-15), Sys-F-8, Sys-F-9, Sys-F-14).
87 : : : *
88 : : : * \anchor aeb_control
89 : : : *
90 : : : */
91 : : 8 : void aeb_control(bool *enable_aeb, bool *alarm_cluster, bool *enable_breaking,
92 : : : bool *lk_seatbelt, bool *lk_doors, double *spd, double *dist, double *acel) {
93 : : : double ttc;
94 : : :
95 : : 8 : ttc = ttc_calc(*dist, *spd, *acel);
96 : : :
97 [ + + + - :[ T F T f : 8 : if ((*enable_aeb) && (ttc > 0.0) && (ttc < THRESHOLD_ALARM)) {
+ + ] T F ]
98 : : 6 : *alarm_cluster = true;
99 : : :
100 [ + + + + :[ T F T F : 6 : if ((ttc < THRESHOLD_BRAKING) && (!*enable_breaking) && (*spd < MAX_SPD_ENABLED)
+ + ] T F T F ]
101 [ + + ]: : 3 : && (*spd > MIN_SPD_ENABLED)) {
102 : : 2 : *enable_breaking = true;
103 [ + + ]: [ T F ]: 2 : if (ttc < (THRESHOLD_BRAKING / 2.0)) {
104 : : 1 : *lk_seatbelt = true;
105 : : 1 : *lk_doors = false;
106 : : : }
107 : : : }
108 : : : } else {
109 : : 2 : *alarm_cluster = false;
110 : : 2 : *enable_breaking = false;
111 : : : }
112 : : 8 : }
113 : : : #endif
|
