git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Navigation/Sources/navdata_client/navdata_polaris.c

   1 #include <sys/time.h>
   2 #include <time.h>
   3
   4 #include <ardrone_tool/Navdata/ardrone_navdata_file.h>
   5
   6 #include "navdata_client/navdata_ihm.h"
   7 #include <ihm/ihm.h>
   8 #include <libPolaris/polaris.h>
   9
  10 #ifdef PC_USE_POLARIS
  11 static vp_os_mutex_t static_POLARIS_mutex;
  12 vp_os_mutex_t *POLARIS_mutex=&static_POLARIS_mutex;
  13 POLARIS_data data_POLARIS;
  14
  15 static int   POLARIS_val_idx=0;
  16
  17 C_RESULT navdata_polaris_init( void* data )
  18 {
  19   return C_OK;
  20 }
  21
  22 C_RESULT navdata_polaris_process( const navdata_unpacked_t* const navdata )
  23 {
  24   struct timeval tv1;
  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;
  37
  38   if( navdata_file != NULL )
  39   {
  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);
  42                 old_tv1=tv1;
  43
  44     vp_os_mutex_lock(POLARIS_mutex);
  45
  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),
  55         (int)tv1.tv_sec,
  56         (int)tv1.tv_usec);
  57     }
  58     else {
  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);
  61     }
  62     vp_os_mutex_unlock(POLARIS_mutex);
  63   }
  64
  65         do {
  66                 if (--POLARIS_val_idx < 0) POLARIS_val_idx = KIHM_N_PT2PLOT-1;
  67
  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;
  73
  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;
  78                 }
  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];
  86
  87
  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];
  93
  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);
 101                 };
 102         } while (POLARIS_val_idx!=ihm_val_idx);
 103
 104
 105   return C_OK;
 106 }
 107
 108 C_RESULT navdata_polaris_release( void )
 109 {
 110   return C_OK;
 111 }
 112
 113 #endif