/*---------------------------------------------------------------------------------------------------------------------
Tests the network connection to the drone by fetching the drone version number
through the FTP server embedded on the drone.
This is how FreeFlight checks if a drone sofware update is required.
The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
---------------------------------------------------------------------------------------------------------------------*/
int test_drone_connection()
{
vp_com_socket_t ftp_client;
static Write ftp_write = NULL;
static Read ftp_read = NULL;
int timeout_windows = 1000; /*milliseconds*/
/* Connects to the FTP server */
wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
ftp_client.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com())))
return -1;
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write)))
return -2;
setsockopt((int32_t)ftp_client.priv,
SOL_SOCKET,
SO_RCVTIMEO,
(const char*)&timeout_windows, sizeof(timeout_windows)
);
/* Clean up */
vp_com_close(wifi_com(), &ftp_client);
return 0;
}
/*---------------------------------------------------------------------------------------------------------------------
Tests the network connection to the drone by fetching the drone version number
through the FTP server embedded on the drone.
This is how FreeFlight checks if a drone sofware update is required.
The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
---------------------------------------------------------------------------------------------------------------------*/
int test_drone_connection()
{
const char * passivdeModeHeader = "\r\n227 PASV ok (";
vp_com_socket_t ftp_client,ftp_client2;
char buffer[1024];
static Write ftp_write = NULL;
static Read ftp_read = NULL;
int bytes_to_send,received_bytes;
int i,L,x[6],port;
vp_os_memset(buffer,0,sizeof(buffer));
/* Connects to the FTP server */
wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
ftp_client.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;
/* Request version file */
bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",
"USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");
ftp_write(&ftp_client, (const int8_t*)buffer,&bytes_to_send);
/* Dirty. We should wait for data to arrive with some kind of synchronization
or make the socket blocking.*/
Sleep(1000);
/* Gets the data port */
received_bytes = sizeof(buffer);
ftp_read(&ftp_client,(int8_t*)buffer,&received_bytes);
if (received_bytes<1) { vp_com_close(wifi_com(), &ftp_client); return -3; }
L=received_bytes-strlen(passivdeModeHeader);
/* Searches for the passive mode acknowlegment from the FTP server */
for (i=0;i<L;i++) {
if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0) break;
}
if (i==L) {
vp_com_close(wifi_com(), &ftp_client); return -4;
}
i+=strlen(passivdeModeHeader);
if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
{ vp_com_close(wifi_com(), &ftp_client); return -5; }
port=(x[4]<<8)+x[5];
/* Connects to the FTP server data port */
wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
ftp_client2.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com())))
{ vp_com_close(wifi_com(), &ftp_client2); return -6; }
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
{ vp_com_close(wifi_com(), &ftp_client2); return -7; }
/* Clean up */
vp_com_close(wifi_com(), &ftp_client);
vp_com_close(wifi_com(), &ftp_client2);
return 0;
}
C_RESULT vp_com_sockopt_ip(vp_com_t* vp_com, vp_com_socket_t* socket, VP_COM_SOCKET_OPTIONS options)
{
C_RESULT res = VP_COM_ERROR;
int s = (int) socket->priv;
if( options & VP_COM_NON_BLOCKING )
{
/*#ifndef USE_MINGW32
int32_t arg = 1;
PRINT("Setting socket %d to non blocking\n", s);
res = ioctl( s, FIONBIO, &arg ) < 0 ? C_FAIL : C_OK;
#endif*/
if( VP_FAILED(res) )
PRINT("error setting non blocking\n");
}
if( options & VP_COM_NO_DELAY )
{
int32_t flag = 1;
PRINT("Disabling the Nagle (TCP No Delay) algorithm for socket %d\n", s);
res = setsockopt( s, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(flag) ) < 0 ? C_FAIL : C_OK;
if( VP_FAILED(res) )
PRINT("error disabling the Nagle algorithm\n");
}
/*
#ifdef __linux__
flags = fcntl(s, F_GETFL, 0);
#endif // __linux__
if( flags >= 0 )
{
flags |= O_NONBLOCK;
flags = fcntl(s, F_SETFL, flags );
res = VP_COM_OK;
}
else
{
DEBUG_PRINT_SDK("Get Socket Options failed because of %d\n", errno);
}
*/
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;
}
//-----------------------------------------------------------------------------
// Name: EnumObjectsCallback()
// Desc: Callback function for enumerating objects (axes, buttons, POVs) on a
// g_pJoystick. This function enables user interface elements for objects
// that are found to exist, and scales axes min/max values.
//-----------------------------------------------------------------------------
BOOL CALLBACK EnumObjectsCallback( const DIDEVICEOBJECTINSTANCE* pdidoi,
VOID* pContext )
{
HWND hDlg = ( HWND )pContext;
static int nSliderCount = 0; // Number of returned slider controls
static int nPOVCount = 0; // Number of returned POV controls
// For axes that are returned, set the DIPROP_RANGE property for the
// enumerated axis in order to scale min/max values.
if( pdidoi->dwType & DIDFT_AXIS )
{
DIPROPRANGE diprg;
diprg.diph.dwSize = sizeof( DIPROPRANGE );
diprg.diph.dwHeaderSize = sizeof( DIPROPHEADER );
diprg.diph.dwHow = DIPH_BYID;
diprg.diph.dwObj = pdidoi->dwType; // Specify the enumerated axis
diprg.lMin = -1000;
diprg.lMax = +1000;
// Set the range for the axis
if( VP_FAILED( g_pJoystick->SetProperty( DIPROP_RANGE, &diprg.diph ) ) )
return DIENUM_STOP;
}
return DIENUM_CONTINUE;
}
C_RESULT open_dx_keyboard(void)
{
HRESULT hr;
HWND hDlg = GetConsoleHwnd();
// Register with the DirectInput subsystem and get a pointer
// to a IDirectInput interface we can use.
// Create a DInput object
if (g_pDI==NULL)
if( VP_FAILED( hr = DirectInput8Create( GetModuleHandle( NULL ), DIRECTINPUT_VERSION,
IID_IDirectInput8, ( VOID** )&g_pDI, NULL ) ) )
return hr;
// Create the connection to the keyboard device
g_pDI->CreateDevice(GUID_SysKeyboard, &fDIKeyboard, NULL);
if (fDIKeyboard)
{
fDIKeyboard->SetDataFormat(&c_dfDIKeyboard);
fDIKeyboard->SetCooperativeLevel(hDlg,DISCL_FOREGROUND | DISCL_EXCLUSIVE);
fDIKeyboard->Acquire();
}
return C_OK;
}
C_RESULT
vp_stages_input_com_stage_transform(vp_stages_input_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = cfg->buffer_size;
out->buffers = (int8_t **) vp_os_malloc (sizeof(int8_t *)+out->size*sizeof(int8_t));
out->buffers[0] = (int8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING && cfg->read != NULL)
{
out->size = cfg->buffer_size;
if(VP_FAILED(cfg->read(&cfg->socket_client, out->buffers[0], &out->size)))
out->status = VP_API_STATUS_ERROR;
}
vp_os_mutex_unlock(&out->lock);
return (VP_SUCCESS);
}
//-----------------------------------------------------------------------------
// Name: Enumg_pJoysticksCallback()
// Desc: Called once for each enumerated g_pJoystick. If we find one, create a
// device interface on it so we can play with it.
//-----------------------------------------------------------------------------
BOOL CALLBACK Enumg_pJoysticksCallback( const DIDEVICEINSTANCE* pdidInstance,
VOID* pContext )
{
DI_ENUM_CONTEXT* pEnumContext = ( DI_ENUM_CONTEXT* )pContext;
HRESULT hr;
if( g_bFilterOutXinputDevices && IsXInputDevice( &pdidInstance->guidProduct ) )
return DIENUM_CONTINUE;
// Skip anything other than the perferred g_pJoystick device as defined by the control panel.
// Instead you could store all the enumerated g_pJoysticks and let the user pick.
if( pEnumContext->bPreferredJoyCfgValid &&
!IsEqualGUID( pdidInstance->guidInstance, pEnumContext->pPreferredJoyCfg->guidInstance ) )
return DIENUM_CONTINUE;
// Obtain an interface to the enumerated g_pJoystick.
hr = g_pDI->CreateDevice( pdidInstance->guidInstance, &g_pJoystick, NULL );
// If it failed, then we can't use this g_pJoystick. (Maybe the user unplugged
// it while we were in the middle of enumerating it.)
if( VP_FAILED( hr ) )
return DIENUM_CONTINUE;
// Stop enumeration. Note: we're just taking the first g_pJoystick we get. You
// could store all the enumerated g_pJoysticks and let the user pick.
if ( wcscmp( (const wchar_t*)&pEnumContext->pPreferredJoyCfg->wszType , TEXT("VID_127F&PID_E018") )==0)
{
JoystickType=GAMEPAD_RADIO_GP;
printf("Using gamepad settings for an Icarus Gamepad.\n");
}
else
if ( wcscmp( (const wchar_t*)&pEnumContext->pPreferredJoyCfg->wszType , TEXT("VID_046D&PID_C21A") )==0)
{
JoystickType=GAMEPAD_LOGITECH_PRECISION;
printf("Using gamepad settings for a Logitech Precision gamepad.\n");
}
else
if ( wcscmp( (const wchar_t*)&pEnumContext->pPreferredJoyCfg->wszType , TEXT("VID_054C&PID_0268") )==0)
{
JoystickType=GAMEPAD_PLAYSTATION3;
printf("Using gamepad settings for a Playstation3 gamepad.\n");
}
else
if ( wcscmp( (const wchar_t*)&pEnumContext->pPreferredJoyCfg->wszType , TEXT("VID_06A3&PID_0836") )==0)
{
JoystickType=JOYSTICK_CYBORG_X;
printf("Using gamepad settings for a CyborgX joystick.\n");
}
else { JoystickType=GAMEPAD_UNKNOWN;
printf("Unknown gamepad. Controls are disabled.\n");
}
return DIENUM_STOP;
}
vp_com_config_t* wifi_config(void)
{
static vp_com_wifi_config_t config =
{
{ 0 },
WIFI_MOBILE_IP,
WIFI_NETMASK,
WIFI_BROADCAST,
WIFI_GATEWAY,
WIFI_SERVER,
WIFI_INFRASTRUCTURE,
WIFI_SECURE,
WIFI_PASSKEY,
{ 0 },
};
struct ifaddrs * ifAddrStructHead = NULL;
struct ifaddrs * ifAddrStruct = NULL;
struct in_addr tmpAddr;
bool_t found = FALSE;
if(strlen(config.itfName) == 0)
{
getifaddrs(&ifAddrStruct);
ifAddrStructHead = ifAddrStruct;
while (!found && (ifAddrStruct != NULL))
{
// Looking for WIFI interface's IP address corresponding to WIFI_BASE_ADDR
if (ifAddrStruct->ifa_addr->sa_family == AF_INET)
{
tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_addr)->sin_addr;
if ( (ntohl(tmpAddr.s_addr) & 0xFFFFFF00) == WIFI_BASE_ADDR )
{
inet_ntop(AF_INET, &tmpAddr, config.localHost, VP_COM_NAME_MAXSIZE);
memcpy(config.itfName, ifAddrStruct->ifa_name, strlen(ifAddrStruct->ifa_name));
if(VP_FAILED(wifi_server_auth(&tmpAddr)))
tmpAddr.s_addr = htonl ( ntohl(tmpAddr.s_addr) - ( ( ( ntohl(tmpAddr.s_addr) & 0xFF ) - 1 ) % 5 ) );
memcpy(config.server, inet_ntoa(tmpAddr), strlen(inet_ntoa(tmpAddr)));
tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_netmask)->sin_addr;
inet_ntop(AF_INET, &tmpAddr, config.netmask, VP_COM_NAME_MAXSIZE);
tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_broadaddr)->sin_addr;
inet_ntop(AF_INET, &tmpAddr, config.broadcast, VP_COM_NAME_MAXSIZE);
found = TRUE;
}
}
ifAddrStruct = ifAddrStruct->ifa_next;
}
if (ifAddrStructHead != NULL)
{
freeifaddrs(ifAddrStructHead);
}
}
return (vp_com_config_t*)&config;
}
C_RESULT vp_api_start_all_threads_tab(thread_table_entry_t* tab)
{
int32_t i = 0;
while(tab[i].name)
{
if(VP_FAILED(vp_api_start_thread_tab(tab,i,0)))
PRINT("Thread %d refused to start\n",(int)i);
i++;
}
return C_OK;
}
AT_CODEC_ERROR_CODE host_write(uint8_t *buffer, int32_t *len)
{
if( func_ptrs.write != NULL )
return func_ptrs.write( buffer, len );
if( atcodec_write != NULL )
{
return VP_FAILED(atcodec_write(&at_socket, buffer, len)) ? AT_CODEC_WRITE_ERROR : AT_CODEC_WRITE_OK;
}
return AT_CODEC_WRITE_OK;
}
C_RESULT video_com_stage_transform(video_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT res;
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = cfg->buffer_size;
out->buffers = (int8_t **) vp_os_malloc (sizeof(int8_t *)+out->size*sizeof(int8_t));
out->buffers[0] = (int8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING && cfg->read != NULL)
{
out->size = cfg->buffer_size;
res = cfg->read(&cfg->socket, out->buffers[0], &out->size);
if( cfg->protocol == VP_COM_UDP )
{
if( out->size == 0 )
{
// Send "1" for Unicast
// Send "2" to enable Multicast
int32_t flag = 1, len = sizeof(flag);
if ( cfg->socket.is_multicast == 1 )
flag = 2;
cfg->write(&cfg->socket, (int8_t*) &flag, &len);
}
}
if( VP_FAILED(res) )
{
out->status = VP_API_STATUS_ERROR;
out->size = 0;
}
if( out->size == 0)
cfg->num_retries++;
else
cfg->num_retries = 0;
}
vp_os_mutex_unlock(&out->lock);
return C_OK;
}
C_RESULT
vp_api_multi_stage_close(vp_api_io_multi_stage_config_t *cfg)
{
uint32_t i;
for(i = 0; i < cfg->nb_stages; i++)
{
if(VP_FAILED(cfg->stages[i].funcs.close(cfg->stages[i].cfg)))
return (VP_FAILURE);
}
return (VP_SUCCESS);
}
C_RESULT
vp_api_multi_stage_open(vp_api_io_multi_stage_config_t *cfg)
{
uint32_t i;
for(i = 0; i < cfg->nb_stages; i++)
{
VP_OS_ASSERT(cfg->stages[i].funcs.open);
VP_OS_ASSERT(cfg->stages[i].funcs.transform);
VP_OS_ASSERT(cfg->stages[i].funcs.close);
if(VP_FAILED(cfg->stages[i].funcs.open(cfg->stages[i].cfg)))
return (VP_FAILURE);
}
return (VP_SUCCESS);
}
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;
}
C_RESULT ardrone_tool_input_update(void)
{
C_RESULT res;
int32_t i;
res = C_OK;
i = 0;
while( VP_SUCCEEDED(res) && i < MAX_NUM_DEVICES )
{
if( devices[i] != NULL && VP_FAILED(devices[i]->update()) )
{
PRINT("Input device %s update failed... it'll be removed\n", devices[i]->name);
ardrone_tool_input_remove_i(i);
res = C_FAIL;
}
i++;
}
ui_pad_update_user_input(&input_state);
return res;
}
C_RESULT video_com_stage_transform(video_com_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT res;
vp_os_mutex_lock(&out->lock);
if (1 == cfg->mustReconnect)
{
PDBG ("Will call connect");
PRINT ("Reconnecting ... ");
res = video_com_stage_connect (cfg);
PRINT ("%s\n", (VP_FAILED (res) ? "FAIL" : "OK"));
cfg->mustReconnect = 0;
}
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = 0;
out->buffers = (uint8_t **) vp_os_malloc (sizeof(uint8_t *)+cfg->buffer_size*sizeof(uint8_t));
out->buffers[0] = (uint8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING && cfg->read != NULL)
{
bool_t nonBlock = (cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE) ? TRUE : FALSE;
out->size = cfg->buffer_size;
if (nonBlock)
{
cfg->socket.block = VP_COM_DONTWAIT;
}
res = cfg->read(&cfg->socket, out->buffers[0], &out->size);
if (! nonBlock && cfg->protocol == VP_COM_UDP && out->size == 0)
{
// Send "1" for Unicast
// Send "2" to enable Multicast
char flag = 1; int32_t len = sizeof(flag);
if ( cfg->socket.is_multicast == 1 )
{
flag = 2;
}
cfg->write(&cfg->socket, (uint8_t*) &flag, &len);
}
bool_t bContinue = TRUE;
if( VP_FAILED(res) )
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if( out->size == 0)
{
if (nonBlock)
{
out->size = -1; // Signal next stage that we don't have data waiting
}
else
{
cfg->num_retries++;
}
bContinue = FALSE;
}
else
{
cfg->num_retries = 0;
}
cfg->socket.block = VP_COM_DONTWAIT;
int32_t readSize = cfg->buffer_size - out->size;
while (TRUE == bContinue)
{
res = cfg->read(&cfg->socket, &(out->buffers[0][out->size]), &readSize);
if( VP_FAILED(res) )
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if (0 == readSize)
{
bContinue = FALSE;
}
out->size += readSize;
readSize = cfg->buffer_size - out->size;
}
cfg->socket.block = VP_COM_DEFAULT;
}
if (NULL != cfg->timeoutFunc && 0 != cfg->timeoutFuncAfterSec)
{
if (cfg->num_retries >= cfg->timeoutFuncAfterSec)
{
cfg->timeoutFunc ();
}
}
vp_os_mutex_unlock(&out->lock);
return C_OK;
}
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;
}
DEFINE_THREAD_ROUTINE( ardrone_control, nomParams )
{
C_RESULT res_wait_navdata = C_OK;
C_RESULT res = C_OK;
uint32_t retry, current_ardrone_state;
int32_t next_index_in_queue;
ardrone_control_event_ptr_t current_event;
retry = 0;
current_event = NULL;
DEBUG_PRINT_SDK("Thread control in progress...\n");
control_socket.is_disable = TRUE;
ardrone_control_connect_to_drone();
while( bContinue
&& !ardrone_tool_exit() )
{
vp_os_mutex_lock(&control_mutex);
control_waited = TRUE;
/* Wait for new navdata to be received. */
res_wait_navdata = vp_os_cond_timed_wait(&control_cond, 1000);
vp_os_mutex_unlock(&control_mutex);
/*
* In case of timeout on the navdata, we assume that there was a problem
* with the Wifi connection.
* It is then safer to close and reopen the control socket (TCP 5559) since
* some OS might stop giving data but not signal any disconnection.
*/
if(VP_FAILED(res_wait_navdata))
{
DEBUG_PRINT_SDK("Timeout while waiting for new navdata.\n");
if(!control_socket.is_disable)
control_socket.is_disable = TRUE;
}
if(control_socket.is_disable)
{
ardrone_control_connect_to_drone();
}
if(VP_SUCCEEDED(res_wait_navdata) && (!control_socket.is_disable))
{
vp_os_mutex_lock(&control_mutex);
current_ardrone_state = ardrone_state;
control_waited = FALSE;
vp_os_mutex_unlock(&control_mutex);
if( ardrone_tool_exit() ) // Test if we received a signal because we are quitting the application
THREAD_RETURN( res );
if( current_event == NULL )
{
vp_os_mutex_lock(&event_queue_mutex);
next_index_in_queue = (end_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
if( next_index_in_queue != start_index_in_queue )
{ // There's an event to process
current_event = ardrone_control_event_queue[next_index_in_queue];
if( current_event != NULL )
{
if( current_event->ardrone_control_event_start != NULL )
{
current_event->ardrone_control_event_start( current_event );
}
}
end_index_in_queue = next_index_in_queue;
retry = 0;
}
vp_os_mutex_unlock(&event_queue_mutex);
}
if( current_event != NULL )
{
switch( current_event->event )
{
case ARDRONE_UPDATE_CONTROL_MODE:
res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
break;
case PIC_UPDATE_CONTROL_MODE:
res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
break;
case LOGS_GET_CONTROL_MODE:
break;
case CFG_GET_CONTROL_MODE:
case CUSTOM_CFG_GET_CONTROL_MODE: /* multiconfiguration support */
res = ardrone_control_configuration_run( current_ardrone_state, (ardrone_control_configuration_event_t*) current_event );
break;
case ACK_CONTROL_MODE:
res = ardrone_control_ack_run( current_ardrone_state, (ardrone_control_ack_event_t *) current_event);
break;
default:
break;
}
if( VP_FAILED(res) )
{
retry ++;
if( retry > current_event->num_retries)
current_event->status = ARDRONE_CONTROL_EVENT_FINISH_FAILURE;
}
else
{
retry = 0;
}
if( current_event->status & ARDRONE_CONTROL_EVENT_FINISH )
{
if( current_event->ardrone_control_event_end != NULL )
current_event->ardrone_control_event_end( current_event );
/* Make the thread read a new event on the next loop iteration */
current_event = NULL;
}
else
{
/* Not changing 'current_event' makes the loop process the same
* event when the next navdata packet arrives. */
}
}
}
}// while
/* Stephane : Bug fix - mutexes were previously detroyed by another thread,
which made ardrone_control crash.*/
vp_os_mutex_destroy(&event_queue_mutex);
vp_os_cond_destroy(&control_cond);
vp_os_mutex_destroy(&control_mutex);
vp_com_close(COM_CONTROL(), &control_socket);
THREAD_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 );
}
C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
{
C_RESULT res = VP_COM_OK;
BOOL reuseaddroption = TRUE;
BOOL exclusiveaddroption = FALSE;
SOCKET s = -1;
struct sockaddr_in name = { 0 };
struct sockaddr_in local_address = { 0 };
struct sockaddr_in remote_address = { 0 };
int err;
int res_setsockopt=0,res_connect=0,res_bind=0;
switch( sck->protocol )
{
case VP_COM_TCP:
s = socket( AF_INET, SOCK_STREAM, IPPROTO_TCP );
res = ( s == INVALID_SOCKET ) ? VP_COM_ERROR : VP_COM_OK;
break;
case VP_COM_UDP:
s = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );
sck->scn = inet_addr(sck->serverHost); // Cache destination in int format
res = ( s == INVALID_SOCKET ) ? VP_COM_ERROR : VP_COM_OK;
break;
default:
sck->type = VP_COM_CLIENT;
res = VP_COM_PARAMERROR;
break;
}
if( VP_FAILED(res) )
{
PRINT("\nSocket opening failed\n");
}
VP_COM_CHECK( res );
// res_setsockopt = setsockopt(s,SOL_SOCKET,SO_REUSEADDR,(char*)&reuseaddroption,sizeof(reuseaddroption));
res_setsockopt = setsockopt(s,SOL_SOCKET,SO_EXCLUSIVEADDRUSE,(char*)&exclusiveaddroption,sizeof(exclusiveaddroption));
name.sin_family = AF_INET;
name.sin_port = htons( sck->port );
switch( sck->type )
{
case VP_COM_CLIENT:
remote_address.sin_family = AF_INET;
remote_address.sin_port = htons( sck->port );
remote_address.sin_addr.s_addr = inet_addr(sck->serverHost);
if ( sck->protocol ==VP_COM_UDP)
{
local_address.sin_addr.s_addr= INADDR_ANY;
local_address.sin_family = AF_INET;
local_address.sin_port = htons( sck->port ); /* Bind to any available port */
res_bind = bind(s,(const struct sockaddr*)&local_address,sizeof(local_address));
err = WSAGetLastError();
res = (res_bind==0)? VP_COM_OK : VP_COM_ERROR; /* Convert from Win32 error code to VP SDK error code */
}
if (VP_SUCCEEDED(res))// && (sck->protocol !=VP_COM_UDP))
{
res_connect = connect( s, (struct sockaddr*)&remote_address, sizeof( remote_address ) );
if( res_connect == -1 ){ res = VP_COM_ERROR; err = WSAGetLastError(); }
}
break;
case VP_COM_SERVER:
/* Local TCP/UDP address on which we wait for connections */
local_address.sin_family = AF_INET;
local_address.sin_port = htons( sck->port );
local_address.sin_addr.s_addr = INADDR_ANY; /* Accept connections on any network interface */
res_bind = bind( s, (const struct sockaddr*)&local_address, sizeof(local_address) );
res = (res_bind==0)? VP_COM_OK : VP_COM_ERROR ; /* Convert from Win32 error code to VP SDK error code */
break;
default:
res = VP_COM_PARAMERROR;
break;
}
if(res == VP_COM_OK)
{
sck->priv = (void*) s;
switch( sck->protocol )
{
case VP_COM_TCP:
if(read) *read = (Read) vp_com_read_socket;
if(write) *write = (Write) vp_com_write_socket;
break;
case VP_COM_UDP:
if(read) *read = (Read) vp_com_read_udp_socket;
if(write) *write = (Write) vp_com_write_udp_socket;
break;
default:
if(read) *read = NULL;
if(write) *write = NULL;
break;
}
}
else
{
closesocket( s );
}
if (sck->block != VP_COM_DEFAULT &&
sck->block != VP_COM_WAITALL &&
sck->block != VP_COM_DONTWAIT)
{
sck->block = VP_COM_DEFAULT;
}
return res;
}
C_RESULT open_dx_gamepad(void)
{
HRESULT hr;
HWND hDlg = GetConsoleHwnd();
// Register with the DirectInput subsystem and get a pointer
// to a IDirectInput interface we can use.
// Create a DInput object
if (g_pDI==NULL)
if( VP_FAILED( hr = DirectInput8Create( GetModuleHandle( NULL ), DIRECTINPUT_VERSION,
IID_IDirectInput8, ( VOID** )&g_pDI, NULL ) ) )
return hr;
if( g_bFilterOutXinputDevices )
SetupForIsXInputDevice();
DIJOYCONFIG PreferredJoyCfg = {0};
DI_ENUM_CONTEXT enumContext;
enumContext.pPreferredJoyCfg = &PreferredJoyCfg;
enumContext.bPreferredJoyCfgValid = false;
IDirectInputJoyConfig8* pJoyConfig = NULL;
if( VP_FAILED( hr = g_pDI->QueryInterface( IID_IDirectInputJoyConfig8, ( void** )&pJoyConfig ) ) )
return hr;
PreferredJoyCfg.dwSize = sizeof( PreferredJoyCfg );
if( SUCCEEDED( pJoyConfig->GetConfig( 0, &PreferredJoyCfg, DIJC_GUIDINSTANCE ) ) ) // This function is expected to fail if no g_pJoystick is attached
enumContext.bPreferredJoyCfgValid = true;
SAFE_RELEASE( pJoyConfig );
// Look for a simple g_pJoystick we can use for this sample program.
if( VP_FAILED( hr = g_pDI->EnumDevices( DI8DEVCLASS_GAMECTRL,
Enumg_pJoysticksCallback,
&enumContext, DIEDFL_ATTACHEDONLY ) ) )
return hr;
if( g_bFilterOutXinputDevices )
CleanupForIsXInputDevice();
// Make sure we got a g_pJoystick
if( g_pJoystick == NULL )
{
//MessageBox( NULL, TEXT( "Joystick not found." ),
// TEXT( "A.R. Drone"),
// MB_ICONERROR | MB_OK );
// EndDialog( hDlg, 0 );
return C_FAIL;
}
// Set the data format to "simple g_pJoystick" - a predefined data format
//
// A data format specifies which controls on a device we are interested in,
// and how they should be reported. This tells DInput that we will be
// passing a DIJOYSTATE2 structure to IDirectInputDevice::GetDeviceState().
if( VP_FAILED( hr = g_pJoystick->SetDataFormat( &c_dfDIJoystick2 ) ) )
return C_FAIL;
// Set the cooperative level to let DInput know how this device should
// interact with the system and with other DInput applications.
if( VP_FAILED( hr = g_pJoystick->SetCooperativeLevel( hDlg , DISCL_EXCLUSIVE |
DISCL_FOREGROUND ) ) )
return C_FAIL;
// Enumerate the g_pJoystick objects. The callback function enabled user
// interface elements for objects that are found, and sets the min/max
// values property for discovered axes.
if( VP_FAILED( hr = g_pJoystick->EnumObjects( EnumObjectsCallback,
( VOID* )hDlg, DIDFT_ALL ) ) )
return C_FAIL;
return C_OK;
}
DEFINE_THREAD_ROUTINE( navdata_update, nomParams )
{
C_RESULT res;
int32_t i, size;
uint32_t cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
struct timeval tv;
#ifdef _WIN32
int timeout_for_windows=1000/*milliseconds*/;
#endif
navdata_t* navdata = (navdata_t*) &navdata_buffer[0];
tv.tv_sec = 1/*second*/;
tv.tv_usec = 0;
res = C_OK;
if( VP_FAILED(vp_com_open(COM_NAVDATA(), &navdata_socket, &navdata_read, &navdata_write)) )
{
printf("VP_Com : Failed to open socket for navdata\n");
res = C_FAIL;
}
if( VP_SUCCEEDED(res) )
{
PRINT("Thread navdata_update in progress...\n");
#ifdef _WIN32
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_for_windows, sizeof(timeout_for_windows));
/* Added by Stephane to force the drone start sending data. */
if(navdata_write)
{
int sizeinit = 5;
navdata_write( (void*)&navdata_socket, (int8_t*)"Init", &sizeinit );
}
#else
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
#endif
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
// if init failed for an handler we set its process function to null
// We keep its release function for cleanup
if( VP_FAILED( ardrone_navdata_handler_table[i].init(ardrone_navdata_handler_table[i].data) ) )
ardrone_navdata_handler_table[i].process = NULL;
i ++;
}
navdata_thread_in_pause = FALSE;
while( VP_SUCCEEDED(res)
&& !ardrone_tool_exit()
&& bContinue )
{
if(navdata_thread_in_pause)
{
vp_os_mutex_lock(&navdata_client_mutex);
num_retries = NAVDATA_MAX_RETRIES + 1;
vp_os_cond_wait(&navdata_client_condition);
vp_os_mutex_unlock(&navdata_client_mutex);
}
if( navdata_read == NULL )
{
res = C_FAIL;
continue;
}
size = NAVDATA_MAX_SIZE;
navdata->header = 0; // Soft reset
res = navdata_read( (void*)&navdata_socket, (int8_t*)&navdata_buffer[0], &size );
#ifdef _WIN32
if( size <= 0 )
#else
if( size == 0 )
#endif
{
// timeout
PRINT("Timeout\n");
ardrone_navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
num_retries++;
}
else
num_retries = 0;
if( VP_SUCCEEDED( res ) )
{
if( navdata->header == NAVDATA_HEADER )
{
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// reset sequence number because of com watchdog
// This code is mandatory because we can have a com watchdog without detecting it on mobile side :
// Reconnection is fast enough (less than one second)
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
ardrone_tool_send_com_watchdog(); // acknowledge
}
if( navdata->sequence > sequence )
{
i = 0;
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &navdata_buffer[0], size - sizeof(navdata_cks_t) );
if( cks == navdata_cks )
{
while( ardrone_navdata_handler_table[i].init != NULL )
{
if( ardrone_navdata_handler_table[i].process != NULL )
ardrone_navdata_handler_table[i].process( &navdata_unpacked );
i++;
}
}
else
{
PRINT("[Navdata] Checksum failed : %d (distant) / %d (local)\n", navdata_cks, cks);
}
}
else
{
PRINT("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
// remaining = sizeof(navdata);
sequence = navdata->sequence;
}
}
}
// Release resources alllocated by handlers
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
ardrone_navdata_handler_table[i].release();
i ++;
}
}
vp_com_close(COM_NAVDATA(), &navdata_socket);
DEBUG_PRINT_SDK("Thread navdata_update ended\n");
return (THREAD_RET)res;
}
int test_drone_connection()
{
const char * passivdeModeHeader = "\r\n227 PASV ok (";
vp_com_socket_t ftp_client,ftp_client2;
char buffer[1024];
static Write ftp_write = NULL;
static Read ftp_read = NULL;
int bytes_to_send,received_bytes;
int i,L,x[6],port;
int timeout_windows = 1000; /*milliseconds*/
vp_os_memset(buffer,0,sizeof(buffer));
wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
ftp_client.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;
setsockopt((int32_t)ftp_client.priv,
SOL_SOCKET,
SO_RCVTIMEO,
(const char*)&timeout_windows, sizeof(timeout_windows)
);
bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",
"USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");
ftp_write(&ftp_client,buffer,&bytes_to_send);
Sleep(1000);
received_bytes = sizeof(buffer);
ftp_read(&ftp_client,buffer,&received_bytes);
if (received_bytes<1) {
vp_com_close(wifi_com(), &ftp_client);
return -3;
}
L=received_bytes-strlen(passivdeModeHeader);
for (i=0; i<L; i++) {
if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0) break;
}
if (i==L) {
vp_com_close(wifi_com(), &ftp_client);
return -4;
}
i+=strlen(passivdeModeHeader);
if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
{
vp_com_close(wifi_com(), &ftp_client);
return -5;
}
port=(x[4]<<8)+x[5];
wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
ftp_client2.protocol = VP_COM_TCP;
if(VP_FAILED(vp_com_init(wifi_com())))
{
vp_com_close(wifi_com(), &ftp_client2);
return -6;
}
if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
{
vp_com_close(wifi_com(), &ftp_client2);
return -7;
}
received_bytes = sizeof(buffer);
ftp_read(&ftp_client2,buffer,&received_bytes);
if (received_bytes>0) {
buffer[min(received_bytes,sizeof(buffer)-1)]=0;
printf("无人机版本 %s 被检测到 ... 按下 <Enter> 开始应用.\n",buffer);
getchar();
}
vp_com_close(wifi_com(), &ftp_client);
vp_com_close(wifi_com(), &ftp_client2);
return 0;
}
C_RESULT video_com_multisocket_stage_transform(video_com_multisocket_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
C_RESULT res;
fd_set rfds;
int retval;
int fs,maxfs;
struct timeval tv;
int i;
bool_t selectTimeout;
vp_os_mutex_lock(&out->lock);
out->size = 0;
for (i=0;i<cfg->nb_sockets;i++) {
if (1 == cfg->configs[i]->mustReconnect)
{
PDBG ("Will call connect");
PRINT ("Reconnecting ... ");
res = C_FAIL;
res = video_com_stage_connect (cfg->configs[i]);
PRINT ("%s\n", (VP_FAILED (res) ? "FAIL" : "OK"));
cfg->configs[i]->mustReconnect = 0;
}
}
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = 0;
out->buffers = (uint8_t **) vp_os_malloc (sizeof(uint8_t *)+cfg->buffer_size*sizeof(uint8_t));
out->buffers[0] = (uint8_t *)(out->buffers+1);
out->indexBuffer = 0;
// out->lineSize not used
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_PROCESSING)
{
/* Check which socket has data to read */
tv.tv_sec = 1;
tv.tv_usec = 0;
if(cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE) { tv.tv_sec = 0; }
FD_ZERO(&rfds);
maxfs=0;
for (i=0;i<cfg->nb_sockets;i++) {
if(cfg->configs[i]->connected) {
fs = (int)cfg->configs[i]->socket.priv;
if (fs>maxfs) maxfs=fs;
FD_SET(fs,&rfds);
}
}
retval = select(maxfs+1, &rfds, NULL, NULL, &tv);
/* Read the socket which has available data */
selectTimeout = FALSE;
i = -1;
if (retval>0)
{
if (cfg->last_active_socket!=-1)
{
i=cfg->last_active_socket;
fs = (int)cfg->configs[i]->socket.priv;
if (cfg->configs[i]->read && FD_ISSET(fs, &rfds))
{
out->size = cfg->configs[i]->buffer_size;
}
else
{
i = -1;
}
}
if (-1 == i)
{
for (i=0;i<cfg->nb_sockets;i++)
{
fs = (int)cfg->configs[i]->socket.priv;
if (cfg->configs[i]->read && FD_ISSET(fs, &rfds)) {
DEBUG_PRINT_SDK("Video multisocket : found data on port %s %d\n",
(cfg->configs[i]->socket.protocol==VP_COM_TCP)?"TCP":"UDP",cfg->configs[i]->socket.port);
cfg->last_active_socket = i;
if (VP_COM_TCP == cfg->configs[i]->protocol)
{
DEBUG_isTcp = 1;
}
else
{
DEBUG_isTcp = 0;
}
out->size = cfg->configs[i]->buffer_size;
break;
}
}
if(i == cfg->nb_sockets)
{
PRINT("%s:%d BUG !!!!!", __FUNCTION__, __LINE__);
selectTimeout = TRUE;
}
}
}
else
{
DEBUG_PRINT_SDK("%s\n",(retval==0)?"timeout":"error");
selectTimeout = TRUE;
}
if (FALSE == selectTimeout)
{
// DEBUG_PRINT_SDK ("Will read on socket %d\n", i);
// Actual first time read
res = cfg->configs[i]->read(&cfg->configs[i]->socket, out->buffers[0], &out->size);
if (VP_FAILED (res))
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->configs[i]->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
// Loop read to empty the socket buffers if needed
cfg->configs[i]->socket.block = VP_COM_DONTWAIT;
bool_t bContinue = TRUE;
int32_t readSize = cfg->configs[i]->buffer_size - out->size;
while (TRUE == bContinue)
{
// DEBUG_PRINT_SDK ("Will read %d octets from socket %d\n", readSize, i);
res = cfg->configs[i]->read(&cfg->configs[i]->socket, &(out->buffers[0][out->size]), &readSize);
if (VP_FAILED (res))
{
PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__);
perror ("Video_com_stage");
cfg->configs[i]->mustReconnect = 1;
out->size = 0;
vp_os_mutex_unlock (&out->lock);
return C_OK;
}
if (0 == readSize)
{
bContinue = FALSE;
}
out->size += readSize;
readSize = cfg->configs[i]->buffer_size - out->size;
}
cfg->configs[i]->socket.block = VP_COM_DEFAULT;
}
/* Resend a connection packet on UDP sockets */
if(!(cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE))
{
if( /* select timed out */ retval==0 )
{
for (i=0;i<cfg->nb_sockets;i++)
{
if( cfg->configs[i]->protocol == VP_COM_UDP )
{
// Send "1" for Unicast
// Send "2" to enable Multicast
char flag = 1; int len = sizeof(flag);
if ( cfg->configs[i]->socket.is_multicast == 1 )
flag = 2;
DEBUG_PRINT_SDK("Video multisocket : sending connection byte on port %s %d\n",
(cfg->configs[i]->socket.protocol==VP_COM_TCP)?"TCP":"UDP",cfg->configs[i]->socket.port);
cfg->configs[i]->write(&cfg->configs[i]->socket, (uint8_t*) &flag, &len);
}
}
}
}
if( (TRUE == selectTimeout || out->size == 0) && (!cfg->forceNonBlocking || (*(cfg->forceNonBlocking) == FALSE)) )
{
cfg->num_retries++;
}
else
cfg->num_retries = 0;
if((selectTimeout == TRUE) && cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE)
{
//PRINT("Debug %s:%d\n",__FUNCTION__,__LINE__);
/* No data are available here, but some are available in the next stage */
/* out->size=0 would restart the pipeline */
out->size=-1;
}
}
vp_os_mutex_unlock(&out->lock);
if (out->size > 0)
{
DEBUG_totalBytes += out->size;
static struct timeval DEBUG_now = {0, 0}, DEBUG_prev = {0, 0};
gettimeofday (&DEBUG_now, NULL);
float DEBUG_timeDiff = ((DEBUG_now.tv_sec - DEBUG_prev.tv_sec) * 1000.0) + ((DEBUG_now.tv_usec - DEBUG_prev.tv_usec) / 1000.0);
if (DEBUG_TIME_BITRATE_CALCULATION_MSEC <= DEBUG_timeDiff)
{
DEBUG_prev.tv_sec = DEBUG_now.tv_sec;
DEBUG_prev.tv_usec = DEBUG_now.tv_usec;
float DEBUG_instantBitrate = 8.0 * (float)(DEBUG_totalBytes) / DEBUG_TIME_BITRATE_CALCULATION_MSEC;
DEBUG_totalBytes = 0;
DEBUG_bitrate = 0.8 * DEBUG_bitrate + 0.2 * DEBUG_instantBitrate;
}
}
return C_OK;
}
//-----------------------------------------------------------------------------
// Enum each PNP device using WMI and check each device ID to see if it contains
// "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then itӳ an XInput device
// Unfortunately this information can not be found by just using DirectInput.
// Checking against a VID/PID of 0x028E/0x045E won't find 3rd party or future
// XInput devices.
//
// This function stores the list of xinput devices in a linked list
// at g_pXInputDeviceList, and IsXInputDevice() searchs that linked list
//-----------------------------------------------------------------------------
HRESULT SetupForIsXInputDevice()
{
IWbemServices* pIWbemServices = NULL;
IEnumWbemClassObject* pEnumDevices = NULL;
IWbemLocator* pIWbemLocator = NULL;
IWbemClassObject* pDevices[20] = {0};
BSTR bstrDeviceID = NULL;
BSTR bstrClassName = NULL;
BSTR bstrNamespace = NULL;
DWORD uReturned = 0;
bool bCleanupCOM = false;
UINT iDevice = 0;
VARIANT var;
HRESULT hr;
// CoInit if needed
hr = CoInitialize( NULL );
bCleanupCOM = SUCCEEDED( hr );
// Create WMI
hr = CoCreateInstance( __uuidof( WbemLocator ),
NULL,
CLSCTX_INPROC_SERVER,
__uuidof( IWbemLocator ),
( LPVOID* )&pIWbemLocator );
if( VP_FAILED( hr ) || pIWbemLocator == NULL )
goto LCleanup;
// Create BSTRs for WMI
bstrNamespace = SysAllocString( L"\\\\.\\root\\cimv2" ); if( bstrNamespace == NULL ) goto LCleanup;
bstrDeviceID = SysAllocString( L"DeviceID" ); if( bstrDeviceID == NULL ) goto LCleanup;
bstrClassName = SysAllocString( L"Win32_PNPEntity" ); if( bstrClassName == NULL ) goto LCleanup;
// Connect to WMI
hr = pIWbemLocator->ConnectServer( bstrNamespace, NULL, NULL, 0L,
0L, NULL, NULL, &pIWbemServices );
if( VP_FAILED( hr ) || pIWbemServices == NULL )
goto LCleanup;
// Switch security level to IMPERSONATE
CoSetProxyBlanket( pIWbemServices, RPC_C_AUTHN_WINNT, RPC_C_AUTHZ_NONE, NULL,
RPC_C_AUTHN_LEVEL_CALL, RPC_C_IMP_LEVEL_IMPERSONATE, NULL, 0 );
// Get list of Win32_PNPEntity devices
hr = pIWbemServices->CreateInstanceEnum( bstrClassName, 0, NULL, &pEnumDevices );
if( VP_FAILED( hr ) || pEnumDevices == NULL )
goto LCleanup;
// Loop over all devices
for(; ; )
{
// Get 20 at a time
hr = pEnumDevices->Next( 10000, 20, pDevices, &uReturned );
if( VP_FAILED( hr ) )
goto LCleanup;
if( uReturned == 0 )
break;
for( iDevice = 0; iDevice < uReturned; iDevice++ )
{
// For each device, get its device ID
hr = pDevices[iDevice]->Get( bstrDeviceID, 0L, &var, NULL, NULL );
if( SUCCEEDED( hr ) && var.vt == VT_BSTR && var.bstrVal != NULL )
{
// Check if the device ID contains "IG_". If it does, then itӳ an XInput device
// Unfortunately this information can not be found by just using DirectInput
if( wcsstr( var.bstrVal, L"IG_" ) )
{
// If it does, then get the VID/PID from var.bstrVal
DWORD dwPid = 0, dwVid = 0;
WCHAR* strVid = wcsstr( var.bstrVal, L"VID_" );
if( strVid && swscanf( strVid, L"VID_%4X", &dwVid ) != 1 )
dwVid = 0;
WCHAR* strPid = wcsstr( var.bstrVal, L"PID_" );
if( strPid && swscanf( strPid, L"PID_%4X", &dwPid ) != 1 )
dwPid = 0;
DWORD dwVidPid = MAKELONG( dwVid, dwPid );
// Add the VID/PID to a linked list
XINPUT_DEVICE_NODE* pNewNode = new XINPUT_DEVICE_NODE;
if( pNewNode )
{
pNewNode->dwVidPid = dwVidPid;
pNewNode->pNext = g_pXInputDeviceList;
g_pXInputDeviceList = pNewNode;
}
}
}
SAFE_RELEASE( pDevices[iDevice] );
}
}
LCleanup:
if( bstrNamespace )
SysFreeString( bstrNamespace );
if( bstrDeviceID )
SysFreeString( bstrDeviceID );
if( bstrClassName )
SysFreeString( bstrClassName );
for( iDevice = 0; iDevice < 20; iDevice++ )
SAFE_RELEASE( pDevices[iDevice] );
SAFE_RELEASE( pEnumDevices );
SAFE_RELEASE( pIWbemLocator );
SAFE_RELEASE( pIWbemServices );
return hr;
}
C_RESULT video_com_stage_connect (video_com_config_t *cfg)
{
#ifdef _WIN32
int timeout_for_windows=1000; /* timeout in milliseconds */
int sizeinit;
#else
struct timeval tv;
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
#endif
C_RESULT res = C_FAIL;
if (TRUE == cfg->connected)
{
PDBG ("Will close");
res = vp_com_close (cfg->com, &cfg->socket);
cfg->connected = FALSE;
if (VP_FAILED (res))
{
PDBG ("Close failed");
return res;
}
}
if( cfg->protocol == VP_COM_PROBE)
{
PRINT("\n\nPROBING\n");
cfg->socket.protocol = VP_COM_TCP;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
PRINT("\n\nTCP\n");
vp_com_close (cfg->com, &cfg->socket);
cfg->protocol = VP_COM_TCP;
}
else
{
PRINT("\n\nUDP\n");
cfg->protocol = VP_COM_UDP;
}
}
if( cfg->protocol == VP_COM_UDP )
{
cfg->socket.protocol = VP_COM_UDP;
cfg->socket.is_multicast = 0; // disable multicast for video
cfg->socket.multicast_base_addr = MULTICAST_BASE_ADDR;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi = SOCKET_BUFFER_SIZE;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
PDBG ("New buffer size : %d", numi);
numi1 = 0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
cfg->connected = TRUE;
}
}
else if( cfg->protocol == VP_COM_TCP )
{
PDBG ("Will open TCP");
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
PDBG ("Success open");
vp_com_sockopt(cfg->com, &cfg->socket, cfg->sockopt);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi = SOCKET_BUFFER_SIZE;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
PDBG ("NEW buffer size : %d", numi);
numi1 = 0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
cfg->connected = TRUE;
}
}
#ifdef _WIN32
sizeinit = strlen("Init");
vp_com_write_socket(&cfg->socket,"Init",&sizeinit);
#endif
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, 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;
}
