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