Rleg  2
 All Data Structures Files Functions Variables Typedefs Macros Groups Pages
datalogger.c
Go to the documentation of this file.
1 #include <stdlib.h>
2 #include <stdio.h>
3 #include <string.h>
4 #if ANU_COMPILE_FOR_XENOMAI
5 #include <native/mutex.h>
6 #else
7 #include <pthread.h>
8 #endif
9 
10 #include <pthread.h>
11 #include "gdatalogger/gqueue.h"
12 #include "gdatalogger/gmatlabdatafile.h"
13 #include "gdatalogger/gdatalogger.h"
14 #include "communication/communication.h"
15 //#include "gmatrix.h"
16 /*#include "imu.h"
17 #include "magnetometer.h"
18 #include "sonar.h"
19 #include "gps.h"
20 #include "sensors.h"
21 #include "calibration.h"
22 #include "estimation.h"
23 #include "control.h"
24 */
25 #include "datalogger.h"
26 
27 // Module variables
28 GDATALOGGER gDataLogger;
29 unsigned int datalogger_initialized = DATALOGGER_NOT_INITIALIZED;
30 unsigned int datalogger_running = DATALOGGER_NOT_RUNNING;
31 #if ANU_COMPILE_FOR_XENOMAI
32 RT_MUTEX datalogger_mutex;
33 #else
34 pthread_mutex_t datalogger_mutex = PTHREAD_MUTEX_INITIALIZER;
35 #endif
36 
37 // Internal functions
38 int datalogger_lock_mutex(void);
39 int datalogger_unlock_mutex(void);
40 
41 int datalogger_init(void)
42 {
43  #if DATALOGGER_MODULE_ENABLED
44  double enabled_variables[16][1];
45  char datalogger_filename[256];
46  #if ANU_COMPILE_FOR_XENOMAI
47  rt_mutex_create(&datalogger_mutex,"Data Logger Mutex");
48  #endif
49  datalogger_lock_mutex();
50 
51  if(datalogger_initialized == DATALOGGER_INITIALIZED)
52  {
53  datalogger_unlock_mutex();
54  return DATALOGGER_FAILURE;
55  }
56 
57  datalogger_running = DATALOGGER_NOT_RUNNING;
58 
59  // Init datalogger
60  strcpy(datalogger_filename, DATALOGGER_FOLDER);
61  strcat(datalogger_filename, "/");
62  strcat(datalogger_filename, DATALOGGER_FILE_NAME);
63  strcat(datalogger_filename, ".mat");
64  #if DATALOGGER_ENABLE_NEW_FILE_ENNUMERATION
65  char datalogger_tmp[256];
66  int i = 0;
67  if(datalogger_file_exists(datalogger_filename) == DATALOGGER_SUCCESS) // File exists
68  {
69  strcpy(datalogger_tmp, DATALOGGER_FOLDER);
70  strcat(datalogger_tmp, "/");
71  strcat(datalogger_tmp, DATALOGGER_FILE_NAME);
72  strcat(datalogger_tmp, "_%d.mat");
73  do
74  {
75  sprintf(datalogger_filename, datalogger_tmp, ++i);
76  }
77  while((datalogger_file_exists(datalogger_filename) == DATALOGGER_SUCCESS) && (i<100));
78  if(i == 100)
79  {
80  datalogger_unlock_mutex();
81  return DATALOGGER_FAILURE;
82  }
83  }
84  #endif //DATALOGGER_ENABLE_NEW_FILE_ENNUMERATION
85 
86  if(!gDataLogger_Init(&gDataLogger, datalogger_filename, NULL))
87  {
88  datalogger_unlock_mutex();
89  return DATALOGGER_FAILURE;
90  }
91 
92  // Declare variables
93  // Arguments for gDataLogger_DeclareVariable:
94  // datalogger pointer, variable name, units, number of rows, number of columns, queue size
95 
96  // Variable that tells MATLAB which variables are enabled
97  enabled_variables[0][0] = (double) DATALOGGER_LOG_TIME;
98  enabled_variables[1][0] = (double) DATALOGGER_LOG_EXECTIMES;
99  enabled_variables[2][0] = (double) DATALOGGER_LOG_RAW_IMU;
100  enabled_variables[3][0] = (double) DATALOGGER_LOG_CALIBRATED_IMU;
101  enabled_variables[4][0] = (double) DATALOGGER_LOG_EFFORTS;
102  enabled_variables[5][0] = (double) DATALOGGER_LOG_MRA;
103  //enabled_variables[12][0] = (double) DATALOGGER_LOG_LOCAL_COORDINATE_SYSTEM;
104  //enabled_variables[13][0] = (double) DATALOGGER_LOG_LOCAL_FIELDS;
105  //enabled_variables[14][0] = (double) DATALOGGER_LOG_ESTIMATOR;
106  //enabled_variables[15][0] = (double) DATALOGGER_LOG_CONTROLLERS;
107 
108  gDataLogger_DeclareVariable(&gDataLogger, "enabled_vars", "", 6, 1, 1);
109  gDataLogger_InsertVariable(&gDataLogger, "enabled_vars", &enabled_variables[0][0]);
110 
111  #if DATALOGGER_LOG_TIME
112  gDataLogger_DeclareVariable(&gDataLogger, "t", "s", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
113  gDataLogger_DeclareVariable(&gDataLogger, "Ts", "s", 1, 1, 1);
114  #endif // DATALOGGER_LOG_TIME
115 
116  #if DATALOGGER_LOG_EXECTIMES
117  gDataLogger_DeclareVariable(&gDataLogger, "t_control_exec", "s", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
118  gDataLogger_DeclareVariable(&gDataLogger, "t_ui_exec", "s", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
119  #endif // DATALOGGER_LOG_EXECTIMES
120 
121  #if DATALOGGER_LOG_RAW_IMU
122  gDataLogger_DeclareVariable(&gDataLogger, "imu_accelerometer_x_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
123  gDataLogger_DeclareVariable(&gDataLogger, "imu_accelerometer_y_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
124  gDataLogger_DeclareVariable(&gDataLogger, "imu_accelerometer_z_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
125  gDataLogger_DeclareVariable(&gDataLogger, "imu_magnetometer_x_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
126  gDataLogger_DeclareVariable(&gDataLogger, "imu_magnetometer_y_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
127  gDataLogger_DeclareVariable(&gDataLogger, "imu_magnetometer_z_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
128  gDataLogger_DeclareVariable(&gDataLogger, "imu_gyrometer_x_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
129  gDataLogger_DeclareVariable(&gDataLogger, "imu_gyrometer_y_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
130  gDataLogger_DeclareVariable(&gDataLogger, "imu_gyrometer_z_raw", "raw", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
131  gDataLogger_DeclareVariable(&gDataLogger, "imu_valid_data", "", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
132  #endif // DATALOGGER_LOG_IMU
133 
134  #if DATALOGGER_LOG_EFFORTS
135  gDataLogger_DeclareVariable(&gDataLogger, "Vin0", "mV", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
136  gDataLogger_DeclareVariable(&gDataLogger, "Vin0_valid_data", "", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
137  #endif
138 
139  #if DATALOGGER_LOG_MRA
140  gDataLogger_DeclareVariable(&gDataLogger, "v_ctl", "mV", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
141  gDataLogger_DeclareVariable(&gDataLogger, "v_ctl_read", "mV", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
142  #endif
143 
144  #if DATALOGGER_LOG_CALIBRATED_IMU
145  gDataLogger_DeclareVariable(&gDataLogger, "imu_accelerometer_x_g", "g", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
146  gDataLogger_DeclareVariable(&gDataLogger, "imu_accelerometer_y_g", "g", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
147  gDataLogger_DeclareVariable(&gDataLogger, "imu_accelerometer_z_g", "g", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
148  gDataLogger_DeclareVariable(&gDataLogger, "imu_magnetometer_x_B", "B", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
149  gDataLogger_DeclareVariable(&gDataLogger, "imu_magnetometer_y_B", "B", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
150  gDataLogger_DeclareVariable(&gDataLogger, "imu_magnetometer_z_B", "B", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
151  gDataLogger_DeclareVariable(&gDataLogger, "imu_gyrometer_x_rads", "rads", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
152  gDataLogger_DeclareVariable(&gDataLogger, "imu_gyrometer_y_rads", "rads", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
153  gDataLogger_DeclareVariable(&gDataLogger, "imu_gyrometer_z_rads", "rads", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
154  gDataLogger_DeclareVariable(&gDataLogger, "imu_calibrated_valid_data", "", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
155  //gDataLogger_DeclareVariable(&gDataLogger, "imu_calibrated_valid_accelerometer_data", "", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
156  //gDataLogger_DeclareVariable(&gDataLogger, "imu_calibrated_valid_gyrometer_data", "", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
157  //gDataLogger_DeclareVariable(&gDataLogger, "imu_calibrated_valid_magnetometer_data", "", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
158  #endif // DATALOGGER_LOG_CALIBRATED_IMU
159 
160 
161 /*
162  #if DATALOGGER_LOG_LOCAL_COORDINATE_SYSTEM
163  gDataLogger_DeclareVariable(&gDataLogger, "initial_altitude", "m", 1, 1, 1);
164  gDataLogger_DeclareVariable(&gDataLogger, "initial_latitude", "radians", 1, 1, 1);
165  gDataLogger_DeclareVariable(&gDataLogger, "initial_longitude", "radians", 1, 1, 1);
166  gDataLogger_DeclareVariable(&gDataLogger, "initial_ecef_x", "m", 1, 1, 1);
167  gDataLogger_DeclareVariable(&gDataLogger, "initial_ecef_y", "m", 1, 1, 1);
168  gDataLogger_DeclareVariable(&gDataLogger, "initial_ecef_z", "m", 1, 1, 1);
169  gDataLogger_DeclareVariable(&gDataLogger, "rotation_matrix", "matrix", 3, 3, 1);
170  #endif // DATALOGGER_LOG_LOCAL_COORDINATE_SYSTEM
171 
172  #if DATALOGGER_LOG_LOCAL_FIELDS
173  gDataLogger_DeclareVariable(&gDataLogger, "local_magnetic", "field", 3, 1, 1);
174  gDataLogger_DeclareVariable(&gDataLogger, "local_gravity", "field", 3, 1, 1);
175  gDataLogger_DeclareVariable(&gDataLogger, "magnetic_magnitude", "uT", 1, 1, 1);
176  gDataLogger_DeclareVariable(&gDataLogger, "gravity_magnitude", "ms2", 1, 1, 1);
177  #endif // DATALOGGER_LOG_LOCAL_FIELDS
178 
179  #if DATALOGGER_LOG_ESTIMATOR
180  gDataLogger_DeclareVariable(&gDataLogger, "estimated_roll_angle_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
181  gDataLogger_DeclareVariable(&gDataLogger, "estimated_pitch_angle_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
182  gDataLogger_DeclareVariable(&gDataLogger, "estimated_yaw_angle_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
183  gDataLogger_DeclareVariable(&gDataLogger, "estimated_q0", "quaternion", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
184  gDataLogger_DeclareVariable(&gDataLogger, "estimated_q1", "quaternion", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
185  gDataLogger_DeclareVariable(&gDataLogger, "estimated_q2", "quaternion", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
186  gDataLogger_DeclareVariable(&gDataLogger, "estimated_q3", "quaternion", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
187  gDataLogger_DeclareVariable(&gDataLogger, "estimated_x_position_meters", "meters", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
188  gDataLogger_DeclareVariable(&gDataLogger, "estimated_y_position_meters", "meters", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
189  gDataLogger_DeclareVariable(&gDataLogger, "estimated_z_position_meters", "meters", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
190  gDataLogger_DeclareVariable(&gDataLogger, "estimated_x_velocity_meters_second", "meters", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
191  gDataLogger_DeclareVariable(&gDataLogger, "estimated_y_velocity_meters_second", "meters", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
192  gDataLogger_DeclareVariable(&gDataLogger, "estimated_z_velocity_meters_second", "meters", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
193  #endif // DATALOGGER_LOG_ESTIMATOR
194 
195  #if DATALOGGER_LOG_CONTROLLERS
196  gDataLogger_DeclareVariable(&gDataLogger, "control_roll_angle_trim_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
197  gDataLogger_DeclareVariable(&gDataLogger, "control_pitch_angle_trim_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
198  gDataLogger_DeclareVariable(&gDataLogger, "control_yaw_angle_trim_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
199  gDataLogger_DeclareVariable(&gDataLogger, "control_left_aileron_trim", "us", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
200  gDataLogger_DeclareVariable(&gDataLogger, "control_right_aileron_trim", "us", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
201  gDataLogger_DeclareVariable(&gDataLogger, "control_elevator_trim", "us", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
202  gDataLogger_DeclareVariable(&gDataLogger, "control_rudder_trim", "us", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
203  gDataLogger_DeclareVariable(&gDataLogger, "control_roll_angle_reference_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
204  gDataLogger_DeclareVariable(&gDataLogger, "control_pitch_angle_reference_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
205  gDataLogger_DeclareVariable(&gDataLogger, "control_yaw_angle_reference_radians", "radians", 1, 1, DATALOGGER_STANDARD_QUEUE_SIZE);
206  #endif // DATALOGGER_LOG_CONTROLLERS
207 */
208  datalogger_initialized = DATALOGGER_INITIALIZED;
209  datalogger_unlock_mutex();
210  #endif //DATALOGGER_MODULE_ENABLED
211 
212  return DATALOGGER_SUCCESS;
213 }
214 
215 int datalogger_close(void)
216 {
217  #if DATALOGGER_MODULE_ENABLED
218  datalogger_lock_mutex();
219  if(datalogger_initialized != DATALOGGER_INITIALIZED)
220  {
221  datalogger_unlock_mutex();
222  return DATALOGGER_ERROR_NOT_INITIALIZED;
223  }
224 
225  gDataLogger_MatfileUpdate(&gDataLogger); // Update data
226  gDataLogger_Close(&gDataLogger); // Close datalogger
227 
228  datalogger_initialized = DATALOGGER_NOT_INITIALIZED;
229  datalogger_unlock_mutex();
230 
231  #if ANU_COMPILE_FOR_XENOMAI
232  rt_mutex_delete(&datalogger_mutex);
233  #endif //ANU_COMPILE_FOR_XENOMAI
234 
235  #endif //DATALOGGER_MODULE_ENABLED
236 
237  return DATALOGGER_SUCCESS;
238 }
239 
240 int datalogger_write_file(void)
241 {
242  #if DATALOGGER_MODULE_ENABLED
243  datalogger_lock_mutex();
244  if(datalogger_initialized != DATALOGGER_INITIALIZED)
245  {
246  datalogger_unlock_mutex();
247  return DATALOGGER_ERROR_NOT_INITIALIZED;
248  }
249  gDataLogger_MatfileUpdate(&gDataLogger); // Empty buffers in the file
250  datalogger_unlock_mutex();
251  #endif
252 
253  return DATALOGGER_SUCCESS;
254 }
255 
256 int datalogger_update_IPC(void)
257 {
258  #if DATALOGGER_MODULE_ENABLED
259  datalogger_lock_mutex();
260  if(datalogger_initialized != DATALOGGER_INITIALIZED)
261  {
262  datalogger_unlock_mutex();
263  return DATALOGGER_ERROR_NOT_INITIALIZED;
264  }
265 
266  gDataLogger_IPCUpdate(&gDataLogger); // Manages IPC
267  datalogger_unlock_mutex();
268  #endif
269  return DATALOGGER_SUCCESS;
270 }
271 
272 int datalogger_update(double t_s, double t_control_exec_s, double t_ui_exec_s, double t0_s, IMU_DATA_STRUCT *pimu_data, EFF_DATA_STRUCT *peff_data, MRA_DATA_STRUCT *pmra_data/*IMUMEASURE *pimu_measure, MAGNETOMETERMEASURE *pmagnetometer_measure, ESTIMATION_DATA_STRUCT *pestimation_data, CONTROL_DATA_STRUCT *pcontrol_data */)
273 {
274  double tmp = 0.0;
275 
276  datalogger_lock_mutex();
277 
278  if(datalogger_initialized != DATALOGGER_INITIALIZED)
279  {
280  datalogger_unlock_mutex();
281  return DATALOGGER_ERROR_NOT_INITIALIZED;
282  }
283 
284  #if DATALOGGER_LOG_TIME
285  double tmp_t = t_s - t0_s;
286  gDataLogger_InsertVariable(&gDataLogger, "t", &tmp_t);
287  #else
288  double tmp_t = 0.0;
289  tmp_t = t_s; // Remove warning
290  #endif // DATALOGGER_LOG_TIME
291 
292  #if DATALOGGER_LOG_EXECTIMES
293  gDataLogger_InsertVariable(&gDataLogger, "t_control_exec", &t_control_exec_s);
294  gDataLogger_InsertVariable(&gDataLogger, "t_ui_exec", &t_ui_exec_s);
295  #else
296  double tmp_e = 0.0; // Remove warning
297  tmp_e = t_control_exec_s; // Remove warning
298  tmp_e = t_ui_exec_s; // Remove warning
299  #endif // DATALOGGER_LOG_EXECTIMES
300 
301 
302  #if DATALOGGER_LOG_RAW_IMU
303  tmp = (double) pimu_data->acc.x;
304  gDataLogger_InsertVariable(&gDataLogger, "imu_accelerometer_x_raw", &tmp);
305  tmp = (double) pimu_data->acc.y;
306  gDataLogger_InsertVariable(&gDataLogger, "imu_accelerometer_y_raw", &tmp);
307  tmp = (double) pimu_data->acc.z;
308  gDataLogger_InsertVariable(&gDataLogger, "imu_accelerometer_z_raw", &tmp);
309  tmp = (double) pimu_data->mag.x;
310  gDataLogger_InsertVariable(&gDataLogger, "imu_magnetometer_x_raw", &tmp);
311  tmp = (double) pimu_data->mag.y;
312  gDataLogger_InsertVariable(&gDataLogger, "imu_magnetometer_y_raw", &tmp);
313  tmp = (double) pimu_data->mag.z;
314  gDataLogger_InsertVariable(&gDataLogger, "imu_magnetometer_z_raw", &tmp);
315  tmp = (double) pimu_data->gyr.x;
316  gDataLogger_InsertVariable(&gDataLogger, "imu_gyrometer_x_raw", &tmp);
317  tmp = (double) pimu_data->gyr.y;
318  gDataLogger_InsertVariable(&gDataLogger, "imu_gyrometer_y_raw", &tmp);
319  tmp = (double) pimu_data->gyr.z;
320  gDataLogger_InsertVariable(&gDataLogger, "imu_gyrometer_z_raw", &tmp);
321  tmp = (double) pimu_data->new_data;
322  gDataLogger_InsertVariable(&gDataLogger, "imu_valid_data", &tmp);
323  #else
324  double tmp_ri = 0; // Remove warning
325  tmp_ri = pimu_data->new_data; // Remove warning ???
326  #endif //DATALOGGER_LOG_RAW_IMU
327 
328  #if DATALOGGER_LOG_EFFORTS
329  tmp = (double) peff_data->F.x;
330  gDataLogger_InsertVariable(&gDataLogger, "Vin0", &tmp);
331  tmp = (double) peff_data->new_data;
332  gDataLogger_InsertVariable(&gDataLogger, "Vin0_valid_data", &tmp);
333  #endif
334 
335  #if DATALOGGER_LOG_MRA
336  tmp = (double) pmra_data->v_ctl;
337  gDataLogger_InsertVariable(&gDataLogger, "v_ctl", &tmp);
338  tmp = (double) pmra_data->v_ctl_read;
339  gDataLogger_InsertVariable(&gDataLogger, "v_ctl_read", &tmp);
340  #endif
341 
342 
343 
344  #if DATALOGGER_LOG_CALIBRATED_IMU
345  gDataLogger_InsertVariable(&gDataLogger, "imu_accelerometer_x_g", &(pimu_data->calib.acc.x));
346  gDataLogger_InsertVariable(&gDataLogger, "imu_accelerometer_y_g", &(pimu_data->calib.acc.y));
347  gDataLogger_InsertVariable(&gDataLogger, "imu_accelerometer_z_g", &(pimu_data->calib.acc.z));
348  gDataLogger_InsertVariable(&gDataLogger, "imu_magnetometer_x_B", &(pimu_data->calib.mag.x));
349  gDataLogger_InsertVariable(&gDataLogger, "imu_magnetometer_y_B", &(pimu_data->calib.mag.y));
350  gDataLogger_InsertVariable(&gDataLogger, "imu_magnetometer_z_B", &(pimu_data->calib.mag.z));
351  gDataLogger_InsertVariable(&gDataLogger, "imu_gyrometer_x_rads", &(pimu_data->calib.gyr.x));
352  gDataLogger_InsertVariable(&gDataLogger, "imu_gyrometer_y_rads", &(pimu_data->calib.gyr.y));
353  gDataLogger_InsertVariable(&gDataLogger, "imu_gyrometer_z_rads", &(pimu_data->calib.gyr.z));
354  tmp = (double) pimu_data->new_data;
355  gDataLogger_InsertVariable(&gDataLogger, "imu_calibrated_valid_data", &tmp);
356  //tmp = pimu_measure->FlagValidAccerometerMeasure;
357  //gDataLogger_InsertVariable(&gDataLogger, "imu_calibrated_valid_accelerometer_data", &tmp);
358  //tmp = pimu_measure->FlagValidGyrometerMeasure;
359  //gDataLogger_InsertVariable(&gDataLogger, "imu_calibrated_valid_gyrometer_data", &tmp);
360  //tmp = pmagnetometer_measure->FlagValidMeasure;
361  //gDataLogger_InsertVariable(&gDataLogger, "imu_calibrated_valid_magnetometer_data", &tmp);
362  #else
363  double tmp_ci = 0.0;
364  tmp_ci = pimu_measure->ax; // Remove warning
365  tmp_ci = pmagnetometer_measure->mx; // Remove warning
366  #endif // DATALOGGER_LOG_CALIBRATED_IMU
367 
368  /*
369  #if DATALOGGER_LOG_ESTIMATOR
370  gDataLogger_InsertVariable(&gDataLogger, "estimated_roll_angle_radians", &(pestimation_data->roll_angle_radians));
371  gDataLogger_InsertVariable(&gDataLogger, "estimated_pitch_angle_radians", &(pestimation_data->pitch_angle_radians));
372  gDataLogger_InsertVariable(&gDataLogger, "estimated_yaw_angle_radians", &(pestimation_data->yaw_angle_radians));
373  gDataLogger_InsertVariable(&gDataLogger, "estimated_q0", &(pestimation_data->q0));
374  gDataLogger_InsertVariable(&gDataLogger, "estimated_q1", &(pestimation_data->q1));
375  gDataLogger_InsertVariable(&gDataLogger, "estimated_q2", &(pestimation_data->q2));
376  gDataLogger_InsertVariable(&gDataLogger, "estimated_q3", &(pestimation_data->q3));
377  gDataLogger_InsertVariable(&gDataLogger, "estimated_x_position_meters", &(pestimation_data->x_position_meters));
378  gDataLogger_InsertVariable(&gDataLogger, "estimated_y_position_meters", &(pestimation_data->y_position_meters));
379  gDataLogger_InsertVariable(&gDataLogger, "estimated_z_position_meters", &(pestimation_data->z_position_meters));
380  gDataLogger_InsertVariable(&gDataLogger, "estimated_x_velocity_meters_second", &(pestimation_data->x_velocity_meters_second));
381  gDataLogger_InsertVariable(&gDataLogger, "estimated_y_velocity_meters_second", &(pestimation_data->y_velocity_meters_second));
382  gDataLogger_InsertVariable(&gDataLogger, "estimated_z_velocity_meters_second", &(pestimation_data->z_velocity_meters_second));
383  #else
384  double tmp_est = 0.0;
385  tmp_est = pestimation_data->roll_angle_radians; // Remove warning
386  #endif // DATALOGGER_LOG_ESTIMATOR
387 
388  #if DATALOGGER_LOG_CONTROLLERS
389  gDataLogger_InsertVariable(&gDataLogger, "control_roll_angle_trim_radians", &(pcontrol_data->roll_angle_trim_radians));
390  gDataLogger_InsertVariable(&gDataLogger, "control_pitch_angle_trim_radians", &(pcontrol_data->pitch_angle_trim_radians));
391  gDataLogger_InsertVariable(&gDataLogger, "control_yaw_angle_trim_radians", &(pcontrol_data->yaw_angle_trim_radians));
392  gDataLogger_InsertVariable(&gDataLogger, "control_left_aileron_trim", &(pcontrol_data->left_aileron_trim));
393  gDataLogger_InsertVariable(&gDataLogger, "control_right_aileron_trim", &(pcontrol_data->right_aileron_trim));
394  gDataLogger_InsertVariable(&gDataLogger, "control_elevator_trim", &(pcontrol_data->elevator_trim));
395  gDataLogger_InsertVariable(&gDataLogger, "control_rudder_trim", &(pcontrol_data->rudder_trim));
396  gDataLogger_InsertVariable(&gDataLogger, "control_roll_angle_reference_radians", &(pcontrol_data->roll_angle_reference_radians));
397  gDataLogger_InsertVariable(&gDataLogger, "control_pitch_angle_reference_radians", &(pcontrol_data->pitch_angle_reference_radians));
398  gDataLogger_InsertVariable(&gDataLogger, "control_yaw_angle_reference_radians", &(pcontrol_data->yaw_angle_reference_radians));
399  #else
400  double tmp_control = 0.0;
401  tmp_control = pcontrol_data->roll_angle_trim_radians;
402  #endif // DATALOGGER_LOG_CONTROLLERS
403 */
404  datalogger_unlock_mutex();
405 
406  return DATALOGGER_SUCCESS;
407 }
408 /*
409 int datalogger_set_local_fields(CALIBRATION_LOCAL_FIELDS_STRUCT *plocal_fields_data)
410 {
411  static int datalogger_local_fields_inserted = DATALOGGER_VARIABLE_NOT_INSERTED;
412 
413  datalogger_lock_mutex();
414 
415  if(datalogger_initialized != DATALOGGER_INITIALIZED)
416  {
417  datalogger_unlock_mutex();
418  return DATALOGGER_ERROR_NOT_INITIALIZED;
419  }
420 
421  #if DATALOGGER_LOG_LOCAL_FIELDS
422  if(plocal_fields_data->system_initialized != CALIBRATION_LOCAL_FIELDS_INITIALIZED)
423  {
424  datalogger_unlock_mutex();
425  return DATALOGGER_FAILURE;
426  }
427  if(datalogger_local_fields_inserted == DATALOGGER_VARIABLE_NOT_INSERTED)
428  {
429  double tmp_matrix[3][1];
430 
431  tmp_matrix[0][0] = PGMATRIX_DATA(plocal_fields_data->local_magnetic, 1, 1);
432  tmp_matrix[1][0] = PGMATRIX_DATA(plocal_fields_data->local_magnetic, 2, 1);
433  tmp_matrix[2][0] = PGMATRIX_DATA(plocal_fields_data->local_magnetic, 3, 1);
434  gDataLogger_InsertVariable(&gDataLogger, "local_magnetic", &tmp_matrix[0][0]);
435 
436  tmp_matrix[0][0] = PGMATRIX_DATA(plocal_fields_data->local_gravity, 1, 1);
437  tmp_matrix[1][0] = PGMATRIX_DATA(plocal_fields_data->local_gravity, 2, 1);
438  tmp_matrix[2][0] = PGMATRIX_DATA(plocal_fields_data->local_gravity, 3, 1);
439  gDataLogger_InsertVariable(&gDataLogger, "local_gravity", &tmp_matrix[0][0]);
440 
441  gDataLogger_InsertVariable(&gDataLogger, "magnetic_magnitude", &(plocal_fields_data->magnetic_magnitude));
442  gDataLogger_InsertVariable(&gDataLogger, "gravity_magnitude", &(plocal_fields_data->gravity_magnitude));
443  }
444  else
445  {
446  datalogger_unlock_mutex();
447  return DATALOGGER_VARIBALE_ALREADY_INSERTED;
448  }
449  #else
450  double tmp = 0.0;
451  tmp = plocal_fields_data->magnetic_magnitude; // Remove warning
452  datalogger_local_fields_inserted = DATALOGGER_VARIABLE_NOT_INSERTED;
453  #endif // DATALOGGER_LOG_LOCAL_FIELDS
454  datalogger_unlock_mutex();
455  return DATALOGGER_SUCCESS;
456 }
457 */
458 /*
459 int datalogger_set_local_coordinate_system(CALIBRATION_LOCAL_COORDINATE_SYSTEM_STRUCT *plocal_coordinate_system_data)
460 {
461  static int datalogger_local_coordinate_inserted = DATALOGGER_VARIABLE_NOT_INSERTED;
462  datalogger_lock_mutex();
463  if(datalogger_initialized != DATALOGGER_INITIALIZED)
464  {
465  datalogger_unlock_mutex();
466  return DATALOGGER_ERROR_NOT_INITIALIZED;
467  }
468 
469  #if DATALOGGER_LOG_LOCAL_COORDINATE_SYSTEM
470  if(plocal_coordinate_system_data->system_initialized != CALIBRATION_LOCAL_COORDINATE_SYSTEM_INITIALIZED)
471  {
472  datalogger_unlock_mutex();
473  return DATALOGGER_FAILURE;
474  }
475 
476  if(datalogger_local_coordinate_inserted == DATALOGGER_VARIABLE_NOT_INSERTED)
477  {
478  datalogger_local_coordinate_inserted = DATALOGGER_VARIABLE_INSERTED;
479  double tmp_matrix[3][3];
480 
481  tmp_matrix[0][0] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 1, 1);
482  tmp_matrix[1][0] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 2, 1);
483  tmp_matrix[2][0] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 3, 1);
484  tmp_matrix[0][1] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 1, 2);
485  tmp_matrix[1][1] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 2, 2);
486  tmp_matrix[2][1] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 3, 2);
487  tmp_matrix[0][2] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 1, 3);
488  tmp_matrix[1][2] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 2, 3);
489  tmp_matrix[2][2] = PGMATRIX_DATA(plocal_coordinate_system_data->protation_matrix, 3, 3);
490 
491  gDataLogger_InsertVariable(&gDataLogger, "initial_altitude", &(plocal_coordinate_system_data->altitude_meters));
492  gDataLogger_InsertVariable(&gDataLogger, "initial_latitude", &(plocal_coordinate_system_data->latitude_radians));
493  gDataLogger_InsertVariable(&gDataLogger, "initial_longitude", &(plocal_coordinate_system_data->longitude_radians));
494  gDataLogger_InsertVariable(&gDataLogger, "initial_ecef_x", &(plocal_coordinate_system_data->x0));
495  gDataLogger_InsertVariable(&gDataLogger, "initial_ecef_y", &(plocal_coordinate_system_data->y0));
496  gDataLogger_InsertVariable(&gDataLogger, "initial_ecef_z", &(plocal_coordinate_system_data->z0));
497  gDataLogger_InsertVariable(&gDataLogger, "rotation_matrix", &tmp_matrix[0][0]);
498  }
499  else
500  {
501  datalogger_unlock_mutex();
502  return DATALOGGER_VARIBALE_ALREADY_INSERTED;
503  }
504  #else
505  double tmp = 0.0;
506  tmp = plocal_coordinate_system_data->altitude_meters; // Remove warning
507  datalogger_local_coordinate_inserted = DATALOGGER_VARIABLE_NOT_INSERTED;
508  #endif // DATALOGGER_LOG_LOCAL_COORDINATE_SYSTEM
509 
510  datalogger_unlock_mutex();
511  return DATALOGGER_SUCCESS;
512 }
513 
514 int datalogger_set_initial_filter_parameters(void)
515 {
516  static int datalogger_filter_parameters_inserted = DATALOGGER_VARIABLE_NOT_INSERTED;
517 
518  datalogger_lock_mutex();
519 
520  if(datalogger_initialized != DATALOGGER_INITIALIZED)
521  {
522  datalogger_unlock_mutex();
523  return DATALOGGER_ERROR_NOT_INITIALIZED;
524  }
525  if(datalogger_filter_parameters_inserted == DATALOGGER_VARIABLE_NOT_INSERTED)
526  {
527  datalogger_filter_parameters_inserted = DATALOGGER_VARIABLE_INSERTED;
528  // TODO! Add X0,P0 and sensor covariances.
529  }
530  else
531  {
532  datalogger_unlock_mutex();
533  return DATALOGGER_VARIBALE_ALREADY_INSERTED;
534  }
535 
536  datalogger_unlock_mutex();
537  return DATALOGGER_SUCCESS;
538 }
539 */
540 int datalogger_set_Ts(double Ts)
541 {
542  static int datalogger_Ts_inserted = DATALOGGER_VARIABLE_NOT_INSERTED;
543 
544  datalogger_lock_mutex();
545 
546  if(datalogger_initialized != DATALOGGER_INITIALIZED)
547  {
548  datalogger_unlock_mutex();
549  return DATALOGGER_ERROR_NOT_INITIALIZED;
550  }
551 
552  #if DATALOGGER_LOG_TIME
553  if(datalogger_Ts_inserted == DATALOGGER_VARIABLE_NOT_INSERTED)
554  {
555  datalogger_Ts_inserted = DATALOGGER_VARIABLE_INSERTED;
556  gDataLogger_InsertVariable(&gDataLogger, "Ts", &Ts);
557  }
558  else
559  {
560  datalogger_unlock_mutex();
561  return DATALOGGER_VARIBALE_ALREADY_INSERTED;
562  }
563  #else
564  double tmp = Ts; // Remove warning
565  #endif // DATALOGGER_LOG_TIME
566 
567  datalogger_unlock_mutex();
568  return DATALOGGER_SUCCESS;
569 }
570 
571 int datalogger_status(void)
572 {
573  datalogger_lock_mutex();
574  if(datalogger_initialized != DATALOGGER_INITIALIZED)
575  {
576  datalogger_unlock_mutex();
577  return DATALOGGER_ERROR_NOT_INITIALIZED;
578  }
579 
580  datalogger_unlock_mutex();
581  return datalogger_running;
582 }
583 
584 int datalogger_start(void)
585 {
586  #if DATALOGGER_MODULE_ENABLED
587  datalogger_lock_mutex();
588  if(datalogger_initialized != DATALOGGER_INITIALIZED)
589  {
590  datalogger_unlock_mutex();
591  return DATALOGGER_ERROR_NOT_INITIALIZED;
592  }
593 
594  datalogger_running = DATALOGGER_RUNNING;
595  datalogger_unlock_mutex();
596  #endif
597 
598  return DATALOGGER_SUCCESS;
599 }
600 
601 int datalogger_stop(void)
602 {
603  #if DATALOGGER_MODULE_ENABLED
604  datalogger_lock_mutex();
605  if(datalogger_initialized != DATALOGGER_INITIALIZED)
606  {
607  datalogger_unlock_mutex();
608  return DATALOGGER_ERROR_NOT_INITIALIZED;
609  }
610 
611  datalogger_running = DATALOGGER_NOT_RUNNING;
612  datalogger_unlock_mutex();
613  #endif
614 
615  return DATALOGGER_SUCCESS;
616 }
617 
618 int datalogger_file_exists(const char *filename)
619 {
620  FILE *file;
621  if((file = fopen(filename, "r")))
622  {
623  fclose(file);
624  return DATALOGGER_SUCCESS;
625  }
626  return DATALOGGER_FAILURE;
627 }
628 
629 int datalogger_lock_mutex(void)
630 {
631  #if ANU_COMPILE_FOR_XENOMAI
632  rt_mutex_acquire(&datalogger_mutex,TM_INFINITE);
633  #else
634  pthread_mutex_lock(&datalogger_mutex);
635  #endif
636  return DATALOGGER_SUCCESS;
637 }
638 
639 int datalogger_unlock_mutex(void)
640 {
641  #if ANU_COMPILE_FOR_XENOMAI
642  rt_mutex_release(&datalogger_mutex);
643  #else
644  pthread_mutex_unlock(&datalogger_mutex);
645  #endif
646  return DATALOGGER_SUCCESS;
647 }
DATALOGGER_ERROR_NOT_INITIALIZED
#define DATALOGGER_ERROR_NOT_INITIALIZED
Definition: datalogger.h:37
DATALOGGER_VARIABLE_NOT_INSERTED
#define DATALOGGER_VARIABLE_NOT_INSERTED
Definition: datalogger.h:31
datalogger_set_Ts
int datalogger_set_Ts(double Ts)
Definition: datalogger.c:540
datalogger_start
int datalogger_start(void)
Definition: datalogger.c:584
DATALOGGER_RUNNING
#define DATALOGGER_RUNNING
Definition: datalogger.h:30
gDataLogger_InsertVariable
int gDataLogger_InsertVariable(PGDATALOGGER pgDataLogger, char *varname, double *varvalue)
Definition: gdatalogger.c:199
IMU_DATA_STRUCT::calibrated::mag
DATA_XYZ_DOUBLE mag
Definition: communication.h:100
MRA_DATA_STRUCT
Struct to control MRA.
Definition: communication.h:129
IMU_DATA_STRUCT
Data of IMU structure.
Definition: communication.h:93
DATA_XYZ::y
short int y
Definition: communication.h:77
DATALOGGER_FILE_NAME
#define DATALOGGER_FILE_NAME
Definition: datalogger.h:10
datalogger_update_IPC
int datalogger_update_IPC(void)
Definition: datalogger.c:256
DATALOGGER_LOG_TIME
#define DATALOGGER_LOG_TIME
Definition: datalogger.h:15
DATALOGGER_STANDARD_QUEUE_SIZE
#define DATALOGGER_STANDARD_QUEUE_SIZE
Definition: datalogger.h:12
datalogger_close
int datalogger_close(void)
Definition: datalogger.c:215
DATALOGGER_LOG_RAW_IMU
#define DATALOGGER_LOG_RAW_IMU
Definition: datalogger.h:17
DATALOGGER_NOT_INITIALIZED
#define DATALOGGER_NOT_INITIALIZED
Definition: datalogger.h:27
DATALOGGER_LOG_EXECTIMES
#define DATALOGGER_LOG_EXECTIMES
Definition: datalogger.h:16
DATALOGGER_LOG_EFFORTS
#define DATALOGGER_LOG_EFFORTS
Definition: datalogger.h:18
IMU_DATA_STRUCT::mag
DATA_XYZ mag
Magnetormeter Vector.
Definition: communication.h:96
datalogger_write_file
int datalogger_write_file(void)
Definition: datalogger.c:240
datalogger_init
int datalogger_init(void)
Definition: datalogger.c:41
IMU_DATA_STRUCT::calibrated::gyr
DATA_XYZ_DOUBLE gyr
Definition: communication.h:99
DATALOGGER_VARIABLE_INSERTED
#define DATALOGGER_VARIABLE_INSERTED
Definition: datalogger.h:32
datalogger_unlock_mutex
int datalogger_unlock_mutex(void)
Definition: datalogger.c:639
datalogger_update
int datalogger_update(double t_s, double t_control_exec_s, double t_ui_exec_s, double t0_s, IMU_DATA_STRUCT *pimu_data, EFF_DATA_STRUCT *peff_data, MRA_DATA_STRUCT *pmra_data)
Definition: datalogger.c:272
DATALOGGER_LOG_CALIBRATED_IMU
#define DATALOGGER_LOG_CALIBRATED_IMU
Definition: datalogger.h:22
datalogger_initialized
unsigned int datalogger_initialized
Definition: datalogger.c:29
DATALOGGER_VARIBALE_ALREADY_INSERTED
#define DATALOGGER_VARIBALE_ALREADY_INSERTED
Definition: datalogger.h:38
datalogger_mutex
pthread_mutex_t datalogger_mutex
Definition: datalogger.c:34
DATALOGGER_LOG_MRA
#define DATALOGGER_LOG_MRA
Definition: datalogger.h:19
gDataLogger
GDATALOGGER gDataLogger
Definition: datalogger.c:28
DATALOGGER_INITIALIZED
#define DATALOGGER_INITIALIZED
Definition: datalogger.h:28
gDataLogger_MatfileUpdate
int gDataLogger_MatfileUpdate(PGDATALOGGER pgDataLogger)
Definition: gdatalogger.c:245
EFF_DATA_STRUCT::new_data
uint8_t new_data
Definition: communication.h:115
EFF_DATA_STRUCT::F
DATA_XYZ F
Definition: communication.h:113
MRA_DATA_STRUCT::v_ctl_read
short int v_ctl_read
Voltage level read from the actuator.
Definition: communication.h:131
IMU_DATA_STRUCT::acc
DATA_XYZ acc
Accel Vector.
Definition: communication.h:94
DATALOGGER_FAILURE
#define DATALOGGER_FAILURE
Definition: datalogger.h:36
DATALOGGER_NOT_RUNNING
#define DATALOGGER_NOT_RUNNING
Definition: datalogger.h:29
DATALOGGER_SUCCESS
#define DATALOGGER_SUCCESS
Definition: datalogger.h:35
gDataLogger_IPCUpdate
int gDataLogger_IPCUpdate(PGDATALOGGER pgDataLogger)
Definition: gdatalogger.c:274
DATA_XYZ::z
short int z
Definition: communication.h:78
gDataLogger_DeclareVariable
int gDataLogger_DeclareVariable(PGDATALOGGER pgDataLogger, char *varname, char *varunit, int nrows, int ncols, int queuesize)
Definition: gdatalogger.c:173
DATALOGGER_FOLDER
#define DATALOGGER_FOLDER
Definition: datalogger.h:11
gDataLogger_Close
int gDataLogger_Close(PGDATALOGGER pgDataLogger)
Definition: gdatalogger.c:362
IMU_DATA_STRUCT::calib
struct IMU_DATA_STRUCT::calibrated calib
datalogger_file_exists
int datalogger_file_exists(const char *filename)
Definition: datalogger.c:618
DATA_XYZ_DOUBLE::y
double y
Definition: communication.h:86
gDataLogger_Init
int gDataLogger_Init(PGDATALOGGER pgDataLogger, char *filename, char *dirname)
Definition: gdatalogger.c:42
IMU_DATA_STRUCT::calibrated::acc
DATA_XYZ_DOUBLE acc
Definition: communication.h:98
IMU_DATA_STRUCT::gyr
DATA_XYZ gyr
Gyrometer Vector.
Definition: communication.h:95
DATA_XYZ_DOUBLE::z
double z
Definition: communication.h:87
DATA_XYZ::x
short int x
Definition: communication.h:76
datalogger_running
unsigned int datalogger_running
Definition: datalogger.c:30
EFF_DATA_STRUCT
Definition: communication.h:112
MRA_DATA_STRUCT::v_ctl
short int v_ctl
Voltage level for control output.
Definition: communication.h:130
IMU_DATA_STRUCT::new_data
uint8_t new_data
Definition: communication.h:104
datalogger_status
int datalogger_status(void)
Definition: datalogger.c:571
DATA_XYZ_DOUBLE::x
double x
Definition: communication.h:85
datalogger_stop
int datalogger_stop(void)
Definition: datalogger.c:601
datalogger_lock_mutex
int datalogger_lock_mutex(void)
Definition: datalogger.c:629
Generated on Tue Mar 18 2014 14:30:11 for Rleg by   doxygen 1.8.6