Branch data MC/DC data Line data Source code
1 : : : /**
2 : : : * @file sensors.c
3 : : : * @brief Sensor module responsible for reading scenario data and converting it into CAN messages.
4 : : : *
5 : : : * This module reads sensor data from a predefined scenario text file and encodes the information
6 : : : * into CAN frames. The resulting frames are sent via a POSIX message queue to other modules.
7 : : : * The data includes vehicle velocity, direction, AEB system status, obstacle presence, and
8 : : : * pedal activation.
9 : : : */
10 : : :
11 : : : #include <stdio.h>
12 : : : #include <stdlib.h>
13 : : : #include <unistd.h>
14 : : : #include "constants.h"
15 : : : #include "mq_utils.h"
16 : : : #include "sensors_input.h"
17 : : : #include <pthread.h>
18 : : : #include <stdbool.h>
19 : : : #include "dbc.h"
20 : : : #include "file_reader.h"
21 : : :
22 : : : void *getSensorsData(void *arg);
23 : : : can_msg conv2CANCarClusterData(bool aeb_system_enabled);
24 : : : can_msg conv2CANVelocityData(bool vehicle_direction, double relative_velocity, double relative_acceleration);
25 : : : can_msg conv2CANObstacleData(bool has_obstacle, double obstacle_distance);
26 : : : can_msg conv2CANPedalsData(bool brake_pedal, bool accelerator_pedal);
27 : : :
28 : : : mqd_t sensors_mq;
29 : : : pthread_t sensors_id;
30 : : : sensors_input_data sensorsData;
31 : : :
32 : : : can_msg can_car_cluster, can_velocity_sensor, can_obstacle_sensor, can_pedals_sensor;
33 : : :
34 : : : #ifndef TEST_MODE
35 : : : int main()
36 : : : {
37 : : : int sensors_thr;
38 : : :
39 : : : sensors_mq = create_mq(SENSORS_MQ);
40 : : :
41 : : : const char *filename = "tcs/cenario.txt";
42 : : : FILE *file = open_file(filename); // uses the modularized function to open the file
43 : : :
44 : : : sensors_thr = pthread_create(&sensors_id, NULL, getSensorsData, file); // Changed the argument from null to file(the last argument)
45 : : : if (sensors_thr != 0)
46 : : : {
47 : : : perror("Sensors: it wasn't possible to create the associated thread\n");
48 : : : exit(52);
49 : : : }
50 : : : sensors_thr = pthread_join(sensors_id, NULL);
51 : : :
52 : : : return 0;
53 : : : }
54 : : :
55 : : : /**
56 : : : * @brief Function that encapsulates data from a file into CAN frames and sends it to the message queue.
57 : : : *
58 : : : * This function is runned by the thread sensors_thr. It calls the other functions of the program
59 : : : * to read data from the file, encode it into CAN frames and send it to sensors message queue.
60 : : : *
61 : : : * @param arg Arguments passed to the thread (in this case it is the file pointer).
62 : : : * @return NULL.
63 : : : *
64 : : : */
65 : : : void* getSensorsData(void *arg)
66 : : : {
67 : : : FILE *file = (FILE *) arg;
68 : : : while (1)
69 : : : {
70 : : : // Read a new line from the file [SwR-9]
71 : : : if (read_sensor_data(file, &sensorsData))
72 : : : {
73 : : : can_car_cluster = conv2CANCarClusterData(sensorsData.aeb_system_enabled);
74 : : : can_velocity_sensor = conv2CANVelocityData(sensorsData.reverse_enabled, sensorsData.relative_velocity, sensorsData.relative_acceleration); // [SwR-10]
75 : : : can_obstacle_sensor = conv2CANObstacleData(sensorsData.has_obstacle, sensorsData.obstacle_distance);
76 : : : can_pedals_sensor = conv2CANPedalsData(sensorsData.brake_pedal, sensorsData.accelerator_pedal);
77 : : :
78 : : : write_mq(sensors_mq, &can_car_cluster);
79 : : : write_mq(sensors_mq, &can_velocity_sensor);
80 : : : write_mq(sensors_mq, &can_obstacle_sensor);
81 : : : write_mq(sensors_mq, &can_pedals_sensor);
82 : : :
83 : : : //printf("New line.\n"); // This line is used for see the break of line
84 : : : }
85 : : : else
86 : : : {
87 : : : // If a new line can't be read, the end of the file was reached
88 : : : printf("EOF reached.\n");
89 : : : break;
90 : : : }
91 : : :
92 : : : sleep(1); // Wait for 1 second before reading the next line
93 : : : }
94 : : :
95 : : : fclose(file);
96 : : : return NULL;
97 : : : }
98 : : : #endif
99 : : :
100 : : : // The location of information in the data frame location, in the following functions,
101 : : : // is according to the dbc file in the requirements specification
102 : : :
103 : : : /**
104 : : : * @brief Function that encapsulates data into the Car Cluster CAN frame.
105 : : : *
106 : : : * @param aeb_system_enabled Argument that refers the power state of the AEB System (ON or OFF).
107 : : : *
108 : : : * @return structure that contains the CAN message ID and Frame.
109 : : : *
110 : : : * \anchor conv2CANCarClusterData
111 : : : *
112 : : : */
113 : : 2 : can_msg conv2CANCarClusterData(bool aeb_system_enabled)
114 : : : {
115 : : 2 : can_msg aux = {.identifier = ID_CAR_C, .dataFrame = BASE_DATA_FRAME};
116 : : :
117 : : : // Enable or disable AEB data encapsulation
118 [ + + ]: [ T F ]: 2 : if (aeb_system_enabled)
119 : : : {
120 : : 1 : aux.dataFrame[0] = 0x01;
121 : : : }
122 : : : else
123 : : : {
124 : : 1 : aux.dataFrame[0] = 0x00;
125 : : : }
126 : : :
127 : : 2 : return aux;
128 : : : }
129 : : :
130 : : :
131 : : : /**
132 : : : * @brief Function that encapsulates data into the Speed CAN frame.
133 : : : *
134 : : : * This function receives data of the vehicle direction, relative velocity and
135 : : : * relative acceleration and encapsulates it into the Speed CAN frame.
136 : : : *
137 : : : * @param vehicle_direction Vehicle direction (forward or reverse).
138 : : : * @param relative_velocity Relative velocity of the vehicle.
139 : : : * @param relative_acceleration Relative acceleration of the vehicle.
140 : : : *
141 : : : * @return structure that contains the CAN message ID and Frame.
142 : : : *
143 : : : * \anchor conv2CANVelocityData
144 : : : *
145 : : : */
146 : : 2 : can_msg conv2CANVelocityData(bool vehicle_direction, double relative_velocity, double relative_acceleration)
147 : : : {
148 : : : //printf("Rel acel: %lf\n", relative_acceleration);
149 : : 2 : can_msg aux = {.identifier = ID_SPEED_S, .dataFrame = BASE_DATA_FRAME};
150 : : :
151 : : : // Vehicle direction (forward or reverse) data encapsulation
152 [ + + ]: [ T F ]: 2 : if (vehicle_direction)
153 : : : {
154 : : 1 : aux.dataFrame[2] = 0x01;
155 : : : }
156 : : : else
157 : : : {
158 : : 1 : aux.dataFrame[2] = 0x00;
159 : : : }
160 : : :
161 : : : // Speed data encapsulation
162 : : 2 : unsigned int data_speed = relative_velocity / RES_SPEED_S;
163 : : : unsigned char ms_speed, ls_speed;
164 : : 2 : ls_speed = data_speed;
165 : : 2 : ms_speed = data_speed >> 8;
166 : : :
167 : : : // Defines most and least significant bytes, according to the DBC specification
168 : : 2 : aux.dataFrame[0] = ls_speed;
169 : : 2 : aux.dataFrame[1] = ms_speed;
170 : : :
171 : : : // Acceleration data encapsulation
172 : : 2 : double aux_acel = relative_acceleration;
173 [ + + ]: [ T F ]: 2 : if(aux_acel < 0){
174 : : 1 : aux_acel *= -1;
175 : : 1 : aux.dataFrame[5] = 0x01;
176 : : : } else {
177 : : 1 : aux.dataFrame[5] = 0x00;
178 : : : }
179 : : :
180 : : 2 : unsigned int data_acel = ((aux_acel * RES_ACCELERATION_DIV_S) - OFFSET_ACCELERATION_S);
181 : : : unsigned char ms_acel, ls_acel;
182 : : 2 : ls_acel = data_acel;
183 : : 2 : ms_acel = data_acel >> 8;
184 : : :
185 : : : // Defines most and least significant bytes, according to the DBC specification
186 : : 2 : aux.dataFrame[3] = ls_acel;
187 : : 2 : aux.dataFrame[4] = ms_acel;
188 : : :
189 : : 2 : return aux;
190 : : : }
191 : : :
192 : : :
193 : : : /**
194 : : : * @brief Function that encapsulates data into the Obstacle CAN frame.
195 : : : *
196 : : : * This function encapsulates the data of the presence of an obstacle and it's distance
197 : : : * to the vehicle into the Obstacle CAN frame.
198 : : : *
199 : : : * @param has_obstacle Argument that refers if an obstacle is present or not.
200 : : : * @param obstacle_distance Distance from the vehicle to the obstacle.
201 : : : *
202 : : : * @return structure that contains the CAN message ID and Frame.
203 : : : *
204 : : : * \anchor conv2CANObstacleData
205 : : : *
206 : : : */
207 : : 2 : can_msg conv2CANObstacleData(bool has_obstacle, double obstacle_distance)
208 : : : {
209 : : 2 : can_msg aux = {.identifier = ID_OBSTACLE_S, .dataFrame = BASE_DATA_FRAME};
210 : : :
211 : : : // Obstacle detection data encapsulation
212 [ + + ]: [ T F ]: 2 : if (has_obstacle)
213 : : : {
214 : : 1 : aux.dataFrame[2] = 0x01;
215 : : : }
216 : : : else
217 : : : {
218 : : 1 : aux.dataFrame[2] = 0x00;
219 : : : }
220 : : :
221 : : : // Obstacle distance data encapsulation
222 : : 2 : unsigned int data_distance = obstacle_distance / RES_OBSTACLE_S;
223 : : : unsigned char ms_distance, ls_distance;
224 : : 2 : ls_distance = data_distance;
225 : : 2 : ms_distance = data_distance >> 8;
226 : : :
227 : : : // Defines most and least significant bytes, according to the DBC specification
228 : : 2 : aux.dataFrame[0] = ls_distance;
229 : : 2 : aux.dataFrame[1] = ms_distance;
230 : : :
231 : : 2 : return aux;
232 : : : }
233 : : :
234 : : :
235 : : : /**
236 : : : * @brief Function that encapsulates data into the Pedals CAN frame.
237 : : : *
238 : : : * This function encapsulates the data of the brake and accelerator pedals into
239 : : : * the Pedals CAN frame.
240 : : : *
241 : : : * @param brake_pedal Argument that refers if the brake is pressed or not.
242 : : : * @param accelerator_pedal Argument that refers if the accelerator is pressed or not.
243 : : : *
244 : : : * @return structure that contains the CAN message ID and Frame.
245 : : : *
246 : : : * \anchor conv2CANPedalsData
247 : : : *
248 : : : */
249 : : 4 : can_msg conv2CANPedalsData(bool brake_pedal, bool accelerator_pedal)
250 : : : {
251 : : 4 : can_msg aux = {.identifier = ID_PEDALS, .dataFrame = BASE_DATA_FRAME};
252 : : :
253 : : : // Brake pedal activation data encapsulation
254 [ + + ]: [ T F ]: 4 : if (brake_pedal)
255 : : : {
256 : : 2 : aux.dataFrame[1] = 0x01;
257 : : : }
258 : : : else
259 : : : {
260 : : 2 : aux.dataFrame[1] = 0x00;
261 : : : }
262 : : :
263 : : : // Accelerator pedal activation data encapsulation
264 [ + + ]: [ T F ]: 4 : if (accelerator_pedal)
265 : : : {
266 : : 2 : aux.dataFrame[0] = 0x01;
267 : : : }
268 : : : else
269 : : : {
270 : : 2 : aux.dataFrame[0] = 0x00;
271 : : : }
272 : : :
273 : : 4 : return aux;
274 : : : }
|
