C_RESULT ardrone_tool_shutdown()
{
C_RESULT res = C_OK;
#ifndef NO_ARDRONE_MAINLOOP
res = ardrone_tool_shutdown_custom();
#endif
// Shutdown subsystems
ardrone_navdata_client_shutdown();
ardrone_control_shutdown();
ardrone_tool_input_shutdown();
JOIN_THREAD(ardrone_control);
JOIN_THREAD(navdata_update);
// Shutdown AT Commands
ATcodec_exit_thread();
ATcodec_Shutdown_Library();
vp_com_disconnect(COM_NAVDATA());
vp_com_shutdown(COM_NAVDATA());
PRINT("Custom ardrone tool ended\n");
return res;
}
C_RESULT ardrone_tool_shutdown()
{
C_RESULT res = C_OK;
res = ardrone_tool_shutdown_custom();
// Shutdown subsystems
ardrone_navdata_client_shutdown();
ardrone_control_shutdown();
ardrone_tool_input_shutdown();
JOIN_THREAD(ardrone_control);
JOIN_THREAD(navdata_update);
fflush (NULL);
// Shutdown AT Commands
ATcodec_exit_thread();
ATcodec_Shutdown_Library();
vp_com_disconnect(COM_NAVDATA());
vp_com_shutdown(COM_NAVDATA());
academy_download_shutdown();
academy_shutdown();
PRINT("Custom ardrone tool ended\n");
return res;
}
C_RESULT
vp_stages_output_com_stage_close(vp_stages_output_com_config_t *cfg)
{
// \todo Faire ca dans le transform en detectant la fin d'une connection
vp_com_close(cfg->com, &cfg->socket_client);
if(cfg->socket.type == VP_COM_SERVER)
vp_com_close(cfg->com, &cfg->socket);
// \todo test
if(cfg->connection && cfg->connection->is_up)
vp_com_disconnect(cfg->com);
vp_com_shutdown(cfg->com);
return (VP_SUCCESS);
}
C_RESULT vp_com_connect(vp_com_t* vp_com, vp_com_connection_t* connection, uint32_t numAttempts)
{
C_RESULT res = VP_COM_OK;
bool_t already_connected;
VP_OS_ASSERT( vp_com != NULL );
if(vp_com->config != NULL)
{
vp_os_mutex_lock(&vp_com->mutex);
already_connected = vp_com->connection && vp_com->connection->is_up == 1;
// TODO voir pour ajouter un test sur l'adresse ethernet de la connection
if( already_connected && vp_com->connection != connection )
{
already_connected = FALSE;
vp_com_disconnect(vp_com);
}
if( !already_connected )
{
res = VP_COM_CONNECT(vp_com, connection, numAttempts);
if( SUCCEED( res ) )
{
vp_com->connection = connection;
vp_com->connection->is_up = 1;
}
}
vp_os_mutex_unlock(&vp_com->mutex);
}
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 );
}
