void beggingForSemaphore(void* p)
{
    SoftTimer myTimer(50);
    while(1)
    {
        if(xSemaphoreTake(buttonSemaphore, portMAX_DELAY))
        {
            uart0_puts("TASK HAS SEMAPHORE!");
            xSemaphoreGive(buttonSemaphore);
            myTimer.reset();
            while(!myTimer.expired())
            {

            }
        }
    }
}
Exemple #2
0
//////////////////////////////////////////////////////////////////////////////////
//
// 入口点函数
//
//////////////////////////////////////////////////////////////////////////////////
int main( void )
{
	lifeInfo emberInfo( 30, 20, 200, 100, "ember" );
	lifeInfo evilInfo( 20, 15, 100, 100, "evil" );

	srand( (unsigned int)time(NULL) );
	timer myTimer( 200 );

	emberManager em( emberInfo, "ember", "testModel1.txt", 
					 evilInfo, "jack", "testModel2.txt" );

	while ( 1 )
	{
		if( myTimer.isTimeToDoSomething() )
		{
			em.update();
		}
	}


	return 0;
}
Exemple #3
0
Foam::fileStat::fileStat(const fileName& fName, const unsigned int maxTime)
{
    // Work on volatile
    volatile bool locIsValid = false;

    timer myTimer(maxTime);

    if (!timedOut(myTimer))
    {
        if (::stat(fName.c_str(), &status_) != 0)
        {
            locIsValid = false;
        }
        else
        {
            locIsValid = true;
        }
    }

    // Copy into (non-volatile, possible register based) member var
    isValid_ = locIsValid;
}
Exemple #4
0
void process()
{
/*
	static int tmc=0;
	tmc++;
	if(tmc==30)
	{
		tmc=0;
		set_indoor();
	}
*/
	int x, y;


	//初始化背景
	if(bInited==0)
	{
		static int initCount=0;
		if(initCount<30)
		{
			initCount++;
			img_copy(&BACK, &Y);
		}
		else
		{
			bInited=1;
			img_fill(&UBAK, 0);
			set_target_pan_tilt(0, 0);
			set_zoom(1);
		}
		return;
	}

	img_fill(&COMP, 0);
	img_fill(&COMPB, 0);
	img_fill(&CLUS, 0);
	img_fill(&LABEL, 0); 

	//计算准备
	for (y = masky1; y < masky2; y++)
		for (x = maskx1; x < maskx2; x++)
		{
			COMP.data[y][x] = abs(Y.data[y][x] - PREV.data[y][x]) > compare_th ? 255 : 0;
			if(Y.data[y][x] < dark || PREV.data[y][x] < dark)
				COMP.data[y][x] = abs(Y.data[y][x] - PREV.data[y][x]) > compare_th*dark_th ? 255 : 0;

			COMPB.data[y][x] = abs(Y.data[y][x] - BACK.data[y][x]) > compare_th2 ? 255 : 0;
			if(Y.data[y][x] < dark || BACK.data[y][x] < dark)
				COMPB.data[y][x] = abs(Y.data[y][x] - BACK.data[y][x]) > compare_th2*dark_th ? 255 : 0;
		}
	img_copy(&PREV, &Y);

	

	//选择性更新背景
	updataBack();

	//聚类
	cluster();

	img_copy(&LABEL, &CLUS);

	//连通域
	int k=label();

	img_plot_rect(&Y, maskx1, masky1, 
			maskx2 - maskx1, masky2 - masky1, 128);

	
	//输出连通域信息
	int i=0;
	mon("count: %d\n", k);
	for(i=0;i<k;i++)
	{
		mon("area%d: %d\n", i, Box[i].area);
		
	}

	float p=0,t=0,z=1;
	int ml=IMG_W,mr=0,cx=0,dx;
	for(i=0;i<k;i++)
	{
		if(Box[i].left < ml) ml = Box[i].left;
		if(Box[i].right > mr) mr = Box[i].right;
	}
	cx=(ml+mr)/2;
	dx=(mr-ml)*1.3;
	
	p = (float)(cx - IMG_W/2) / (IMG_W/2) * (PAN_RANGE/2);
	

	z = CAM_RANGE / ((float)dx / IMG_W * PAN_RANGE);

	if(z<1.0)
		z=1;
	
	printf("--------%d\n",k);

	myTimer(&p,&t,&z,k);
	set_target_pan_tilt(p, t);
	set_zoom(z);
	
	
	mon("PZ:%f,%f\n", p, z);
	
}
bool Foam::ping
(
    const word& destName,
    const label destPort,
    const label timeOut
)
{
    char *serverAddress;
    struct in_addr *ptr;
    struct hostent *hostPtr;
    volatile int sockfd;
    struct sockaddr_in destAddr;      // will hold the destination addr
    u_int addr;

    if ((hostPtr = gethostbyname(destName.c_str())) == NULL)
    {
        FatalErrorIn
        (
            "Foam::ping(const word&, const label)"
        )   << "gethostbyname error " << h_errno << " for host " << destName
            << abort(FatalError);
    }

    // Get first of the SLL of addresses
    serverAddress = *(hostPtr->h_addr_list);
    ptr = reinterpret_cast<struct in_addr*>(serverAddress);
    addr = ptr->s_addr;

    // Allocate socket
    sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd < 0)
    {
        FatalErrorIn
        (
            "Foam::ping(const word&, const label)"
        )   << "socket error"
            << abort(FatalError);
    }

    // Fill sockaddr_in structure with dest address and port
    memset (reinterpret_cast<char *>(&destAddr), '\0', sizeof(destAddr));
    destAddr.sin_family = AF_INET;
    destAddr.sin_port = htons(ushort(destPort));
    destAddr.sin_addr.s_addr = addr;


    timer myTimer(timeOut);

    if (timedOut(myTimer))
    {
        // Setjmp from timer jumps back to here
        fdClose(sockfd);
        return false;
    }

    if
    (
        connect
        (
            sockfd,
            reinterpret_cast<struct sockaddr*>(&destAddr),
            sizeof(struct sockaddr)
        ) != 0
    )
    {
        // Connection refused. Check if network was actually used or not.

        int connectErr = errno;

        fdClose(sockfd);

        if (connectErr == ECONNREFUSED)
        {
            return true;
        }
        //perror("connect");

        return false;
    }

    fdClose(sockfd);

    return true;
}
Exemple #6
0
void marginalize(
			std::vector<aslam::backend::DesignVariable*>& inDesignVariables,
			std::vector<aslam::backend::ErrorTerm*>& inErrorTerms,
			int numberOfInputDesignVariablesToRemove,
			bool useMEstimator,
			boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm>& outPriorErrorTermPtr,
			Eigen::MatrixXd& outCov,
			std::vector<aslam::backend::DesignVariable*>& outDesignVariablesInRTop,
			size_t numTopRowsInCov,
			size_t /* numThreads */)
{
		  SM_WARN_STREAM_COND(inDesignVariables.size() == 0, "Zero input design variables in the marginalizer!");

		  // check for duplicates!
		  std::unordered_set<aslam::backend::DesignVariable*> inDvSetHT;
		  for(auto it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
		  {
			  auto ret = inDvSetHT.insert(*it);
			  SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate design variables in input list!");
		  }
		  std::unordered_set<aslam::backend::ErrorTerm*> inEtSetHT;
		  for(auto it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
		  {
			  auto ret = inEtSetHT.insert(*it);
			  SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate error term in input list!");
		  }
		  SM_DEBUG_STREAM("NO duplicates in input design variables or input error terms found.");

          // Partition the design varibles into removed/remaining.
		  int dimOfDesignVariablesToRemove = 0;
		  std::vector<aslam::backend::DesignVariable*> remainingDesignVariables;
		  int k = 0;
		  size_t dimOfDvsInTopBlock = 0;
		  for(std::vector<aslam::backend::DesignVariable*>::const_iterator it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
		  {
			  if (k < numberOfInputDesignVariablesToRemove)
			  {
				  dimOfDesignVariablesToRemove += (*it)->minimalDimensions();
			  } else
			  {
				  remainingDesignVariables.push_back(*it);
			  }

			  if(dimOfDvsInTopBlock < numTopRowsInCov)
			  {
				  outDesignVariablesInRTop.push_back(*it);
			  }
			  dimOfDvsInTopBlock += (*it)->minimalDimensions();
			  k++;
		  }

		  // store original block indices to prevent side effects
		  std::vector<int> originalBlockIndices;
		  std::vector<int> originalColumnBase;
			// assign block indices
			int columnBase = 0;
			for (size_t i = 0; i < inDesignVariables.size(); ++i) {
				originalBlockIndices.push_back(inDesignVariables[i]->blockIndex());
				originalColumnBase.push_back(inDesignVariables[i]->columnBase());

				inDesignVariables[i]->setBlockIndex(i);
				inDesignVariables[i]->setColumnBase(columnBase);
			  columnBase += inDesignVariables[i]->minimalDimensions();
			}

			int dim = 0;
			std::vector<size_t> originalRowBase;
			for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
			{
				originalRowBase.push_back((*it)->rowBase());
				(*it)->setRowBase(dim);
				dim += (*it)->dimension();
			}

		  aslam::backend::DenseQrLinearSystemSolver qrSolver;
          qrSolver.initMatrixStructure(inDesignVariables, inErrorTerms, false);

		  SM_INFO_STREAM("Marginalization optimization problem initialized with " << inDesignVariables.size() << " design variables and " << inErrorTerms.size() << " error terrms");
		  SM_INFO_STREAM("The Jacobian matrix is " << dim << " x " << columnBase);

		  qrSolver.evaluateError(1, useMEstimator);
		  qrSolver.buildSystem(1, useMEstimator);


		  const Eigen::MatrixXd& jacobian = qrSolver.getJacobian();
		  const Eigen::VectorXd& b = qrSolver.e();

		  // check dimension of jacobian
		  int jrows = jacobian.rows();
		  int jcols = jacobian.cols();

		  int dimOfRemainingDesignVariables = jcols - dimOfDesignVariablesToRemove;
		  //int dimOfPriorErrorTerm = jrows;

		  // check the rank
		  Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(jacobian);
		  //lu_decomp.setThreshold(1e-20);
		  double threshold = lu_decomp.threshold();
		  int rank = lu_decomp.rank();
		  int fullRank = std::min(jacobian.rows(), jacobian.cols());
		  SM_DEBUG_STREAM("Rank of jacobian: " << rank << " (full rank: " << fullRank << ", threshold: " << threshold << ")");
		  bool rankDeficient = rank < fullRank;
		  if(rankDeficient)
		  {
			  SM_WARN("Marginalization jacobian is rank deficient!");
		  }
		  //SM_ASSERT_FALSE(aslam::Exception, rankDeficient, "Right now, we don't want the jacobian to be rank deficient - ever...");

		  Eigen::MatrixXd R_reduced;
		  Eigen::VectorXd d_reduced;
		  if (jrows < jcols)
		  {
			  SM_THROW(aslam::Exception, "underdetermined LSE!");
			  // underdetermined LSE, don't do QR
			  R_reduced = jacobian.block(0, dimOfDesignVariablesToRemove, jrows, jcols - dimOfDesignVariablesToRemove);
			  d_reduced = b;

		  } else {
			  // PTF: Do we know what will happen when the jacobian matrix is rank deficient?
			  // MB: yes, bad things!

              // do QR decomposition
			  sm::timing::Timer myTimer("QR Decomposition");
              Eigen::HouseholderQR<Eigen::MatrixXd> qr(jacobian);
			  Eigen::MatrixXd Q = qr.householderQ();
			  Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
			  Eigen::VectorXd d = Q.transpose()*b;
			  myTimer.stop();

			  if(numTopRowsInCov > 0)
			  {
				sm::timing::Timer myTimer("Covariance computation");
				Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(R);
				Eigen::MatrixXd Rinv = lu_decomp.inverse();
				Eigen::MatrixXd covariance = Rinv * Rinv.transpose();
				outCov = covariance.block(0, 0, numTopRowsInCov, numTopRowsInCov);
				myTimer.stop();
			  }

//			  size_t numRowsToKeep = rank - dimOfDesignVariablesToRemove;
//			  SM_ASSERT_TRUE_DBG(aslam::Exception, rankDeficient || (numRowsToKeep == dimOfRemainingDesignVariables), "must be the same if full rank!");

			  // get the top left block
			  SM_ASSERT_GE(aslam::Exception, R.rows(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " rows of R because it only has " << R.rows() << " rows.");
			  SM_ASSERT_GE(aslam::Exception, R.cols(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " cols of R because it only has " << R.cols() << " cols.");
			  //outRtop = R.block(0, 0, numTopRowsInRtop, numTopRowsInRtop);

              // cut off the zero rows at the bottom
              R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables, dimOfRemainingDesignVariables);
			  //R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, numRowsToKeep, dimOfRemainingDesignVariables);

              d_reduced = d.segment(dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables);
              //d_reduced = d.segment(dimOfDesignVariablesToRemove, numRowsToKeep);
              //dimOfPriorErrorTerm = dimOfRemainingDesignVariables;
		  }

		  // now create the new error term
		  boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> err(new aslam::backend::MarginalizationPriorErrorTerm(remainingDesignVariables, d_reduced, R_reduced));

		  outPriorErrorTermPtr.swap(err);

		  // restore initial block indices to prevent side effects
          for (size_t i = 0; i < inDesignVariables.size(); ++i) {
              inDesignVariables[i]->setBlockIndex(originalBlockIndices[i]);
              inDesignVariables[i]->setColumnBase(originalColumnBase[i]);
          }
          int index = 0;
          for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
          {
              (*it)->setRowBase(originalRowBase[index++]);
          }
}
Exemple #7
0
int main(int argc, char *argv[])
{
	WSADATA wsa;
	struct sockaddr_in server, address;
	int max_clients = 30, activity, addrlen, i, valread;

	//size of our receive buffer, this is string length.
	int MAXRECV = 1024;
	//set of socket descriptors
	fd_set readfds;
	//1 extra for null character, string termination
	char *buffer;
	buffer = (char*)malloc((MAXRECV + 1) * sizeof(char));

	for (i = 0; i < TOTAL_POSSIBLE_NEIGHBOURS; i++)
	{
		client_socket[i] = 0;
	}

	printf("\nInitialising Winsock...");
	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
	{
		printf("Failed. Error Code : %d", WSAGetLastError());
		exit(EXIT_FAILURE);
	}

	printf("Initialised.\n");

	//Create a socket
	if ((master = socket(AF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET)
	{
		printf("Could not create socket : %d", WSAGetLastError());
		exit(EXIT_FAILURE);
	}

	printf("Socket created.\n");

	//Prepare the sockaddr_in structure
	server.sin_family = AF_INET;
	server.sin_addr.s_addr = INADDR_ANY;
	server.sin_port = htons(MY_PORT);

	//Bind
	if (bind(master, (struct sockaddr *)&server, sizeof(server)) == SOCKET_ERROR)
	{
		printf("Bind failed with error code : %d", WSAGetLastError());
		exit(EXIT_FAILURE);
	}

	puts("Bind done");

	//Listen to incoming connections
	listen(master, 3);

	//Accept and incoming connection
	puts("Waiting for incoming connections...\n\n");

	addrlen = sizeof(struct sockaddr_in);

	extend_network(bootstrap_ip, port, activeNeighbours);
	myTimer(hTimerQueue);

	while (TRUE)
	{
		//clear the socket fd set
		FD_ZERO(&readfds);
		//add master socket to fd set
		FD_SET(master, &readfds);
		//add child sockets to fd set
		for (i = 0; i < 5; i++)
		{
			s = client_socket[i];
			if (s > 0)
			{
				FD_SET(s, &readfds);
			}
		}

		//wait for an activity on any of the sockets, timeout is NULL , so wait indefinitely
		activity = select(0, &readfds, NULL, NULL, NULL);

		if (activity == SOCKET_ERROR)
		{
			printf("[activity] select call failed with error code : %d", WSAGetLastError());
			getchar();
			exit(EXIT_FAILURE);
		}

		//If something happened on the master socket , then its an incoming connection
		if (FD_ISSET(master, &readfds))
		{
			if ((new_socket = accept(master, (struct sockaddr *)&address, (int *)&addrlen)) < 0)
			{
				perror("accept");
				getchar();
				exit(EXIT_FAILURE);
			}

			//inform user of socket number - used in send and receive commands
			printf("New connection , socket fd is %d , ip is : %s , port : %d \n", new_socket, inet_ntoa(address.sin_addr), ntohs(address.sin_port));

			//add new socket to array of sockets
			for (i = 0; i < max_clients; i++)
			{
				if (client_socket[i] == 0)
				{
					client_socket[i] = new_socket;
					printf("Adding to list of sockets at index %d \n", i);
					break;
				}
			}
		}

		//else its some IO operation on some other socket :)
		for (i = 0; i < max_clients; i++)
		{
			s = client_socket[i];
			//if client presend in read sockets             
			if (FD_ISSET(s, &readfds))
			{
				//get details of the client
				getpeername(s, (struct sockaddr*)&address, (int*)&addrlen);

				//Check if it was for closing , and also read the incoming message
				//recv does not place a null terminator at the end of the string (whilst printf %s assumes there is one).
				valread = recv(s, buffer, MAXRECV, 0);

				if (valread == SOCKET_ERROR)
				{
					int error_code = WSAGetLastError();
					if (error_code == WSAECONNRESET)
					{
						//Somebody disconnected , get his details and print
						printf("Host disconnected unexpectedly , ip %s , port %d \n", inet_ntoa(address.sin_addr), ntohs(address.sin_port));
						//removeFromNeighboursList(i, inet_ntoa(address.sin_addr));		
						//Close the socket and mark as 0 in list for reuse
						closesocket(s);
						client_socket[i] = 0;
					}
					else
					{
						printf("recv failed with error code : %d", error_code);
					}
				}
				if (valread == 0)
				{
					//Somebody disconnected , get his details and print
					printf("Host disconnected , ip %s , port %d \n", inet_ntoa(address.sin_addr), ntohs(address.sin_port));
					//removeFromNeighboursList(i, inet_ntoa(address.sin_addr));
					//Close the socket and mark as 0 in list for reuse
					closesocket(s);
					client_socket[i] = 0;
				}

				//Echo back the message that came in
				else
				{
					//add null character, if you want to use with printf/puts or other string handling functions
					process_receive(buffer, i);
				}
			}
		}
	}

	if (WaitForSingleObject(gDoneEvent, INFINITE) != WAIT_OBJECT_0)
		printf("WaitForSingleObject failed (%d)\n", GetLastError());

	closesocket(s);
	WSACleanup();

	getchar();

	return 0;
}