DEFINE_THREAD_ROUTINE(video_update_thread, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES]; vp_api_picture_t picture; video_com_config_t icc; vlib_stage_decoding_config_t vec; vp_stages_yuv2rgb_config_t yuv2rgbconf; /// Picture configuration picture.format = PIX_FMT_YUV420P; picture.width = STREAM_WIDTH; picture.height = STREAM_HEIGHT; picture.framerate = 30; picture.y_buf = (uint8_t *)vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT ); picture.cr_buf = (uint8_t *)vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT / 4 ); picture.cb_buf = (uint8_t *)vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT / 4 ); picture.y_line_size = STREAM_WIDTH; picture.cb_line_size = STREAM_WIDTH / 2; picture.cr_line_size = STREAM_WIDTH / 2; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf )); icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); vec.width = STREAM_WIDTH; vec.height = STREAM_HEIGHT; vec.picture = &picture; #ifdef USE_VIDEO_YUV vec.luma_only = FALSE; #else vec.luma_only = TRUE; #endif // USE_VIDEO_YUV vec.block_mode_enable = TRUE; vec.luma_only = FALSE; yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24; if( CAMIF_H_CAMERA_USED == CAMIF_CAMERA_OVTRULY_UPSIDE_DOWN_ONE_BLOCKLINE_LESS ) yuv2rgbconf.mode = VP_STAGES_YUV2RGB_MODE_UPSIDE_DOWN; pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB; stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf; stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; stages[pipeline.nb_stages].cfg = (void *)&vec; stages[pipeline.nb_stages].funcs = vp_stages_export_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; PIPELINE_HANDLE pipeline_handle; res = vp_api_open(&pipeline, &pipeline_handle); if( SUCCEED(res) ) { while( SUCCEED(vp_api_run(&pipeline, &out)) ) { } vp_api_close(&pipeline, &pipeline_handle); } return (THREAD_RET)0; }
DEFINE_THREAD_ROUTINE(video_stage, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES]; vp_api_picture_t picture; vlib_stage_decoding_config_t vec; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&picture, 0, sizeof( picture )); //#ifdef RECORD_VIDEO // video_stage_recorder_config_t vrc; //#endif /// Picture configuration picture.format = /*PIX_FMT_YUV420P */ PIX_FMT_RGB565; picture.width = 512; picture.height = 512; picture.framerate = 15; picture.y_buf = vp_os_malloc( picture.width * picture.height * 2); picture.cr_buf = NULL; picture.cb_buf = NULL; picture.y_line_size = picture.width * 2; picture.cb_line_size = 0; picture.cr_line_size = 0; icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); vec.width = 512; vec.height = 512; vec.picture = &picture; vec.luma_only = FALSE; vec.block_mode_enable = TRUE; pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; /* #ifdef RECORD_VIDEO stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vrc; stages[pipeline.nb_stages].funcs = video_recorder_funcs; pipeline.nb_stages++; #endif */ stages[pipeline.nb_stages].type = VP_API_OUTPUT_LCD; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = video_stage_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; if( !ardrone_tool_exit() ) { PRINT("\nvideo stage thread initialisation\n\n"); // NICK video_stage_init(); res = vp_api_open(&pipeline, &pipeline_handle); if( VP_SUCCEEDED(res) ) { int loop = VP_SUCCESS; out.status = VP_API_STATUS_PROCESSING; #ifdef RECORD_VIDEO { DEST_HANDLE dest; dest.stage = 2; dest.pipeline = pipeline_handle; vp_api_post_message( dest, PIPELINE_MSG_START, NULL, (void*)NULL); } #endif video_stage_in_pause = FALSE; while( !ardrone_tool_exit() && (loop == VP_SUCCESS) ) { if(video_stage_in_pause) { vp_os_mutex_lock(&video_stage_mutex); icc.num_retries = VIDEO_MAX_RETRIES; vp_os_cond_wait(&video_stage_condition); vp_os_mutex_unlock(&video_stage_mutex); } if( VP_SUCCEEDED(vp_api_run(&pipeline, &out)) ) { if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) { loop = VP_SUCCESS; } } else loop = -1; // Finish this thread } vp_api_close(&pipeline, &pipeline_handle); } } PRINT(" video stage thread ended\n\n"); return (THREAD_RET)0; }
DEFINE_THREAD_ROUTINE(video_stage, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES]; vp_api_picture_t picture; video_com_config_t icc; vlib_stage_decoding_config_t vec; vp_stages_yuv2rgb_config_t yuv2rgbconf; #ifdef RECORD_VIDEO video_stage_recorder_config_t vrc; #endif /// Picture configuration picture.format = PIX_FMT_YUV420P; picture.width = QVGA_WIDTH; picture.height = QVGA_HEIGHT; picture.framerate = 30; picture.y_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT ); picture.cr_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.cb_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.y_line_size = QVGA_WIDTH; picture.cb_line_size = QVGA_WIDTH / 2; picture.cr_line_size = QVGA_WIDTH / 2; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf )); icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); vec.width = QVGA_WIDTH; vec.height = QVGA_HEIGHT; vec.picture = &picture; vec.block_mode_enable = TRUE; vec.luma_only = FALSE; yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24; #ifdef RECORD_VIDEO vrc.fp = NULL; #endif pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; #ifdef RECORD_VIDEO stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vrc; stages[pipeline.nb_stages].funcs = video_recorder_funcs; pipeline.nb_stages++; #endif // RECORD_VIDEO stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB; stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf; stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; stages[pipeline.nb_stages].cfg = NULL; stages[pipeline.nb_stages].funcs = vp_stages_output_gtk_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; /* Processing of a pipeline */ if( !ardrone_tool_exit() ) { PRINT("\n Video stage thread initialisation\n\n"); res = vp_api_open(&pipeline, &pipeline_handle); if( SUCCEED(res) ) { int loop = SUCCESS; out.status = VP_API_STATUS_PROCESSING; while( !ardrone_tool_exit() && (loop == SUCCESS) ) { if( SUCCEED(vp_api_run(&pipeline, &out)) ) { if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) { loop = SUCCESS; } } else loop = -1; // Finish this thread } vp_api_close(&pipeline, &pipeline_handle); } } PRINT(" Video stage thread ended\n\n"); return (THREAD_RET)0; }
PROTO_THREAD_ROUTINE(app,argv) { vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES_MAX]; vp_os_memset( &icc, 0, sizeof(icc) ); vp_os_memset( &bpc, 0, sizeof(bpc) ); vp_os_memset( &osc, 0, sizeof(osc) ); icc.com = COM_VIDEO(); icc.buffer_size = ACQ_WIDTH*ACQ_HEIGHT*3/2; icc.socket.protocol = VP_COM_TCP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, 5555, SERVER_HOST); /// Picture configuration picture.format = PIX_FMT_YUV420P; picture.width = ACQ_WIDTH; picture.height = ACQ_HEIGHT; picture.framerate = 30; picture.y_buf = vp_os_malloc( ACQ_WIDTH*ACQ_HEIGHT ); picture.cr_buf = vp_os_malloc( ACQ_WIDTH*ACQ_HEIGHT/4 ); picture.cb_buf = vp_os_malloc( ACQ_WIDTH*ACQ_HEIGHT/4 ); picture.y_line_size = ACQ_WIDTH; picture.cb_line_size = ACQ_WIDTH / 2; picture.cr_line_size = ACQ_WIDTH / 2; picture.y_pad = 0; picture.c_pad = 0; bpc.picture = &picture; bpc.block_mode_enable = FALSE; bpc.y_buffer_size = ACQ_WIDTH * ACQ_HEIGHT; bpc.y_blockline_size = ACQ_WIDTH * 16; // each blockline have 16 lines bpc.y_current_size = 0; bpc.num_frames = 0; bpc.y_buf_ptr = NULL; bpc.luma_only = FALSE; bpc.cr_buf_ptr = NULL; bpc.cb_buf_ptr = NULL; osc.width = ACQ_WIDTH; osc.height = ACQ_HEIGHT; osc.bpp = 16; osc.window_width = ACQ_WIDTH; osc.window_height = ACQ_HEIGHT; osc.pic_width = ACQ_WIDTH; osc.pic_height = ACQ_HEIGHT; osc.y_size = ACQ_WIDTH*ACQ_HEIGHT; osc.c_size = (ACQ_WIDTH*ACQ_HEIGHT) >> 2; pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages++].funcs = vp_stages_input_com_funcs; stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&bpc; stages[pipeline.nb_stages++].funcs = vp_stages_buffer_to_picture_funcs; stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; stages[pipeline.nb_stages].cfg = (void *)&osc; stages[pipeline.nb_stages++].funcs = vp_stages_output_sdl_funcs; pipeline.stages = &stages[0]; vp_api_open(&pipeline, &pipeline_handle); out.status = VP_API_STATUS_PROCESSING; while(SUCCEED(vp_api_run(&pipeline, &out)) && (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING)) { } vp_api_close(&pipeline, &pipeline_handle); return EXIT_SUCCESS; }
DEFINE_THREAD_ROUTINE(video_stage, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES]; vp_api_picture_t picture; video_com_config_t icc; vlib_stage_decoding_config_t vec; vp_stages_yuv2rgb_config_t yuv2rgbconf; #ifdef RECORD_VIDEO video_stage_recorder_config_t vrc; #endif /// Picture configuration picture.format = PIX_FMT_YUV420P; picture.width = QVGA_WIDTH; picture.height = QVGA_HEIGHT; picture.framerate = 30; picture.y_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT ); picture.cr_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.cb_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.y_line_size = QVGA_WIDTH; picture.cb_line_size = QVGA_WIDTH / 2; picture.cr_line_size = QVGA_WIDTH / 2; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf )); icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); vec.width = QVGA_WIDTH; vec.height = QVGA_HEIGHT; vec.picture = &picture; vec.block_mode_enable = TRUE; vec.luma_only = FALSE; yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24; #ifdef RECORD_VIDEO vrc.fp = NULL; #endif pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; #ifdef RECORD_VIDEO stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vrc; stages[pipeline.nb_stages].funcs = video_recorder_funcs; pipeline.nb_stages++; #endif // RECORD_VIDEO stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB; stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf; stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; stages[pipeline.nb_stages].cfg = NULL; stages[pipeline.nb_stages].funcs = vp_stages_output_gtk_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; /* Processing of a pipeline */ if( !ardrone_tool_exit() ) { PRINT("Video stage thread initialization\n"); res = vp_api_open(&pipeline, &pipeline_handle); // PRINT("VP_API OPENED\n"); if( SUCCEED(res) ) { int loop = SUCCESS; out.status = VP_API_STATUS_PROCESSING; cvStartWindowThread(); #define frontWindow "DroneView" cvNamedWindow(frontWindow, CV_WINDOW_AUTOSIZE); frontImgStream = cvCreateImage(cvSize(picture.width, picture.height), IPL_DEPTH_8U, 3); #define bottomWindow "BomberView" if(extractBottomImage) cvNamedWindow(bottomWindow, CV_WINDOW_AUTOSIZE); bottomImgStream = cvCreateImage(cvSize(bottomWidth, bottomHeight), IPL_DEPTH_8U, 3); IplImage *frame; CvCapture *capture; if(USEWEBCAM) { capture = cvCaptureFromCAM(WEBCAMINDEX); if(!capture) { printf("ERROR: Cannot Initialize Webcam.\n"); loop = !SUCCESS; } } while( !ardrone_tool_exit() && (loop == SUCCESS) ) { if(!USEWEBCAM) { if( SUCCEED(vp_api_run(&pipeline, &out)) ) { if (vec.controller.num_frames==0) continue; /*int i; for(i = 0; i < (picture.width)*(picture.height); i++) { frontImgStream->imageData[i*3] = picture.y_buf[i]; frontImgStream->imageData[i*3+1] = picture.y_buf[i]; frontImgStream->imageData[i*3+2] = picture.y_buf[i]; } */ cvCvtColor(rgbHeader, frontImgStream, CV_RGB2BGR); //[for colour] if(extractBottomImage) { int j = 0, i; for(i = 0; j < bottomHeight*bottomWidth; i = i%picture.width >= bottomWidth-1 ? i - (i%picture.width) + picture.width : i+1) { bottomImgStream->imageData[j*3] = frontImgStream->imageData[i*3]; bottomImgStream->imageData[j*3+1] = frontImgStream->imageData[i*3+1]; bottomImgStream->imageData[j*3+2] = frontImgStream->imageData[i*3+2]; frontImgStream->imageData[i*3] = 0; frontImgStream->imageData[i*3+1] = 0; frontImgStream->imageData[i*3+2] = 0; j++; } } //cvLine(frontImgStream, cvPoint(picture.width/2, 0), cvPoint(picture.width/2, picture.height), CV_RGB(0,255,0), 1, CV_AA, 0 ); cvShowImage(frontWindow, frontImgStream); if(extractBottomImage) cvShowImage(bottomWindow, bottomImgStream); if(CAPTUREIMAGESTREAM) { char filename[256]; struct timeval t; gettimeofday(&t, NULL); sprintf(filename, "%d.%06d.jpg", (int)t.tv_sec, (int)t.tv_usec); if(frontImgStream != NULL && cvSaveImage(filename, cvCloneImage(frontImgStream), 0)) printf("Image dumped to %s\n", filename); else printf("Error dumping image...\n"); if(extractBottomImage) { sprintf(filename, "%d.%06dbottom.jpg", (int)t.tv_sec, (int)t.tv_usec); if(bottomImgStream != NULL && cvSaveImage(filename, cvCloneImage(bottomImgStream), 0)) printf("Bottom Image dumped to %s\n", filename); else printf("Error dumping bottom image...\n"); } } } else loop = -1; } else //use webcam { frame = cvQueryFrame(capture); if(!frame) break; cvResize(frame, frontImgStream, CV_INTER_LINEAR); cvShowImage(frontWindow, frontImgStream); cvWaitKey(1); } } cvDestroyWindow(frontWindow); if(extractBottomImage) cvDestroyWindow(bottomWindow); //cvReleaseImage(&imgBottom); //cvDestroyWindow(bottomWindow); cvReleaseImage(&frontImgStream); vp_api_close(&pipeline, &pipeline_handle); } } PRINT(" Video stage thread ended\n\n"); return (THREAD_RET)0; }
DEFINE_THREAD_ROUTINE(video_recorder, data) { if (1 >= ARDRONE_VERSION ()) // Run only for ARDrone 2 and upper { return (THREAD_RET)0; } C_RESULT res = C_OK; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[3]; PIPELINE_HANDLE video_recorder_pipeline_handle; video_recorder_thread_param_t *video_recorder_thread_param = (video_recorder_thread_param_t *)data; video_stage_tcp_config_t tcpConf; if(video_recorder_thread_param != NULL) { CHANGE_THREAD_PRIO(video_recorder, video_recorder_thread_param->priority); video_stage_encoded_recorder_config.finish_callback = video_recorder_thread_param->finish_callback; } vp_os_memset (&record_icc, 0x0, sizeof (record_icc)); record_icc.com = COM_VIDEO (); record_icc.buffer_size = (8*1024); record_icc.protocol = VP_COM_TCP; record_icc.forceNonBlocking = &tcpConf.tcpStageHasMoreData; COM_CONFIG_SOCKET_VIDEO (&record_icc.socket, VP_COM_CLIENT, VIDEO_RECORDER_PORT, wifi_ardrone_ip); record_icc.timeoutFunc = &video_stage_encoded_recorder_com_timeout; record_icc.timeoutFuncAfterSec = 10; vp_os_memset (&tcpConf, 0, sizeof (tcpConf)); tcpConf.maxPFramesPerIFrame = 30; tcpConf.frameMeanSize = 160*1024; tcpConf.latencyDrop = 0; pipeline.nb_stages = 0; pipeline.stages = &stages[0]; // Com stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&record_icc; stages[pipeline.nb_stages++].funcs = video_com_funcs; // TCP stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void *) &tcpConf; stages[pipeline.nb_stages++].funcs = video_stage_tcp_funcs; // Record stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void *) &video_stage_encoded_recorder_config; stages[pipeline.nb_stages++].funcs = video_encoded_recorder_funcs; while (FALSE == isInit) { printf ("Waiting for init\n"); } if (! ardrone_tool_exit ()) { PRINT ("Video recorder thread initialisation\n"); res = vp_api_open (&pipeline, &video_recorder_pipeline_handle); if (SUCCEED (res)) { int loop = SUCCESS; out.status = VP_API_STATUS_PROCESSING; while (! ardrone_tool_exit () && (SUCCESS == loop)) { if (video_recorder_in_pause) { vp_os_mutex_lock (&video_recorder_mutex); record_icc.num_retries = VIDEO_MAX_RETRIES; vp_os_cond_wait (&video_recorder_condition); vp_os_mutex_unlock (&video_recorder_mutex); } if (SUCCEED (vp_api_run (&pipeline, &out))) { if ((VP_API_STATUS_PROCESSING == out.status) || (VP_API_STATUS_STILL_RUNNING == out.status)) { loop = SUCCESS; } else loop = -1; } else loop = -1; } vp_api_close (&pipeline, &video_recorder_pipeline_handle); } } PRINT ("Video recorder thread ended\n"); return (THREAD_RET)res; }
/* The video processing thread. This function can be kept as it is by most users. It automatically receives the video stream in a loop, decode it, and then call the 'output_rendering_device_stage_transform' function for each decoded frame. */ DEFINE_THREAD_ROUTINE(video_stage, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES]; vp_api_picture_t picture; video_com_config_t icc; vlib_stage_decoding_config_t vec; vp_stages_yuv2rgb_config_t yuv2rgbconf; /* Picture configuration */ picture.format = PIX_FMT_YUV420P; picture.width = DRONE_VIDEO_MAX_WIDTH; picture.height = DRONE_VIDEO_MAX_HEIGHT; picture.framerate = 15; //picture.framerate = 3; picture.y_buf = vp_os_malloc( DRONE_VIDEO_MAX_WIDTH * DRONE_VIDEO_MAX_HEIGHT ); picture.cr_buf = vp_os_malloc( DRONE_VIDEO_MAX_WIDTH * DRONE_VIDEO_MAX_HEIGHT / 4 ); picture.cb_buf = vp_os_malloc( DRONE_VIDEO_MAX_WIDTH * DRONE_VIDEO_MAX_HEIGHT / 4 ); picture.y_line_size = DRONE_VIDEO_MAX_WIDTH; picture.cb_line_size = DRONE_VIDEO_MAX_WIDTH / 2; picture.cr_line_size = DRONE_VIDEO_MAX_WIDTH / 2; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf )); /* Video socket configuration */ icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); /* Video decoder configuration */ /* Size of the buffers used for decoding This must be set to the maximum possible video resolution used by the drone The actual video resolution will be stored by the decoder in vec.controller (see vlib_stage_decode.h) */ vec.width = DRONE_VIDEO_MAX_WIDTH; vec.height = DRONE_VIDEO_MAX_HEIGHT; vec.picture = &picture; vec.block_mode_enable = TRUE; vec.luma_only = FALSE; yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24; /* Video pipeline building */ pipeline.nb_stages = 0; /* Video stream reception */ stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; /* Video stream decoding */ stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; /* YUV to RGB conversion YUV format is used by the video stream protocol Remove this stage if your rendering device can handle YUV data directly */ stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB; stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf; stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs; pipeline.nb_stages++; /* User code */ stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; /* Set to VP_API_OUTPUT_SDL even if SDL is not used */ stages[pipeline.nb_stages].cfg = (void*)&vec; /* give the decoder information to the renderer */ stages[pipeline.nb_stages].funcs = vp_stages_output_rendering_device_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; /* Processing of a pipeline */ if( !ardrone_tool_exit() ) { PRINT("\n Video stage thread initialisation\n\n"); // switch to vertical camera //ardrone_at_zap(ZAP_CHANNEL_VERT); res = vp_api_open(&pipeline, &pipeline_handle); if( VP_SUCCEEDED(res) ) { int loop = VP_SUCCESS; out.status = VP_API_STATUS_PROCESSING; while( !ardrone_tool_exit() && (loop == VP_SUCCESS) ) { if( VP_SUCCEEDED(vp_api_run(&pipeline, &out)) ) { if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) { loop = VP_SUCCESS; } } else loop = -1; // Finish this thread } vp_api_close(&pipeline, &pipeline_handle); } } PRINT(" Video stage thread ended\n\n"); return (THREAD_RET)0; }
DEFINE_THREAD_ROUTINE(video_stage, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t * stages; video_stage_merge_slices_config_t merge_slices_cfg; uint8_t i; specific_parameters_t * params = (specific_parameters_t *)(data); if (1 == params->needSetPriority) { CHANGE_THREAD_PRIO (video_stage, params->priority); } vp_os_memset(&icc_tcp, 0, sizeof ( icc_tcp)); vp_os_memset(&icc_udp, 0, sizeof ( icc_udp)); // Video Communication config icc_tcp.com = COM_VIDEO(); icc_tcp.buffer_size = (1024*1024); icc_tcp.protocol = VP_COM_TCP; COM_CONFIG_SOCKET_VIDEO(&icc_tcp.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); // Video Communication config icc_udp.com = COM_VIDEO(); icc_udp.buffer_size = (1024*1024); icc_udp.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc_udp.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); icc.nb_sockets = 2; icc.configs = icc_tab; icc.forceNonBlocking = &(tcpConf.tcpStageHasMoreData); icc_tab[1] = &icc_tcp; icc_tab[0] = &icc_udp; icc.buffer_size = (1024*1024); vp_os_memset(&vec, 0, sizeof ( vec)); stages = (vp_api_io_stage_t*) (vp_os_calloc( NB_STAGES + params->pre_processing_stages_list->length + params->post_processing_stages_list->length, sizeof (vp_api_io_stage_t) )); vec.src_picture = params->in_pic; vec.dst_picture = params->out_pic; vp_os_memset(&tcpConf, 0, sizeof ( tcpConf)); tcpConf.maxPFramesPerIFrame = 30; tcpConf.frameMeanSize = 160*1024; tcpConf.tcpStageHasMoreData = FALSE; tcpConf.latencyDrop = 1; pipeline.name = "video_stage_pipeline"; pipeline.nb_stages = 0; pipeline.stages = &stages[0]; //ENCODED FRAME PROCESSING STAGES stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *) &icc; stages[pipeline.nb_stages++].funcs = video_com_multisocket_funcs; printf("thread_video_stage: adding multisocket_funcs as %d th stage (input type) to video_pipeline of AR.Drone with version %d.%d.%d\n", pipeline.nb_stages, ardroneVersion.majorVersion, ardroneVersion.minorVersion, ardroneVersion.revision ); stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void *) &tcpConf; stages[pipeline.nb_stages++].funcs = video_stage_tcp_funcs; printf("thread_video_stage: adding tcp_funcs to video_pipeline as %d nd stage (filter type) of AR.Drone\n", pipeline.nb_stages ); // Record Encoded video if(1 == ARDRONE_VERSION()) { ardrone_academy_stage_recorder_config.dest.pipeline = video_pipeline_handle; ardrone_academy_stage_recorder_config.dest.stage = pipeline.nb_stages; stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&ardrone_academy_stage_recorder_config; stages[pipeline.nb_stages++].funcs = ardrone_academy_stage_recorder_funcs; printf("thread_video_stage: adding academy_recorder_funcs to video_pipeline as %d rd stage (filter type) of AR.Drone\n", pipeline.nb_stages ); } else { // Nothing to do for AR.Drone 2 as we have a separated thread for recording } //PRE-DECODING STAGES ==> recording, ... for(i=0;i<params->pre_processing_stages_list->length;i++){ stages[pipeline.nb_stages].type = params->pre_processing_stages_list->stages_list[i].type; stages[pipeline.nb_stages].cfg = params->pre_processing_stages_list->stages_list[i].cfg; stages[pipeline.nb_stages++].funcs = params->pre_processing_stages_list->stages_list[i].funcs; } printf("thread_video_stage: adding pre_processing_stages_list to video_pipeline (now %d stages) of AR.Drone\n", pipeline.nb_stages ); stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void *)&merge_slices_cfg; stages[pipeline.nb_stages++].funcs = video_stage_merge_slices_funcs; printf("thread_video_stage: adding merge_slices_funcs to video_pipeline (now %d stages) of AR.Drone\n", pipeline.nb_stages ); //DECODING STAGES stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*) &vec; stages[pipeline.nb_stages++].funcs = video_decoding_funcs; printf("thread_video_stage: adding video_decoding_funcs to video_pipeline (now %d stages) of AR.Drone\n", pipeline.nb_stages ); //POST-DECODING STAGES ==> transformation, display, ... for(i=0;i<params->post_processing_stages_list->length;i++){ stages[pipeline.nb_stages].type = params->post_processing_stages_list->stages_list[i].type; stages[pipeline.nb_stages].cfg = params->post_processing_stages_list->stages_list[i].cfg; stages[pipeline.nb_stages++].funcs = params->post_processing_stages_list->stages_list[i].funcs; } printf("thread_video_stage: adding post_processing_stages_list to video_pipeline (now %d stages) of AR.Drone\n", pipeline.nb_stages ); if (!ardrone_tool_exit()) { PRINT("\nvideo stage thread initialisation\n\n"); res = vp_api_open(&pipeline, &video_pipeline_handle); if (SUCCEED(res)) { int loop = SUCCESS; out.status = VP_API_STATUS_PROCESSING; while (!ardrone_tool_exit() && (loop == SUCCESS)) { if (video_stage_in_pause) { vp_os_mutex_lock(&video_stage_mutex); icc.num_retries = VIDEO_MAX_RETRIES; vp_os_cond_wait(&video_stage_condition); vp_os_mutex_unlock(&video_stage_mutex); } if (SUCCEED(vp_api_run(&pipeline, &out))) { if ((out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING)) { loop = SUCCESS; } } else loop = -1; // Finish this thread } vp_os_free(params->pre_processing_stages_list->stages_list); vp_os_free(params->post_processing_stages_list->stages_list); vp_os_free(params->pre_processing_stages_list); vp_os_free(params->post_processing_stages_list); vp_os_free(params->in_pic); vp_os_free(params->out_pic); vp_os_free(params); vp_api_close(&pipeline, &video_pipeline_handle); } } PRINT("\nvideo stage thread ended\n\n"); return (THREAD_RET) 0; }
DEFINE_THREAD_ROUTINE(video_stage, data) { C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[NB_STAGES]; vp_api_picture_t picture; video_com_config_t icc; vlib_stage_decoding_config_t vec; vp_stages_yuv2rgb_config_t yuv2rgbconf; #ifdef RECORD_VIDEO video_stage_recorder_config_t vrc; #endif /// Picture configuration picture.format = PIX_FMT_YUV420P; picture.width = QVGA_WIDTH; picture.height = QVGA_HEIGHT; picture.framerate = 30; picture.y_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT ); picture.cr_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.cb_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.y_line_size = QVGA_WIDTH; picture.cb_line_size = QVGA_WIDTH / 2; picture.cr_line_size = QVGA_WIDTH / 2; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf )); icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); vec.width = QVGA_WIDTH; vec.height = QVGA_HEIGHT; vec.picture = &picture; vec.block_mode_enable = TRUE; vec.luma_only = FALSE; yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24; #ifdef RECORD_VIDEO vrc.fp = NULL; #endif pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; #ifdef RECORD_VIDEO PRINT("\n Record Video\n\n"); stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vrc; stages[pipeline.nb_stages].funcs = video_recorder_funcs; pipeline.nb_stages++; #endif // RECORD_VIDEO stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB; stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf; stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; stages[pipeline.nb_stages].cfg = NULL; stages[pipeline.nb_stages].funcs = vp_stages_output_gtk_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; /* Processing of a pipeline */ if( !ardrone_tool_exit() ) { PRINT("\n Video stage thread initialisation\n\n"); if ((sockfd = socket(AF_UNIX,SOCK_STREAM,0)) < 0) { printf("error creating socket"); } bzero((char *) &serv_addr, sizeof(serv_addr)); serv_addr.sun_family = AF_UNIX; unlink("/tmp/video"); strcpy(serv_addr.sun_path, "/tmp/video"); servlen=strlen(serv_addr.sun_path) + sizeof(serv_addr.sun_family); if(bind(sockfd,(struct sockaddr *)&serv_addr,servlen)<0) { printf("error binding socket"); } listen(sockfd,5); clilen = sizeof(serv_addr); newsockfd = accept( sockfd,(struct sockaddr *)&serv_addr,&clilen); printf("1 sockfd, %d", sockfd); PRINT("\n Video socket initialisation\n\n"); res = vp_api_open(&pipeline, &pipeline_handle); if( SUCCEED(res) ) { int loop = SUCCESS; out.status = VP_API_STATUS_PROCESSING; while( !ardrone_tool_exit() && (loop == SUCCESS) ) { if( SUCCEED(vp_api_run(&pipeline, &out)) ) { if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) { loop = SUCCESS; } } else loop = -1; // Finish this thread //printf("%s\n", pixbuf_data); //printf("!%d sizeof *\n", sizeof(pixbuf_data)); //printf("!%d --- %d\n", n, errno); } vp_api_close(&pipeline, &pipeline_handle); } } PRINT(" Video stage thread ended\n\n"); return (THREAD_RET)0; }
DEFINE_THREAD_ROUTINE(drone_video_stage_thread, data) { PIPELINE_HANDLE pipeline_handle; printf("Vision thread started\n"); C_RESULT res; vp_api_io_pipeline_t pipeline; vp_api_io_data_t out; vp_api_io_stage_t stages[DRONE_NB_STAGES]; vp_api_picture_t picture; video_com_config_t icc; vlib_stage_decoding_config_t vec; vp_stages_yuv2rgb_config_t yuv2rgbconf; /// Picture configuration picture.format = PIX_FMT_YUV420P; picture.width = QVGA_WIDTH; picture.height = QVGA_HEIGHT; picture.framerate = 30; picture.y_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT ); picture.cr_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.cb_buf = vp_os_malloc( QVGA_WIDTH * QVGA_HEIGHT / 4 ); picture.y_line_size = QVGA_WIDTH; picture.cb_line_size = QVGA_WIDTH / 2; picture.cr_line_size = QVGA_WIDTH / 2; vp_os_memset(&icc, 0, sizeof( icc )); vp_os_memset(&vec, 0, sizeof( vec )); vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf )); icc.com = COM_VIDEO(); icc.buffer_size = 100000; icc.protocol = VP_COM_UDP; COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip); vec.width = QVGA_WIDTH; vec.height = QVGA_HEIGHT; vec.picture = &picture; vec.block_mode_enable = TRUE; vec.luma_only = FALSE; yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24; pipeline.nb_stages = 0; stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET; stages[pipeline.nb_stages].cfg = (void *)&icc; stages[pipeline.nb_stages].funcs = video_com_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER; stages[pipeline.nb_stages].cfg = (void*)&vec; stages[pipeline.nb_stages].funcs = vlib_decoding_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB; stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf; stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs; pipeline.nb_stages++; stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; stages[pipeline.nb_stages].cfg = NULL; stages[pipeline.nb_stages].funcs = vp_video_output_funcs; pipeline.nb_stages++; pipeline.stages = &stages[0]; pthread_mutex_lock(&drone_sharedDataMutex); int doRun = drone_doRun; pthread_mutex_unlock(&drone_sharedDataMutex); /* Processing of a pipeline */ if (doRun) { res = 0; res = vp_api_open(&pipeline, &pipeline_handle); if (SUCCEED(res)) { int loop = SUCCESS; out.status = VP_API_STATUS_PROCESSING; while (doRun && (loop == SUCCESS)) { pthread_mutex_lock(&drone_sharedDataMutex); doRun = drone_doRun; pthread_mutex_unlock(&drone_sharedDataMutex); if( SUCCEED(vp_api_run(&pipeline, &out)) ) { if ((out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING)) { loop = SUCCESS; } } } fprintf(stderr, "BUG and WORKAROUND: vp_api_close was never called because it is buggy\n"); // vp_api_close(&pipeline, &pipeline_handle); // <- TODO: do not call this - fix this function in SDK } } vp_os_free(picture.y_buf); vp_os_free(picture.cr_buf); vp_os_free(picture.cb_buf); printf("Vision thread terminated\n"); return (THREAD_RET) 0; }