C_RESULT vp_com_serial_init(void)
{
vp_os_mutex_init(&wait_sync_mutex);
vp_os_mutex_init(&write_sync_mutex);
return VP_COM_OK;
}
bool_t dct_init(void)
{
vp_os_mutex_init(&dct_start_mutex);
vp_os_cond_init(&dct_start_cond, &dct_start_mutex);
vp_os_mutex_init(&critical_section);
current_io_buffer = NULL;
result_io_buffer = NULL;
return TRUE;
}
static void atcodec_init( AT_CODEC_FUNCTIONS_PTRS *funcs )
{
if( funcs != NULL)
{
memcpy(&func_ptrs, funcs, sizeof(*funcs));
}
#if defined (_MSC_VER)
AT_CODEC_FUNCTIONS_PTRS ptrs =
{
/*init*/host_init,
/*shutdown*/host_shutdown,
/*enable*/host_enable,
/*open*/host_open,
/*close*/host_close,
/*write*/host_write,
/*read*/host_read
};
#else
AT_CODEC_FUNCTIONS_PTRS ptrs =
{
.init = host_init,
.shutdown = host_shutdown,
.enable = host_enable,
.open = host_open,
.close = host_close,
.read = host_read,
.write = host_write,
};
#endif
vp_os_mutex_init(&at_mutex);
ATcodec_Init_Library( &ptrs );
}
void
ATcodec_Init_Library_Tree (ATcodec_Tree_t *tree, AT_CODEC_FUNCTIONS_PTRS *funcs)
{
VP_OS_ASSERT(funcs);
VP_OS_ASSERT(funcs->open);
VP_OS_ASSERT(funcs->read);
VP_OS_ASSERT(funcs->enable);
VP_OS_ASSERT(funcs->write);
VP_OS_ASSERT(funcs->close);
VP_OS_ASSERT(funcs->init);
VP_OS_ASSERT(funcs->shutdown);
ATcodec_Tree_init(tree, sizeof(ATcodec_Message_Data_t), 1);
memcpy(&func_ptrs, funcs, sizeof(*funcs));
vp_os_mutex_init(&ATcodec_cond_mutex);
vp_os_cond_init(&ATcodec_wait_cond, &ATcodec_cond_mutex);
if(func_ptrs.init() != AT_CODEC_INIT_OK)
{
ATCODEC_PRINT("ATcodec Init error\n");
}
else
{
ATcodec_Tree_print(tree);
atcodec_lib_init_ok = 1;
ATcodec_Message_len = 0;
}
}
C_RESULT vp_com_init_server(void)
{
server_init_not_finished = TRUE;
vp_os_mutex_init(&server_initialisation_mutex);
vp_os_cond_init(&server_initialisation_wait, &server_initialisation_mutex);
return C_OK;
}
C_RESULT ardrone_control_init(void)
{
COM_CONFIG_SOCKET_CONTROL(&control_socket, VP_COM_CLIENT, CONTROL_PORT, wifi_ardrone_ip);
control_waited = FALSE;
ardrone_state = 0;
vp_os_mutex_init(&control_mutex);
vp_os_cond_init(&control_cond, &control_mutex);
vp_os_mutex_init(&event_queue_mutex);
start_index_in_queue = 0;
end_index_in_queue = (start_index_in_queue - 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
return C_OK;
}
TClientList *clientList_create(int32_t maxClients) {
TClientList *cl = vp_os_malloc(sizeof(TClientList));
cl->maxClients = maxClients;
cl->numClients = 0;
cl->clients = vp_os_malloc(maxClients * sizeof(TClient));
clientList_removeAllClients(cl);
vp_os_mutex_init(&cl->mutex);
return cl;
}
inline C_RESULT navdata_init( void* data )
{
LOGD(TAG, "navdata_init");
vp_os_mutex_init(&instance_navdata_mutex);
vp_os_mutex_lock( &instance_navdata_mutex);
navdata_reset(&inst_nav);
bIsInitialized = TRUE;
vp_os_mutex_unlock( &instance_navdata_mutex);
return C_OK;
}
void ardrone_tool_init_timers_and_mutex()
{
// Initalize mutex and condition
vp_os_mutex_init(&ardrone_tool_mutex);
ardrone_tool_in_pause = FALSE;
// Init subsystems
ardrone_timer_reset(&ardrone_tool_timer);
ardrone_timer_update(&ardrone_tool_timer);
}
C_RESULT ardrone_tool_init( const char* ardrone_ip, size_t n, AT_CODEC_FUNCTIONS_PTRS *ptrs, const char *appname, const char *usrname)
{
// Initalize mutex and condition
vp_os_mutex_init(&ardrone_tool_mutex);
ardrone_tool_in_pause = FALSE;
// Initialize ardrone_control_config structures;
ardrone_tool_reset_configuration();
// ardrone_control_config_default initialisation. Sould not be modified after that !
vp_os_memcpy ((void *)&ardrone_control_config_default, (const void *)&ardrone_control_config, sizeof (ardrone_control_config_default));
// initialization of application defined default values
vp_os_memcpy ((void *)&ardrone_application_default_config, (const void *)&ardrone_control_config, sizeof (ardrone_application_default_config));
//Fill structure AT codec and built the library AT commands.
if( ptrs != NULL )
ardrone_at_init_with_funcs( ardrone_ip, n, ptrs );
else
ardrone_at_init( ardrone_ip, n );
// Save appname/appid for reconnections
if (NULL != appname)
{
ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
}
// Save usrname/usrid for reconnections
if (NULL != usrname)
{
ardrone_gen_usrid (usrname, usr_id, usr_name, sizeof (usr_name));
}
// Create pseudorandom session id
ardrone_gen_sessionid (ses_id, ses_name, sizeof (ses_name));
// Init subsystems
ardrone_timer_reset(&ardrone_tool_timer);
ardrone_timer_update(&ardrone_tool_timer);
ardrone_tool_input_init();
ardrone_control_init();
ardrone_tool_configuration_init();
ardrone_navdata_client_init();
//Opens a connection to AT port.
ardrone_at_open();
START_THREAD(navdata_update, 0);
START_THREAD(ardrone_control, 0);
// Send start up configuration
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
return C_OK;
}
C_RESULT overlay_stage_open(vlib_stage_decoding_config_t *cfg)
{
vp_os_mutex_init( &overlay_video_config.mutex );
// vp_os_mutex_lock( &overlay_video_config.mutex );
// vp_os_mutex_unlock( &overlay_video_config.mutex );
initFpsCounter();
return C_OK;
}
C_RESULT video_stage_open(vlib_stage_decoding_config_t *cfg)
{
vp_os_mutex_init( &video_stage_config.mutex );
vp_os_mutex_lock( &video_stage_config.mutex );
video_stage_config.data = vp_os_malloc(512 * 512 * 4);
vp_os_memset(video_stage_config.data, 0x0, 512 * 512 * 4);
vp_os_mutex_unlock( &video_stage_config.mutex );
return C_OK;
}
void academy_upload_init(void)
{
if(!academy_upload_started)
{
vp_os_mutex_init(&academy_upload.mutex);
vp_os_cond_init(&academy_upload.cond, &academy_upload.mutex);
academy_upload.connected = FALSE;
vp_os_memset(&academy_upload.user, 0, sizeof(academy_user_t));
academy_upload_started = TRUE;
vp_os_thread_create(thread_academy_upload, (THREAD_PARAMS)&academy_upload, &academy_upload_thread);
}
}
static inline C_RESULT navdata_init( void* data )
{
vp_os_mutex_init( &inst_nav_mutex );
vp_os_mutex_lock( &inst_nav_mutex);
navdata_reset(&inst_nav);
vp_os_mutex_unlock( &inst_nav_mutex);
writeToFile = FALSE;
return C_OK;
}
C_RESULT ardrone_tool_init_custom(int argc, char **argv)
{
/* Registering for a new device of game controller */
/* Start all threads of your application */
//control_data = (control_data_t *) malloc (sizeof(control_data_t);
vp_os_mutex_init( &control_data_lock );
START_THREAD( video_stage, NULL );
START_THREAD( ardrone_control, NULL );
START_THREAD( thread1, NULL );
return C_OK;
}
C_RESULT ardrone_navdata_client_init(void)
{
C_RESULT res;
COM_CONFIG_SOCKET_NAVDATA(&navdata_socket, VP_COM_CLIENT, NAVDATA_PORT, wifi_ardrone_ip);
navdata_socket.protocol = VP_COM_UDP;
navdata_socket.is_multicast = 1; // enable multicast for Navdata
navdata_socket.multicast_base_addr = MULTICAST_BASE_ADDR;
vp_os_mutex_init(&navdata_client_mutex);
vp_os_cond_init(&navdata_client_condition, &navdata_client_mutex);
res = C_OK;
return res;
}
C_RESULT video_stage_encoded_recorder_open(video_stage_encoded_recorder_config_t *cfg)
{
if (0 == cfg->fps)
{
cfg->fps = 30;
}
cfg->startRec=VIDEO_ENCODED_STOPPED;
cfg->lastStreamId = UINT16_MAX;
cfg->currentStreamId = UINT16_MAX;
cfg->video = NULL;
vp_os_mutex_init (&cfg->videoMutex);
ardrone_video_remove_all (VIDEO_ENCODED_FILE_DEFAULT_PATH);
return C_OK;
}
static void init_thread(void) {
struct sigaction sigsuspend, sigresume;
vp_os_mutex_init(&thread_mutex);
sigresume.sa_flags = 0;
sigsuspend.sa_flags = 0;
sigemptyset(&sigresume.sa_mask);
sigemptyset(&sigsuspend.sa_mask);
sigaddset(&sigsuspend.sa_mask, SIGSUSPEND);
sigaddset(&sigresume.sa_mask, SIGRESUME);
sigresume.sa_handler = resumeSignalHandler;
sigsuspend.sa_handler = suspendSignalHandler;
sigaction(SIGRESUME,&sigresume,NULL);
sigaction(SIGSUSPEND,&sigsuspend,NULL);
}
C_RESULT
vp_stages_output_sdl_stage_open(vp_stages_output_sdl_config_t *cfg)
{
vp_os_mutex_init(&xlib_mutex);
if(SDL_Init(SDL_INIT_TIMER|SDL_INIT_VIDEO))
{
PRINT("Error initializing SDL\n");
return (VP_FAILURE);
}
#if defined(_CK4215_) && defined(WIN32)
child_windows = NULL;
exit_pipeline = FALSE;
#ifdef _TEST_CK4215_
show_hide = TRUE;//show
#endif
#endif
return (VP_SUCCESS);
}
/*--------------------------------------------------------------------
The delegate object calls this method during initialization of an
ARDrone application
--------------------------------------------------------------------*/
C_RESULT ardrone_tool_init_custom(int argc, char **argv)
{
/* Change the console title */
vp_os_mutex_init(&consoleMutex);
system("cls");
SetConsoleTitle(TEXT("Parrot A.R. Drone SDK Demo for Windows"));
//CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)&HmiStart,NULL,0,0);
//CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)&HmiStart2,NULL,0,0);
/* Registering for a new device of game controller */
ardrone_tool_input_add( &dx_keyboard );
ardrone_tool_input_add( &dx_gamepad );
/* Start all threads of your application */
START_THREAD( directx_renderer_thread , NULL);
START_THREAD( video_stage, NULL );
return C_OK;
}
C_RESULT video_com_stage_register (video_com_config_t *cfg)
{
static int runOnce = 1;
if (1 == runOnce)
{
vp_os_mutex_init (®isterMutex);
runOnce = 0;
}
vp_os_mutex_lock (®isterMutex);
nbRegisteredStages++;
registeredStages = vp_os_realloc (registeredStages, nbRegisteredStages * sizeof (video_com_config_t *));
C_RESULT retVal = C_FAIL;
if (NULL != registeredStages)
{
registeredStages[nbRegisteredStages - 1] = cfg;
retVal = C_OK;
}
vp_os_mutex_unlock (®isterMutex);
return retVal;
}
C_RESULT ardrone_academy_navdata_init(void* data)
{
navdata_state.wasEmergency = FALSE;
navdata_state.needSetEmergency = FALSE;
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
navdata_state.record_state = TAKEOFF_STATE_IDLE;
navdata_state.userbox_state = USERBOX_STATE_STOPPED;
navdata_state.isTakeOff = FALSE;
navdata_state.isEmergency = FALSE;
navdata_state.cameraIsReady = FALSE;
navdata_state.usbIsReady = FALSE;
navdata_state.usbRecordInProgress = FALSE;
navdata_state.userbox_time = (time_t)0;
navdata_state.takeoff_time = (time_t)0;
navdata_state.usbFreeTime = 0;
navdata_state.droneStoppedUsbRecording = FALSE;
navdata_state.takeOffWasCancelled = FALSE;
navdata_state.internalRecordInProgress = FALSE;
navdata_state.lastState = 0;
vp_os_mutex_init (&navdata_state.aan_mutex);
return C_OK;
}
C_RESULT videoServer_init() {
videoServer_clientList = clientList_create(VIDEOSERVER_MAXCLIENTS);
/* Build frame packet buffer */
videoServer_framePacketLength = sizeof(VideoFrameHeader) + videoServer_frameWidth * videoServer_frameHeight * (videoServer_frameBpp / 8);
frameBuffer = vp_os_malloc(videoServer_framePacketLength);
vp_os_mutex_init(&frameBufferMutex);
vp_os_cond_init(&frameBufferCond, &frameBufferMutex);
/* Create server socket */
videoServerSocket.type = VP_COM_SERVER;
videoServerSocket.protocol = VP_COM_TCP;
videoServerSocket.block = VP_COM_DONTWAIT;
videoServerSocket.is_multicast = 0;
videoServerSocket.port = VIDEO_SERVER_PORT;
/* if (FAILED(vp_com_open(&globalCom, &videoServerSocket, NULL, NULL))) {
vp_os_cond_destroy(&frameBufferCond);
vp_os_mutex_destroy(&frameBufferMutex);
vp_os_free(frameBuffer);
vp_os_mutex_destroy(&clientListMutex);
PRINT("[VideoServer] Unable to open server socket\n");
return C_FAIL;
}*/
/* We create the server socket manually, as we need to set the reuse flag before binding it and Parrot's SDK doesn't allow that */
bool_t error = TRUE;
videoServerSocket.priv = (void *)socket(AF_INET, SOCK_STREAM, 0);
if ((int)videoServerSocket.priv >= 0) {
struct sockaddr_in serverAddress;
/* Try reusing the address */
int s = 1;
setsockopt((int)videoServerSocket.priv, SOL_SOCKET, SO_REUSEADDR, &s, sizeof(int));
/* Bind to address and port */
bzero((char *)&serverAddress, sizeof(serverAddress));
serverAddress.sin_family = AF_INET;
serverAddress.sin_addr.s_addr = INADDR_ANY;
serverAddress.sin_port = htons(videoServerSocket.port);
if (bind((int)videoServerSocket.priv, (struct sockaddr *) &serverAddress, sizeof(struct sockaddr_in)) >= 0)
{
error = FALSE;
} else close((int)videoServerSocket.priv);
}
if (error) {
vp_os_cond_destroy(&frameBufferCond);
vp_os_mutex_destroy(&frameBufferMutex);
vp_os_free(frameBuffer);
if (videoServer_clientList != NULL) {
clientList_destroy(videoServer_clientList);
videoServer_clientList = NULL;
}
PRINT("[VideoServer] Unable to open server socket\n");
return C_FAIL;
}
/* Set server socket timeout */
struct timeval tm;
tm.tv_sec = 0;
tm.tv_usec = ACCEPT_TIMEOUT * 1e6;
setsockopt((int32_t)videoServerSocket.priv, SOL_SOCKET, SO_RCVTIMEO, &tm, sizeof(tm));
videoTranscoder_init();
videoServerStarted = TRUE;
vp_os_mutex_init(&settingsMutex);
return C_OK;
}
C_RESULT ardrone_tool_init_custom(void)
{
should_exit = 0;
vp_os_mutex_init(&navdata_lock);
vp_os_mutex_init(&video_lock);
vp_os_mutex_init(&twist_lock);
//jdeDriver = new ARDroneDriver();
int _w, _h;
if (IS_ARDRONE2)
{
ardrone_application_default_config.video_codec = H264_360P_CODEC;
_w = D2_STREAM_WIDTH;
_h = D2_STREAM_HEIGHT;
}
else if (IS_ARDRONE1)
{
ardrone_application_default_config.video_codec = UVLC_CODEC;
_w = D1_STREAM_WIDTH;
_h = D1_STREAM_HEIGHT;
}
else
{
printf("Something must be really wrong with the SDK!");
}
realtime_navdata=false;
realtime_video=false;
looprate=LOOPRATE;
// SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
// THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
ardrone_application_default_config.bitrate_ctrl_mode = VBC_MODE_DISABLED;
if (IS_ARDRONE2)
{
ardrone_application_default_config.max_bitrate = 4000;
}
ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK;
ardrone_application_default_config.video_channel = ZAP_CHANNEL_HORI;
ardrone_application_default_config.control_level = (0 << CONTROL_LEVEL_COMBINED_YAW);
ardrone_application_default_config.flying_mode = FLYING_MODE_FREE_FLIGHT;
jdeDriver->configureDrone((char*)CONFIG_FILE_PATH);
cam_state=jdeDriver->getParameter("video_channel",0);
char buffer[MULTICONFIG_ID_SIZE+1];
sprintf(buffer,"-%s",usr_id);
printf("Deleting Profile %s\n",buffer);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_id, buffer, NULL);
sprintf(buffer,"-%s",app_id);
printf("Deleting Application %s\n",buffer);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_id, buffer, NULL);
// Now continue with the rest of the initialization
ardrone_tool_input_add(&teleop);
uint8_t post_stages_index = 0;
//Alloc structs
specific_parameters_t * params = (specific_parameters_t *)vp_os_calloc(1,sizeof(specific_parameters_t));
specific_stages_t * driver_pre_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
specific_stages_t * driver_post_stages = (specific_stages_t*)vp_os_calloc(1, sizeof(specific_stages_t));
vp_api_picture_t * in_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
vp_api_picture_t * out_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
in_picture->width = _w;
in_picture->height = _h;
out_picture->framerate = 20;
out_picture->format = PIX_FMT_RGB24;
out_picture->width = _w;
out_picture->height = _h;
out_picture->y_buf = (uint8_t*) vp_os_malloc( _w * _h * 3 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = _w * 3;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
//Alloc the lists
driver_pre_stages->stages_list = NULL;
driver_post_stages->stages_list = (vp_api_io_stage_t*)vp_os_calloc(NB_DRIVER_POST_STAGES,sizeof(vp_api_io_stage_t));
driver_post_stages->stages_list[post_stages_index].name = "ExtractData";
driver_post_stages->stages_list[post_stages_index].type = VP_API_OUTPUT_SDL;
driver_post_stages->stages_list[post_stages_index].cfg = NULL;
driver_post_stages->stages_list[post_stages_index++].funcs = vp_stages_export_funcs;
driver_pre_stages->length = 0;
driver_post_stages->length = post_stages_index;
params->in_pic = in_picture;
params->out_pic = out_picture;
params->pre_processing_stages_list = driver_pre_stages;
params->post_processing_stages_list = driver_post_stages;
params->needSetPriority = 1;
params->priority = 31;
// Using the provided threaded pipeline implementation from SDK
START_THREAD(video_stage, params);
video_stage_init();
if (ARDRONE_VERSION() >= 2)
{
START_THREAD (video_recorder, NULL);
video_recorder_init ();
video_recorder_resume_thread();
}
// Threads do not start automatically
video_stage_resume_thread();
ardrone_tool_set_refresh_time(25);
//jdeDriver->configure_drone();
jdeDriver->initInterfaces();
START_THREAD(update_jde, jdeDriver);
return C_OK;
}
/*
\brief Initialization of the video rendering stage.
*/
C_RESULT output_rendering_device_stage_open( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_init(&video_update_lock);
return (VP_SUCCESS);
}
void video_recorder_init (void)
{
vp_os_mutex_init (&video_recorder_mutex);
vp_os_cond_init (&video_recorder_condition, &video_recorder_mutex);
isInit = TRUE;
}
void video_stage_init(void)
{
vp_os_mutex_init(&video_stage_mutex);
vp_os_cond_init(&video_stage_condition, &video_stage_mutex);
}
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");
}
}
C_RESULT ardrone_tool_init( const char* ardrone_ip, size_t n, AT_CODEC_FUNCTIONS_PTRS *ptrs, const char *appname, const char *usrname, const char *rootdir, const char *flightdir, int flight_storing_size, academy_download_new_media academy_download_new_media_func)
{
C_RESULT res;
// Initalize mutex and condition
vp_os_mutex_init(&ardrone_tool_mutex);
ardrone_tool_in_pause = FALSE;
// Initialize ardrone_control_config structures;
ardrone_tool_reset_configuration();
// ardrone_control_config_default initialisation. Sould not be modified after that !
vp_os_memcpy ((void *)&ardrone_control_config_default, (const void *)&ardrone_control_config, sizeof (ardrone_control_config_default));
// initialization of application defined default values
vp_os_memcpy ((void *)&ardrone_application_default_config, (const void *)&ardrone_control_config, sizeof (ardrone_application_default_config));
// Save appname/appid for reconnections
if (NULL != appname)
{
ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
}
// Save usrname/usrid for reconnections
if (NULL != usrname)
{
ardrone_gen_usrid (usrname, usr_id, usr_name, sizeof (usr_name));
}
// Create pseudorandom session id
ardrone_gen_sessionid (ses_id, ses_name, sizeof (ses_name));
// copy root directory
if((rootdir != NULL) && strlen(rootdir) < ROOT_NAME_SIZE)
strcpy(root_dir, rootdir);
//Fill structure AT codec and built the library AT commands.
if( ptrs != NULL )
ardrone_at_init_with_funcs( ardrone_ip, n, ptrs );
else
ardrone_at_init( ardrone_ip, n );
// Init subsystems
ardrone_timer_reset(&ardrone_tool_timer);
ardrone_timer_update(&ardrone_tool_timer);
ardrone_tool_input_init();
ardrone_control_init();
ardrone_tool_configuration_init();
ardrone_navdata_client_init();
PRINT("Before usbdata_client_init\n");
ardrone_usbdata_client_init(); //add Alex
// Init custom tool
res = ardrone_tool_init_custom();
//Opens a connection to AT port.
ardrone_at_open();
START_THREAD(navdata_update, 0);
START_THREAD(usbdata_update, 0);
START_THREAD(ardrone_control, 0);
// Send start up configuration
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
academy_download_init(academy_download_new_media_func);
academy_init(flightdir, flight_storing_size);
return res;
}
C_RESULT ardrone_tool_init(int argc, char **argv)
{
C_RESULT res;
int32_t b_value = FALSE;
// Initalize mutex and condition
vp_os_mutex_init(&ardrone_tool_mutex);
ardrone_tool_in_pause = FALSE;
// Initialize ardrone_control_config structures;
ardrone_tool_reset_configuration();
// ardrone_control_config_default initialisation. Sould not be modified after that !
vp_os_memcpy ((void *)&ardrone_control_config_default, (const void *)&ardrone_control_config, sizeof (ardrone_control_config_default));
// initialization of application defined default values
vp_os_memcpy ((void *)&ardrone_application_default_config, (const void *)&ardrone_control_config, sizeof (ardrone_application_default_config));
ardrone_application_default_config.navdata_demo = b_value;
// Save appname/appid for reconnections
if (NULL != argv[0])
{
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));
}
// Create pseudorandom session id
ardrone_gen_sessionid (ses_id, ses_name, sizeof (ses_name));
//Fill structure AT codec and built the library AT commands.
ardrone_at_init( wifi_ardrone_ip, strlen( wifi_ardrone_ip) );
// Init subsystems
ardrone_timer_reset(&ardrone_tool_timer);
ardrone_timer_update(&ardrone_tool_timer);
ardrone_tool_input_init();
ardrone_control_init();
ardrone_tool_configuration_init();
ardrone_navdata_client_init();
// Init custom tool
res = ardrone_tool_init_custom(argc, argv);
//Opens a connection to AT port.
ardrone_at_open();
START_THREAD(navdata_update, 0);
START_THREAD(ardrone_control, 0);
// Send start up configuration
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
return res;
}
