Ejemplo n.º 1
0
Graph *generateRandomGraph(int numNodes) {
  
 Graph *g;

 int i, stride, n;

 g = (Graph *) malloc(sizeof(Graph));
 g->vertices = (VertexRecord *) malloc(numNodes*sizeof(VertexRecord));
 g->V = numNodes;
 
 g->E = 4* g->V ;
 g->edges = (int *) malloc( (1 + g->E) * sizeof(int));
 stride = intSqrt(g->V);

 n = 0;
 for (i = 0; i<g->V; i++) {
   g->vertices[i].index = i;
   g->vertices[i].firstEdge = n;
   g->vertices[i].numEdges = 4;
   g->vertices[i].weight = 1.0;

   g->edges[n++] = (i + numNodes - 1) % numNodes;
   g->edges[n++] = (i + 1) % numNodes;
   
   g->edges[n++] = (i +numNodes - stride) % numNodes;
   g->edges[n++] = (i + stride) % numNodes;
 
 }
return g;
}
Ejemplo n.º 2
0
void Estimator2(int16_t *IMUDataInt,double *angle)
{

	int16_t * imuBiases;
	int16_t IMUData[6];
	imuBiases = GetIMUBiases();
	// subtract the bias from acc and gyro:
	for (int j=0; j<6; j++)
		IMUData[j]=IMUDataInt[j] - imuBiases[j]/20.0;
		
	IMUData[2] +=256; //need to feal the gravity
	
	int16_t accPhi = fixATan2(IMUData[1],IMUData[2]);
	int16_t accTeta = fixATan2(IMUData[0],intSqrt((int32_t)IMUData[1]*IMUData[1]+(int32_t)IMUData[2]*IMUData[2]));

	
	angle[0] = accPhi;
	angle[1] = accTeta;

	for(uint8_t i=0;i<2;i++){
		setOutputData(angle[i]*180 / PI,i+9);
	}
}