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_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;
}
AT_CODEC_ERROR_CODE host_open( void )
{
static bool_t init_ok = FALSE;
if( func_ptrs.open != NULL )
return func_ptrs.open();
if( !init_ok )
{
COM_CONFIG_SOCKET_AT(&at_socket, VP_COM_CLIENT, AT_PORT, wifi_ardrone_ip);
at_socket.protocol = VP_COM_UDP;
if(VP_FAILED(vp_com_init(COM_AT())))
{
PRINT ("Failed to init AT\n");
vp_com_shutdown( COM_AT() );
return AT_CODEC_OPEN_ERROR;
}
if(VP_FAILED(vp_com_open(COM_AT(), &at_socket, &atcodec_read, &atcodec_write)))
{
PRINT ("Failed to open AT\n");
return AT_CODEC_OPEN_ERROR;
}
// set send_buffer to a low value to limit latency
int32_t sendbuf = AT_MUTEX_SNDBUF_SIZE;
if ( setsockopt((int32_t)at_socket.priv, SOL_SOCKET, SO_SNDBUF, &sendbuf, sizeof(sendbuf)) )
{
PRINT ("Error setting SND_BUF for AT socket\n");
}
int opt = IPTOS_PREC_NETCONTROL;
int res = setsockopt((int)at_socket.priv, IPPROTO_IP, IP_TOS, &opt, (socklen_t)sizeof(opt));
if (res)
{
perror("AT stage - setting Live video socket IP Type Of Service : ");
}
else
{
printf ("Set IP_TOS ok\n");
}
init_ok = TRUE;
}
return AT_CODEC_OPEN_OK;
}
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);
}
AT_CODEC_ERROR_CODE host_open( void )
{
static bool_t init_ok = FALSE;
if( func_ptrs.open != NULL )
return func_ptrs.open();
if( !init_ok )
{
COM_CONFIG_SOCKET_AT(&at_socket, VP_COM_CLIENT, 0, wifi_ardrone_ip);
at_socket.protocol = VP_COM_UDP;
at_socket.remotePort = AT_PORT;
if(VP_FAILED(vp_com_init(COM_AT())))
{
PRINT ("Failed to init AT\n");
vp_com_shutdown( COM_AT() );
return AT_CODEC_OPEN_ERROR;
}
if(VP_FAILED(vp_com_open(COM_AT(), &at_socket, &atcodec_read, &atcodec_write)))
{
PRINT ("Failed to open AT\n");
return AT_CODEC_OPEN_ERROR;
}
// set send_buffer to a low value to limit latency
int32_t sendbuf = AT_MUTEX_SNDBUF_SIZE;
if ( setsockopt((int32_t)at_socket.priv, SOL_SOCKET, SO_SNDBUF, &sendbuf, sizeof(sendbuf)) )
{
PRINT ("Error setting SND_BUF for AT socket\n");
}
/*
* On android, with IP_TOS set, certain devices can't connect to AR.Drone 1
* So we just disable this functionnality to avoid these cases.
*/
#ifdef USE_ANDROID
if (IS_ARDRONE2)
{
#endif
int opt = IPTOS_PREC_NETCONTROL;
int res = setsockopt((int)at_socket.priv, IPPROTO_IP, IP_TOS, &opt, (socklen_t)sizeof(opt));
if (res)
{
perror("AT stage - setting Live video socket IP Type Of Service : ");
}
else
{
printf ("Set IP_TOS ok\n");
}
#ifdef USE_ANDROID
}
#endif
init_ok = TRUE;
}
return AT_CODEC_OPEN_OK;
}
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 );
}
C_RESULT wifi_server_auth(struct in_addr *addr)
{
Read read = NULL;
Write write = NULL;
C_RESULT result = C_FAIL;
int numretries = 1;
uint8_t recvString[BUFFER_SIZE]; /* Buffer for received string */
int recvStringLen; /* Length of received string */
struct timeval tv = {0, 500000};
int on=1;
const uint8_t msg[] = AUTH_MSG;
struct in_addr to;
struct in_addr from;
vp_com_socket_t socket;
// Initialize sending socket
to.s_addr = htonl(WIFI_BROADCAST_ADDR);
COM_CONFIG_SOCKET_AUTH(&socket, VP_COM_CLIENT, AUTH_PORT, inet_ntoa(to));
socket.protocol = VP_COM_UDP;
if(VP_FAILED(vp_com_init(COM_AUTH())))
{
printf("Failed to init Authentification\n");
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
if(VP_FAILED(vp_com_open(COM_AUTH(), &socket, &read, &write)))
{
printf("Failed to open Authentification\n");
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
if (setsockopt((int)socket.priv, SOL_SOCKET, SO_BROADCAST,(char *)&on,sizeof(on)) < 0)
{
printf("Failed to set socket option Authentification\n");
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
if (setsockopt((int)socket.priv, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
{
printf("Failed to set socket option Authentification\n");
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
do
{
int len = strlen((char*)msg);
if(write != NULL)
{
if (VP_FAILED(write(&socket, msg, &len)))
{
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return C_FAIL;
}
}
printf("Wait authentification\n");
do
{
if(read != NULL)
{
recvStringLen = BUFFER_SIZE;
if(VP_FAILED(read(&socket, recvString, &recvStringLen)))
{
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH());
return C_FAIL;
}
}
recvString[recvStringLen] = '\0';
}
while((recvStringLen != 0) && (strcmp((char *)recvString, AUTH_MSG) == 0));
}
while((strcmp((char *)recvString, AUTH_MSG_OK) != 0) && (numretries++ < AUTH_MAX_NUMRETRIES));
if(strcmp((char*)recvString, AUTH_MSG_OK) == 0)
{
from.s_addr = socket.scn;
printf("Authentification ok from %s:%d\n", inet_ntoa(from), socket.port);
memcpy(addr, &from, sizeof(struct in_addr));
result = C_OK;
}
vp_com_close(COM_AUTH(), &socket);
vp_com_shutdown( COM_AUTH() );
return result;
}
