vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_file.c

   1 #include <time.h>
   2 #ifndef _WIN32
   3         #include <sys/time.h>
   4 #else
   5
   6  #include <sys/timeb.h>
   7  #include <Winsock2.h>  // for timeval structure
   8
   9  int gettimeofday (struct timeval *tp, void *tz)
  10  {
  11          struct _timeb timebuffer;
  12          _ftime (&timebuffer);
  13          tp->tv_sec = (long)timebuffer.time;
  14          tp->tv_usec = (long)timebuffer.millitm * 1000;
  15          return 0;
  16  }
  17 #endif
  18
  19 #include <stdlib.h>
  20
  21 #include <VP_Os/vp_os_malloc.h>
  22 #include <VP_Os/vp_os_print.h>
  23
  24 #include <ardrone_tool/ardrone_tool.h>
  25 #include <ardrone_tool/Navdata/ardrone_navdata_file.h>
  26 #include <ardrone_tool/UI/ardrone_tool_ui.h>
  27
  28 uint32_t num_picture_decoded = 0;
  29
  30 extern float32_t nd_iphone_gaz;
  31 extern float32_t nd_iphone_yaw;
  32 extern int32_t nd_iphone_flag;
  33 extern float32_t nd_iphone_phi;
  34 extern float32_t nd_iphone_theta;
  35
  36 // Public declaration of navdata_file allowing other handlers to write into
  37 FILE* navdata_file = NULL;
  38
  39 // Private declaration of navdata_file
  40 // Allow this handler to disable other handlers that write in navdata file
  41 static FILE* navdata_file_private = NULL;
  42
  43 static void ardrone_navdata_file_print_version( void )
  44 {
  45   unsigned int i;
  46   fprintf(navdata_file,"VERSION 19b\n");  // TODO : CHANGE VERSION NUMBER EVERY TIME THE FILE STRUCTURE CHANGES
  47   fprintf(navdata_file,
  48   "Control_state [-]; ARDrone_state [-]; Time [s]; \
  49     AccX_raw [LSB]; AccY_raw [LSB]; AccZ_raw [LSB]; \
  50     GyroX_raw [LSB]; GyroY_raw [LSB]; GyroZ_raw [LSB]; GyroX_110_raw [LSB]; GyroY_110_raw [LSB];Battery_Voltage_raw [mV]; \
  51     Alim_3V3 [LSB]; vrefEpson [LSB]; vrefIDG [LSB]; flag_echo_ini [LSB]; us_debut_echo [LSB]; us_fin_echo [LSB]; us_association_echo; us_distance_echo [LSB];\
  52      us_courbe_temps [LSB]; us_courbe_valeur [LSB]; us_courbe_ref [LSB];\
  53     Accs_temperature [K]; Gyro_temperature [LSB]; AccX_phys_filt [mg]; AccY_phys_filt [mg]; AccZ_phys_filt [mg]; \
  54     GyroX_phys_filt [deg/s]; GyroY_phys_filt [deg/s]; GyroZ_phys_filt [deg/s];\
  55     GyroX_offset [deg/s]; GyroY_offset [deg/s]; GyroZ_offset [deg/s]; \
  56     Theta_acc [mdeg]; Phi_acc [mdeg];\
  57     Theta_ref_embedded [mdeg]; Phi_ref_embedded [mdeg]; Psi_ref_embedded [mdeg]; Theta_ref_int [mdeg]; Phi_ref_int [mdeg]; \
  58     Pitch_ref_embedded [mdeg]; Roll_ref_embedded [mdeg]; Yaw_ref_embedded [mdeg/s]; \
  59     Theta_trim_embedded [mdeg]; Phi_trim_embedded [mdeg]; Yaw_trim_embedded [mdeg/s]; \
  60     Pitch_rc_embedded [-]; Roll_rc_embedded [-]; Yaw_rc_embedded [-]; Gaz_rc_embedded [-]; Ag_rc_embedded [-]; User_Input [-]; \
  61     PWM1 [PWM]; PWM2 [PWM]; PWM3 [PWM]; PWM4 [PWM];\
  62     SAT_PWM1 [PWM]; SAT_PWM2 [PWM]; SAT_PWM3 [PWM]; SAT_PWM4 [PWM];\
  63     Gaz_feed_forward [PWM]; Gaz_altitude [PWM]; Altitude_integral [mm/s]; Vz_Ref [mm/s] ;\
  64     u_pitch [PWM]; u_roll [PWM]; u_yaw [PWM]; yaw_u_I [PWM];\
  65     u_pitch_planif [PWM]; u_roll_planif [PWM]; u_yaw_planif [PWM]; u_gaz_planif [PWM];\
  66     Current_motor1 [mA]; Current_motor2 [mA]; Current_motor3 [mA]; Current_motor4 [mA];\
  67     Altitude_vision [mm]; Altitude_vz [mm/s]; Altitude_ref_embedded [mm]; Altitude_raw [mm]; \
  68     Observer AccZ [m/s2]; Observer altitude US [m]; Estimated Altitude[m]; Estimated Vz [m/s]; Estimated acc bias [m/s2];\
  69     Observer state [-]; vb 1; vb 2;     Observer flight state [-];\
  70     Vision_tx_raw [-]; Vision_ty_raw [-]; Vision_tz_raw [-];   Vision_State [-]; Vision_defined [-];\
  71     Vision_phi_trim[rad]; Vision_phi_ref_prop[rad]; Vision_theta_trim[rad]; Vision_theta_ref_prop[rad];\
  72     Vx_body [mm/s]; Vy_body [mm/s]; Vz_body [mm/s];\
  73     New_raw_picture [-]; T_capture; F_capture; P_capture; delta_Phi; delta_Theta; delta_Psi; Alt_capture [-]; time_capture [s];\
  74         Demo_vbat [-]; Demo_theta [mdeg]; Demo_phi [mdeg]; Demo_psi [mdeg];  Demo_altitude [mm];  Demo_vx [mm/s]; Demo_vy [mm/s]; Demo_vz [mm/s];Demo_num_frames [-];\
  75     Demo_detect_rot_m11 [-]; Demo_detect_rot_m12 [-]; Demo_detect_rot_m13 [-]; Demo_detect_rot_m21 [-]; Demo_detect_rot_m22 [-]; Demo_detect_rot_m23 [-];\
  76     Demo_detect_rot_m31 [-]; Demo_detect_rot_m32 [-]; Demo_detect_rot_m33 [-]; Demo_detect_trans_v1 [-]; Demo_detect_trans_v2 [-]; Demo_detect_trans_v3 [-]; \
  77         Demo_detect_tag_index [-]; Demo_camera_type [-]; Demo_drone_camera_rot_m11 [-]; Demo_drone_camera_rot_m12 [-]; Demo_drone_camera_rot_m13 [-];\
  78     Demo_drone_camera_rot_m21 [-]; Demo_drone_camera_rot_m22 [-]; Demo_drone_camera_rot_m23 [-]; Demo_drone_camera_rot_m31 [-]; Demo_drone_camera_rot_m32 [-];\
  79     Demo_drone_camera_rot_m33 [-]; Demo_drone_camera_trans_x [-]; Demo_drone_camera_trans_y [-]; Demo_drone_camera_trans_z [-];\
  80         nd_iphone_flag [-]; nd_iphone_phi [-]; nd_iphone_theta [-]; nd_iphone_gaz [-]; nd_iphone_yaw [-];\
  81         quant [-]; encoded_frame_size [bytes]; encoded_frame_number [-]; atcmd_ref_seq [-]; atcmd_mean_ref_gap [ms]; atcmd_var_ref_gap [SU]; atcmd_ref_quality[-];");
  82
  83   for(i = 0 ; i < DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT ; i++)
  84     fprintf(navdata_file, "Locked_%u; X_%u; Y_%u; ", i, i, i);
  85
  86   fprintf(navdata_file, "Nb_detected; ");
  87
  88   for(i = 0 ; i < 4 ; i++)
  89     fprintf(navdata_file, "Type_%u; Xd_%u; Yd_%u; W_%u; H_%u; D_%u; O_%u; ", i, i, i, i, i, i, i);
  90
  91   fprintf(navdata_file, "Perf_szo [ms]; Perf_corners [ms]; Perf_compute [ms]; Perf_tracking [ms]; Perf_trans [ms]; Perf_update [ms]; ");
  92
  93         for(i=0; i<NAVDATA_MAX_CUSTOM_TIME_SAVE; i++)
  94                 fprintf(navdata_file, "Perf_Custom_%u; ", i);
  95
  96   // tags after "flag_new_picture" will be written on the IHM side
  97   fprintf(navdata_file, "Watchdog Control [-]; flag_new_picture [-]; Sample time [s]; ");
  98
  99   #ifdef PC_USE_POLARIS
 100     fprintf( navdata_file,
 101     "POLARIS_X [mm]; POLARIS_Y [mm]; POLARIS_Z [mm]; \
 102   POLARIS_QX [deg]; POLARIS_QY [deg]; POLARIS_QZ [deg];\
 103   POLARIS_Q0 [deg]; Time s [s]; Time us [us]; ");
 104   #endif
 105
 106   #ifdef USE_TABLE_PILOTAGE
 107       fprintf( navdata_file, " Table_Pilotage_position [mdeg]; Table_Pilotage_vitesse [deg/s]; ");
 108   #endif
 109 }
 110
 111 struct tm *navdata_atm = NULL;
 112
 113 C_RESULT ardrone_navdata_file_init( void* data )
 114 {
 115   char filename[1024];
 116   struct timeval tv;
 117   time_t temptime;
 118   const char* path = (const char*) data;
 119
 120   gettimeofday(&tv,NULL);
 121   temptime = (time_t)tv.tv_sec;
 122   navdata_atm = localtime(&temptime);
 123   if( path != NULL )
 124   {
 125     sprintf(filename, "%s/mesures_%04d%02d%02d_%02d%02d%02d.txt",
 126             path,
 127             navdata_atm->tm_year+1900, navdata_atm->tm_mon+1, navdata_atm->tm_mday,
 128             navdata_atm->tm_hour, navdata_atm->tm_min, navdata_atm->tm_sec);
 129   }
 130   else
 131   {
 132     sprintf(filename, "mesures_%04d%02d%02d_%02d%02d%02d.txt",
 133             navdata_atm->tm_year+1900, navdata_atm->tm_mon+1, navdata_atm->tm_mday,
 134             navdata_atm->tm_hour, navdata_atm->tm_min, navdata_atm->tm_sec);
 135   }
 136
 137   // private for instance
 138   navdata_file_private = fopen(filename, "wb");
 139
 140   return navdata_file_private != NULL ? C_OK : C_FAIL;
 141 }
 142
 143 C_RESULT ardrone_navdata_file_process( const navdata_unpacked_t* const pnd )
 144 {
 145   uint32_t i;
 146   char str[50];
 147   int32_t* locked_ptr;
 148   screen_point_t* point_ptr;
 149   struct timeval time;
 150
 151   gettimeofday(&time,NULL);
 152
 153   if( navdata_file_private == NULL )
 154     return C_FAIL;
 155
 156   if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
 157     return C_OK;
 158
 159   if( navdata_file == NULL )
 160   {
 161     navdata_file = navdata_file_private;
 162
 163     if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
 164     {
 165       printf("Receiving navdata demo\n");
 166     }
 167     else
 168     {
 169       printf("Receiving all navdata\n");
 170     }
 171     ardrone_navdata_file_print_version();
 172   }
 173
 174   // Handle the case where user asked for a new navdata file
 175   if( navdata_file != navdata_file_private )
 176   {
 177     fclose(navdata_file);
 178     navdata_file = navdata_file_private;
 179
 180     if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
 181     {
 182       printf("Receiving navdata demo\n");
 183     }
 184     else
 185     {
 186       printf("Receiving all navdata\n");
 187     }
 188     ardrone_navdata_file_print_version();
 189   }
 190
 191         vp_os_memset(&str[0], 0, sizeof(str));
 192
 193     fprintf( navdata_file,"\n" );
 194     fprintf( navdata_file, "%u; %u", (unsigned int) pnd->navdata_demo.ctrl_state, (unsigned int) pnd->ardrone_state );
 195
 196     sprintf( str, "%d.%06d", (int)((pnd->navdata_time.time & TSECMASK) >> TSECDEC), (int)(pnd->navdata_time.time & TUSECMASK) );
 197     fprintf( navdata_file, ";%s", str );
 198
 199     fprintf( navdata_file, "; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u",
 200                             (unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_X],
 201                             (unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Y],
 202                             (unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Z],
 203                             (unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_X],
 204                             (unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_Y],
 205                             (unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_Z],
 206                             (unsigned int) pnd->navdata_raw_measures.raw_gyros_110[0],
 207                             (unsigned int) pnd->navdata_raw_measures.raw_gyros_110[1],
 208                             (unsigned int) pnd->navdata_raw_measures.vbat_raw,
 209                             (unsigned int) pnd->navdata_phys_measures.alim3V3,
 210                             (unsigned int) pnd->navdata_phys_measures.vrefEpson,
 211                             (unsigned int) pnd->navdata_phys_measures.vrefIDG,
 212                             (unsigned int) pnd->navdata_raw_measures.flag_echo_ini,
 213                             (unsigned int) pnd->navdata_raw_measures.us_debut_echo,
 214                             (unsigned int) pnd->navdata_raw_measures.us_fin_echo,
 215                             (unsigned int) pnd->navdata_raw_measures.us_association_echo,
 216                             (unsigned int) pnd->navdata_raw_measures.us_distance_echo,
 217                             (unsigned int) pnd->navdata_raw_measures.us_courbe_temps,
 218                             (unsigned int) pnd->navdata_raw_measures.us_courbe_valeur,
 219                             (unsigned int) pnd->navdata_raw_measures.us_courbe_ref );
 220
 221     fprintf( navdata_file, "; %f; %04u; % 5f; % 5f; % 5f; % 6f; % 6f; % 6f",
 222                             pnd->navdata_phys_measures.accs_temp,
 223                             (unsigned int)pnd->navdata_phys_measures.gyro_temp,
 224                             pnd->navdata_phys_measures.phys_accs[ACC_X],
 225                             pnd->navdata_phys_measures.phys_accs[ACC_Y],
 226                             pnd->navdata_phys_measures.phys_accs[ACC_Z],
 227                             pnd->navdata_phys_measures.phys_gyros[GYRO_X],
 228                             pnd->navdata_phys_measures.phys_gyros[GYRO_Y],
 229                             pnd->navdata_phys_measures.phys_gyros[GYRO_Z]);
 230
 231     fprintf( navdata_file, "; % f; % f; % f",
 232                             pnd->navdata_gyros_offsets.offset_g[GYRO_X],
 233                             pnd->navdata_gyros_offsets.offset_g[GYRO_Y],
 234                             pnd->navdata_gyros_offsets.offset_g[GYRO_Z] );
 235
 236     fprintf( navdata_file, "; % f; % f",
 237                             pnd->navdata_euler_angles.theta_a,
 238                             pnd->navdata_euler_angles.phi_a);
 239
 240     fprintf( navdata_file, ";  %04d; %04d; %04d; %04d; %04d; %06d; %06d; %06d",
 241                             (int) pnd->navdata_references.ref_theta,
 242                             (int) pnd->navdata_references.ref_phi,
 243                             (int) pnd->navdata_references.ref_psi,
 244                             (int) pnd->navdata_references.ref_theta_I,
 245                             (int) pnd->navdata_references.ref_phi_I,
 246                             (int) pnd->navdata_references.ref_pitch,
 247                             (int) pnd->navdata_references.ref_roll,
 248                             (int) pnd->navdata_references.ref_yaw );
 249
 250     fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f",
 251                             pnd->navdata_trims.euler_angles_trim_theta,
 252                             pnd->navdata_trims.euler_angles_trim_phi,
 253                             pnd->navdata_trims.angular_rates_trim_r );
 254
 255     fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %04u",
 256                             (int) pnd->navdata_rc_references.rc_ref_pitch,
 257                             (int) pnd->navdata_rc_references.rc_ref_roll,
 258                             (int) pnd->navdata_rc_references.rc_ref_yaw,
 259                             (int) pnd->navdata_rc_references.rc_ref_gaz,
 260                             (int) pnd->navdata_rc_references.rc_ref_ag,
 261                             (unsigned int) ui_get_user_input() );
 262
 263     fprintf( navdata_file, "; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03d; % f; % f; %03d; %03d; %03d; % f; %03d; %03d; %03d; % f; %04d; %04d; %04d; %04d",
 264                             (unsigned int) pnd->navdata_pwm.motor1,
 265                             (unsigned int) pnd->navdata_pwm.motor2,
 266                             (unsigned int) pnd->navdata_pwm.motor3,
 267                             (unsigned int) pnd->navdata_pwm.motor4,
 268                             (unsigned int) pnd->navdata_pwm.sat_motor1,
 269                             (unsigned int) pnd->navdata_pwm.sat_motor2,
 270                             (unsigned int) pnd->navdata_pwm.sat_motor3,
 271                             (unsigned int) pnd->navdata_pwm.sat_motor4,
 272                             (int) pnd->navdata_pwm.gaz_feed_forward,
 273                             pnd->navdata_pwm.gaz_altitude,
 274                             pnd->navdata_pwm.altitude_integral,
 275                             pnd->navdata_pwm.vz_ref,
 276                             (int) pnd->navdata_pwm.u_pitch,
 277                             (int) pnd->navdata_pwm.u_roll,
 278                             (int) pnd->navdata_pwm.u_yaw,
 279                             pnd->navdata_pwm.yaw_u_I,
 280                             (int) pnd->navdata_pwm.u_pitch_planif,
 281                             (int) pnd->navdata_pwm.u_roll_planif,
 282                             (int) pnd->navdata_pwm.u_yaw_planif,
 283                             pnd->navdata_pwm.u_gaz_planif,
 284                             (int) pnd->navdata_pwm.current_motor1,
 285                             (int) pnd->navdata_pwm.current_motor2,
 286                             (int) pnd->navdata_pwm.current_motor3,
 287                             (int) pnd->navdata_pwm.current_motor4 );
 288
 289     fprintf( navdata_file, "; %04d; %f; %04d; %04u",
 290                             (int) pnd->navdata_altitude.altitude_vision,
 291                             pnd->navdata_altitude.altitude_vz,
 292                             (int) pnd->navdata_altitude.altitude_ref,
 293                             (unsigned int) pnd->navdata_altitude.altitude_raw );
 294
 295    fprintf( navdata_file, "; %f; %f; %f; %f; %f; %04u; %f; %f; %04u",
 296                             pnd->navdata_altitude.obs_accZ,
 297                             pnd->navdata_altitude.obs_alt,
 298                             pnd->navdata_altitude.obs_x.v[0],
 299                             pnd->navdata_altitude.obs_x.v[1],
 300                             pnd->navdata_altitude.obs_x.v[2],
 301                             pnd->navdata_altitude.obs_state,
 302                                                                                                                 pnd->navdata_altitude.est_vb.v[0],
 303                             pnd->navdata_altitude.est_vb.v[1],
 304                             pnd->navdata_altitude.est_state );
 305
 306                 vp_os_memset(&str[0], 0, sizeof(str));
 307     sprintf( str, "%d.%06d", (int)((pnd->navdata_vision.time_capture & TSECMASK) >> TSECDEC), (int)(pnd->navdata_vision.time_capture & TUSECMASK) );
 308
 309     fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f; %u; %u; % f;% f;% f;% f; % f; % f; % f; %u; % f; % f; % f; % f; % f; % f; % d; %s",
 310                             pnd->navdata_vision_raw.vision_tx_raw,
 311                             pnd->navdata_vision_raw.vision_ty_raw,
 312                             pnd->navdata_vision_raw.vision_tz_raw,
 313                             (unsigned int) pnd->navdata_vision.vision_state,
 314                             (unsigned int) pnd->vision_defined,
 315                             pnd->navdata_vision.vision_phi_trim,
 316                             pnd->navdata_vision.vision_phi_ref_prop,
 317                             pnd->navdata_vision.vision_theta_trim,
 318                             pnd->navdata_vision.vision_theta_ref_prop,
 319                             pnd->navdata_vision.body_v.x,
 320                             pnd->navdata_vision.body_v.y,
 321                             pnd->navdata_vision.body_v.z,
 322                             (unsigned int) pnd->navdata_vision.new_raw_picture,
 323                             pnd->navdata_vision.theta_capture,
 324                             pnd->navdata_vision.phi_capture,
 325                             pnd->navdata_vision.psi_capture,
 326                             pnd->navdata_vision.delta_phi,
 327                             pnd->navdata_vision.delta_theta,
 328                             pnd->navdata_vision.delta_psi,
 329                             (int)pnd->navdata_vision.altitude_capture,
 330                             str );
 331
 332     fprintf( navdata_file, "; %04u",
 333                              (unsigned int) pnd->navdata_demo.vbat_flying_percentage );
 334
 335      fprintf( navdata_file, "; % f; % f; % f",
 336                              pnd->navdata_demo.theta,
 337                              pnd->navdata_demo.phi,
 338                              pnd->navdata_demo.psi );
 339
 340      fprintf( navdata_file, "; %04d",
 341                              (int) pnd->navdata_demo.altitude );
 342
 343      fprintf( navdata_file, "; %f; %f; %f ",
 344                                                          pnd->navdata_demo.vx,
 345                              pnd->navdata_demo.vy,
 346                              pnd->navdata_demo.vz );
 347
 348      fprintf( navdata_file, "; %04u", (unsigned int) pnd->navdata_demo.num_frames );
 349
 350      fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.detection_camera_rot.m11,
 351                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m12,
 352                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m13,
 353                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m21,
 354                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m22,
 355                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m23,
 356                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m31,
 357                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m32,
 358                                                                                                                                         pnd->navdata_demo.detection_camera_rot.m33);
 359
 360      fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.detection_camera_trans.x,
 361                                                                                         pnd->navdata_demo.detection_camera_trans.y,
 362                                                                                         pnd->navdata_demo.detection_camera_trans.z);
 363
 364      fprintf( navdata_file, "; %04u; %04u",
 365                              (unsigned int) pnd->navdata_demo.detection_tag_index,
 366                              (unsigned int) pnd->navdata_demo.detection_camera_type);
 367
 368      fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.drone_camera_rot.m11,
 369                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m12,
 370                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m13,
 371                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m21,
 372                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m22,
 373                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m23,
 374                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m31,
 375                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m32,
 376                                                                                                                                         pnd->navdata_demo.drone_camera_rot.m33);
 377
 378      fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.drone_camera_trans.x,
 379                                                                                         pnd->navdata_demo.drone_camera_trans.y,
 380                                                                                         pnd->navdata_demo.drone_camera_trans.z);
 381
 382      fprintf( navdata_file, "; %d; %f; %f; %f; %f",
 383                                                                                         (int)nd_iphone_flag,
 384                                                                                         nd_iphone_phi,
 385                                                                                         nd_iphone_theta,
 386                                                                                         nd_iphone_gaz,
 387                                                                                         nd_iphone_yaw);
 388
 389            fprintf( navdata_file, "; %d; %d; %d; %d; %d; %f; %d",
 390                            pnd->navdata_video_stream.quant,
 391                            pnd->navdata_video_stream.frame_size,
 392                            pnd->navdata_video_stream.frame_number,
 393                            pnd->navdata_video_stream.atcmd_ref_seq,
 394                            pnd->navdata_video_stream.atcmd_mean_ref_gap,
 395                            pnd->navdata_video_stream.atcmd_var_ref_gap,
 396                            pnd->navdata_video_stream.atcmd_ref_quality);
 397
 398
 399
 400     locked_ptr  = (int32_t*) &pnd->navdata_trackers_send.locked[0];
 401     point_ptr   = (screen_point_t*) &pnd->navdata_trackers_send.point[0];
 402
 403     for(i = 0; i < DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT; i++)
 404     {
 405       fprintf( navdata_file, "; %d; %u; %u",
 406                             (int) *locked_ptr++,
 407                             (unsigned int) point_ptr->x,
 408                             (unsigned int) point_ptr->y );
 409       point_ptr++;
 410     }
 411
 412     fprintf( navdata_file, "; %u", (unsigned int) pnd->navdata_vision_detect.nb_detected );
 413     for(i = 0 ; i < 4 ; i++)
 414     {
 415       fprintf( navdata_file, "; %u; %u; %u; %u; %u; %u; %f",
 416                             (unsigned int) pnd->navdata_vision_detect.type[i],
 417                             (unsigned int) pnd->navdata_vision_detect.xc[i],
 418                             (unsigned int) pnd->navdata_vision_detect.yc[i],
 419                             (unsigned int) pnd->navdata_vision_detect.width[i],
 420                             (unsigned int) pnd->navdata_vision_detect.height[i],
 421                             (unsigned int) pnd->navdata_vision_detect.dist[i],
 422                             pnd->navdata_vision_detect.orientation_angle[i]);
 423     }
 424
 425     fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f",
 426                           pnd->navdata_vision_perf.time_szo,
 427                           pnd->navdata_vision_perf.time_corners,
 428                           pnd->navdata_vision_perf.time_compute,
 429                           pnd->navdata_vision_perf.time_tracking,
 430                           pnd->navdata_vision_perf.time_trans,
 431                           pnd->navdata_vision_perf.time_update );
 432
 433     for(i = 0 ; i < NAVDATA_MAX_CUSTOM_TIME_SAVE ; i++)
 434     {
 435       fprintf( navdata_file, "; %f", pnd->navdata_vision_perf.time_custom[i]);
 436         }
 437
 438     fprintf( navdata_file, "; %d", (int) pnd->navdata_watchdog.watchdog );
 439
 440     fprintf( navdata_file, "; %u", (unsigned int) num_picture_decoded );
 441
 442     sprintf( str, "%d.%06d", (int)time.tv_sec, (int)time.tv_usec);
 443     fprintf( navdata_file, "; %s", str );
 444
 445   return C_OK;
 446 }
 447
 448 C_RESULT ardrone_navdata_file_release( void )
 449 {
 450   if( navdata_file != NULL )
 451   {
 452     navdata_file = NULL;
 453
 454     fprintf(navdata_file_private,"\n");
 455
 456     fclose( navdata_file_private );
 457
 458     navdata_file_private = NULL;
 459   }
 460
 461   return C_OK;
 462 }