Exemple #1
0
C_RESULT ardrone_tool_setup_com( const char* ssid )
{
	LOGII("not in lib");
  C_RESULT res = C_OK;

#ifdef CHECK_WIFI_CONFIG
  if( FAILED(vp_com_init(COM_NAVDATA())) )
  {
	  DEBUG_PRINT_SDK("VP_Com : Failed to init com for navdata\n");
	  vp_com_shutdown(COM_NAVDATA());
	  res = C_FAIL;
  }

  vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);

  if( SUCCEED(res) && FAILED(vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA())) )
  {
	  DEBUG_PRINT_SDK("VP_Com : Failed to configure com for navdata\n");
	  vp_com_shutdown(COM_NAVDATA());
	  res = C_FAIL;
  }

  if( ssid != NULL )
  {
	  strncpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid, VP_COM_NAME_MAXSIZE-1 );
		((vp_com_wifi_connection_t*)wifi_connection())->networkName[VP_COM_NAME_MAXSIZE-1]='\0';
  }

  if( SUCCEED(res) && FAILED(vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS)))
  {
	  DEBUG_PRINT_SDK("VP_Com: Failed to connect for navdata\n");
	  vp_com_shutdown(COM_NAVDATA());
	  res = C_FAIL;
  }
#else  
  vp_com_init(COM_NAVDATA());
  vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
  vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());

  if( ssid != NULL )
	{
		strncpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid, VP_COM_NAME_MAXSIZE-1 );
		((vp_com_wifi_connection_t*)wifi_connection())->networkName[VP_COM_NAME_MAXSIZE-1]='\0';
	}

  vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
  ((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
#endif

  return res;
}
C_RESULT ardrone_tool_setup_com( const char* ssid )
{
    C_RESULT res = C_OK;

    vp_com_init(COM_NAVDATA());
    vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
    vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
    ((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
    return res;
}
C_RESULT
vp_stages_output_com_stage_open(vp_stages_output_com_config_t *cfg)
{
  C_RESULT res;

  res = vp_com_init(cfg->com);

  if( VP_SUCCEEDED(res) )
  {
    vp_com_local_config(cfg->com, cfg->config);

    if(cfg->connection && !cfg->connection->is_up)
    {
      res = vp_com_connect(cfg->com, cfg->connection, 1);
    }
  }

  if( VP_SUCCEEDED(res) && VP_FAILED(vp_com_open(cfg->com, &cfg->socket, 0, &cfg->write)))
    res = C_FAIL;

  if( VP_SUCCEEDED(res) )
  {
    if(cfg->socket.type == VP_COM_SERVER)
    {
      res = vp_com_wait_connections(cfg->com, &cfg->socket, &cfg->socket_client, 1);
    }
    else
    {
      vp_os_memcpy(&cfg->socket_client, &cfg->socket, sizeof(vp_com_socket_t));
    }

    vp_com_sockopt(cfg->com, &cfg->socket_client, cfg->sockopt);
  }

  // \todo test
  return res;
}
DEFINE_THREAD_ROUTINE_STACK( vp_com_server, thread_params, VP_COM_THREAD_SERVER_STACK_SIZE )
{

  vp_com_socket_t client_sockets[VP_COM_THREAD_NUM_MAX_CLIENTS];
  struct timeval tv, *ptv;

  // This thread setup connection then loop & wait for a socket event
  vp_com_server_thread_param_t* params = (vp_com_server_thread_param_t*) thread_params;

  int32_t i, rc, ncs, s, max = 0, num_server_sockets = params->num_servers, num_client_sockets = 0;
  vp_com_socket_t* server_sockets = params->servers;
  fd_set read_fs;

  vp_os_memset( client_sockets, 0, sizeof( client_sockets ));

  if(VP_FAILED(vp_com_init(params->com)))
  {
    DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to init com\n");
    vp_com_shutdown(params->com);
  }
  else if(VP_FAILED(vp_com_local_config(params->com, params->config)))
  {
    DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to configure com\n");
    vp_com_shutdown(params->com);
  }
  else if(VP_FAILED(vp_com_connect(params->com, params->connection, 1)))
  {
    DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to connect\n");
    vp_com_shutdown(params->com);
  }
  else
  {
    vp_os_mutex_lock(&server_initialisation_mutex);
    vp_os_cond_signal(&server_initialisation_wait);
    vp_os_mutex_unlock(&server_initialisation_mutex);

    server_init_not_finished = FALSE;

    for( i = 0; i < num_server_sockets; i++ )
    {
      if(VP_FAILED( vp_com_open_socket(&server_sockets[i], NULL, NULL) ))
      {
        DEBUG_PRINT_SDK("[VP_COM_SERVER] Unable to open server socket\n");
        server_sockets[i].is_disable = TRUE;
      }
      else
      {
        listen((int32_t)server_sockets[i].priv, server_sockets[i].queue_length);
      }
    }

    params->run = TRUE;

    while( params->run == TRUE )
    {
      if( params->timer_enable == FALSE || ( params->wait_sec == 0 && params->wait_usec == 0 ) )
      {
        ptv = NULL;
      }
      else
      {
        tv.tv_sec   = params->wait_sec;
        tv.tv_usec  = params->wait_usec;
        ptv         = &tv;
      }

      FD_ZERO(&read_fs);
      max = vp_com_fill_read_fs( &server_sockets[0], num_server_sockets, 0, &read_fs );
      max = vp_com_fill_read_fs( &client_sockets[0], num_client_sockets, max, &read_fs );

      rc = select( max + 1, &read_fs, NULL, NULL, ptv );
      if( rc == -1 && ( errno == EINTR || errno == EAGAIN ) )
        continue;

      if( rc == 0 )
      {
        DEBUG_PRINT_SDK("[VP_COM_SERVER] select timeout\n");

        vp_com_close_client_sockets(&client_sockets[0], num_client_sockets);
        num_client_sockets = 0;

        params->timer_enable  = FALSE;
        vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
      }

      for( i = 0; i < num_server_sockets && rc != 0; i++ )
      {
        s = (int32_t) server_sockets[i].priv;

        if( ( !server_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
        {
          rc --;

          // Recycle previously released sockets
          for( ncs = 0; ncs < num_client_sockets && client_sockets[ncs].priv != NULL; ncs++ );

          if( ncs < VP_COM_THREAD_NUM_MAX_CLIENTS)
          {
            if( VP_SUCCEEDED(vp_com_client_open_socket(&server_sockets[i], &client_sockets[ncs])) && ( ncs == num_client_sockets ) )
              num_client_sockets ++;
          }
        }
      }

      for( i = 0; i < num_client_sockets && rc != 0; i++ )
      {
        s = (int32_t) client_sockets[i].priv;
        if( ( !client_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
        {
          rc--;

          vp_com_client_receive( &client_sockets[i] );
        }
      }
    }

    for( i = 0; i < num_server_sockets; i++ )
    {
      vp_com_close_socket(&server_sockets[i]);
    }
  }

  vp_com_disconnect(params->com);
  vp_com_shutdown(params->com);


  THREAD_RETURN( 0 );
}