C_RESULT output_gtk_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
/* Get a reference to the last decoded picture */
IplImage *src; //Image Structure
char c=0;
src = cvCreateImageHeader( cvSize(QVGA_WIDTH,QVGA_HEIGHT),8,3 );
vp_os_mutex_lock(&video_update_lock); // Taking Mutex for Video
src->imageData = (uint8_t*)in->buffers[0];
cvCvtColor(src, src, CV_RGB2BGR );
cvShowImage("Image", src);
c = cvWaitKey(20); //Wait Require to show image and take input from keyboard,
vp_os_mutex_lock(&control_video_lock); //Taking Mutex for Image
if( global_video_feed == NULL )
global_video_feed = cvCreateImage( cvSize(QVGA_WIDTH,QVGA_HEIGHT),8,3 );
cvCopy( src,global_video_feed,NULL );
vp_os_mutex_unlock( &control_video_lock ); //Releasing Mutex for Image
vp_os_mutex_unlock( &video_update_lock ); //Releasing Mutex for video
cvReleaseImageHeader(&src);
if( c==27 )
{
end_all_threads = 1;
return( 1 );
}
else if( c == 's' || c == 'S' )
{
vp_os_mutex_lock( &control_data_lock ); // Locking Control Data Lock
if(control_data.start == 1)
control_data.start = 0;
else
control_data.start = 1;
vp_os_mutex_unlock( &control_data_lock );
vp_os_delay(1000);
}
else if( c == 'c' || c == 'C' )
{
if(nextChannel == ZAP_CHANNEL_HORI)
{
nextChannel = ZAP_CHANNEL_VERT;
ardrone_tool_configuration_addevent_video_channel( &nextChannel,NULL );
vp_os_delay(500);
}
else
{
nextChannel = ZAP_CHANNEL_HORI;
ardrone_tool_configuration_addevent_video_channel( &nextChannel,NULL );
vp_os_delay(500);
}
}
//vp_os_mutex_unlock( &control_data_lock );
return (SUCCESS);
}
DEFINE_THREAD_ROUTINE(mobile_main, data)
{
C_RESULT res = C_FAIL;
vp_com_wifi_config_t *config = NULL;
mobile_main_param_t *param = (mobile_main_param_t *)data;
ardroneEngineCallback callback = param->callback;
vp_os_memset(drone_address, 0x0, sizeof(drone_address));
// TODO(johnb): Make this autodetect based on network interfaces
while(((config = (vp_com_wifi_config_t *)wifi_config()) != NULL) && (strcmp(config->itfName, WIFI_ITFNAME) != 0))
{
PRINT("Wait WIFI connection !\n");
vp_os_delay(250);
}
// Get drone_address
vp_os_memcpy(drone_address, config->server, strlen(config->server));
PRINT("Drone address %s\n", drone_address);
// Get iphone_mac_address
get_iphone_mac_address(config->itfName);
PRINT("Iphone MAC Address %s\n", iphone_mac_address);
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
{
START_THREAD(video_stage, NULL);
res = ardrone_tool_init(drone_address, strlen(drone_address), NULL, param->appName, param->usrName);
callback(ARDRONE_ENGINE_INIT_OK);
ardrone_tool_set_refresh_time(1000 / kAPS);
while( SUCCEED(res) && bContinue == TRUE )
{
ardrone_tool_update();
}
JOIN_THREAD(video_stage);
res = ardrone_tool_shutdown();
}
vp_os_free (data);
return (THREAD_RET)res;
}
DEFINE_THREAD_ROUTINE( comm_control, data )
{
int seq_no = 0,prev_start = 0;
control_data_t l_control;
PRINT( "Initilizing Thread 1\n" );
while( end_all_threads == 0 )
{
vp_os_delay(10);
//PRINT("E:%d,S:%d\n\n",end_all_threads,control_data.start );
vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex
l_control = control_data;
vp_os_mutex_unlock( &control_data_lock );
if( prev_start == 0 && l_control.start == 1 ) //To Start Drone
{
ardrone_tool_set_ui_pad_start( 1 );
prev_start = 1;
}
else if( prev_start == 1 && l_control.start == 0 ) //To Stop the Drone
{
ardrone_tool_set_ui_pad_start( 0 );
prev_start = 0;
}
//PRINT("YAW : %f\n\n",l_control.yaw);
ardrone_at_set_progress_cmd( ++seq_no,l_control.roll,l_control.pitch,l_control.gaz,l_control.yaw); //command to make drone move.
//ardrone_at_set_progress_cmd( ++seq_no,0,0,0,-1);
}
PRINT( "Communication Thread Ending\n" );
return C_OK;
}
DEFINE_THREAD_ROUTINE(mobile_main, data)
{
C_RESULT res = C_FAIL;
char drone_address[64];
unsigned long theAddr;
ardroneEngineCallback callback = (ardroneEngineCallback)data;
vp_os_memset(drone_address, 0x0, sizeof(drone_address));
while((theAddr = deviceIPAddress(WIFI_ITFNAME, iphone_mac_address)) == LOCALHOST)
{
PRINT("Wait WIFI connection !\n");
vp_os_delay(250);
}
struct in_addr drone_addr;
drone_addr.s_addr = htonl( ntohl((in_addr_t)theAddr) - 1 );
vp_os_memcpy(drone_address, inet_ntoa(drone_addr), strlen(inet_ntoa(drone_addr)));
PRINT("Drone address %s\n", drone_address);
PRINT("Iphone MAC Address %s\n", iphone_mac_address);
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
{
res = ardrone_tool_init(drone_address, strlen(drone_address), NULL);
callback(ARDRONE_ENGINE_INIT_OK);
if(SUCCEED(res))
{
START_THREAD(video_stage, NULL);
res = ardrone_tool_set_refresh_time(1000 / kAPS);
while( SUCCEED(res) && bContinue == TRUE )
{
res = ardrone_tool_update();
}
JOIN_THREAD(video_stage);
}
res = ardrone_tool_shutdown();
}
return (THREAD_RET)0;
}
PROTO_THREAD_ROUTINE(escaper,nomParams)
{
SDL_Event event;
while(!pipeline_opened)
{
vp_os_delay(100);
}
while(pipeline_opened)
{
vp_os_mutex_lock(&xlib_mutex);
if(SDL_PollEvent(&event))
{
vp_os_mutex_unlock(&xlib_mutex);
switch(event.type)
{
case SDL_KEYDOWN:
case SDL_KEYUP:
{
SDL_KeyboardEvent *kb_event = (SDL_KeyboardEvent *)&event;
switch(kb_event->keysym.sym)
{
case SDLK_ESCAPE:
exit(1);
break;
default:
break;
}
}
default:
break;
}
}
else
{
vp_os_mutex_unlock(&xlib_mutex);
}
}
return (THREAD_RET)0;
}
DEFINE_THREAD_ROUTINE( thread1, data )
{
int seq_no = 0,prev_start = 0;
PRINT( "Initilizing Thread 1\n" );
while(1)
{
vp_os_delay(100);
vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex
if( prev_start == 0 && control_data.start == 1 )
{
ardrone_tool_set_ui_pad_start( 1 );
prev_start = 1;
}
else if( prev_start == 1 && control_data.start == 0 )
{
ardrone_tool_set_ui_pad_start( 0 );
prev_start = 0;
}
ardrone_at_set_progress_cmd( ++seq_no,control_data.roll, control_data.pitch, control_data.gaz, control_data.yaw); //command to make drone move.
vp_os_mutex_unlock( &control_data_lock );
}
}
void vp_os_delay_us(uint32_t us)
{
vp_os_delay(us/1000);
}
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;
}
DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Server,data,ATCODEC_STACK_SIZE)
{
ATcodec_Tree_t *tree = &default_tree;
AT_CODEC_ERROR_CODE res;
int32_t v_loop, v_read, len, nb_cmd = 0;
char buffer[INTERNAL_BUFFER_SIZE]; // user-defined
char global_buffer[INTERNAL_BUFFER_SIZE];
char safety[16]; // Absorbs data overflowing from global_buffer.
int global_len=0;
v_continue = 1;
PRINT("Thread AT Commands Server Start\n");
while(!atcodec_lib_init_ok)
{
vp_os_thread_yield();
}
while(v_continue)
{
vp_os_memset(buffer,0,sizeof(buffer));
vp_os_memset(global_buffer,0,sizeof(global_buffer));
global_len=0;
vp_os_memset(safety,0,sizeof(safety));
// open and init
if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK) {
v_continue = 0;
}
for ( v_loop = 1 ; v_loop && func_ptrs.enable() == AT_CODEC_ENABLE_OK; )
{
v_read = 1;
do
{
// wait so that thread can give the hand : delay user-defined / OS-dependent
// ...
vp_os_delay(ATCODEC_SERVER_YIELD_DELAY);
//vp_os_thread_yield();
/* In case of reading from packets, we clear the incoming buffer.
* Splitting AT commands into several packets would be a bad idea since packet order in not guaranteed in UDP.
*/
if (at_codec_reading_mode==ATCODEC_READ_FROM_PACKETS) {
vp_os_memset(global_buffer,0,sizeof(global_buffer));
global_len=0;
}
/*
* Read some bytes; this function blocks until some data are made
* available by the VP_COM thread.
*/
len = sizeof(buffer); //INTERNAL_BUFFER_SIZE/*/2*/; // user-defined
res = func_ptrs.read((int8_t*)&buffer[0], (int32_t*)&len);
if(res == AT_CODEC_READ_OK)
{
if(len > 0)
{
// process characters and update v_read
// \todo Do not use nb_cmd ?
/* Data are accumulated in the global buffer until at least one '\r' is found. */
if((nb_cmd = append_reception(&buffer[0], len, &global_buffer[0], &global_len,sizeof(global_buffer))) > 0)
{
v_read = 0;
}
else if(nb_cmd == -1) /* no \r found in the global_buffer*/
{
// a buffer overflow occurs
switch(at_codec_reading_mode)
{
case ATCODEC_READ_FROM_STREAM:
PRINT("AT Codec buffer was filled before a full AT commands was received.");
break;
case ATCODEC_READ_FROM_PACKETS:
PRINT("AT Codec received a packet with no complete AT command or buffer was too small to store the whole packet.");
break;
}
//ATCODEC_PRINT("Overflow\n");
/* In case of overflow, a TCP connection should be reinitialized in order to resynchronize
* the client and the server. Otherwise there is no way to find the beginning of the next AT Command.
* For a UDP connection, we assume all packets begin with an AT Command, and we just wait
* for the next packet to arrive.
*/
if (at_codec_reading_mode==ATCODEC_READ_FROM_STREAM) {
v_loop = 0;
}
}
else
{
v_read = 1;
}
}
else
{
if(len < 0)
{
ATCODEC_PRINT("read returns a neg length\n");
v_loop = 0;
}
}
}
else /* if (res == AT_CODEC_READ_OK) */
{
// an error occurred
ATCODEC_PRINT("an error occurs\n");
v_loop = 0;
}
}
while (v_read && v_loop);
// process what has been received if no error occurs
if(v_loop)
{
// ...
if(process_received_data(tree, nb_cmd, &global_buffer[0], &global_len) != ATCODEC_TRUE)
{
ATCODEC_PRINT("process_received returns false\n");
v_loop = 0;
}
}
}/*for*/
// close and un-init : user-defined
if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
v_continue = 0;
}/* while */
if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
{
ATCODEC_PRINT("ATcodec Shutdown error\n");
}
return((THREAD_RET)0);
}
C_RESULT vp_com_serial_open(vp_com_serial_config_t* config, vp_com_connection_t* connection, vp_com_socket_t* socket, Read* read, Write* write)
{
struct termios tio;
speed_t speed;
VP_OS_ASSERT(config->blocking == 0 || config->blocking == 1);
if(config->blocking == 0)
socket->priv = (void *)open(&config->itfName[0], O_RDWR|O_NOCTTY|O_NONBLOCK);
else
socket->priv = (void *)open(&config->itfName[0], O_RDWR|O_NOCTTY);
if(((int)socket->priv) == -1)
{
PRINT("Unable to \"open\" serial device");
return (VP_COM_ERROR);
}
/* get current serial port settings */
if(tcgetattr((int)socket->priv, &tio) != 0)
{
PRINT("Serial device configuration failure (%s)", strerror(errno));
close((int)socket->priv);
return (VP_COM_ERROR);
}
tio_save = tio;
if(config->sync)
{
/* set serial settings */
speed = (speed_t)config->initial_baudrate;
cfsetispeed(&tio, speed);
cfsetospeed(&tio, speed);
cfmakeraw(&tio);
if(tcsetattr((int)socket->priv, TCSANOW, &tio) != 0)
{
PRINT("Serial device configuration failure (%s)", strerror(errno));
close((int)socket->priv);
return (VP_COM_ERROR);
}
if(socket->type == VP_COM_CLIENT)
{
if(FAILED(vp_com_serial_write_sync(config, socket)))
return (VP_COM_ERROR);
vp_os_delay(VP_COM_SYNC_DELAY);
}
else if(socket->type == VP_COM_SERVER)
{
if(FAILED(vp_com_serial_wait_sync(config, socket)))
return (VP_COM_ERROR);
vp_os_delay(VP_COM_SYNC_DELAY);
}
}
/* set serial settings */
speed = (speed_t)config->baudrate;
cfsetispeed(&tio, speed);
cfsetospeed(&tio, speed);
cfmakeraw(&tio);
if(tcsetattr((int)socket->priv, TCSANOW, &tio) != 0)
{
PRINT("Serial device configuration failure (%s)", strerror(errno));
close((int)socket->priv);
return (VP_COM_ERROR);
}
if(read) *read = (Read) vp_com_serial_read;
if(write) *write = (Write) vp_com_serial_write;
return (VP_COM_OK);
}
DEFINE_THREAD_ROUTINE(main_application_thread, data)
{
/* Mutexes for synchronisation */
vp_os_mutex_init(&test_mutex);
vp_os_cond_init(&test_condition,&test_mutex);
vp_os_delay(1000);
//fflush(stdin);
printf("[IMPORTANT] Please check you deleted all configuration files on the drone before starting.\n");
printf("\n Resetting the drone to the default configuration \n");
/* Switch to the default configuration file */
/* Always start by setting the session, then the application, then the user */
set_string(session_id,"00000000");
set_string(application_id,"00000000");
set_string(profile_id,"00000000");
/* Suppress all other settings. A session and an application were automatically created
* at application startup by ARDrone Tool ; we must delete them.
* The '-' symbol means 'delete'.
* '-all' deletes all the existing configurations.
*/
set_string(session_id,"-all");
set_string(application_id,"-all");
set_string(profile_id,"-all");
set_int(navdata_demo,1);
mypause();
/* Ask the drone configuration and compare it to the expected values */
/* Send time to the drone */
//gettimeofday(&tv,NULL);
//set_int(time,(int32_t)tv.tv_sec);
title("\nGetting the drone configuration ...\n");
ARDRONE_TOOL_CONFIGURATION_GET(test_callback); vp_os_cond_wait(&test_condition);
title("Comparing received values to the config_keys.h defaults ...\n");
#define ARDRONE_CONFIG_KEY_IMM_a9(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK,SCOPE) \
test_float(NAME,DEFAULT);
#undef ARDRONE_CONFIG_KEY_REF_a9
#define ARDRONE_CONFIG_KEY_STR_a9(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK,SCOPE) \
test_string(NAME,DEFAULT);
#include <config_keys.h>
/* Modify one value of each category and see what happens */
/* To test the 'COMMON' category */
set_string(ardrone_name,"TEST_CONFIG");
/* To test the 'APPLIS' category */
test_nint(bitrate_ctrl_mode,1); /* 0 is the default value */
set_int(bitrate_ctrl_mode,1);
/* To test the 'PROFILE' (aka. 'USER') category */
test_nfloat(outdoor_euler_angle_max,F1);
set_float(outdoor_euler_angle_max,F1);
/* To test the 'SESSION' category */
test_string(leds_anim,"0,0,0");
set_string(leds_anim,"1,1,1");
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
title("\nGetting the drone configuration ...\n");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG"); /* CAT_COMMON param */
test_int(bitrate_ctrl_mode,1); /* CAT_APPLI param */
test_float(outdoor_euler_angle_max,F1); /* CAT_USER param */
test_string(leds_anim,"1,1,1"); /* CAT_SESSION param */
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
/*---------------------------------------------------------------------------------------------------------*/
/* Create an application configuration file */
title("\nCreating the application ID 11111111 ...\n");
set_string(application_id,"11111111");
set_string(application_desc,APPDESC1);
title("\nGetting the drone configuration ...\n");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG"); /* CAT_COMMON param; its value should not be changed by creating an application config. */
test_int(bitrate_ctrl_mode,0); /* 0 is the default value ; default values are affected to newly created configurations */
test_float(outdoor_euler_angle_max,F1); /* CAT_USER param ; its value should not be changed by creating an application config. */
test_int(detect_type,CAD_TYPE_NONE); /* CAT_SESSION param; its value should not be changed by creating an application config. */
test_string(application_id,"11111111");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc,APPDESC1);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
set_int(bitrate_ctrl_mode,1);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1);
set_int(bitrate_ctrl_mode,0);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,0);
mypause();
title("\nGoing back to the default application configuration ...\n");
set_string(application_id,"00000000");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1); /* 1 was the value of the config.ini configuration */
test_float(outdoor_euler_angle_max,F1);
test_int(detect_type,CAD_TYPE_NONE);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc,DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
mypause();
title(" Going back to the newly created application configuration ...\n");
set_string(application_id,"11111111");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,0);
test_float(outdoor_euler_angle_max,F1);
test_int(detect_type,CAD_TYPE_NONE);
test_string(application_id,"11111111");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , APPDESC1);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
set_string(application_id,"00000000");
mypause();
/*-----------------------------------------------------------------------------------------------*/
/* Create a user profile configuration file */
title("\nCreating the user profile ID 22222222 ...\n");
set_string(profile_id,"22222222");
set_string(profile_desc,PROFDESC2);
title("\nGetting the drone configuration ...\n");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1); /* 1 was the value of the config.ini configuration */
test_float(outdoor_euler_angle_max,default_outdoor_euler_angle_ref_max);
test_int(detect_type,CAD_TYPE_NONE);
test_string(application_id,"00000000");
test_string(profile_id,"22222222");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, PROFDESC2);//DEFAULT_PROFILE_DESC
test_string(session_desc, DEFAULT_SESSION_DESC);
set_float(outdoor_euler_angle_max,F2);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
mypause();
title("\nGoing back to the default user profile configuration ...\n");
set_string(profile_id,"00000000");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
/* The default value for this is 1 after the first test */
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1); /* 1 was the value of the config.ini configuration */
test_float(outdoor_euler_angle_max,F1);
test_int(detect_type,CAD_TYPE_NONE);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
mypause();
title(" Going back to the newly created user profile configuration ...\n");
set_string(profile_id,"22222222");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1); /* 1 was the value of the config.ini configuration */
test_float(outdoor_euler_angle_max,F2);
test_int(detect_type,CAD_TYPE_NONE);
test_string(ardrone_name,"TEST_CONFIG");
test_string(application_id,"00000000");
test_string(profile_id,"22222222");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, PROFDESC2);//DEFAULT_PROFILE_DESC
test_string(session_desc, DEFAULT_SESSION_DESC);
set_string(profile_id,"00000000");
mypause();
/*-----------------------------------------------------------------------------------------------*/
/* Create a session configuration file */
title("\nCreating the session ID 33333333 ...\n");
set_string(session_id,"33333333");
title("\nGetting the drone configuration ...\n");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG"); /* CAT_COMMON param; its value should not be changed by creating a session */
test_int(bitrate_ctrl_mode,1); /* CAT_APPLI param; its value should not be changed by creating a session */
test_float(outdoor_euler_angle_max,F1); /* value in default config.ini */
test_int(detect_type,CAD_TYPE_NONE); /* CAT_SESSION param; its value should be the default one */
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"33333333");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC ); //DEFAULT_SESSION_DESC);
/* Test changing a value in the session */
set_int(detect_type,CAD_TYPE_VISION);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_int(detect_type,CAD_TYPE_VISION);
mypause();
title("\nGoing back to the default session configuration ...\n");
set_string(session_id,"00000000");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1); /* value in default config.ini */
test_float(outdoor_euler_angle_max,F1); /* value in default config.ini */
test_int(detect_type,CAD_TYPE_NONE);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
mypause();
title(" Going back to the newly created session configuration ...\n");
set_string(session_id,"33333333");
set_string(session_desc,SESSDESC3);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1); /* value in default config.ini */
test_float(outdoor_euler_angle_max,F1); /* value in default config.ini */
test_int(detect_type,CAD_TYPE_VISION);
test_string(ardrone_name,"TEST_CONFIG");
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"33333333");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, SESSDESC3 ); //DEFAULT_SESSION_DESC);
set_int(detect_type,CAD_TYPE_COCARDE);
set_string(session_id,"00000000");
mypause();
/*-----------------------------------------------------------------------------------------------*/
/* Create a session configuration file */
title("\nCreating the session ID 44444444 ...\n");
set_string(session_id,"44444444");
title("\nGetting the drone configuration ...\n");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_int(detect_type,CAD_TYPE_NONE);
test_string(ardrone_name,"TEST_CONFIG");
test_int(bitrate_ctrl_mode,1);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"44444444");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC ); //FAULT_SESSION_DESC);
set_int(detect_type,CAD_TYPE_VISION);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_int(detect_type,CAD_TYPE_VISION);
set_string(application_id,"11111111");
set_string(profile_id,"22222222");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG"); set_string(ardrone_name,"TEST_CONFIG2");
test_string(application_id,"11111111");
test_string(profile_id,"22222222");
test_string(session_id,"44444444");
test_string(application_desc , APPDESC1);
test_string(profile_desc, PROFDESC2);
test_string(session_desc, DEFAULT_SESSION_DESC);
mypause();
while(1){
title("\nGoing back to the default session ...\n");
set_string(session_id,"00000000");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG2");
test_int(bitrate_ctrl_mode,1);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
mypause();
title("\nGoing back to the 44444444 session ...\n");
set_string(session_id,"44444444");
set_string(session_desc,SESSDESC4);
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG2");
test_int(bitrate_ctrl_mode,0);
test_string(application_id,"11111111");
test_string(profile_id,"22222222");
test_string(session_id,"44444444");
test_string(application_desc , APPDESC1);
test_string(profile_desc, PROFDESC2);
test_string(session_desc, SESSDESC4);
mypause();
title("\nGoing back to the default session ...\n");
set_string(session_id,"00000000");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG2"); set_string(ardrone_name,"TEST_CONFIG3");
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
mypause();
title(" Going back to the 33333333 session ...\n");
set_string(session_id,"33333333");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG3");
test_int(bitrate_ctrl_mode,1); /* value in default config.ini */
test_float(outdoor_euler_angle_max,F1); /* value in default config.ini */
test_int(detect_type,CAD_TYPE_COCARDE);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"33333333");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, SESSDESC3);
mypause();
title("\nGoing back to the 44444444 session ...\n");
set_string(session_id,"44444444");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG3"); set_string(ardrone_name,"TEST_CONFIG4");
test_int(bitrate_ctrl_mode,0);
test_float(outdoor_euler_angle_max,F2);
test_int(detect_type,CAD_TYPE_VISION);
test_string(application_id,"11111111");
test_string(profile_id,"22222222");
test_string(session_id,"44444444");
test_string(application_desc , APPDESC1);
test_string(profile_desc, PROFDESC2);
test_string(session_desc, SESSDESC4);
mypause();
title(" Going back to the 33333333 session ...\n");
set_string(session_id,"33333333");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_int(bitrate_ctrl_mode,1);
test_int(detect_type,CAD_TYPE_COCARDE);
test_string(ardrone_name,"TEST_CONFIG4");
test_int(bitrate_ctrl_mode,1);
test_float(outdoor_euler_angle_max,F1);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"33333333");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, SESSDESC3);
mypause();
title("\nGoing back to the default session ...\n");
set_string(session_id,"00000000");
{ARDRONE_TOOL_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
test_string(ardrone_name,"TEST_CONFIG4"); /* common parameter whose value should never be reset */
test_int(bitrate_ctrl_mode,1);
test_float(outdoor_euler_angle_max,F1);
test_int(detect_type,CAD_TYPE_NONE);
test_string(application_id,"00000000");
test_string(profile_id,"00000000");
test_string(session_id,"00000000");
test_string(application_desc , DEFAULT_APPLICATION_DESC);
test_string(profile_desc, DEFAULT_PROFILE_DESC);
test_string(session_desc, DEFAULT_SESSION_DESC);
set_string(ardrone_name,"TEST_CONFIG2");
mypause();
/*-----------------------------------------------------------------------------------------------*/
/* Read the list of custom configurations */
title("\nReading the list of custom configuration files ...\n");
{ARDRONE_TOOL_CUSTOM_CONFIGURATION_GET(test_callback);vp_os_cond_wait(&test_condition);}
printf("\n\n ====================================== \n\n");
if(available_configurations[CAT_APPLI].nb_configurations!=1) { printf("Bad number of custom applis.\n"); }
if(available_configurations[CAT_USER].nb_configurations!=1) { printf("Bad number of custom applis.\n"); }
if(available_configurations[CAT_SESSION].nb_configurations!=2) { printf("Bad number of custom applis.\n"); }
if (available_configurations[CAT_APPLI].list[0].id==NULL) { printf("Unexpected custom app 0 NULL\n"); }
else if (strcmp(available_configurations[CAT_APPLI].list[0].id,"11111111")) { printf("Unexpected custom app : <%s>\n",available_configurations[CAT_APPLI].list[0].id); }
if (available_configurations[CAT_USER].list[0].id==NULL) { printf("Unexpected custom user 0 NULL\n"); }
else if (strcmp(available_configurations[CAT_USER].list[0].id,"22222222")) { printf("Unexpected custom profile : <%s>\n",available_configurations[CAT_USER].list[0].id); }
if (available_configurations[CAT_SESSION].list[0].id==NULL) { printf("Unexpected custom session 0 NULL\n"); }
else if (strcmp(available_configurations[CAT_SESSION].list[0].id,"33333333")) { printf("Unexpected custom session 0 : <%s>\n",available_configurations[CAT_SESSION].list[0].id); }
if (available_configurations[CAT_SESSION].list[1].id==NULL) { printf("Unexpected custom session 1 NULL\n"); }
else if (strcmp(available_configurations[CAT_SESSION].list[1].id,"44444444")) { printf("Unexpected custom session 1 : <%s>\n",available_configurations[CAT_SESSION].list[1].id); }
if (available_configurations[CAT_APPLI].list[0].description==NULL) { printf("Unexpected custom appli descrition 0 NULL\n"); }
else if (strcmp(available_configurations[CAT_APPLI].list[0].description,APPDESC1)) { printf("Unexpected custom app description : <%s>\n",available_configurations[CAT_APPLI].list[0].description); }
if (available_configurations[CAT_USER].list[0].description==NULL) { printf("Unexpected custom user 0 description NULL\n"); }
else if (strcmp(available_configurations[CAT_USER].list[0].description,PROFDESC2)) { printf("Unexpected custom profile description : <%s>\n",available_configurations[CAT_USER].list[0].description); }
if (available_configurations[CAT_SESSION].list[0].description==NULL) { printf("Unexpected custom session 0 description NULL\n"); }
else if (strcmp(available_configurations[CAT_SESSION].list[0].description,SESSDESC3)) { printf("Unexpected custom session 0 description : <%s>\n",available_configurations[CAT_SESSION].list[0].description); }
if (available_configurations[CAT_SESSION].list[1].description==NULL) { printf("Unexpected custom session 1 description NULL\n"); }
else if (strcmp(available_configurations[CAT_SESSION].list[1].description,SESSDESC4)) { printf("Unexpected custom session 1 description : <%s>\n",available_configurations[CAT_SESSION].list[1].description); }
printf(" Custom config list checked.\n");
mypause();
/*-----------------------------------------------------------------------------------------------*/
printf("\n\n ====================================== \n\n");
title("\nEnd of the test.\n");
}
}
void mypause()
{
printf("Press a key to continue...\n");
vp_os_delay(1000);
return;
}
DEFINE_THREAD_ROUTINE(academy_upload, data)
{
char *directoryList = NULL;
char *fileList = NULL;
char dirname[ACADEMY_MAX_FILENAME];
_ftp_t *academy_ftp = NULL;
_ftp_status academy_status;
char *ptr = NULL;
academy_upload_t *academy = (academy_upload_t *)data;
printf("Start thread %s\n", __FUNCTION__);
while( academy_upload_started && !ardrone_tool_exit() )
{
vp_os_mutex_lock(&academy->mutex);
vp_os_memset(&academy->user, 0, sizeof(academy_user_t));
academy->callback = &academy_upload_private_callback;
academy->connected = FALSE;
academy_upload_resetState(academy);
vp_os_cond_wait(&academy->cond);
vp_os_mutex_unlock(&academy->mutex);
while(academy_upload_started && academy->connected)
{
academy_status = FTP_FAIL;
academy_upload_nextState(academy);
switch(academy->state.state)
{
case ACADEMY_STATE_CONNECTION:
{
struct dirent *dirent = NULL;
DIR *dir = NULL;
academy_status = FTP_FAIL;
// Check if flight_* directory exist in local dir
if((dir = opendir(flight_dir)) != NULL)
{
struct stat statbuf;
while(FTP_FAILED(academy_status) && (dirent = readdir(dir)) != NULL)
{
if((strncmp(dirent->d_name, "flight_", strlen("flight_")) == 0))
{
char local_dir[ACADEMY_MAX_FILENAME];
sprintf(dirname, "%s", dirent->d_name);
sprintf(local_dir, "%s/%s", flight_dir, dirname);
if((stat(local_dir, &statbuf) == 0) && S_ISDIR(statbuf.st_mode))
academy_status = FTP_SUCCESS;
}
}
closedir(dir);
}
if(FTP_SUCCEDED(academy_status))
{
if(academy_ftp == NULL)
academy_ftp = ftpConnectFromName(ACADEMY_SERVERNAME, ACADEMY_PORT, academy->user.username, academy->user.password, &academy_status);
}
}
break;
case ACADEMY_STATE_PREPARE_PROCESS:
academy_status = ftpCd(academy_ftp, "/Uploaded");
if(FTP_FAILED(academy_status))
{
ftpMkdir(academy_ftp, "/Uploaded");
academy_status = ftpCd(academy_ftp, "/Uploaded");
}
if(FTP_SUCCEDED(academy_status))
{
academy_status = ftpList(academy_ftp, &directoryList, NULL);
if(FTP_SUCCEDED(academy_status))
{
bool_t found = FALSE;
char *next_dir = NULL;
while(!found && (ptr = academy_get_next_item_with_prefix(directoryList, &next_dir, "flight_", TRUE)))
{
if(strcmp(ptr, dirname) == 0)
{
found = TRUE;
academy_upload_setState(academy, ACADEMY_STATE_FINISH_PROCESS);
}
}
if(directoryList != NULL)
{
vp_os_free(directoryList);
directoryList = NULL;
}
}
}
break;
case ACADEMY_STATE_PROCESS:
academy_status = ftpCd(academy_ftp, "/Uploading");
if(FTP_FAILED(academy_status))
{
ftpMkdir(academy_ftp, "/Uploading");
academy_status = ftpCd(academy_ftp, "/Uploading");
}
if(FTP_SUCCEDED(academy_status))
{
ftpMkdir(academy_ftp, dirname);
academy_status = ftpCd(academy_ftp, dirname);
if(FTP_SUCCEDED(academy_status))
{
char local_dir[ACADEMY_MAX_FILENAME];
struct dirent *dirent = NULL;
DIR *dir = NULL;
sprintf(local_dir, "%s/%s", flight_dir, dirname);
if((dir = opendir(local_dir)) != NULL)
{
char local_filename[ACADEMY_MAX_FILENAME];
struct stat statbuf;
while(FTP_SUCCEDED(academy_status) && ((dirent = readdir(dir)) != NULL))
{
if((strncmp(dirent->d_name, "picture_", strlen("picture_")) == 0) || (strncmp(dirent->d_name, "userbox_", strlen("userbox_")) == 0))
{
sprintf(local_filename, "%s/%s", local_dir, dirent->d_name);
if((stat(local_filename, &statbuf) == 0) && !S_ISDIR(statbuf.st_mode))
{
PA_DEBUG("Put %s from local directory to server...", dirent->d_name);
academy_status = ftpPut(academy_ftp, local_filename, dirent->d_name, 1, NULL);
if(FTP_SUCCEDED(academy_status))
PA_DEBUG("OK\n");
else
PA_DEBUG("ERROR\n");
}
}
}
closedir(dir);
}
}
}
break;
case ACADEMY_STATE_FINISH_PROCESS:
{
char local_dir[ACADEMY_MAX_FILENAME];
char src[ACADEMY_MAX_FILENAME];
char dest[ACADEMY_MAX_FILENAME];
sprintf(src, "/Uploading/%s", dirname);
sprintf(dest, "/Uploaded/%s", dirname);
academy_status = ftpRename(academy_ftp, src, dest);
// Penser à supprimer le fichier local
academy_status = FTP_FAIL;
sprintf(local_dir, "%s/%s", flight_dir, dirname);
if(!ftw(local_dir, &academy_remove, 1))
{
rmdir(local_dir);
academy_status = FTP_SUCCESS;
}
}
break;
case ACADEMY_STATE_DISCONNECTION:
if(academy_ftp != NULL)
ftpClose(&academy_ftp);
academy_status = FTP_SUCCESS;
break;
default:
// Nothing to do
break;
}
if(FTP_SUCCEDED(academy_status))
{
PA_DEBUG("continue state from %d to %d\n", academy->state.state, (academy->state.state + 1) % ACADEMY_STATE_MAX);
academy_upload_stateOK(academy);
}
else
{
PA_DEBUG("stop\n");
academy_upload_stateERROR(academy);
if(fileList != NULL)
{
vp_os_free(fileList);
fileList = NULL;
}
if(academy_ftp)
ftpClose(&academy_ftp);
vp_os_delay(1000);
}
}
if(fileList != NULL)
{
vp_os_free(fileList);
fileList = NULL;
}
if(academy_ftp)
ftpClose(&academy_ftp);
} // while
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;
}
DEFINE_THREAD_ROUTINE( control_system, data )
{
IplImage *current_frame, *color_pos, *src_hsi,*Connection;
control_data_t l_control, prev_control;
structure_COM CntrMass, Prev_COM;
current_frame = cvCreateImage( cvSize(QVGA_WIDTH,QVGA_HEIGHT),8,3 );
color_pos = cvCreateImage( cvSize(QVGA_WIDTH,QVGA_HEIGHT),8,1 );
Connection = cvCreateImage(cvGetSize(color_pos),8,1);
Prev_COM.n = -1;
PRINT("STARTING CONTROL SYSTEM \n");
while( end_all_threads==0 )
{
vp_os_mutex_lock( &control_video_lock );
if( global_video_feed==NULL )
{
vp_os_delay(20);
vp_os_mutex_unlock( &control_video_lock );
continue;
}
cvCopy( global_video_feed,current_frame,NULL );
vp_os_mutex_unlock( &control_video_lock );
vp_os_mutex_lock( &control_data_lock );
prev_control = control_data;
vp_os_mutex_unlock( &control_data_lock );
#ifdef Runtime
cvZero( color_pos );
BackProjectHSI( current_frame,color_pos,cvScalar( COLOR2DETECT ), 20 ); // RED: 175,220,160,0
cvMorphologyEx( color_pos,color_pos,NULL,NULL,CV_MOP_OPEN,1 );
CntrMass = calcCenterOfMass( color_pos );
if( Prev_COM.n == -1 )
{
if( CntrMass.n > Pix_Count_thresh )
Prev_COM = CntrMass;
}
else if( CntrMass.n>Pix_Count_thresh )
{
uchar *ptr, *ptr_src;
ptr_src = (uchar *)color_pos->imageData + Prev_COM.y * color_pos->widthStep + Prev_COM.x * color_pos->nChannels;
if( *ptr_src > 127 )
{
CalcConnectedComponents( color_pos, Connection, 5 );
ptr = (uchar *)Connection->imageData + Prev_COM.y * Connection->widthStep + Prev_COM.x * Connection->nChannels;
RemoveAllComponentExcept( Connection,color_pos,*ptr );
CntrMass = calcCenterOfMass( color_pos );
Prev_COM = CntrMass;
}
else
Prev_COM.n = -1;
//CalcConnectedComponents( color_pos, Connection, 5 );
//ptr = (uchar *)Connection->imageData + Prev_COM.y * Connection->widthStep + Prev_COM.x * Connection->nChannels;
//RemoveAllComponentExcept( Connection,color_pos,*ptr );
//CntrMass = calcCenterOfMass( color_pos );
//Prev_COM = CntrMass;
}
else
Prev_COM.n = -1;
MakeControlZero( &l_control );
if( CntrMass.n>Pix_Count_thresh )
{
CvMemStorage* storage = cvCreateMemStorage(0);
float *p;
cvCircle( current_frame, cvPoint(CntrMass.x,CntrMass.y), 5, CV_RGB(255,255,255), 1, 8, 0 );
l_control.yaw = (float)(CntrMass.x-(QVGA_WIDTH/2))/(QVGA_WIDTH/2);
l_control.gaz = -(float)(CntrMass.y-(QVGA_HEIGHT/2))/(QVGA_HEIGHT/2);
if( CntrMass.n > 700 && CntrMass.n < 20000 )
{
if(CntrMass.n > 10000 && CntrMass.n<50000)
{
CntrMass.n = 10000;
//l_control.pitch = -0.33;
}
//else
//l_control.pitch = (float)(( (float)CntrMass.n - (float)5000 )/20000);
}
PRINT( "number of Pixels: %f\n\n",l_control.pitch );
/*cvCanny( color_pos, color_pos, 10, 100, 3 );
CvSeq* circles =cvHoughCircles(color_pos, storage, CV_HOUGH_GRADIENT, 1, 30, 100, 10, 5, 25 );
if( circles->total > 0 )
{
p = (float *)cvGetSeqElem( circles, 0 );
cvCircle( color_pos, cvPoint((int) p[0],(int) p[1]), (int) p[2], cvScalar(255,0,0,0), 1, 8, 0 );
l_control.pitch = ( p[2]-15 )/20;
//PRINT( "Radius: %f\n\n",p[2] );
}
cvShowImage("Image", color_pos);
cvWaitKey(20);*/
}
//cvShowImage("Image", current_frame);
//cvWaitKey(20);
//cvShowImage( "BackProjection",color_pos );
//cvWaitKey(20);
#endif
#ifdef Calc
{
uchar *ptr_src;
CvScalar current_sclr;
src_hsi = cvCreateImage( cvGetSize(current_frame),current_frame->depth,3 );
cvCvtColor( current_frame,src_hsi,CV_BGR2HSV ); // Red Ball:170,200,130
//cvShowImage("Image", current_frame);
//cvShowImage( "BackProjection",src_hsi );
//cvWaitKey(30);
ptr_src = (uchar *)src_hsi->imageData + (QVGA_HEIGHT/4) * current_frame->widthStep + (QVGA_WIDTH/4) * current_frame->nChannels;
current_sclr = cvScalar( ptr_src[0],ptr_src[1],ptr_src[2],0 );
PRINT( "Pix :- %d,%d,%d\n",ptr_src[0],ptr_src[1],ptr_src[2] );
//if( calcDist( current_sclr,cvScalar(170,200,130,0) )<100 )
// PRINT("CENTER EQUAL\n\n");
//l_control.yaw = -1;
}
#endif
#ifdef Runtime
vp_os_mutex_lock( &control_data_lock );
//control_data.yaw = -1;
control_data = GradualChangeOfControl( l_control,prev_control,20 );
//PRINT( "YAW: %f\n\n",control_data.yaw );
vp_os_mutex_unlock( &control_data_lock );
#endif
vp_os_delay(10);
}
cvReleaseImageHeader(¤t_frame);
cvReleaseImage(&Connection);
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;
}
