C_RESULT ardrone_tool_shutdown_custom() {
#ifdef USE_TABLE_PILOTAGE
JOIN_THREAD(novadem);
#endif // USE_TABLE_PILOTAGE
#ifdef PC_USE_POLARIS
JOIN_THREAD(polaris);
#endif // PC_USE_POLARIS
#ifdef USE_ARDRONE_VICON
JOIN_THREAD(vicon);
#endif // USE_ARDRONE_VICON
JOIN_THREAD(ihm);
video_stage_resume_thread(); //Resume thread to kill it !
JOIN_THREAD(video_stage);
if (2 <= ARDRONE_VERSION ())
{
video_recorder_resume_thread ();
JOIN_THREAD (video_recorder);
}
#ifdef RAW_CAPTURE
JOIN_THREAD(raw_capture);
#endif
JOIN_THREAD(remote_console);
/*ardrone_tool_input_remove( &gamepad );
ardrone_tool_input_remove( &radioGP );
ardrone_tool_input_remove( &ps3pad );
ardrone_tool_input_remove( &wiimote_device );*/
ardrone_tool_input_remove(&control_device);
return C_OK;
}
C_RESULT ardrone_tool_shutdown_custom ()
{
video_stage_resume_thread(); //Resume thread to kill it !
JOIN_THREAD(video_stage);
if (2 <= ARDRONE_VERSION ())
{
video_recorder_resume_thread ();
JOIN_THREAD (video_recorder);
}
return C_OK;
}
void parrot_ardrone_notify_resume()
{
video_stage_resume_thread();
if (IS_LEAST_ARDRONE2)
{
video_recorder_resume_thread();
}
ardrone_tool_resume();
LOGD(TAG, "AR.Drone Tool Resume [OK]");
}
C_RESULT ardrone_tool_shutdown_custom ()
{
video_stage_resume_thread(); //Resume thread to kill it !
JOIN_THREAD(video_stage);
JOIN_THREAD(kinect);
if (2 <= ARDRONE_VERSION ()) {
video_recorder_resume_thread ();
JOIN_THREAD (video_recorder);
}
//JOIN_THREAD(main_application_thread);
//ardrone_tool_input_remove(&input_controller);
return C_OK;
}
C_RESULT ardrone_tool_shutdown_custom (){
//King of the Hill threads
JOIN_THREAD(wiimote_logic);
JOIN_THREAD(drone_logic);
JOIN_THREAD(score_logic);
video_stage_resume_thread(); //Resume thread to kill it !
JOIN_THREAD(video_stage);
if (2 <= ARDRONE_VERSION ())
{
video_recorder_resume_thread ();
JOIN_THREAD (video_recorder);
}
return C_OK;
}
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;
}
