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;
}
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);
}
}
