static void* navdata_loop(void *arg)
{
uint8_t msg[NAVDATA_BUFFER_SIZE];
navdata_unpacked_t navdata_unpacked;
unsigned int cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
int sockfd = -1, addr_in_size;
struct sockaddr_in *my_addr, *from;
INFO("NAVDATA thread starting (thread=%d)...\n", (int)pthread_self());
navdata_open_server();
addr_in_size = sizeof(struct sockaddr_in);
navdata_t* navdata = (navdata_t*) &msg[0];
from = (struct sockaddr_in *)vp_os_malloc(addr_in_size);
my_addr = (struct sockaddr_in *)vp_os_malloc(addr_in_size);
assert(from);
assert(my_addr);
vp_os_memset((char *)my_addr,(char)0,addr_in_size);
my_addr->sin_family = AF_INET;
my_addr->sin_addr.s_addr = htonl(INADDR_ANY);
my_addr->sin_port = htons( NAVDATA_PORT );
if((sockfd = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
INFO ("socket: %s\n", strerror(errno));
goto fail;
};
if(bind(sockfd, (struct sockaddr *)my_addr, addr_in_size) < 0){
INFO ("bind: %s\n", strerror(errno));
goto fail;
};
{
struct timeval tv;
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
setsockopt( sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
}
INFO("[NAVDATA] Ready to receive incomming data\n");
while ( nav_thread_alive ) {
int size;
size = recvfrom (sockfd, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)from,
(socklen_t *)&addr_in_size);
//INFO ("SIZE %d \n", size);
if( size == 0)
{
INFO ("Navdata lost connection \n");
navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
}
// if (size == 24)
// {
// const char cmds[] = "AT*COMWDG=0\r";
// at_write ((int8_t*)cmds, strlen( cmds ));
//
// const char cmds2[] = "AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\r";
// at_write ((int8_t*)cmds2, strlen( cmds2 ));
// }
if( navdata->header == NAVDATA_HEADER )
{
mykonos_state = navdata->ardrone_state;
if(ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// INFO ("[NAVDATA] Detect com watchdog\n");
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
{
//INFO ("[NAVDATA] Send com watchdog\n");
const char cmds[] = "AT*COMWDG=0\r";
at_write ((int8_t*)cmds, strlen( cmds ));
}
}
if( navdata->sequence > sequence )
{
if ( ardrone_get_mask_from_state( mykonos_state, ARDRONE_NAVDATA_DEMO_MASK ))
{
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &msg[0], size - sizeof(navdata_cks_t) );
if( cks != navdata_cks )
{
INFO("NAVADATA wrong CKS\n");
}
//INFO("VALUE = %f \n", navdata_unpacked.navdata_demo.theta);
}
}
else
{
INFO ("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
sequence = navdata->sequence;
}
}
fail:
vp_os_free(from);
vp_os_free(my_addr);
if (sockfd >= 0){
close(sockfd);
}
if (navdata_udp_socket >= 0){
close(navdata_udp_socket);
navdata_udp_socket = -1;
}
INFO("NAVDATA thread stopping\n");
return NULL;
}
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;
}
C_RESULT ardrone_tool_init_custom (void)
{
// Reset global data
bzero(&g_navdata, sizeof(g_navdata));
g_autopilot = FALSE;
//int32_t altMax = 1400;
//uint32_t altMin = 1300;
// ARDRONE_TOOL_CONFIGURATION_ADDEVENT (altitude_max, &altMax, NULL);
// 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_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_720P_CODEC;
in_picture->width = 1280;
in_picture->height = 720;*/
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;
//bool_t isOutdoor = TRUE;
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (outdoor, &isOutdoor, NULL);
/**
* Start the video thread (and the video recorder thread for AR.Drone 2)
*/
START_THREAD(video_stage, params);
video_stage_init();
//Set max and minimum altitude flight
//uint32_t altMin = 500;
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (altitude_min, &altMin, NULL);
/** START THE KEYBOARD CONTROL THREAD**/
//START_THREAD(KeyboardController, NULL);
video_stage_resume_thread ();
return C_OK;
}
PROTO_THREAD_ROUTINE(app,argv)
{
vp_api_picture_t picture;
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t stages[NB_STAGES];
vp_stages_input_file_config_t ifc;
vp_stages_output_sdl_config_t osc;
mjpeg_stage_decoding_config_t dec;
vp_os_memset(&ifc,0,sizeof(vp_stages_input_file_config_t));
ifc.name = ((char**)argv)[1];
ifc.buffer_size = (ACQ_WIDTH*ACQ_HEIGHT*3)/2;
osc.width = WINDOW_WIDTH;
osc.height = WINDOW_HEIGHT;
osc.bpp = 16;
osc.window_width = WINDOW_WIDTH;
osc.window_height = WINDOW_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;
/// Picture configuration
picture.format = PIX_FMT_YUV420P;
picture.width = ACQ_WIDTH;
picture.height = ACQ_HEIGHT;
picture.framerate = 15;
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;
dec.picture = &picture;
dec.out_buffer_size = 4096;
stages[0].type = VP_API_INPUT_FILE;
stages[0].cfg = (void *)&ifc;
stages[0].funcs = vp_stages_input_file_funcs;
stages[1].type = VP_API_FILTER_DECODER;
stages[1].cfg = (void *)&dec;
stages[1].funcs = mjpeg_decoding_funcs;
stages[2].type = VP_API_OUTPUT_SDL;
stages[2].cfg = (void *)&osc;
stages[2].funcs = vp_stages_output_sdl_funcs;
pipeline.nb_stages = NB_STAGES;
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_os_delay( 30 );
}
vp_api_close(&pipeline, &pipeline_handle);
return EXIT_SUCCESS;
}
/**
* \brief Runs a control event managing the configuration file.
* This functions handles reading the configuration information
* sent by the drone on the TCP 'control' socket on port 5559.
* Called by the 'ardrone_control' thread loop in 'ardrone_control.c'.
*/
C_RESULT ardrone_control_configuration_run( uint32_t ardrone_state, ardrone_control_configuration_event_t* event )
{
int32_t buffer_size;
char *start_buffer, *buffer;
char *value, *param, *c;
bool_t ini_buffer_end, ini_buffer_more;
C_RESULT res = C_OK;
int bytes_to_read;
/* Multiconfiguration support */
static char *custom_configuration_list_buffer = NULL;
static int custom_configuration_list_buffer_size = 0;
static int custom_configuration_list_data_size = 0;
#define CUSTOM_CFG_BLOCK_SIZE 1024
int i;
switch( event->config_state )
{
case CONFIG_REQUEST_INI:
if( ardrone_state & ARDRONE_COMMAND_MASK )
{
/* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
}
else
{
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the configuration.\n",__FILE__,__FUNCTION__,__LINE__);
ini_buffer_index = 0;
ardrone_at_configuration_get_ctrl_mode();
set_state(event, CONFIG_RECEIVE_INI);
}
break;
/* multi-configuration support */
case CUSTOM_CONFIG_REQUEST:
DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
if( ardrone_state & ARDRONE_COMMAND_MASK )
{
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
}
else
{
DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the custom config.\n",__FILE__,__FUNCTION__,__LINE__);
custom_configuration_list_buffer = NULL;
custom_configuration_list_buffer_size = 0;
custom_configuration_list_data_size = 0;
ardrone_at_custom_configuration_get_ctrl_mode();
set_state(event, CUSTOM_CONFIG_RECEIVE);
}
break;
case CONFIG_RECEIVE_INI:
DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
// Read data coming from ARDrone
buffer_size = ARDRONE_CONTROL_CONFIGURATION_INI_BUFFER_SIZE - ini_buffer_index;
res = ardrone_control_read( &ini_buffer[ini_buffer_index], &buffer_size );
DEBUG_CONFIG_RECEIVE("Received <<%s>>\n",&ini_buffer[ini_buffer_index]);
if(VP_SUCCEEDED(res))
{
buffer_size += ini_buffer_index;
ini_buffer[buffer_size]=0; // Makes sure the buffer ends with a zero
// Parse received data
if( buffer_size > 0 )
{
//DEBUG_CONFIG_RECEIVE(" Searching value\n");
ini_buffer_end = ini_buffer[buffer_size-1] == '\0'; // End of configuration data
ini_buffer_more = ini_buffer[buffer_size-1] != '\n'; // Need more configuration data to end parsing current line
//if (ini_buffer_end) DEBUG_CONFIG_RECEIVE(" ini_buffer_end\n");
//if (ini_buffer_more) DEBUG_CONFIG_RECEIVE(" ini_buffer_more\n");
start_buffer = (char*)&ini_buffer[0];
buffer = strchr(start_buffer, '\n');
//DEBUG_CONFIG_RECEIVE("Le start buffer : <<%s>>\n",start_buffer);
while( buffer != NULL )
{
//DEBUG_CONFIG_RECEIVE(" Found an '\\n' \n");
value = start_buffer;
param = strchr(value, '=');
*buffer = '\0';
*param = '\0';
// Remove spaces at end of strings
c = param - 1;
while( *c == ' ' )
{
*c = '\0';
c = c-1;
}
c = buffer-1;
while( *c == ' ' )
{
*c = '\0';
c = c-1;
}
// Remove spaces at beginning of strings
while( *value == ' ' )
{
value = value + 1;
}
param = param + 1;
while( *param == ' ' )
{
param = param + 1;
}
DEBUG_CONFIG_RECEIVE(" Decoding from control stream : <%s>=<%s>\n",value,param);
iniparser_setstring( event->ini_dict, value, param );
start_buffer = buffer + 1;
buffer = strchr(start_buffer, '\n');
}
if( ini_buffer_end )
{
/* Reading the condfiguration is finished, we ask the drone
* to clear the ACK bit */
ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
set_state(event, CONFIG_RECEIVED);
}
else if( ini_buffer_more )
{
// Compute number of bytes to copy
int32_t size = (int32_t)&ini_buffer[buffer_size] - (int32_t)start_buffer;
vp_os_memcpy( &ini_buffer[0], start_buffer, size );
ini_buffer_index = size;
}
else
{
/* The previous line was completely consumed - next data
* from the network will be stored at the beginning of the
* buffer. */
ini_buffer_index = 0;
}
}
}
else
{
res = C_FAIL;
DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
if(!(ardrone_state & ARDRONE_COMMAND_MASK))
set_state(event, CONFIG_REQUEST_INI);
}
break;
case CUSTOM_CONFIG_RECEIVE:
DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
/* Read data until a zero byte is received */
/* Clear old data from the buffer when receiving the first bytes */
if (custom_configuration_list_buffer!=NULL && custom_configuration_list_data_size==0){
vp_os_memset(custom_configuration_list_buffer,0,custom_configuration_list_buffer_size);
}
/* Enlarge the buffer if needed */
if (custom_configuration_list_data_size==custom_configuration_list_buffer_size)
{
custom_configuration_list_buffer_size += CUSTOM_CFG_BLOCK_SIZE;
custom_configuration_list_buffer = vp_os_realloc(custom_configuration_list_buffer,custom_configuration_list_buffer_size);
}
/* Read data at the end of the buffer */
bytes_to_read = custom_configuration_list_buffer_size - custom_configuration_list_data_size;
res = ardrone_control_read( &custom_configuration_list_buffer[custom_configuration_list_data_size], &bytes_to_read );
DEBUG_CONFIG_RECEIVE(" Reading %i bytes of the custom config. list\n",bytes_to_read);
for (i=0;i<bytes_to_read;i++) { DEBUG_CONFIG_RECEIVE("<%i>",custom_configuration_list_buffer[i]); }
/* Searches a zero */
if (VP_SUCCEEDED(res))
{
custom_configuration_list_data_size += bytes_to_read;
for (i=bytes_to_read;i>0;i--){
if (custom_configuration_list_buffer[custom_configuration_list_data_size - i] == 0) {
/* Reading the condfiguration is finished, we ask the drone
* to clear the ACK bit */
DEBUG_CONFIG_RECEIVE("Finished receiving\n");
ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
set_state(event, CUSTOM_CONFIG_RECEIVED);
ardrone_control_reset_custom_configurations_list(available_configurations);
ardrone_control_read_custom_configurations_list(custom_configuration_list_buffer,
custom_configuration_list_data_size,
available_configurations);
/* Clean up */
vp_os_sfree(&custom_configuration_list_buffer);
custom_configuration_list_buffer_size = 0;
custom_configuration_list_data_size = 0;
res = C_OK;
break;
}
}
}
else{
/* No data are available on the control socket. */
DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__);
/* Reset the buffer */
custom_configuration_list_data_size = 0;
/* The request for the configuration file should have been acknowledged by the drone.
* If not, another request is sent. */
if(!(ardrone_state & ARDRONE_COMMAND_MASK)) set_state(event, CUSTOM_CONFIG_REQUEST);
}
break;
case CONFIG_RECEIVED:
case CUSTOM_CONFIG_RECEIVED:
/* We finished reading the configuration, we wait for the drone to reset the ACK bit */
if( ardrone_state & ARDRONE_COMMAND_MASK )
{
/* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */
PRINT("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__);
ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0);
}
else
{
PRINT("%s %s %i - Finished.\n",__FILE__,__FUNCTION__,__LINE__);
event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS;
}
res = C_OK;
break;
default:
res = C_FAIL;
break;
}
return res;
}
C_RESULT ardrone_navdata_file_process( const navdata_unpacked_t* const pnd )
{
uint32_t i;
char str[50];
int32_t* locked_ptr;
screen_point_t* point_ptr;
struct timeval time;
gettimeofday(&time,NULL);
if( navdata_file_private == NULL )
return C_FAIL;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
return C_OK;
if( navdata_file == NULL )
{
navdata_file = navdata_file_private;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
{
printf("Receiving navdata demo\n");
}
else
{
printf("Receiving all navdata\n");
}
ardrone_navdata_file_print_version();
}
// Handle the case where user asked for a new navdata file
if( navdata_file != navdata_file_private )
{
fclose(navdata_file);
navdata_file = navdata_file_private;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
{
printf("Receiving navdata demo\n");
}
else
{
printf("Receiving all navdata\n");
}
ardrone_navdata_file_print_version();
}
vp_os_memset(&str[0], 0, sizeof(str));
fprintf( navdata_file,"\n" );
fprintf( navdata_file, "%u; %u", (unsigned int) pnd->navdata_demo.ctrl_state, (unsigned int) pnd->ardrone_state );
sprintf( str, "%d.%06d", (int)((pnd->navdata_time.time & TSECMASK) >> TSECDEC), (int)(pnd->navdata_time.time & TUSECMASK) );
fprintf( navdata_file, ";%s", str );
fprintf( navdata_file, "; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u",
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_X],
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Y],
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Z],
(unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_X],
(unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_Y],
(unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_Z],
(unsigned int) pnd->navdata_raw_measures.raw_gyros_110[0],
(unsigned int) pnd->navdata_raw_measures.raw_gyros_110[1],
(unsigned int) pnd->navdata_raw_measures.vbat_raw,
(unsigned int) pnd->navdata_phys_measures.alim3V3,
(unsigned int) pnd->navdata_phys_measures.vrefEpson,
(unsigned int) pnd->navdata_phys_measures.vrefIDG,
(unsigned int) pnd->navdata_raw_measures.flag_echo_ini,
(unsigned int) pnd->navdata_raw_measures.us_debut_echo,
(unsigned int) pnd->navdata_raw_measures.us_fin_echo,
(unsigned int) pnd->navdata_raw_measures.us_association_echo,
(unsigned int) pnd->navdata_raw_measures.us_distance_echo,
(unsigned int) pnd->navdata_raw_measures.us_courbe_temps,
(unsigned int) pnd->navdata_raw_measures.us_courbe_valeur,
(unsigned int) pnd->navdata_raw_measures.us_courbe_ref );
fprintf( navdata_file, "; %f; %04u; % 5f; % 5f; % 5f; % 6f; % 6f; % 6f",
pnd->navdata_phys_measures.accs_temp,
(unsigned int)pnd->navdata_phys_measures.gyro_temp,
pnd->navdata_phys_measures.phys_accs[ACC_X],
pnd->navdata_phys_measures.phys_accs[ACC_Y],
pnd->navdata_phys_measures.phys_accs[ACC_Z],
pnd->navdata_phys_measures.phys_gyros[GYRO_X],
pnd->navdata_phys_measures.phys_gyros[GYRO_Y],
pnd->navdata_phys_measures.phys_gyros[GYRO_Z]);
fprintf( navdata_file, "; % f; % f; % f",
pnd->navdata_gyros_offsets.offset_g[GYRO_X],
pnd->navdata_gyros_offsets.offset_g[GYRO_Y],
pnd->navdata_gyros_offsets.offset_g[GYRO_Z] );
fprintf( navdata_file, "; % f; % f",
pnd->navdata_euler_angles.theta_a,
pnd->navdata_euler_angles.phi_a);
fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %06d; %06d; %06d",
(int) pnd->navdata_references.ref_theta,
(int) pnd->navdata_references.ref_phi,
(int) pnd->navdata_references.ref_psi,
(int) pnd->navdata_references.ref_theta_I,
(int) pnd->navdata_references.ref_phi_I,
(int) pnd->navdata_references.ref_pitch,
(int) pnd->navdata_references.ref_roll,
(int) pnd->navdata_references.ref_yaw );
fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f",
pnd->navdata_trims.euler_angles_trim_theta,
pnd->navdata_trims.euler_angles_trim_phi,
pnd->navdata_trims.angular_rates_trim_r );
fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %04u",
(int) pnd->navdata_rc_references.rc_ref_pitch,
(int) pnd->navdata_rc_references.rc_ref_roll,
(int) pnd->navdata_rc_references.rc_ref_yaw,
(int) pnd->navdata_rc_references.rc_ref_gaz,
(int) pnd->navdata_rc_references.rc_ref_ag,
(unsigned int) ui_get_user_input() );
fprintf( navdata_file, "; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03d; % f; % f; %03d; %03d; %03d; % f; %03d; %03d; %03d; % f; %04d; %04d; %04d; %04d",
(unsigned int) pnd->navdata_pwm.motor1,
(unsigned int) pnd->navdata_pwm.motor2,
(unsigned int) pnd->navdata_pwm.motor3,
(unsigned int) pnd->navdata_pwm.motor4,
(unsigned int) pnd->navdata_pwm.sat_motor1,
(unsigned int) pnd->navdata_pwm.sat_motor2,
(unsigned int) pnd->navdata_pwm.sat_motor3,
(unsigned int) pnd->navdata_pwm.sat_motor4,
(int) pnd->navdata_pwm.gaz_feed_forward,
pnd->navdata_pwm.gaz_altitude,
pnd->navdata_pwm.altitude_integral,
pnd->navdata_pwm.vz_ref,
(int) pnd->navdata_pwm.u_pitch,
(int) pnd->navdata_pwm.u_roll,
(int) pnd->navdata_pwm.u_yaw,
pnd->navdata_pwm.yaw_u_I,
(int) pnd->navdata_pwm.u_pitch_planif,
(int) pnd->navdata_pwm.u_roll_planif,
(int) pnd->navdata_pwm.u_yaw_planif,
pnd->navdata_pwm.u_gaz_planif,
(int) pnd->navdata_pwm.current_motor1,
(int) pnd->navdata_pwm.current_motor2,
(int) pnd->navdata_pwm.current_motor3,
(int) pnd->navdata_pwm.current_motor4 );
fprintf( navdata_file, "; %04d; %f; %04d; %04u",
(int) pnd->navdata_altitude.altitude_vision,
pnd->navdata_altitude.altitude_vz,
(int) pnd->navdata_altitude.altitude_ref,
(unsigned int) pnd->navdata_altitude.altitude_raw );
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %04u; %f; %f; %04u",
pnd->navdata_altitude.obs_accZ,
pnd->navdata_altitude.obs_alt,
pnd->navdata_altitude.obs_x.v[0],
pnd->navdata_altitude.obs_x.v[1],
pnd->navdata_altitude.obs_x.v[2],
pnd->navdata_altitude.obs_state,
pnd->navdata_altitude.est_vb.v[0],
pnd->navdata_altitude.est_vb.v[1],
pnd->navdata_altitude.est_state );
vp_os_memset(&str[0], 0, sizeof(str));
sprintf( str, "%d.%06d", (int)((pnd->navdata_vision.time_capture & TSECMASK) >> TSECDEC), (int)(pnd->navdata_vision.time_capture & TUSECMASK) );
fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f; %u; %u; % f;% f;% f;% f; % f; % f; % f; %u; % f; % f; % f; % f; % f; % f; % d; %s",
pnd->navdata_vision_raw.vision_tx_raw,
pnd->navdata_vision_raw.vision_ty_raw,
pnd->navdata_vision_raw.vision_tz_raw,
(unsigned int) pnd->navdata_vision.vision_state,
(unsigned int) pnd->vision_defined,
pnd->navdata_vision.vision_phi_trim,
pnd->navdata_vision.vision_phi_ref_prop,
pnd->navdata_vision.vision_theta_trim,
pnd->navdata_vision.vision_theta_ref_prop,
pnd->navdata_vision.body_v.x,
pnd->navdata_vision.body_v.y,
pnd->navdata_vision.body_v.z,
(unsigned int) pnd->navdata_vision.new_raw_picture,
pnd->navdata_vision.theta_capture,
pnd->navdata_vision.phi_capture,
pnd->navdata_vision.psi_capture,
pnd->navdata_vision.delta_phi,
pnd->navdata_vision.delta_theta,
pnd->navdata_vision.delta_psi,
(int)pnd->navdata_vision.altitude_capture,
str );
fprintf( navdata_file, "; %04u",
(unsigned int) pnd->navdata_demo.vbat_flying_percentage );
fprintf( navdata_file, "; % f; % f; % f",
pnd->navdata_demo.theta,
pnd->navdata_demo.phi,
pnd->navdata_demo.psi );
fprintf( navdata_file, "; %04d",
(int) pnd->navdata_demo.altitude );
fprintf( navdata_file, "; %f; %f; %f ",
pnd->navdata_demo.vx,
pnd->navdata_demo.vy,
pnd->navdata_demo.vz );
fprintf( navdata_file, "; %04u", (unsigned int) pnd->navdata_demo.num_frames );
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.detection_camera_rot.m11,
pnd->navdata_demo.detection_camera_rot.m12,
pnd->navdata_demo.detection_camera_rot.m13,
pnd->navdata_demo.detection_camera_rot.m21,
pnd->navdata_demo.detection_camera_rot.m22,
pnd->navdata_demo.detection_camera_rot.m23,
pnd->navdata_demo.detection_camera_rot.m31,
pnd->navdata_demo.detection_camera_rot.m32,
pnd->navdata_demo.detection_camera_rot.m33);
fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.detection_camera_trans.x,
pnd->navdata_demo.detection_camera_trans.y,
pnd->navdata_demo.detection_camera_trans.z);
fprintf( navdata_file, "; %04u; %04u",
(unsigned int) pnd->navdata_demo.detection_tag_index,
(unsigned int) pnd->navdata_demo.detection_camera_type);
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.drone_camera_rot.m11,
pnd->navdata_demo.drone_camera_rot.m12,
pnd->navdata_demo.drone_camera_rot.m13,
pnd->navdata_demo.drone_camera_rot.m21,
pnd->navdata_demo.drone_camera_rot.m22,
pnd->navdata_demo.drone_camera_rot.m23,
pnd->navdata_demo.drone_camera_rot.m31,
pnd->navdata_demo.drone_camera_rot.m32,
pnd->navdata_demo.drone_camera_rot.m33);
fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.drone_camera_trans.x,
pnd->navdata_demo.drone_camera_trans.y,
pnd->navdata_demo.drone_camera_trans.z);
fprintf( navdata_file, "; %d; %f; %f; %f; %f",
(int)nd_iphone_flag,
nd_iphone_phi,
nd_iphone_theta,
nd_iphone_gaz,
nd_iphone_yaw);
fprintf( navdata_file, "; %d; %d; %d; %d; %d; %f; %d",
pnd->navdata_video_stream.quant,
pnd->navdata_video_stream.frame_size,
pnd->navdata_video_stream.frame_number,
pnd->navdata_video_stream.atcmd_ref_seq,
pnd->navdata_video_stream.atcmd_mean_ref_gap,
pnd->navdata_video_stream.atcmd_var_ref_gap,
pnd->navdata_video_stream.atcmd_ref_quality);
locked_ptr = (int32_t*) &pnd->navdata_trackers_send.locked[0];
point_ptr = (screen_point_t*) &pnd->navdata_trackers_send.point[0];
for(i = 0; i < DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT; i++)
{
fprintf( navdata_file, "; %d; %u; %u",
(int) *locked_ptr++,
(unsigned int) point_ptr->x,
(unsigned int) point_ptr->y );
point_ptr++;
}
fprintf( navdata_file, "; %u", (unsigned int) pnd->navdata_vision_detect.nb_detected );
for(i = 0 ; i < 4 ; i++)
{
fprintf( navdata_file, "; %u; %u; %u; %u; %u; %u; %f",
(unsigned int) pnd->navdata_vision_detect.type[i],
(unsigned int) pnd->navdata_vision_detect.xc[i],
(unsigned int) pnd->navdata_vision_detect.yc[i],
(unsigned int) pnd->navdata_vision_detect.width[i],
(unsigned int) pnd->navdata_vision_detect.height[i],
(unsigned int) pnd->navdata_vision_detect.dist[i],
pnd->navdata_vision_detect.orientation_angle[i]);
}
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f",
pnd->navdata_vision_perf.time_szo,
pnd->navdata_vision_perf.time_corners,
pnd->navdata_vision_perf.time_compute,
pnd->navdata_vision_perf.time_tracking,
pnd->navdata_vision_perf.time_trans,
pnd->navdata_vision_perf.time_update );
for(i = 0 ; i < NAVDATA_MAX_CUSTOM_TIME_SAVE ; i++)
{
fprintf( navdata_file, "; %f", pnd->navdata_vision_perf.time_custom[i]);
}
fprintf( navdata_file, "; %d", (int) pnd->navdata_watchdog.watchdog );
fprintf( navdata_file, "; %u", (unsigned int) num_picture_decoded );
sprintf( str, "%d.%06d", (int)time.tv_sec, (int)time.tv_usec);
fprintf( navdata_file, "; %s", str );
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;
}
/*---------------------------------------------------------------------------------------------------------------------
Tests the network connection to the drone by fetching the drone version number
through the FTP server embedded on the drone.
This is how FreeFlight checks if a drone sofware update is required.
The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
---------------------------------------------------------------------------------------------------------------------*/
int test_drone_connection()
{
const char * passivdeModeHeader = "\r\n227 PASV ok (";
vp_com_socket_t ftp_client,ftp_client2;
char buffer[1024];
static Write ftp_write = NULL;
static Read ftp_read = NULL;
int bytes_to_send,received_bytes;
int i,L,x[6],port;
int timeout_windows = 1000; /*milliseconds*/
vp_os_memset(buffer,0,sizeof(buffer));
/* Connects to the FTP server */
wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
ftp_client.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;
setsockopt((int32_t)ftp_client.priv,
SOL_SOCKET,
SO_RCVTIMEO,
(const char*)&timeout_windows, sizeof(timeout_windows)
);
/* Request version file */
bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",
"USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");
ftp_write(&ftp_client,buffer,&bytes_to_send);
/* Dirty. We should wait for data to arrive with some kind of synchronization
or make the socket blocking.*/
Sleep(1000);
/* Gets the data port */
received_bytes = sizeof(buffer);
ftp_read(&ftp_client,buffer,&received_bytes);
if (received_bytes<1) { vp_com_close(wifi_com(), &ftp_client); return -3; }
L=received_bytes-strlen(passivdeModeHeader);
/* Searches for the passive mode acknowlegment from the FTP server */
for (i=0;i<L;i++) {
if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0) break;
}
if (i==L) {
vp_com_close(wifi_com(), &ftp_client); return -4;
}
i+=strlen(passivdeModeHeader);
if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
{ vp_com_close(wifi_com(), &ftp_client); return -5; }
port=(x[4]<<8)+x[5];
/* Connects to the FTP server data port */
wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
ftp_client2.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com())))
{ vp_com_close(wifi_com(), &ftp_client2); return -6; }
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
{ vp_com_close(wifi_com(), &ftp_client2); return -7; }
/* Gets the data */
received_bytes = sizeof(buffer);
ftp_read(&ftp_client2,buffer,&received_bytes);
if (received_bytes>0) {
buffer[min(received_bytes,sizeof(buffer)-1)]=0;
printf("Drone version %s detected ... press <Enter> to start the application.\n",buffer);
getchar();
}
/* Clean up */
vp_com_close(wifi_com(), &ftp_client);
vp_com_close(wifi_com(), &ftp_client2);
return 0;
}
int main(int argc, char **argv)
{
vp_api_picture_t video_picture;
vp_api_picture_t buffer_blockline;
#ifdef BLOCK_MODE
vp_api_picture_t video_blockline;
#endif
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t stages[NB_STAGES_MAX];
vp_os_memset(&ivc,0,sizeof(ivc));
vp_os_memset(&ofc,0,sizeof(ofc));
vp_os_memset(&occ,0,sizeof(occ));
// CAMIF config
ivc.camera = "/dev/video1";
ivc.i2c = "/dev/i2c-0";
#ifdef OV_CAM
ivc.camera_configure = &camera_configure_ov77xx;
#else
#ifdef CRESYN_CAM
ivc.camera_configure = &camera_configure_cresyn;
#else
#error no cam selected
#endif
#endif
ivc.resolution = VGA;
ivc.nb_buffers = 8;
ivc.format = V4L2_PIX_FMT_YUV420;
ivc.y_pad = 0;
ivc.c_pad = 0;
ivc.offset_delta = 0;
#ifdef BLOCK_MODE
ivc.vp_api_blockline = &video_blockline;
#endif
ivc.vp_api_picture = &video_picture;
ivc.use_chrominances = 1;
ivc.framerate = 15;
// BUFFER
#ifdef BLOCK_MODE
bbc.picture = &video_blockline;
#endif
// ENCODER
vec.width = 320;
vec.height = 240;
#ifdef BLOCK_MODE
vec.block_mode_enable = TRUE;
#else
vec.block_mode_enable = FALSE;
#endif
vec.picture = &video_picture;
// OUTPUT FILE config
ofc.name = "/tmp/out.yuv";
// COM CONFIG
occ.com = wired_com();
occ.config = wired_config();
occ.connection = wired_connection();
occ.socket.type = VP_COM_SERVER;
occ.socket.protocol = VP_COM_TCP;
occ.socket.port = 5555;
occ.buffer_size = 640*480*3/2;
// build pipeline
pipeline.nb_stages = 0;
stages[pipeline.nb_stages].type = VP_API_INPUT_CAMIF;
stages[pipeline.nb_stages].cfg = (void *)&ivc;
stages[pipeline.nb_stages++].funcs = V4L2_funcs;
#ifdef BLOCK_MODE
#ifdef BUFFER
stages[pipeline.nb_stages].type = VP_API_INPUT_CAMIF;
stages[pipeline.nb_stages].cfg = (void *)&bbc;
stages[pipeline.nb_stages++].funcs = blockline_to_buffer_funcs;
#endif
#endif
#ifdef ENCODE
stages[pipeline.nb_stages].type = VP_API_FILTER_ENCODER;
stages[pipeline.nb_stages].cfg = (void*)&vec;
stages[pipeline.nb_stages++].funcs = vlib_encoding_funcs;
#endif
#ifdef FILE_OUTPUT
stages[pipeline.nb_stages].type = VP_API_OUTPUT_FILE;
stages[pipeline.nb_stages].cfg = (void *)&ofc;
stages[pipeline.nb_stages++].funcs = vp_stages_output_file_funcs;
#endif
#ifdef ETHERNET_OUTPUT
stages[pipeline.nb_stages].type = VP_API_OUTPUT_SOCKET;
stages[pipeline.nb_stages].cfg = (void *)&occ;
stages[pipeline.nb_stages++].funcs = vp_stages_output_com_funcs;
#endif
pipeline.stages = &stages[0];
// launch pipelines
vp_api_open(&pipeline, &pipeline_handle);
out.status = VP_API_STATUS_PROCESSING;
/////////////////////////////////////////////////////////////////////////////////////////
uint16_t i = 0;
struct timeval time1,time2;
printf("Pipeline launched....\n");
gettimeofday(&time1,NULL);
while(SUCCEED(vp_api_run(&pipeline, &out)) && (out.status == VP_API_STATUS_PROCESSING))
{
i++;
//printf("image %d \n",i);
if (i>50)
break;
//vp_os_delay(20);
}
/////////////////////////////////////////////////////////////////////////////////////////
gettimeofday(&time2,NULL);
int seconde = time2.tv_sec-time1.tv_sec;
int ms = time2.tv_usec-time1.tv_usec;
if (ms<0)
{
seconde--;
ms += 1000000;
}
ms = ms/1000;
float fps = ((float)i*1000)/(float)(seconde*1000+ms);
printf("temps ecoule : %d.%d / nombre image : %d average fps : %f\n",seconde,ms,i,fps);
vp_api_close(&pipeline, &pipeline_handle);
return EXIT_SUCCESS;
}
int ardrone_tool_main(int argc, char **argv)
{
C_RESULT res;
const char* old_locale;
const char* appname = argv[0];
int argc_backup = argc;
char** argv_backup = argv;
char * drone_ip_address = NULL;
struct in_addr drone_ip_address_in;
bool_t show_usage = FAILED( ardrone_tool_check_argc_custom(argc) ) ? TRUE : FALSE;
argc--; argv++;
while( argc && *argv[0] == '-' )
{
if( !strcmp(*argv, "-ip") && ( argc > 1 ) )
{
drone_ip_address = *(argv+1);
printf("Using custom ip address %s\n",drone_ip_address);
argc--; argv++;
}
else if( !strcmp(*argv, "-?") || !strcmp(*argv, "-h") || !strcmp(*argv, "-help") || !strcmp(*argv, "--help") )
{
ardrone_tool_usage( appname );
exit( 0 );
}
else if( !ardrone_tool_parse_cmd_line_custom( *argv ) )
{
printf("Option %s not recognized\n", *argv);
show_usage = TRUE;
}
argc--; argv++;
}
/*if( show_usage || (argc != 0) )
{
ardrone_tool_usage( appname );
exit(-1);
}*/
/* After a first analysis, the arguments are restored so they can be passed to the user-defined functions */
argc=argc_backup;
argv=argv_backup;
old_locale = setlocale(LC_NUMERIC, "en_GB.UTF-8");
if( old_locale == NULL )
{
PRINT("You have to install new locales in your dev environment! (avoid the need of conv_coma_to_dot)\n");
PRINT("As root, do a \"dpkg-reconfigure locales\" and add en_GB.UTF8 to your locale settings\n");
}
else
{
PRINT("Setting locale to %s\n", old_locale);
}
vp_com_wifi_config_t *config = (vp_com_wifi_config_t*)wifi_config();
if(config)
{
vp_os_memset( &wifi_ardrone_ip[0], 0, sizeof(wifi_ardrone_ip) );
if(drone_ip_address && inet_aton(drone_ip_address,&drone_ip_address_in)!=0)
{
/* If the drone IP address was given on the command line and is valid */
printf("===================+> %s\n", drone_ip_address);
strncpy( &wifi_ardrone_ip[0], drone_ip_address, sizeof(wifi_ardrone_ip)-1);
}
else
{
printf("===================+> %s\n", config->server);
strncpy( &wifi_ardrone_ip[0], config->server, sizeof(wifi_ardrone_ip)-1);
}
}
while (-1 == getDroneVersion (root_dir, wifi_ardrone_ip, &ardroneVersion))
{
printf ("Getting AR.Drone version ...\n");
vp_os_delay (250);
}
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed. It means either:\n");
PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
PRINT("\t* wifi device is not present (on your pc or on your card)\n");
PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
PRINT("\t* wifi device has no antenna\n");
}
else
{
// Save appname/appid for reconnections
char *appname = NULL;
int lastSlashPos;
/* Cut the invoking name to the last / or \ character on the command line
* This avoids using differents app_id for applications called from different directories
* e.g. if argv[0] is "Build/Release/ardrone_navigation", appname will point to "ardrone_navigation" only
*/
for (lastSlashPos = strlen (argv[0])-1; lastSlashPos > 0 && argv[0][lastSlashPos] != '/' && argv[0][lastSlashPos] != '\\'; lastSlashPos--);
appname = &argv[0][lastSlashPos+1];
ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
res = ardrone_tool_init(wifi_ardrone_ip, strlen(wifi_ardrone_ip), NULL, appname, NULL, NULL, NULL, MAX_FLIGHT_STORING_SIZE, NULL);
while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
{
res = ardrone_tool_update();
}
res = ardrone_tool_shutdown();
}
if( old_locale != NULL )
{
setlocale(LC_NUMERIC, old_locale);
}
return SUCCEED(res) ? 0 : -1;
}
C_RESULT ardrone_timer_reset(ardrone_timer_t* timer)
{
vp_os_memset(timer, 0, sizeof(ardrone_timer_t));
return C_OK;
}
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;
}
void ardrone_control_read_custom_configurations_list(/*in*/char * buffer,
/*in*/int buffer_size,
/*out*/custom_configuration_list_t *available_configurations)
{
custom_configuration_list_t * current_scope = NULL;
char id[CUSTOM_CONFIGURATION_ID_LENGTH+1];
char description[1024];
int index = 0;
char * pindex; int i,j;
char * end_of_buffer;
index = 0;
pindex = buffer;
end_of_buffer = buffer + buffer_size;
DEBUG_CONFIG_RECEIVE("Decoding %i bytes",buffer_size);
DEBUG_CONFIG_RECEIVE("\n");
while(1)
{
//DEBUG_CONFIG_RECEIVE("Analysing <"); for (i=index;i<buffer_size;i++) DEBUG_CONFIG_RECEIVE("[%i]",buffer[i]); DEBUG_CONFIG_RECEIVE(">\n");
/* Go to the beginning of a section */
while((*pindex)!='[') { index++; pindex++; if (pindex==end_of_buffer) return; }
/* Search the end of the section name */
for (;index<buffer_size;index++) { if (buffer[index]==13 ) { buffer[index]=0; break; } } if(index==buffer_size) return;
/* Search the corresponding category */
for (j=0;j<NB_CONFIG_CATEGORIES;j++){
if ( strcmp(custom_configuration_headers[j],pindex)==0 ){
/* Found the category */
current_scope = &available_configurations[j];
DEBUG_CONFIG_RECEIVE(" Found Scope <%s>\n",custom_configuration_headers[j]);
break;
}
}
if (j==NB_CONFIG_CATEGORIES) { DEBUG_CONFIG_RECEIVE("Unknown category."); return ;}
/* Reset the list */
if (current_scope!=NULL)
{
current_scope->nb_configurations = 0;
if (current_scope->list!=NULL) { vp_os_free(current_scope->list); current_scope->list = NULL; }
}
/* Points on the first ID */
index++;
pindex=buffer+index;
/* Read the IDs */
while(pindex<end_of_buffer && (*pindex)!='[' && (*pindex)!=0)
{
vp_os_memset(id,0,sizeof(id));
vp_os_memset(description,0,sizeof(description));
//DEBUG_CONFIG_RECEIVE("Now scanning <%c> %i\n",*pindex,index);
for (;index<buffer_size;index++) { if (buffer[index]==',' || buffer[index]=='\r') { buffer[index]=0; break; } } if(index==buffer_size) return;
strncpy(id,pindex,sizeof(id));
index++;
pindex=buffer+index;
for (;index<buffer_size;index++) { if (buffer[index]==0 || buffer[index]=='\r') { buffer[index]=0; break; } } if(index==buffer_size) return;
strncpy(description,pindex,sizeof(description));
DEBUG_CONFIG_RECEIVE(" Found ID <%s> description <%s>\n",id,description);
index++;
pindex=buffer+index;
/* Store the found ID */
/* Increase the size of the list by one element */
current_scope->list = vp_os_realloc(current_scope->list,sizeof(*current_scope->list)*(current_scope->nb_configurations+1));
/* Store the new element */
strncpy(current_scope->list[current_scope->nb_configurations].id,
id,
sizeof(current_scope->list[current_scope->nb_configurations].id) );
strncpy(current_scope->list[current_scope->nb_configurations].description,
description,
sizeof(current_scope->list[current_scope->nb_configurations].description) );
current_scope->nb_configurations++;
}
}
return;
}
void create_image_window( void )
{
/* Image display main window */
/* ------------------------- */
int k;
printf("Creating the Video window.\n");
// Image main window
ihm_ImageWin = gtk_window_new( GTK_WINDOW_TOPLEVEL);
gtk_container_set_border_width(GTK_CONTAINER(ihm_ImageWin), 10);
gtk_window_set_title(GTK_WINDOW(ihm_ImageWin), ihm_ImageTitle);
gtk_signal_connect(GTK_OBJECT(ihm_ImageWin), "destroy", G_CALLBACK(ihm_ImageWinDestroy), NULL );
// Boxes
ihm_ImageVBox = gtk_vbox_new(FALSE, 0);
ihm_VideoStream_VBox = gtk_vbox_new(FALSE, 0);
ihm_ImageVBoxPT = gtk_vbox_new(FALSE, 0);
//hBox_vision_state = gtk_hbox_new(FALSE, 0);
for (k=0; k<NB_IMAGES_H_BOXES; k++) ihm_ImageHBox[k] = gtk_hbox_new(FALSE, 0);
// Frames
for (k=0; k<NB_IMAGES_FRAMES; k++) ihm_ImageFrames[k] = gtk_frame_new( ihm_ImageFrameCaption[k] );
// Entries
for (k=0; k<NB_IMAGES_ENTRIES; k++) {
ihm_ImageEntry[k] = gtk_entry_new();
gtk_widget_set_size_request(ihm_ImageEntry[k], 80, 20);
}
// Video Stream
for (k=0; k<NB_VIDEO_STREAM_WIDGET; k++) ihm_VideoStreamLabel[k] = gtk_label_new( ihm_ImageVideoStreamCaption[k] );
video_bitrateEntry = gtk_entry_new();
gtk_widget_set_size_request(video_bitrateEntry, 150, 20);
video_codecList = gtk_combo_box_new_text();
gtk_combo_box_insert_text( (GtkComboBox*)video_codecList, 0, (const gchar*)"UVLC");
gtk_combo_box_insert_text( (GtkComboBox*)video_codecList, 1, (const gchar*)"P264");
gtk_widget_set_size_request(video_codecList, 150, 20);
video_bitrateModeList = gtk_combo_box_new_text();
gtk_combo_box_insert_text( (GtkComboBox*)video_bitrateModeList, 0, (const gchar*)"None");
gtk_combo_box_insert_text( (GtkComboBox*)video_bitrateModeList, 1, (const gchar*)"Adaptative");
gtk_combo_box_insert_text( (GtkComboBox*)video_bitrateModeList, 2, (const gchar*)"Manual");
gtk_widget_set_size_request(video_codecList, 150, 20);
for (k=0; k<NB_IMAGES_ENTRIES; k++) ihm_ImageLabel[k] = gtk_label_new( ihm_ImageEntryCaption[k] );
/* Creates buttons and links them to callbacks */
for (k=0; k<NB_IMAGES_BUTTONS; k++)
{
ihm_ImageButton[k] = gtk_button_new();// ihm_ImageButtonCaption[k] );
gtk_button_set_label((GtkButton*)ihm_ImageButton[k] ,ihm_ImageButtonCaption[k]);
switch (k)
{
case UPDATE_VISION_PARAMS_BUTTON:
g_signal_connect( G_OBJECT(ihm_ImageButton[k]), "clicked", G_CALLBACK(ihm_sendVisionConfigParams), (gpointer)k );
break;
case RAW_CAPTURE_BUTTON:
g_signal_connect( G_OBJECT(ihm_ImageButton[k]), "clicked", G_CALLBACK(ihm_RAWCapture), (gpointer)k );
break;
case ZAPPER_BUTTON:
g_signal_connect( G_OBJECT(ihm_ImageButton[k]), "clicked", G_CALLBACK(ihm_Zapper), (gpointer)k );
break;
case FULLSCREEN_BUTTON:
g_signal_connect( G_OBJECT(ihm_ImageButton[k]), "clicked", G_CALLBACK(ihm_VideoFullScreen), (gpointer)k );
break;
default:
g_signal_connect( G_OBJECT(ihm_ImageButton[k]), "clicked", G_CALLBACK(ihm_ImageButtonCB), (gpointer)k );
}
}
GdkColor color;
gdk_color_parse ("red", &color);
gtk_widget_modify_text (ihm_ImageLabel[RAW_CAPTURE_BUTTON], GTK_STATE_NORMAL, &color);
video_bitrateButton = gtk_button_new_with_label( "Send" );
g_signal_connect(G_OBJECT(video_bitrateButton), "clicked", G_CALLBACK(ihm_send_VideoBitrate), 0 );
g_signal_connect(G_OBJECT(video_codecList), "changed", G_CALLBACK(ihm_send_VideoCodec), 0 );
g_signal_connect(G_OBJECT(video_bitrateModeList), "changed", G_CALLBACK(ihm_send_VideoBitrateMode), 0 );
/* Creates input boxes (aka. entries) */
char label_vision_default_val[NB_IMAGES_ENTRIES] ;
tab_vision_config_params[0] = DEFAULT_CS;
tab_vision_config_params[1] = DEFAULT_NB_PAIRS;
tab_vision_config_params[2] = DEFAULT_LOSS_PER;
tab_vision_config_params[3] = DEFAULT_NB_TRACKERS_WIDTH;
tab_vision_config_params[4] = DEFAULT_NB_TRACKERS_HEIGHT;
tab_vision_config_params[5] = DEFAULT_SCALE;
tab_vision_config_params[6] = DEFAULT_TRANSLATION_MAX;
tab_vision_config_params[7] = DEFAULT_MAX_PAIR_DIST;
tab_vision_config_params[8] = DEFAULT_NOISE;
for (k=0; k<NB_IMAGES_ENTRIES; k++) {
if (k==FAKE_ENTRY) continue;
sprintf(label_vision_default_val, "%d", tab_vision_config_params[k]);
gtk_entry_set_text( GTK_ENTRY(ihm_ImageEntry[k]), label_vision_default_val);
}
gtk_entry_set_text( GTK_ENTRY(video_bitrateEntry), "frame size (bytes)");
/* Builds the vision state frame */
vp_os_memset(label_vision_state_value, 0, sizeof(label_vision_state_value));
strcat(label_vision_state_value, "Not Connected");
label_vision_values = (GtkLabel*) gtk_label_new(label_vision_state_value);
gtk_container_add( GTK_CONTAINER(ihm_ImageFrames[STATE_FRAME]), (GtkWidget*) label_vision_values );
/* Builds the vision parameters frame */
/* First line of parameters */
for (k=CS_ENTRY; k<NB_TH_ENTRY; k++) {
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAM_HBOX1]), ihm_ImageLabel[k], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAM_HBOX1]), ihm_ImageEntry[k], FALSE , FALSE, 0);
}
/* Second line of parameters */
for (k=NB_TH_ENTRY; k<NB_IMAGES_ENTRIES; k++) {
if (k==FAKE_ENTRY) continue;
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAM_HBOX2]), ihm_ImageLabel[k], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAM_HBOX2]), ihm_ImageEntry[k], FALSE , FALSE, 0);
}
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAM_HBOX2]), ihm_ImageLabel[FAKE_ENTRY], FALSE , FALSE, 0); // To fill space
/* Fuses the two line in a single VBox */
gtk_box_pack_start(GTK_BOX(ihm_ImageVBoxPT), ihm_ImageHBox[TRACKING_PARAM_HBOX1], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX(ihm_ImageVBoxPT), ihm_ImageHBox[TRACKING_PARAM_HBOX2], FALSE , FALSE, 0);
/* Builds the whole parameter block */
gtk_container_add(GTK_CONTAINER(ihm_ImageFrames[TRACKING_PARAMETERS_FRAME]), ihm_ImageVBoxPT );
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAMS_HBOX]), ihm_ImageFrames[TRACKING_PARAMETERS_FRAME], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[TRACKING_PARAMS_HBOX]), ihm_ImageButton[UPDATE_VISION_PARAMS_BUTTON], TRUE , FALSE, 5);
for (k=TZ_KNOWN_BUTTON; k<=SE3_BUTTON; k++)
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[TRACKING_OPTION_HBOX]), ihm_ImageButton[k], TRUE , FALSE, 0);
for (k=PROJ_OVERSCENE_BUTTON; k<=FLAT_GROUND_BUTTON; k++)
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[COMPUTING_OPTION_HBOX]), ihm_ImageButton[k], TRUE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), ihm_VideoStreamLabel[CODEC_TYPE_LIST], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), video_codecList, TRUE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), ihm_VideoStreamLabel[BITRATE_MODE_LIST], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), video_bitrateModeList, TRUE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), ihm_VideoStreamLabel[MANUAL_BITRATE_ENTRY], FALSE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), video_bitrateEntry, TRUE , FALSE, 0);
gtk_box_pack_start(GTK_BOX( ihm_ImageHBox[VIDEO_STREAM_HBOX]), video_bitrateButton, TRUE , FALSE, 0);
/* */
gtk_container_add(GTK_CONTAINER( ihm_ImageFrames[TRACKING_OPTION_FRAME]) , ihm_ImageHBox[TRACKING_OPTION_HBOX] );
gtk_container_add(GTK_CONTAINER( ihm_ImageFrames[COMPUTING_OPTION_FRAME]), ihm_ImageHBox[COMPUTING_OPTION_HBOX] );
gtk_container_add(GTK_CONTAINER( ihm_ImageFrames[VIDEO_STREAM_FRAME]) , ihm_ImageHBox[VIDEO_STREAM_HBOX] );
/* Frame where to show buttons controlling how the drone video is displayed */
displayvbox = gtk_vbox_new(FALSE,0);
gtk_box_pack_start(GTK_BOX(displayvbox), ihm_ImageButton[RAW_CAPTURE_BUTTON], FALSE , FALSE, 5);
gtk_box_pack_start(GTK_BOX(displayvbox), ihm_ImageButton[ZAPPER_BUTTON], FALSE , FALSE, 5);
gtk_box_pack_start(GTK_BOX(displayvbox), ihm_ImageButton[FULLSCREEN_BUTTON], FALSE , FALSE, 5);
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[VIDEO_DISPLAY_HBOX]),ihm_VideoStream_VBox,FALSE,FALSE,5);
gtk_box_pack_start(GTK_BOX(ihm_ImageHBox[VIDEO_DISPLAY_HBOX]),displayvbox,FALSE,FALSE,5);
gtk_container_add(GTK_CONTAINER( ihm_ImageFrames[VIDEO_DISPLAY_FRAME]) , ihm_ImageHBox[VIDEO_DISPLAY_HBOX] );
/* Builds the final window */
gtk_box_pack_start(GTK_BOX(ihm_ImageVBox), ihm_ImageFrames[VIDEO_DISPLAY_FRAME], FALSE, FALSE, 5);
gtk_box_pack_start(GTK_BOX(ihm_ImageVBox), ihm_ImageFrames[STATE_FRAME], FALSE, FALSE, 5);
gtk_box_pack_start(GTK_BOX(ihm_ImageVBox), ihm_ImageHBox[TRACKING_PARAMS_HBOX], FALSE, FALSE, 5);
gtk_box_pack_start(GTK_BOX(ihm_ImageVBox), ihm_ImageFrames[TRACKING_OPTION_FRAME], FALSE, FALSE, 5);
gtk_box_pack_start(GTK_BOX(ihm_ImageVBox), ihm_ImageFrames[COMPUTING_OPTION_FRAME], FALSE, FALSE, 5);
gtk_box_pack_start(GTK_BOX(ihm_ImageVBox), ihm_ImageFrames[VIDEO_STREAM_FRAME], FALSE, FALSE, 5);
gtk_container_add(GTK_CONTAINER(ihm_ImageWin), ihm_ImageVBox);
image_vision_window_view = WINDOW_HIDE;
image_vision_window_status = WINDOW_OPENED;
/* Set the callback for the checkbox inside the main application window */
g_signal_connect(G_OBJECT(button_show_image), "clicked", G_CALLBACK(ihm_showImage), (gpointer) ihm_ImageWin);
g_signal_connect(G_OBJECT(button_show_image2), "clicked", G_CALLBACK(ihm_showImage), (gpointer) ihm_ImageWin);
}
/* 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(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 = 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_stages_output_gtk_funcs;
pipeline.nb_stages++;
pipeline.stages = &stages[0];
nextChannel = ZAP_CHANNEL_HORI;
/* Processing of a pipeline */
if( !ardrone_tool_exit() )
{
PRINT("\n Video stage thread initialisation1\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;
}
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;
}
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;
}
/**
* @fn ardrone_sendata_unpack_all:
* @param sendata_unpacked sendata_unpacked in which to store the sendata.
* @param sendata One packet read from the port SENDATA.
* @param Checksum of sendata
* @brief Disassembles a buffer of received sendata, and dispatches
* it inside 'sendata_unpacked' structure.
*/
C_RESULT ardrone_sendata_unpack_all(sendata_unpacked_t* sendata_unpacked, sendata_t* sendata, uint32_t* cks)
{
C_RESULT res;
sendata_cks_t sendata_cks = { 0 };
sendata_option_t* sendata_option_ptr;
sendata_option_ptr = (sendata_option_t*) &sendata->options[0];
vp_os_memset( sendata_unpacked, 0, sizeof(*sendata_unpacked) );
res = C_OK;
#ifdef DEBUG_SENDATA_C
printf("Received sendatas tags :");
#endif
while( sendata_option_ptr != NULL )
{
// Check if we have a valid option
if( sendata_option_ptr->size == 0 )
{
PRINT("One sendata option (%d) is not a valid option because its size is zero\n", sendata_option_ptr->tag);
sendata_option_ptr = NULL;
res = C_FAIL;
}
else
{
if( sendata_option_ptr->tag <= SENDATA_NUM_TAGS){
#ifdef DEBUG_SENDATA_C
printf("[%d]",sendata_option_ptr->tag);
#endif
sendata_unpacked->last_sendata_refresh |= SENDATA_OPTION_MASK(sendata_option_ptr->tag);
}
switch( sendata_option_ptr->tag )
{
#define SENDATA_OPTION(STRUCTURE,NAME,TAG) \
case TAG: \
sendata_option_ptr = ardrone_sendata_unpack( sendata_option_ptr, sendata_unpacked->NAME); \
break;
#define SENDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) SENDATA_OPTION(STRUCTURE,NAME,TAG)
#define SENDATA_OPTION_CKS(STRUCTURE,NAME,TAG) {}
#include <ardrone_tool/Sendata/sendata_keys.h>
case SENDATA_CKS_TAG:
sendata_option_ptr = ardrone_sendata_unpack( sendata_option_ptr, sendata_cks );
*cks = sendata_cks.cks;
sendata_option_ptr = NULL; // End of structure
break;
default:
PRINT("Tag %d is an unknown sendata option tag\n", (int) sendata_option_ptr->tag);
sendata_option_ptr = (sendata_option_t *)(((uint32_t)sendata_option_ptr) + sendata_option_ptr->size);
break;
}
}
}
#ifdef DEBUG_SENDATA_C
printf("\n");
#endif
return res;
}
