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_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;
}
DEFINE_THREAD_ROUTINE(app_main, data)
{
C_RESULT res = C_FAIL;
vp_com_wifi_config_t* config = NULL;
JNIEnv* env = NULL;
if (g_vm) {
(*g_vm)->AttachCurrentThread (g_vm, (JNIEnv **) &env, NULL);
}
bContinue = TRUE;
mobile_main_param_t *param = data;
video_recorder_thread_param_t video_recorder_param;
video_recorder_param.priority = VIDEO_RECORDER_THREAD_PRIORITY;
video_recorder_param.finish_callback = param->academy_download_callback_func;
vp_os_memset(&ardrone_info, 0x0, sizeof(ardrone_info_t));
while ((config = (vp_com_wifi_config_t *)wifi_config()) != NULL && strlen(config->itfName) == 0)
{
//Waiting for wifi initialization
vp_os_delay(250);
if (ardrone_tool_exit() == TRUE) {
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_DISCONNECTED);
}
return 0;
}
}
vp_os_memcpy(&ardrone_info.drone_address[0], config->server, strlen(config->server));
while (-1 == getDroneVersion (param->root_dir, &ardrone_info.drone_address[0], &ardroneVersion))
{
LOGD (TAG, "Getting AR.Drone version");
vp_os_delay (250);
}
sprintf(&ardrone_info.drone_version[0], "%u.%u.%u", ardroneVersion.majorVersion, ardroneVersion.minorVersion, ardroneVersion.revision);
LOGD (TAG, "ARDrone Version : %s\n", &ardrone_info.drone_version[0]);
LOGI(TAG, "Drone Family: %d", ARDRONE_VERSION());
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
LOGII("Setup com failed");
LOGW(TAG, "Wifi initialization failed. It means either:");
LOGW(TAG, "\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
LOGW(TAG, "\t* wifi device is not present (on your pc or on your card)\n");
LOGW(TAG, "\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
LOGW(TAG, "\t* ap is not up (reboot card or remove wifi usb dongle)\n");
LOGW(TAG, "\t* wifi device has no antenna\n");
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_ERR_NO_WIFI);
}
}
else
{
LOGII("ardrone_tool_setup_com [OK]");
#define NB_IPHONE_PRE_STAGES 0
#define NB_IPHONE_POST_STAGES 2
//Alloc structs
specific_parameters_t * params = (specific_parameters_t *)vp_os_calloc(1, sizeof(specific_parameters_t));
specific_stages_t * iphone_pre_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
specific_stages_t * iphone_post_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
vp_api_picture_t * in_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
vp_api_picture_t * out_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
in_picture->width = STREAM_WIDTH;
in_picture->height = STREAM_HEIGHT;
out_picture->framerate = 20;
out_picture->format = PIX_FMT_RGB565;
out_picture->width = STREAM_WIDTH;
out_picture->height = STREAM_HEIGHT;
out_picture->y_buf = vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT * 2 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = STREAM_WIDTH * 2;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
//Define the list of stages size
iphone_pre_stages->length = NB_IPHONE_PRE_STAGES;
iphone_post_stages->length = NB_IPHONE_POST_STAGES;
//Alloc the lists
iphone_pre_stages->stages_list = NULL;
iphone_post_stages->stages_list = (vp_api_io_stage_t*)vp_os_calloc(iphone_post_stages->length,sizeof(vp_api_io_stage_t));
//Fill the POST-stages------------------------------------------------------
int postStageNumber = 0;
vp_os_memset (&vlat, 0x0, sizeof (vlat));
vlat.state = 0;
vlat.last_decoded_frame_info= (void *)&vec;
iphone_post_stages->stages_list[postStageNumber].type = VP_API_FILTER_DECODER;
iphone_post_stages->stages_list[postStageNumber].cfg = (void *)&vlat;
iphone_post_stages->stages_list[postStageNumber++].funcs = vp_stages_latency_estimation_funcs;
vp_os_memset (&ovsc, 0x0, sizeof (ovsc));
ovsc.video_decoder = &vec;
iphone_post_stages->stages_list[postStageNumber].type = VP_API_OUTPUT_LCD;
iphone_post_stages->stages_list[postStageNumber].cfg = (void *)&ovsc;
iphone_post_stages->stages_list[postStageNumber++].funcs = opengl_video_stage_funcs;
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = iphone_pre_stages;
params->post_processing_stages_list = iphone_post_stages;
#if USE_THREAD_PRIORITIES
params->needSetPriority = 1;
params->priority = VIDEO_THREAD_PRIORITY;
#else
params->needSetPriority = 0;
params->priority = 0;
#endif
START_THREAD(video_stage, params);
if (IS_LEAST_ARDRONE2)
{
START_THREAD (video_recorder, (void *)&video_recorder_param);
LOGD(TAG, "Video recorder thread start [OK]");
}
res = ardrone_tool_init(&ardrone_info.drone_address[0], strlen(&ardrone_info.drone_address[0]), NULL, param->app_name, param->user_name, param->root_dir, param->flight_dir, param->flight_storing_size, param->academy_download_callback_func);
if(SUCCEED(res))
{
ardrone_tool_input_add(&virtual_gamepad);
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_CONNECTED_OK);
}
} else {
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_UNKNOWN_ERR);
}
bContinue = FALSE;
}
res = ardrone_tool_set_refresh_time(1000 / kAPS);
#if USE_THREAD_PRIORITIES
CHANGE_THREAD_PRIO (app_main, AT_THREAD_PRIORITY);
CHANGE_THREAD_PRIO (navdata_update, NAVDATA_THREAD_PRIORITY);
CHANGE_THREAD_PRIO (ardrone_control, NAVDATA_THREAD_PRIORITY);
#endif
while( SUCCEED(res) && bContinue == TRUE )
{
ardrone_tool_update();
}
JOIN_THREAD(video_stage);
if (IS_LEAST_ARDRONE2)
{
JOIN_THREAD (video_recorder);
}
/* Unregistering for the current device */
ardrone_tool_input_remove( &virtual_gamepad );
res = ardrone_tool_shutdown();
LOGD(TAG, "AR.Drone tool shutdown [OK]");
if (param != NULL && param->callback != NULL) {
param->callback(env, param->obj, ARDRONE_MESSAGE_DISCONNECTED);
}
}
vp_os_free (data);
data = NULL;
(*env)->DeleteGlobalRef(env, param->obj);
if (g_vm) {
(*g_vm)->DetachCurrentThread (g_vm);
}
LOGI(TAG, "app_main thread has been stopped.");
return (THREAD_RET) res;
}
