C_RESULT ardrone_tool_init_custom (void)
{
// Reset global data
bzero(&g_navdata, sizeof(g_navdata));
g_autopilot = FALSE;
g_pass_button = 0;
// Register a new game controller
ardrone_tool_input_add( &gamepad );
ardrone_application_default_config.navdata_demo = TRUE;
ardrone_application_default_config.navdata_options =
(NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) |
NAVDATA_OPTION_FULL_MASK |
NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) |
NAVDATA_OPTION_MASK(NAVDATA_GAMES_TAG) |
NAVDATA_OPTION_MASK(NAVDATA_MAGNETO_TAG) |
NAVDATA_OPTION_MASK(NAVDATA_HDVIDEO_STREAM_TAG) |
NAVDATA_OPTION_MASK(NAVDATA_WIFI_TAG));
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));
if (IS_ARDRONE2)
{
ardrone_application_default_config.video_codec = H264_360P_CODEC;
in_picture->width = 640;
in_picture->height = 360;
}
else
{
ardrone_application_default_config.video_codec = P264_CODEC;
in_picture->width = 320;
in_picture->height = 240;
}
ardrone_application_default_config.video_channel = ZAP_CHANNEL_HORI;
ardrone_application_default_config.bitrate_ctrl_mode = 1;
/**
* Allocate useful structures :
* - index counter
* - thread param structure and its substructures
*/
uint8_t stages_index = 0;
specific_parameters_t *params = (specific_parameters_t *)vp_os_calloc (1, sizeof (specific_parameters_t));
specific_stages_t *example_pre_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
specific_stages_t *example_post_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
out_picture->framerate = 20; // Drone 1 only, must be equal to drone target FPS
out_picture->format = PIX_FMT_RGB24;
out_picture->width = in_picture->width;
out_picture->height = in_picture->height;
// One buffer, three bytes per pixel
out_picture->y_buf = vp_os_malloc ( out_picture->width * out_picture->height * 3 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = out_picture->width * 3;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
example_pre_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (1, sizeof (vp_api_io_stage_t));
example_post_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (1, sizeof (vp_api_io_stage_t));
/**
* Fill the POST stage list
* - name and type are debug infos only
* - cfg is the pointer passed as "cfg" in all the stages calls
* - funcs is the pointer to the stage functions
*/
stages_index = 0;
vp_os_memset (&dispCfg, 0, sizeof (video_cfg_t));
dispCfg.width = in_picture->width;
dispCfg.height = in_picture->height;
example_post_stages->stages_list[stages_index].name = "Decoded display"; // Debug info
example_post_stages->stages_list[stages_index].type = VP_API_OUTPUT_SDL; // Debug info
example_post_stages->stages_list[stages_index].cfg = &dispCfg;
example_post_stages->stages_list[stages_index++].funcs = video_funcs;
example_post_stages->length = stages_index;
/**
* Fill thread params for the ardrone_tool video thread
* - in_pic / out_pic are reference to our in_picture / out_picture
* - pre/post stages lists are references to our stages lists
* - needSetPriority and priority are used to control the video thread priority
* -> if needSetPriority is set to 1, the thread will try to set its priority to "priority"
* -> if needSetPriority is set to 0, the thread will keep its default priority (best on PC)
*/
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = example_pre_stages;
params->post_processing_stages_list = example_post_stages;
params->needSetPriority = 0;
params->priority = 0;
/**
* Start the video thread (and the video recorder thread for AR.Drone 2)
*/
START_THREAD(video_stage, params);
video_stage_init();
video_stage_resume_thread ();
return C_OK;
}
C_RESULT ardrone_tool_init_custom(void)
{
/**
* Set application default configuration
*
* In this example, we use the AR.FreeFlight configuration :
* - Demo navdata rate (15Hz)
* - Useful additionnal navdata packets enabled (detection, games, video record, wifi quality estimation ...)
* - Adaptive video enabled (bitrate_ctrl_mode) -> video bitrate will change according to the available bandwidth
*/
ardrone_application_default_config.navdata_demo = TRUE;
//use this -> NAVDATA_OPTION_FULL_MASK
//or coment the line below if detection doesn't work
ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK;//(NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) | NAVDATA_OPTION_MASK(NAVDATA_GAMES_TAG) | NAVDATA_OPTION_MASK(NAVDATA_MAGNETO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_HDVIDEO_STREAM_TAG) | NAVDATA_OPTION_MASK(NAVDATA_WIFI_TAG));
if (IS_ARDRONE2){
ardrone_application_default_config.video_codec = drone2Codec;
} else {
ardrone_application_default_config.video_codec = drone1Codec;
}
ardrone_application_default_config.video_channel = videoChannel;
ardrone_application_default_config.bitrate_ctrl_mode = 1;
/**
* Define the number of video stages we'll add before/after decoding
*/
#define EXAMPLE_PRE_STAGES 1
#define EXAMPLE_POST_STAGES 1
/**
* Allocate useful structures :
* - index counter
* - thread param structure and its substructures
*/
uint8_t stages_index = 0;
specific_parameters_t *params = (specific_parameters_t *)vp_os_calloc (1, sizeof (specific_parameters_t));
specific_stages_t *example_pre_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
specific_stages_t *example_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));
/**
* Fill the vp_api_pictures used for video decodig
* --> out_picture->format is mandatory for AR.Drone 1 and 2. Other lines are only necessary for AR.Drone 1 video decoding
*/
in_picture->width = 640; // Drone 1 only : Must be greater than the drone 1 picture size (320)
in_picture->height = 360; // Drone 1 only : Must be greater that the drone 1 picture size (240)
out_picture->framerate = 20; // Drone 1 only, must be equal to drone target FPS
out_picture->format = PIX_FMT_RGB24; // MANDATORY ! Only RGB24, RGB565 are supported
out_picture->width = in_picture->width;
out_picture->height = in_picture->height;
// Alloc Y, CB, CR bufs according to target format
uint32_t bpp = 0;
switch (out_picture->format)
{
case PIX_FMT_RGB24:
// One buffer, three bytes per pixel
bpp = 3;
out_picture->y_buf = vp_os_malloc ( out_picture->width * out_picture->height * bpp );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = out_picture->width * bpp;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
break;
case PIX_FMT_RGB565:
// One buffer, two bytes per pixel
bpp = 2;
out_picture->y_buf = vp_os_malloc ( out_picture->width * out_picture->height * bpp );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = out_picture->width * bpp;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
break;
default:
fprintf (stderr, "Wrong video format, must be either PIX_FMT_RGB565 or PIX_FMT_RGB24\n");
exit (-1);
break;
}
/**
* Allocate the stage lists
*
* - "pre" stages are called before video decoding is done
* -> A pre stage get the encoded video frame (including PaVE header for AR.Drone 2 frames) as input
* -> A pre stage MUST NOT modify these data, and MUST pass it to the next stage
* - Typical "pre" stage : Encoded video recording for AR.Drone 1 (recording for AR.Drone 2 is handled differently)
*
* - "post" stages are called after video decoding
* -> The first post stage will get the decoded video frame as its input
* --> Video frame format depend on out_picture->format value (RGB24 / RGB565)
* -> A post stage CAN modify the data, as ardrone_tool won't process it afterwards
* -> All following post stages will use the output of the previous stage as their inputs
* - Typical "post" stage : Display the decoded frame
*/
example_pre_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (EXAMPLE_PRE_STAGES, sizeof (vp_api_io_stage_t));
example_post_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (EXAMPLE_POST_STAGES, sizeof (vp_api_io_stage_t));
/**
* Fill the PRE stage list
* - name and type are debug infos only
* - cfg is the pointer passed as "cfg" in all the stages calls
* - funcs is the pointer to the stage functions
*/
stages_index = 0;
vp_os_memset (&precfg, 0, sizeof (pre_stage_cfg_t));
strncpy (precfg.outputName, encodedFileName, 255);
example_pre_stages->stages_list[stages_index].name = "Encoded Dumper"; // Debug info
example_pre_stages->stages_list[stages_index].type = VP_API_FILTER_DECODER; // Debug info
example_pre_stages->stages_list[stages_index].cfg = &precfg;
example_pre_stages->stages_list[stages_index++].funcs = pre_stage_funcs;
example_pre_stages->length = stages_index;
/**
* Fill the POST stage list
* - name and type are debug infos only
* - cfg is the pointer passed as "cfg" in all the stages calls
* - funcs is the pointer to the stage functions
*/
stages_index = 0;
vp_os_memset (&dispCfg, 0, sizeof (display_stage_cfg_t));
dispCfg.bpp = bpp;
dispCfg.decoder_info = in_picture;
example_post_stages->stages_list[stages_index].name = "Decoded display"; // Debug info
example_post_stages->stages_list[stages_index].type = VP_API_OUTPUT_SDL; // Debug info
example_post_stages->stages_list[stages_index].cfg = &dispCfg;
example_post_stages->stages_list[stages_index++].funcs = display_stage_funcs;
example_post_stages->length = stages_index;
/**
* Fill thread params for the ardrone_tool video thread
* - in_pic / out_pic are reference to our in_picture / out_picture
* - pre/post stages lists are references to our stages lists
* - needSetPriority and priority are used to control the video thread priority
* -> if needSetPriority is set to 1, the thread will try to set its priority to "priority"
* -> if needSetPriority is set to 0, the thread will keep its default priority (best on PC)
*/
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = example_pre_stages;
params->post_processing_stages_list = example_post_stages;
params->needSetPriority = 0;
params->priority = 0;
//set the tag detection
ENEMY_COLORS_TYPE enemyColors = ARDRONE_DETECTION_COLOR_ORANGE_BLUE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (enemy_colors, &enemyColors, NULL);
CAD_TYPE detectType = CAD_TYPE_MULTIPLE_DETECTION_MODE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detect_type, &detectType, NULL);
uint32_t detectH = TAG_TYPE_MASK (TAG_TYPE_SHELL_TAG_V2);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_h, &detectH, NULL);
/**
* Start the video thread (and the video recorder thread for AR.Drone 2)
*/
START_THREAD(video_stage, params);
START_THREAD (video_recorder, NULL);
video_stage_init();
if (2 <= ARDRONE_VERSION ())
{
START_THREAD (video_recorder, NULL);
video_recorder_init ();
}
video_stage_resume_thread();
//King of the Hill threads
START_THREAD(wiimote_logic, NULL);
START_THREAD(drone_logic, NULL);
START_THREAD(score_logic, NULL);
return C_OK;
}
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;
}
C_RESULT ardrone_tool_init_custom(void)
{
should_exit = 0;
vp_os_mutex_init(&navdata_lock);
vp_os_mutex_init(&video_lock);
vp_os_mutex_init(&twist_lock);
//jdeDriver = new ARDroneDriver();
int _w, _h;
if (IS_ARDRONE2)
{
ardrone_application_default_config.video_codec = H264_360P_CODEC;
_w = D2_STREAM_WIDTH;
_h = D2_STREAM_HEIGHT;
}
else if (IS_ARDRONE1)
{
ardrone_application_default_config.video_codec = UVLC_CODEC;
_w = D1_STREAM_WIDTH;
_h = D1_STREAM_HEIGHT;
}
else
{
printf("Something must be really wrong with the SDK!");
}
realtime_navdata=false;
realtime_video=false;
looprate=LOOPRATE;
// SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
// THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
ardrone_application_default_config.bitrate_ctrl_mode = VBC_MODE_DISABLED;
if (IS_ARDRONE2)
{
ardrone_application_default_config.max_bitrate = 4000;
}
ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK;
ardrone_application_default_config.video_channel = ZAP_CHANNEL_HORI;
ardrone_application_default_config.control_level = (0 << CONTROL_LEVEL_COMBINED_YAW);
ardrone_application_default_config.flying_mode = FLYING_MODE_FREE_FLIGHT;
jdeDriver->configureDrone((char*)CONFIG_FILE_PATH);
cam_state=jdeDriver->getParameter("video_channel",0);
char buffer[MULTICONFIG_ID_SIZE+1];
sprintf(buffer,"-%s",usr_id);
printf("Deleting Profile %s\n",buffer);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_id, buffer, NULL);
sprintf(buffer,"-%s",app_id);
printf("Deleting Application %s\n",buffer);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_id, buffer, NULL);
// Now continue with the rest of the initialization
ardrone_tool_input_add(&teleop);
uint8_t post_stages_index = 0;
//Alloc structs
specific_parameters_t * params = (specific_parameters_t *)vp_os_calloc(1,sizeof(specific_parameters_t));
specific_stages_t * driver_pre_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
specific_stages_t * driver_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 = _w;
in_picture->height = _h;
out_picture->framerate = 20;
out_picture->format = PIX_FMT_RGB24;
out_picture->width = _w;
out_picture->height = _h;
out_picture->y_buf = (uint8_t*) vp_os_malloc( _w * _h * 3 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = _w * 3;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
//Alloc the lists
driver_pre_stages->stages_list = NULL;
driver_post_stages->stages_list = (vp_api_io_stage_t*)vp_os_calloc(NB_DRIVER_POST_STAGES,sizeof(vp_api_io_stage_t));
driver_post_stages->stages_list[post_stages_index].name = "ExtractData";
driver_post_stages->stages_list[post_stages_index].type = VP_API_OUTPUT_SDL;
driver_post_stages->stages_list[post_stages_index].cfg = NULL;
driver_post_stages->stages_list[post_stages_index++].funcs = vp_stages_export_funcs;
driver_pre_stages->length = 0;
driver_post_stages->length = post_stages_index;
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = driver_pre_stages;
params->post_processing_stages_list = driver_post_stages;
params->needSetPriority = 1;
params->priority = 31;
// Using the provided threaded pipeline implementation from SDK
START_THREAD(video_stage, params);
video_stage_init();
if (ARDRONE_VERSION() >= 2)
{
START_THREAD (video_recorder, NULL);
video_recorder_init ();
video_recorder_resume_thread();
}
// Threads do not start automatically
video_stage_resume_thread();
ardrone_tool_set_refresh_time(25);
//jdeDriver->configure_drone();
jdeDriver->initInterfaces();
START_THREAD(update_jde, jdeDriver);
return C_OK;
}
C_RESULT ardrone_tool_init_custom(void) {
ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK /*&
~(NAVDATA_OPTION_MASK(NAVDATA_TRACKERS_SEND_TAG)
| NAVDATA_OPTION_MASK(NAVDATA_VISION_OF_TAG)
| NAVDATA_OPTION_MASK(NAVDATA_VISION_PERF_TAG)
| NAVDATA_OPTION_MASK(NAVDATA_VISION_TAG))*/;
if (IS_ARDRONE2)
{
ardrone_application_default_config.video_codec = H264_360P_CODEC;
}
else
{
ardrone_application_default_config.video_codec = UVLC_CODEC;
}
ardrone_application_default_config.bitrate_ctrl_mode = 1;
/// Init specific code for application
ardrone_navdata_handler_table[NAVDATA_IHM_PROCESS_INDEX].data = &cfg;
// Add inputs
//ardrone_tool_input_add( &gamepad );
/*ardrone_tool_input_add( &radioGP );
ardrone_tool_input_add( &ps3pad );
ardrone_tool_input_add( &joystick );
ardrone_tool_input_add( &wiimote_device );*/
load_ini();
printf("Default control : %s (0x%08x, %s)\n", default_control->name, default_control->serial, default_control->filename);
ardrone_tool_input_add(&control_device);
cfg.default_control = default_control;
cfg.devices = devices;
#ifdef USE_ARDRONE_VICON
START_THREAD(vicon, &cfg);
#endif // USE_ARDRONE_VICON
START_THREAD(ihm, &cfg);
#ifdef RAW_CAPTURE
START_THREAD(raw_capture, &cfg);
#endif
START_THREAD(remote_console, &cfg);
/************************ VIDEO STAGE CONFIG ******************************/
#define NB_NAVIGATION_POST_STAGES 10
uint8_t post_stages_index = 0;
//Alloc structs
specific_parameters_t * params = (specific_parameters_t *)vp_os_calloc(1,sizeof(specific_parameters_t));
specific_stages_t * navigation_pre_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
specific_stages_t * navigation_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_RGB24;
out_picture->width = STREAM_WIDTH;
out_picture->height = STREAM_HEIGHT;
out_picture->y_buf = vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT * 3 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = STREAM_WIDTH * 3;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
//Alloc the lists
navigation_pre_stages->stages_list = NULL;
navigation_post_stages->stages_list = (vp_api_io_stage_t*)vp_os_calloc(NB_NAVIGATION_POST_STAGES,sizeof(vp_api_io_stage_t));
//Fill the POST-stages------------------------------------------------------
vp_os_memset(&vlat, 0, sizeof( vlat ));
vlat.state = 0;
vlat.last_decoded_frame_info = (void*)&vec;
navigation_post_stages->stages_list[post_stages_index].name = "(latency estimator)";
navigation_post_stages->stages_list[post_stages_index].type = VP_API_FILTER_DECODER;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)&vlat;
navigation_post_stages->stages_list[post_stages_index++].funcs = vp_stages_latency_estimation_funcs;
#ifdef RECORD_RAW_VIDEO
vp_os_memset(&vrc, 0, sizeof( vrc ));
#warning Recording RAW video option enabled in Navigation.
vrc.stage = 3;
#warning We have to get the stage number an other way
vp_os_memset(&vrc, 0, sizeof(vrc));
navigation_post_stages->stages_list[post_stages_index].name = "(raw video recorder)";
navigation_post_stages->stages_list[post_stages_index].type = VP_API_FILTER_DECODER;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)&vrc;
navigation_post_stages->stages_list[post_stages_index++].funcs = video_recorder_funcs;
#endif // RECORD_RAW_VIDEO
#if defined(FFMPEG_SUPPORT) && defined(RECORD_FFMPEG_VIDEO)
#warning Recording FFMPEG (reencoding)video option enabled in Navigation.
vp_os_memset(&ffmpeg_vrc, 0, sizeof(ffmpeg_vrc));
ffmpeg_vrc.numframes = &vec.controller.num_frames;
ffmpeg_vrc.stage = pipeline.nb_stages;
navigation_post_stages->stages_list[post_stages_index].name = "(ffmpeg recorder)";
navigation_post_stages->stages_list[post_stages_index].type = VP_API_FILTER_DECODER;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)&ffmpeg_vrc;
navigation_post_stages->stages_list[post_stages_index++].funcs = video_ffmpeg_recorder_funcs;
#endif
vp_os_memset(&draw_trackers_cfg, 0, sizeof( draw_trackers_funcs ));
draw_trackers_cfg.last_decoded_frame_info = (void*)&vec;
navigation_post_stages->stages_list[post_stages_index].type = VP_API_FILTER_DECODER;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)&draw_trackers_cfg;
navigation_post_stages->stages_list[post_stages_index++].funcs = draw_trackers_funcs;
vp_os_memset(>kconf, 0, sizeof( gtkconf ));
gtkconf.rowstride = out_picture->width * 3;
gtkconf.last_decoded_frame_info = (void*)&vec;
gtkconf.desired_display_width = 0; /* auto */
gtkconf.desired_display_height = 0; /* auto */
gtkconf.gdk_interpolation_mode = 0; /* fastest */
navigation_post_stages->stages_list[post_stages_index].name = "(Gtk display)";
navigation_post_stages->stages_list[post_stages_index].type = VP_API_OUTPUT_SDL;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)>kconf;
navigation_post_stages->stages_list[post_stages_index++].funcs = vp_stages_output_gtk_funcs;
//Define the list of stages size
navigation_pre_stages->length = 0;
navigation_post_stages->length = post_stages_index;
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = navigation_pre_stages;
params->post_processing_stages_list = navigation_post_stages;
params->needSetPriority = 0;
params->priority = 0;
START_THREAD(video_stage, params);
video_stage_init();
if (2 <= ARDRONE_VERSION ())
{
START_THREAD (video_recorder, NULL);
video_recorder_init ();
}
else
{
printf ("Don't start ... version is %d\n", ARDRONE_VERSION ());
}
/************************ END OF VIDEO STAGE CONFIG ***********************/
#ifdef PC_USE_POLARIS
START_THREAD(polaris, &cfg);
#endif // PC_USE_POLARIS
#ifdef USE_TABLE_PILOTAGE
START_THREAD(novadem, (void*) ("/dev/ttyUSB0"));
#endif // USE_TABLE_PILOTAGE
return C_OK;
}
/* The delegate object calls this method during initialization of an ARDrone application */
C_RESULT ardrone_tool_init_custom(void)
{
if (customPad)
ardrone_tool_input_add( &fpad );
//init_gui(0, NULL); /// Creating the GUI
START_THREAD(gui, NULL); // Starting the GUI thread
START_THREAD(imgproc,NULL);
START_THREAD(ttiofiles,NULL);
ardrone_application_default_config.video_channel = ZAP_CHANNEL_VERT;
// ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &ZAP_CHANNEL_VERT, NULL);
/************************ VIDEO STAGE CONFIG ******************************/
#define NB_NAVIGATION_POST_STAGES 10
uint8_t post_stages_index = 0;
//Alloc structs
specific_parameters_t * params = (specific_parameters_t *)vp_os_calloc(1,sizeof(specific_parameters_t));
specific_stages_t * navigation_pre_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
specific_stages_t * navigation_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_RGB24;
out_picture->width = STREAM_WIDTH;
out_picture->height = STREAM_HEIGHT;
out_picture->y_buf = vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT * 3 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = STREAM_WIDTH * 3;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
//Alloc the lists
navigation_pre_stages->stages_list = NULL;
navigation_post_stages->stages_list = (vp_api_io_stage_t*)vp_os_calloc(NB_NAVIGATION_POST_STAGES,sizeof(vp_api_io_stage_t));
//Fill the POST-stages------------------------------------------------------
vp_os_memset(&vlat, 0, sizeof( vlat ));
vlat.state = 0;
vlat.last_decoded_frame_info = (void*)&vec;
navigation_post_stages->stages_list[post_stages_index].name = "(latency estimator)";
navigation_post_stages->stages_list[post_stages_index].type = VP_API_FILTER_DECODER;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)&vlat;
navigation_post_stages->stages_list[post_stages_index++].funcs = vp_stages_latency_estimation_funcs;
vp_os_memset(&draw_trackers_cfg, 0, sizeof( draw_trackers_funcs ));
draw_trackers_cfg.last_decoded_frame_info = (void*)&vec;
navigation_post_stages->stages_list[post_stages_index].type = VP_API_FILTER_DECODER;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)&draw_trackers_cfg;
navigation_post_stages->stages_list[post_stages_index++].funcs = draw_trackers_funcs;
vp_os_memset(>kconf, 0, sizeof( gtkconf ));
gtkconf.rowstride = out_picture->width * 3;
gtkconf.last_decoded_frame_info = (void*)&vec;
gtkconf.desired_display_width = 0; /* auto */
gtkconf.desired_display_height = 0; /* auto */
gtkconf.gdk_interpolation_mode = 0; /* fastest */
navigation_post_stages->stages_list[post_stages_index].name = "(Gtk display)";
navigation_post_stages->stages_list[post_stages_index].type = VP_API_OUTPUT_SDL;
navigation_post_stages->stages_list[post_stages_index].cfg = (void*)>kconf;
navigation_post_stages->stages_list[post_stages_index++].funcs = vp_stages_output_gtk_funcs;
//Define the list of stages size
navigation_pre_stages->length = 0;
navigation_post_stages->length = post_stages_index;
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = navigation_pre_stages;
params->post_processing_stages_list = navigation_post_stages;
params->needSetPriority = 0;
params->priority = 0;
START_THREAD(video_stage, params);
video_stage_init();
video_stage_resume_thread ();
/************************ END OF VIDEO STAGE CONFIG ***********************/
return C_OK;
//START_THREAD( video_stage, NULL );
return C_OK;
}//*/
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;
}
