4 #include <ardrone_tool/Navdata/ardrone_navdata_file.h>
6 #include "navdata_client/navdata_ihm.h"
8 #include <libPolaris/polaris.h>
11 static vp_os_mutex_t static_POLARIS_mutex;
12 vp_os_mutex_t *POLARIS_mutex=&static_POLARIS_mutex;
13 POLARIS_data data_POLARIS;
15 static int POLARIS_val_idx=0;
17 C_RESULT navdata_polaris_init( void* data )
22 C_RESULT navdata_polaris_process( const navdata_unpacked_t* const navdata )
25 static struct timeval old_tv1;
26 static float32_t polaris_old_x = 0.0f;
27 static float32_t polaris_old_y = 0.0f;
28 static float32_t polaris_old_z = 0.0f;
29 static float32_t polaris_old_vx = 0.0f;
30 static float32_t polaris_old_vy = 0.0f;
31 static float32_t polaris_old_vz = 0.0f;
32 static float32_t polaris_old_qx = 0.0f;
33 static float32_t polaris_old_qy = 0.0f;
34 float32_t polaris_delta_t_s = 0.0f;
35 float32_t polaris_cos_psi = 0.0f;
36 float32_t polaris_sin_psi = 0.0f;
38 if( navdata_file != NULL )
40 gettimeofday( &tv1, NULL );
41 polaris_delta_t_s=((tv1.tv_sec*1000000+tv1.tv_usec)/1000000.0)-((old_tv1.tv_sec*1000000+old_tv1.tv_usec)/1000000.0);
44 vp_os_mutex_lock(POLARIS_mutex);
46 if( BAD_FLOAT != data_POLARIS.y ) {
47 fprintf(navdata_file,"; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % d; % 6d",
48 (double) (data_POLARIS.x),
49 (double) (data_POLARIS.y),
50 (double) (data_POLARIS.z),
51 (double) (data_POLARIS.qx),
52 (double) (data_POLARIS.qy),
53 (double) (data_POLARIS.qz),
54 (double) (data_POLARIS.q0),
59 fprintf(navdata_file,"; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % 4.2f; % d; % 6d",
60 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (int32_t)tv1.tv_sec, (int32_t)tv1.tv_usec);
62 vp_os_mutex_unlock(POLARIS_mutex);
66 if (--POLARIS_val_idx < 0) POLARIS_val_idx = KIHM_N_PT2PLOT-1;
68 ihm_CA[KIHM_CURVE_VX].tval[2][POLARIS_val_idx] = polaris_old_vx;
69 ihm_CA[KIHM_CURVE_VY].tval[2][POLARIS_val_idx] = polaris_old_vy;
70 ihm_CA[KIHM_CURVE_VZ].tval[2][POLARIS_val_idx] = polaris_old_vz;
71 ihm_CA[KIHM_CURVE_PHI].tval[5][POLARIS_val_idx] = polaris_old_qx;
72 ihm_CA[KIHM_CURVE_THETA].tval[5][POLARIS_val_idx] = polaris_old_qy;
74 if (BAD_FLOAT==data_POLARIS.y) {
75 ihm_CA[KIHM_CURVE_VX].tval[2][POLARIS_val_idx] = 0.0f;
76 ihm_CA[KIHM_CURVE_VY].tval[2][POLARIS_val_idx] = 0.0f;
77 ihm_CA[KIHM_CURVE_VZ].tval[2][POLARIS_val_idx] = 0.0f;
79 else if(data_POLARIS.y != polaris_old_y) {
80 sincosf(data_POLARIS.qz * DEG_TO_RAD, &polaris_sin_psi, &polaris_cos_psi);
81 ihm_CA[KIHM_CURVE_VX].tval[2][POLARIS_val_idx] = (double) (polaris_cos_psi * (data_POLARIS.x - polaris_old_x)
82 +polaris_sin_psi * (data_POLARIS.y - polaris_old_y))
83 /polaris_delta_t_s ; // POLARIS en x
84 polaris_old_x = (double) (data_POLARIS.x);
85 polaris_old_vx = ihm_CA[KIHM_CURVE_VX].tval[2][POLARIS_val_idx];
88 ihm_CA[KIHM_CURVE_VY].tval[2][POLARIS_val_idx] = (double) (-polaris_sin_psi * (data_POLARIS.x - polaris_old_x)
89 +polaris_cos_psi * (data_POLARIS.y - polaris_old_y))
90 /polaris_delta_t_s ; // POLARIS en y
91 polaris_old_y = (double) (data_POLARIS.y);
92 polaris_old_vy = ihm_CA[KIHM_CURVE_VY].tval[2][POLARIS_val_idx];
94 ihm_CA[KIHM_CURVE_VZ].tval[2][POLARIS_val_idx] = (double) (data_POLARIS.z - polaris_old_z)/polaris_delta_t_s; // POLARIS en z
95 polaris_old_z = (double) (data_POLARIS.z);
96 polaris_old_vz = ihm_CA[KIHM_CURVE_VZ].tval[2][POLARIS_val_idx];
97 ihm_CA[KIHM_CURVE_PHI].tval[5][POLARIS_val_idx] = (double) (data_POLARIS.qx); // POLARIS en qx
98 polaris_old_qx =(double) (data_POLARIS.qx);
99 ihm_CA[KIHM_CURVE_THETA].tval[5][POLARIS_val_idx] = (double) (data_POLARIS.qy); // POLARIS en qy
100 polaris_old_qy = (double) (data_POLARIS.qy);
102 } while (POLARIS_val_idx!=ihm_val_idx);
108 C_RESULT navdata_polaris_release( void )