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

   1 /*
   2  * @raw_capture.c
   3  * @author aurelien.morelle@parrot.com
   4  * @date 2008/05/29
   5  *
   6  * Raw video capture from Mykonos RAM
   7  *
   8  */
   9 #include <stdio.h>
  10 #include <stdlib.h>
  11 #include <sys/time.h>
  12 #include <time.h>
  13
  14 #include <VP_Api/vp_api.h>
  15 #include <VP_Api/vp_api_thread_helper.h>
  16 #include <VP_Api/vp_api_error.h>
  17 #include <VP_Api/vp_api_stage.h>
  18 #include <VP_Stages/vp_stages_io_com.h>
  19 #include <VP_Stages/vp_stages_io_file.h>
  20
  21 #include <VP_Os/vp_os_print.h>
  22 #include <VP_Os/vp_os_malloc.h>
  23
  24 #include <ardrone_tool/ardrone_tool.h>
  25 #include <ardrone_tool/Com/config_com.h>
  26 #include <Vision/vision_stage.h>
  27
  28 #define NB_STAGES_MAX 10
  29
  30 PIPELINE_HANDLE raw_capture_pipeline_handle;
  31
  32
  33 // useful ?
  34 extern int exit_ihm_program;
  35
  36
  37 C_RESULT
  38 raw_capture_output_file_stage_open(vp_stages_output_file_config_t *cfg)
  39 {
  40   return (SUCCESS);
  41 }
  42
  43
  44 C_RESULT
  45 raw_capture_output_file_stage_transform(vp_stages_output_file_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
  46 {
  47   static int total_size = 0, local_size, size2read;
  48   static int start = 1;
  49
  50   static struct {
  51     int32_t size2read;
  52     vp_stages_vision_config_t saved_cfg;
  53   } special_struct;
  54
  55   struct timeval tv;
  56   struct tm atm;
  57
  58   static char raw_filename[256];
  59   char struct_filename[256];
  60   FILE *f;
  61
  62   vp_os_mutex_lock(&out->lock);
  63
  64   if(in->status == VP_API_STATUS_PROCESSING && in->size > 0)
  65     {
  66       local_size = in->size;
  67
  68       if(start)
  69         {
  70           if(!cfg->f)
  71             {
  72               gettimeofday(&tv,NULL);
  73               localtime_r(&tv.tv_sec, &atm);
  74               sprintf(&raw_filename[0], "raw_%04d%02d%02d_%02d%02d%02d.yuv", atm.tm_year+1900, atm.tm_mon+1, atm.tm_mday, atm.tm_hour, atm.tm_min, atm.tm_sec);
  75               sprintf(&struct_filename[0], "vision_state_%04d%02d%02d_%02d%02d%02d.raw", atm.tm_year+1900, atm.tm_mon+1, atm.tm_mday, atm.tm_hour, atm.tm_min, atm.tm_sec);
  76               cfg->name = &raw_filename[0];
  77               cfg->f = fopen(cfg->name, "wb");
  78               PRINT("Open %s                      \n", cfg->name);
  79             }
  80
  81           if(total_size < sizeof(special_struct))
  82             {
  83               vp_os_memcpy(total_size+(int8_t*)&special_struct, &in->buffers[in->indexBuffer][0], min(sizeof(special_struct)-total_size, local_size));
  84               local_size -= min(sizeof(special_struct)-total_size, in->size);
  85               total_size += min(sizeof(special_struct)-total_size, in->size);
  86             }
  87
  88           if(total_size == sizeof(special_struct))
  89             {
  90               size2read = special_struct.size2read;
  91               f = fopen(&struct_filename[0], "wb");
  92               fwrite(&special_struct.saved_cfg, sizeof(int8_t), sizeof(special_struct.saved_cfg), f);
  93               fclose(f);
  94               start = 0;
  95               total_size = 0;
  96             }
  97         }
  98
  99       if(start == 0 && local_size > 0)
 100         {
 101           fwrite(&in->buffers[in->indexBuffer][0], sizeof(int8_t), local_size*sizeof(int8_t), cfg->f);
 102           total_size += local_size;
 103           PRINT("red size = %07d / total size = %07d\r", total_size, size2read);
 104           if(total_size == size2read)
 105             {
 106               total_size = 0;
 107               start = 1;
 108               fclose(cfg->f);
 109               cfg->f = NULL;
 110               PRINT("Close %s                    \n", cfg->name);
 111             }
 112         }
 113     }
 114
 115   out->status = in->status;
 116
 117   vp_os_mutex_unlock(&out->lock);
 118
 119   return (SUCCESS);
 120 }
 121
 122
 123 C_RESULT
 124 raw_capture_output_file_stage_close(vp_stages_output_file_config_t *cfg)
 125 {
 126   return (SUCCESS);
 127 }
 128
 129
 130 const vp_api_stage_funcs_t raw_capture_output_file_funcs =
 131 {
 132   (vp_api_stage_handle_msg_t) NULL,
 133   (vp_api_stage_open_t) raw_capture_output_file_stage_open,
 134   (vp_api_stage_transform_t) raw_capture_output_file_stage_transform,
 135   (vp_api_stage_close_t) raw_capture_output_file_stage_close
 136 };
 137
 138
 139 DEFINE_THREAD_ROUTINE(raw_capture, data)
 140 {
 141   C_RESULT res;
 142
 143   vp_api_io_pipeline_t    pipeline;
 144   vp_api_io_data_t        out;
 145   vp_api_io_stage_t       stages[NB_STAGES_MAX];
 146
 147   vp_stages_input_com_config_t    icc;
 148   vp_stages_output_file_config_t  ofc;
 149
 150   vp_os_memset(&icc,        0, sizeof( icc ));
 151   vp_os_memset(&ofc,        0, sizeof( ofc ));
 152
 153   icc.com                 = COM_RAW_CAPTURE();
 154   icc.config              = COM_CONFIG_RAW_CAPTURE();
 155   icc.connection          = COM_CONNECTION_RAW_CAPTURE();
 156   icc.buffer_size         = 16*16*3;
 157   COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, RAW_CAPTURE_PORT, wifi_ardrone_ip);
 158
 159   ofc.name = "toto.out";
 160
 161   pipeline.nb_stages = 0;
 162
 163   stages[pipeline.nb_stages].type    = VP_API_INPUT_SOCKET;
 164   stages[pipeline.nb_stages].cfg     = (vp_stages_input_com_config_t *)&icc;
 165   stages[pipeline.nb_stages++].funcs = vp_stages_input_com_funcs;
 166
 167   stages[pipeline.nb_stages].type    = VP_API_OUTPUT_FILE;
 168   stages[pipeline.nb_stages].cfg     = (vp_stages_output_file_config_t *)&ofc;
 169   stages[pipeline.nb_stages++].funcs = raw_capture_output_file_funcs;
 170
 171   pipeline.stages = &stages[0];
 172
 173   PRINT("\n   Raw video capture thread initialisation\n\n");
 174
 175   res = vp_api_open(&pipeline, &raw_capture_pipeline_handle);
 176
 177   if( SUCCEED(res) )
 178   {
 179     int loop = SUCCESS;
 180     out.status = VP_API_STATUS_PROCESSING;
 181
 182     while( !ardrone_tool_exit() && (loop == SUCCESS) )
 183     {
 184       if( SUCCEED(vp_api_run(&pipeline, &out)) ) {
 185         if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) {
 186           loop = SUCCESS;
 187         }
 188         else loop = -1; // Finish this thread
 189       }
 190     }
 191
 192     vp_api_close(&pipeline, &raw_capture_pipeline_handle);
 193   }
 194
 195   PRINT("\n   Raw video capture thread closed\n\n");
 196
 197   return (THREAD_RET)0;
 198 }
 199