vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Video/video_stage.c

   1 /*
   2  *  video_stage.c
   3  *  Test
   4  *
   5  *  Created by Frédéric D'HAEYER on 22/02/10.
   6  *  Copyright 2010 Parrot SA. All rights reserved.
   7  *
   8  */
   9 #include <ardrone_tool/Video/video_stage.h>
  10
  11 #define NB_STAGES 5
  12
  13 PIPELINE_HANDLE pipeline_handle;
  14 static bool_t video_stage_in_pause = TRUE;
  15 static vp_os_cond_t video_stage_condition;
  16 static vp_os_mutex_t video_stage_mutex;
  17 static video_com_config_t icc;
  18 static video_stage_config_t video_stage_config;
  19
  20 const vp_api_stage_funcs_t video_stage_funcs =
  21 {
  22         (vp_api_stage_handle_msg_t) NULL,
  23         (vp_api_stage_open_t) video_stage_open,
  24         (vp_api_stage_transform_t) video_stage_transform,
  25         (vp_api_stage_close_t) video_stage_close
  26 };
  27
  28 C_RESULT video_stage_open(vlib_stage_decoding_config_t *cfg)
  29 {
  30         vp_os_mutex_init( &video_stage_config.mutex );
  31
  32         vp_os_mutex_lock( &video_stage_config.mutex );
  33         video_stage_config.data = vp_os_malloc(512 * 512 * 4);
  34         vp_os_memset(video_stage_config.data, 0x0, 512 * 512 * 4);
  35         vp_os_mutex_unlock( &video_stage_config.mutex );
  36
  37         return C_OK;
  38 }
  39
  40 C_RESULT video_stage_transform(vlib_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
  41 {
  42         vp_os_mutex_lock( &out->lock );
  43
  44         if(out->status == VP_API_STATUS_INIT)
  45         {
  46                 out->status = VP_API_STATUS_PROCESSING;
  47         }
  48
  49         if( in->status == VP_API_STATUS_ENDED )
  50         {
  51                 out->status = in->status;
  52         }
  53
  54         if(out->status == VP_API_STATUS_PROCESSING )
  55         {
  56                 vp_os_mutex_lock( &video_stage_config.mutex );
  57
  58                 if(cfg->num_picture_decoded > video_stage_config.num_picture_decoded)
  59                 {
  60                         video_stage_config.num_picture_decoded = cfg->num_picture_decoded;
  61                         video_stage_config.num_frame = cfg->controller.num_frames;
  62                         video_stage_config.bytesPerPixel        = 2;
  63                         video_stage_config.widthImage           = cfg->controller.width;
  64                         video_stage_config.heightImage          = cfg->controller.height;
  65
  66                         if (video_stage_config.data != NULL)
  67                         {
  68                                 vp_os_memcpy(video_stage_config.data, cfg->picture->y_buf, cfg->picture->width * cfg->picture->height );
  69                         }
  70
  71                         out->numBuffers = in->numBuffers;
  72                         out->indexBuffer = in->indexBuffer;
  73                         out->buffers = in->buffers;
  74                 }
  75
  76                 vp_os_mutex_unlock( &video_stage_config.mutex );
  77         }
  78
  79         vp_os_mutex_unlock( &out->lock );
  80
  81         return C_OK;
  82 }
  83
  84 C_RESULT video_stage_close(vlib_stage_decoding_config_t *cfg)
  85 {
  86         vp_os_free(video_stage_config.data);
  87
  88         return C_OK;
  89 }
  90
  91 video_stage_config_t* video_stage_get(void)
  92 {
  93         return &video_stage_config;
  94 }
  95
  96 void video_stage_init(void)
  97 {
  98         vp_os_mutex_init(&video_stage_mutex);
  99         vp_os_cond_init(&video_stage_condition, &video_stage_mutex);
 100 }
 101
 102 void video_stage_suspend_thread(void)
 103 {
 104         vp_os_mutex_lock(&video_stage_mutex);
 105         video_stage_in_pause = TRUE;
 106         vp_os_mutex_unlock(&video_stage_mutex);
 107 }
 108
 109 void video_stage_resume_thread(void)
 110 {
 111         vp_os_mutex_lock(&video_stage_mutex);
 112         vp_os_cond_signal(&video_stage_condition);
 113         video_stage_in_pause = FALSE;
 114         vp_os_mutex_unlock(&video_stage_mutex);
 115 }
 116
 117 DEFINE_THREAD_ROUTINE(video_stage, data)
 118 {
 119         C_RESULT res;
 120
 121         vp_api_io_pipeline_t    pipeline;
 122         vp_api_io_data_t        out;
 123         vp_api_io_stage_t       stages[NB_STAGES];
 124
 125         vp_api_picture_t picture;
 126
 127         vlib_stage_decoding_config_t    vec;
 128
 129         vp_os_memset(&icc,          0, sizeof( icc ));
 130         vp_os_memset(&vec,          0, sizeof( vec ));
 131         vp_os_memset(&picture,      0, sizeof( picture ));
 132
 133 //#ifdef RECORD_VIDEO
 134 //      video_stage_recorder_config_t vrc;
 135 //#endif
 136
 137         /// Picture configuration
 138         picture.format        = PIX_FMT_RGB565;
 139
 140         picture.width         = 512;
 141         picture.height        = 512;
 142         picture.framerate     = 15;
 143
 144         picture.y_buf   = vp_os_malloc( picture.width * picture.height * 2);
 145         picture.cr_buf  = NULL;
 146         picture.cb_buf  = NULL;
 147
 148         picture.y_line_size   = picture.width * 2;
 149         picture.cb_line_size  = 0;
 150         picture.cr_line_size  = 0;
 151
 152         icc.com                 = COM_VIDEO();
 153         icc.buffer_size         = 100000;
 154         icc.protocol            = VP_COM_UDP;
 155         COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
 156
 157         vec.width               = 512;
 158         vec.height              = 512;
 159         vec.picture             = &picture;
 160         vec.luma_only           = FALSE;
 161         vec.block_mode_enable   = TRUE;
 162
 163         pipeline.nb_stages = 0;
 164
 165         stages[pipeline.nb_stages].type    = VP_API_INPUT_SOCKET;
 166         stages[pipeline.nb_stages].cfg     = (void *)&icc;
 167         stages[pipeline.nb_stages].funcs   = video_com_funcs;
 168         pipeline.nb_stages++;
 169
 170         stages[pipeline.nb_stages].type    = VP_API_FILTER_DECODER;
 171         stages[pipeline.nb_stages].cfg     = (void*)&vec;
 172         stages[pipeline.nb_stages].funcs   = vlib_decoding_funcs;
 173         pipeline.nb_stages++;
 174
 175 /*
 176 #ifdef RECORD_VIDEO
 177         stages[pipeline.nb_stages].type    = VP_API_FILTER_DECODER;
 178         stages[pipeline.nb_stages].cfg     = (void*)&vrc;
 179         stages[pipeline.nb_stages].funcs   = video_recorder_funcs;
 180         pipeline.nb_stages++;
 181 #endif
 182 */
 183         stages[pipeline.nb_stages].type    = VP_API_OUTPUT_LCD;
 184         stages[pipeline.nb_stages].cfg     = (void*)&vec;
 185         stages[pipeline.nb_stages].funcs   = video_stage_funcs;
 186         pipeline.nb_stages++;
 187
 188         pipeline.stages = &stages[0];
 189
 190         if( !ardrone_tool_exit() )
 191         {
 192                 PRINT("\nvideo stage thread initialisation\n\n");
 193
 194                 res = vp_api_open(&pipeline, &pipeline_handle);
 195
 196                 if( SUCCEED(res) )
 197                 {
 198                         int loop = SUCCESS;
 199                         out.status = VP_API_STATUS_PROCESSING;
 200 #ifdef RECORD_VIDEO
 201                         {
 202                                 DEST_HANDLE dest;
 203                                 dest.stage = 2;
 204                                 dest.pipeline = pipeline_handle;
 205                                 vp_api_post_message( dest, PIPELINE_MSG_START, NULL, (void*)NULL);
 206                         }
 207 #endif
 208
 209                         while( !ardrone_tool_exit() && (loop == SUCCESS) )
 210                         {
 211                                 if(video_stage_in_pause)
 212                                 {
 213                                         vp_os_mutex_lock(&video_stage_mutex);
 214                                         icc.num_retries = VIDEO_MAX_RETRIES;
 215                                         vp_os_cond_wait(&video_stage_condition);
 216                                         vp_os_mutex_unlock(&video_stage_mutex);
 217                                 }
 218
 219                                 if( SUCCEED(vp_api_run(&pipeline, &out)) ) {
 220                                         if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) {
 221                                                 loop = SUCCESS;
 222                                         }
 223                                 }
 224                                 else loop = -1; // Finish this thread
 225                         }
 226
 227                         vp_api_close(&pipeline, &pipeline_handle);
 228                 }
 229         }
 230
 231         PRINT("   video stage thread ended\n\n");
 232
 233         return (THREAD_RET)0;
 234 }
 235
 236 uint32_t video_stage_get_num_retries(void)
 237 {
 238         return icc.num_retries;
 239 }