/*---------------------------------------------------------------------------------------------------------------------
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;
}
示例#5
0
//-----------------------------------------------------------------------------
// 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;
}
示例#6
0
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);
}
示例#8
0
//-----------------------------------------------------------------------------
// 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;
}
示例#11
0
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;
}
示例#16
0
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;
}
示例#18
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;
}
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;
}
示例#22
0
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;
}
示例#26
0
//-----------------------------------------------------------------------------
// 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;
}
示例#28
0
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;
}