/*---------------------------------------------------------------------------------------------------------------------
---------------------------------------------------------------------------------------------------------------------*/
C_RESULT ardrone_tool_init(int argc, char **argv)
{
C_RESULT res;
//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_tool_input_init();
ardrone_control_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);
ardrone_tool_configure();
// 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;
}
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 ardrone_tool_init(int argc, char **argv)
{
C_RESULT res;
ardrone_at_init( wifi_ardrone_ip, strlen( wifi_ardrone_ip) );
ardrone_timer_reset(&ardrone_tool_timer);
ardrone_control_init();
ardrone_navdata_client_init();
res = ardrone_tool_init_custom(argc, argv);
ardrone_at_open();
START_THREAD(ardrone_control, 0);
ardrone_tool_configure();
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
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;
}
C_RESULT update_gamepad(void)
{
static int32_t x = 0, y = 0;
static bool_t refresh_values = FALSE;
ssize_t res;
static struct js_event js_e_buffer[64];
static int32_t start = 0;
input_state_t* input_state;
static int32_t theta_trim = 0;
static int32_t phi_trim = 0;
static int32_t yaw_trim = 0;
res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
if( !res || (res < 0 && errno == EAGAIN) )
return C_OK;
if( res < 0 )
return C_FAIL;
if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
return C_OK;
// Buffer decomposition in blocs (if the last is incomplete, it's ignored)
int32_t idx = 0;
refresh_values = FALSE;
input_state = ardrone_tool_get_input_state();
for (idx = 0; idx < res / sizeof(struct js_event); idx++)
{
if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
{
break;
}
else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
{
switch( js_e_buffer[idx].number )
{
case PAD_AG :
ardrone_tool_set_ui_pad_ag(js_e_buffer[idx].value);
break;
case PAD_AB :
ardrone_tool_set_ui_pad_ab(js_e_buffer[idx].value);
break;
case PAD_AD :
ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
break;
case PAD_AH :
ardrone_tool_set_ui_pad_ah(js_e_buffer[idx].value);
break;
case PAD_L1 :
if( js_e_buffer[idx].value )
{
yaw_trim = 0;
if( input_state->r2 )
{
yaw_trim = -1;
}
else
{
ardrone_tool_set_ui_pad_l1(1);
}
// ui_pad_yaw_trim( yaw_trim );
}
else
{
yaw_trim = 0;
ardrone_tool_set_ui_pad_l1(0);
// ui_pad_yaw_trim( yaw_trim );
}
break;
case PAD_R1 :
if( js_e_buffer[idx].value )
{
yaw_trim = 0;
if( input_state->r2 )
{
yaw_trim = 1;
}
else
{
ardrone_tool_set_ui_pad_r1(1);
}
// ui_pad_yaw_trim( yaw_trim );
}
else
{
yaw_trim = 0;
ardrone_tool_set_ui_pad_r1(0);
// ui_pad_yaw_trim( yaw_trim );
}
break;
case PAD_L2 :
ardrone_tool_set_ui_pad_l2(js_e_buffer[idx].value);
if( !js_e_buffer[idx].value )
{
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
}
break;
case PAD_R2 :
ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
if( !js_e_buffer[idx].value )
ardrone_at_set_flat_trim();
break;
case PAD_SELECT :
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
break;
case PAD_START :
if( js_e_buffer[idx].value )
{
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
}
break;
default:
break;
}
}
else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
{
refresh_values = TRUE;
switch( js_e_buffer[idx].number )
{
case PAD_X:
x = ( js_e_buffer[idx].value + 1 ) >> 15;
break;
case PAD_Y:
y = ( js_e_buffer[idx].value + 1 ) >> 15;
break;
default:
break;
}
}
else
{// TODO: default: ERROR (non-supported)
}
}
if(refresh_values)// Axis values to refresh
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;
}
