// read X/Y/Z registers and swap endian void ITG3200getXYZ() { int i; #ifndef USE_ITG3200_INTERRUPT // poll status until raw data ready itgregs[INT_STATUS] = 0; while (!(itgregs[INT_STATUS] & RAW_RDY_EN)) MasterI2C0Read(ITG3200_SLAVE_ADDRESS,&itgregs[INT_STATUS],INT_STATUS,1); // read status #endif #if 1 itgready = false; MasterI2C0Read(ITG3200_SLAVE_ADDRESS,&itgregs[XOUT_H],XOUT_H,6); // read X,Y,Z memcpy(ITG3200values,&itgregs[XOUT_H],6); for (i=0; i<3; i++) swapb((unsigned short *)&ITG3200values[i]); #else // sanity check that we are talking I2C_Send1(ITG3200_SLAVE_ADDRESS, XOUT_H); ITG3200values[0] = I2C_Recv2(ITG3200_SLAVE_ADDRESS); // read ITG3200_WHO_AM_I I2C_Send1(ITG3200_SLAVE_ADDRESS, YOUT_H); ITG3200values[1] = I2C_Recv2(ITG3200_SLAVE_ADDRESS); // read ITG3200_WHO_AM_I I2C_Send1(ITG3200_SLAVE_ADDRESS, ZOUT_H); ITG3200values[2] = I2C_Recv2(ITG3200_SLAVE_ADDRESS); // read ITG3200_WHO_AM_I for (i=0; i<3; i++) swapb((unsigned short *)&ITG3200values[i]); #endif }
void qhsort(void* start, int n, int sz, int (*compar)(void* a, void* b, void* handle), void* handle) { byte *vec = (byte*) start; int left=1, right=n-1; const int pivot = 0; assert(sort_depth < 16); if (n <= 1) return; sort_depth++; while (left < right) { while (left < n && compar(ELEM(left), ELEM(pivot), handle) < 0) { left++; } while (right >= 0 && compar(ELEM(right), ELEM(pivot), handle) > 0) { right--; } if (left < right) { swapb(ELEM(left), ELEM(right), sz); } } if (compar(ELEM(right), ELEM(pivot), handle) < 0) { swapb(ELEM(right), ELEM(pivot), sz); } qhsort(start, right, sz, compar, handle); qhsort(ELEM(right+1), n-right-1, sz, compar, handle); sort_depth--; }
/* * Internal helper functions for manipulating the heap */ static void swaph( MxHeap *h, int i, int j ) { MxHeapable *hi, *hj; swapb( &(h->data), i, j ); hi = (MxHeapable *) getpb ( &(h->data), i ); hj = (MxHeapable *) getpb ( &(h->data), j ); set_heap_pos( hi, i ); set_heap_pos( hj, j ); }
// read X/Y/Z registers and swap endian void ITG3200getXYZ(float *pfGyroX, float *pfGyroY,float *pfGyroZ) { int i; float fFactor; int16_t i16Temp; itgready = false; MasterI2C0Read(ITG3200_SLAVE_ADDRESS,&itgregs[XOUT_H],XOUT_H,6); // read X,Y,Z //DEBUG_TP6(1); // This code takes 2.7 us memcpy(ITG3200values,&itgregs[XOUT_H],6); for (i=0; i<3; i++) swapb((unsigned short *)&ITG3200values[i]); //DEBUG_TP6(0); // // Get the gyroscope conversion factor for the current data format. // fFactor = g_fMPU9150GyroFactors[3]; //DEBUG_TP5(1); // This code takes 1.7 us i16Temp = (int16_t)itgregs[XOUT_H]; i16Temp <<= 8; i16Temp += itgregs[XOUT_L]; *pfGyroX = (float)i16Temp; *pfGyroX *= fFactor; i16Temp = (int16_t)itgregs[YOUT_H]; i16Temp <<= 8; i16Temp += itgregs[YOUT_L]; *pfGyroY = (float)i16Temp; *pfGyroY *= fFactor; i16Temp = (int16_t)itgregs[ZOUT_H]; i16Temp <<= 8; i16Temp += itgregs[ZOUT_L]; *pfGyroZ = (float)i16Temp; *pfGyroZ *= fFactor; //DEBUG_TP5(0); }
void removeb( Block *b, int which ) { swapb( b, which, b->used - 1 ); chopb( b ); }