C_RESULT ardrone_tool_setup_com( const char* ssid )
{
LOGII("not in lib");
C_RESULT res = C_OK;
#ifdef CHECK_WIFI_CONFIG
if( FAILED(vp_com_init(COM_NAVDATA())) )
{
DEBUG_PRINT_SDK("VP_Com : Failed to init com for navdata\n");
vp_com_shutdown(COM_NAVDATA());
res = C_FAIL;
}
vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
if( SUCCEED(res) && FAILED(vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA())) )
{
DEBUG_PRINT_SDK("VP_Com : Failed to configure com for navdata\n");
vp_com_shutdown(COM_NAVDATA());
res = C_FAIL;
}
if( ssid != NULL )
{
strncpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid, VP_COM_NAME_MAXSIZE-1 );
((vp_com_wifi_connection_t*)wifi_connection())->networkName[VP_COM_NAME_MAXSIZE-1]='\0';
}
if( SUCCEED(res) && FAILED(vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS)))
{
DEBUG_PRINT_SDK("VP_Com: Failed to connect for navdata\n");
vp_com_shutdown(COM_NAVDATA());
res = C_FAIL;
}
#else
vp_com_init(COM_NAVDATA());
vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
if( ssid != NULL )
{
strncpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid, VP_COM_NAME_MAXSIZE-1 );
((vp_com_wifi_connection_t*)wifi_connection())->networkName[VP_COM_NAME_MAXSIZE-1]='\0';
}
vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
#endif
return res;
}
C_RESULT ardrone_tool_setup_com( const char* ssid )
{
C_RESULT res = C_OK;
vp_com_init(COM_NAVDATA());
vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
return res;
}
C_RESULT
vp_stages_output_com_stage_open(vp_stages_output_com_config_t *cfg)
{
C_RESULT res;
res = vp_com_init(cfg->com);
if( VP_SUCCEEDED(res) )
{
vp_com_local_config(cfg->com, cfg->config);
if(cfg->connection && !cfg->connection->is_up)
{
res = vp_com_connect(cfg->com, cfg->connection, 1);
}
}
if( VP_SUCCEEDED(res) && VP_FAILED(vp_com_open(cfg->com, &cfg->socket, 0, &cfg->write)))
res = C_FAIL;
if( VP_SUCCEEDED(res) )
{
if(cfg->socket.type == VP_COM_SERVER)
{
res = vp_com_wait_connections(cfg->com, &cfg->socket, &cfg->socket_client, 1);
}
else
{
vp_os_memcpy(&cfg->socket_client, &cfg->socket, sizeof(vp_com_socket_t));
}
vp_com_sockopt(cfg->com, &cfg->socket_client, cfg->sockopt);
}
// \todo test
return res;
}
DEFINE_THREAD_ROUTINE_STACK( vp_com_server, thread_params, VP_COM_THREAD_SERVER_STACK_SIZE )
{
vp_com_socket_t client_sockets[VP_COM_THREAD_NUM_MAX_CLIENTS];
struct timeval tv, *ptv;
// This thread setup connection then loop & wait for a socket event
vp_com_server_thread_param_t* params = (vp_com_server_thread_param_t*) thread_params;
int32_t i, rc, ncs, s, max = 0, num_server_sockets = params->num_servers, num_client_sockets = 0;
vp_com_socket_t* server_sockets = params->servers;
fd_set read_fs;
vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
if(VP_FAILED(vp_com_init(params->com)))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to init com\n");
vp_com_shutdown(params->com);
}
else if(VP_FAILED(vp_com_local_config(params->com, params->config)))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to configure com\n");
vp_com_shutdown(params->com);
}
else if(VP_FAILED(vp_com_connect(params->com, params->connection, 1)))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to connect\n");
vp_com_shutdown(params->com);
}
else
{
vp_os_mutex_lock(&server_initialisation_mutex);
vp_os_cond_signal(&server_initialisation_wait);
vp_os_mutex_unlock(&server_initialisation_mutex);
server_init_not_finished = FALSE;
for( i = 0; i < num_server_sockets; i++ )
{
if(VP_FAILED( vp_com_open_socket(&server_sockets[i], NULL, NULL) ))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Unable to open server socket\n");
server_sockets[i].is_disable = TRUE;
}
else
{
listen((int32_t)server_sockets[i].priv, server_sockets[i].queue_length);
}
}
params->run = TRUE;
while( params->run == TRUE )
{
if( params->timer_enable == FALSE || ( params->wait_sec == 0 && params->wait_usec == 0 ) )
{
ptv = NULL;
}
else
{
tv.tv_sec = params->wait_sec;
tv.tv_usec = params->wait_usec;
ptv = &tv;
}
FD_ZERO(&read_fs);
max = vp_com_fill_read_fs( &server_sockets[0], num_server_sockets, 0, &read_fs );
max = vp_com_fill_read_fs( &client_sockets[0], num_client_sockets, max, &read_fs );
rc = select( max + 1, &read_fs, NULL, NULL, ptv );
if( rc == -1 && ( errno == EINTR || errno == EAGAIN ) )
continue;
if( rc == 0 )
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] select timeout\n");
vp_com_close_client_sockets(&client_sockets[0], num_client_sockets);
num_client_sockets = 0;
params->timer_enable = FALSE;
vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
}
for( i = 0; i < num_server_sockets && rc != 0; i++ )
{
s = (int32_t) server_sockets[i].priv;
if( ( !server_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
{
rc --;
// Recycle previously released sockets
for( ncs = 0; ncs < num_client_sockets && client_sockets[ncs].priv != NULL; ncs++ );
if( ncs < VP_COM_THREAD_NUM_MAX_CLIENTS)
{
if( VP_SUCCEEDED(vp_com_client_open_socket(&server_sockets[i], &client_sockets[ncs])) && ( ncs == num_client_sockets ) )
num_client_sockets ++;
}
}
}
for( i = 0; i < num_client_sockets && rc != 0; i++ )
{
s = (int32_t) client_sockets[i].priv;
if( ( !client_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
{
rc--;
vp_com_client_receive( &client_sockets[i] );
}
}
}
for( i = 0; i < num_server_sockets; i++ )
{
vp_com_close_socket(&server_sockets[i]);
}
}
vp_com_disconnect(params->com);
vp_com_shutdown(params->com);
THREAD_RETURN( 0 );
}
