diff options
Diffstat (limited to 'cphidgetspatial.c')
-rw-r--r-- | cphidgetspatial.c | 99 |
1 files changed, 49 insertions, 50 deletions
diff --git a/cphidgetspatial.c b/cphidgetspatial.c index 2feccce..463d3ab 100644 --- a/cphidgetspatial.c +++ b/cphidgetspatial.c @@ -208,6 +208,12 @@ static void updateTimestamp(CPhidgetSpatialHandle phid, int time) phid->lastTimeCounter = time; } +static void updateLatestDataTime(CPhidgetSpatialHandle phid, int i) +{ + phid->latestDataTime.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) / 1000000; + phid->latestDataTime.microseconds = (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) % 1000000; +} + //dataInput - parses device packets CPHIDGETDATA(Spatial) int i = 0, j = 0, count = 0, dataRate = phid->dataRate, cal; @@ -290,9 +296,7 @@ CPHIDGETDATA(Spatial) else phid->dataBuffer[phid->bufferWritePtr].acceleration[2] = accelUncalib[2] * phid->accelGain2[2] + accelUncalib[0] * phid->accelFactor1[2] + accelUncalib[1] * phid->accelFactor2[2]; - - phid->latestDataTime.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) / 1000000; - phid->latestDataTime.microseconds = (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) % 1000000; + updateLatestDataTime(phid, i); phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime; @@ -323,49 +327,6 @@ CPHIDGETDATA(Spatial) } break; } - // 1043 (1049 Replacement v300) and 1041 (low-cost 0/0/3 v200) - case PHIDUID_SPATIAL_ACCEL_3AXIS_1041: - case PHIDUID_SPATIAL_ACCEL_3AXIS_1043: - { - int time; - int analogOrDigital = ((unsigned short)buffer[1]<<8) + (unsigned short)buffer[2]; - count = buffer[0]; - - if(count == 0) - goto done; - - //this timestamp is for the latest data - time = ((unsigned short)buffer[3]<<8) + (unsigned short)buffer[4]; - updateTimestamp(phid, time); - - //add data to data buffer - for(i=0;i<count;i++) - { - int countOffset = i * 6; //Each set of samples is 6 bytes - for(j=0;j<3;j++) - { - int indexOffset = j * 2; //Each value is 2 bytes - short accelData = (signed short)((unsigned short)buffer[5 + indexOffset + countOffset]<<8) + (unsigned short)buffer[6 + indexOffset + countOffset]; - - //digital accel - if(analogOrDigital & (0x01 << i)) - phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = (double)accelData / SPATIAL_MMA8451Q_BITS_PER_G; - //analog accel - else - phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = (double)accelData / SPATIAL_KXR94_2050_w_AD7689_BITS_PER_G; - } - - phid->latestDataTime.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) / 1000000; - phid->latestDataTime.microseconds = (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) % 1000000; - - phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime; - - phid->bufferWritePtr++; - if(phid->bufferWritePtr >= SPATIAL_DATA_BUFFER_SIZE) - phid->bufferWritePtr = 0; - } - break; - } case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056: case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN: //top 2 bits in buffer[0] are packet type @@ -526,8 +487,7 @@ CPHIDGETDATA(Spatial) phid->dataBuffer[phid->bufferWritePtr].magneticField[2] = PUNK_DBL; } - phid->latestDataTime.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) / 1000000; - phid->latestDataTime.microseconds = (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) % 1000000; + updateLatestDataTime(phid, i); phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime; @@ -602,7 +562,47 @@ CPHIDGETDATA(Spatial) break; } break; + case PHIDUID_SPATIAL_ACCEL_3AXIS_1041: + case PHIDUID_SPATIAL_ACCEL_3AXIS_1043: + { + int time; + int analogOrDigital = ((unsigned short)buffer[1]<<8) + (unsigned short)buffer[2]; + count = buffer[0]; + + if(count == 0) + goto done; + //this timestamp is for the latest data + time = ((unsigned short)buffer[3]<<8) + (unsigned short)buffer[4]; + updateTimestamp(phid, time); + + //add data to data buffer + for(i=0;i<count;i++) + { + int countOffset = i * 6; //Each set of samples is 6 bytes + for(j=0;j<3;j++) + { + int indexOffset = j * 2; //Each value is 2 bytes + short accelData = (signed short)((unsigned short)buffer[5 + indexOffset + countOffset]<<8) + (unsigned short)buffer[6 + indexOffset + countOffset]; + + //digital accel + if(analogOrDigital & (0x01 << i)) + phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = (double)accelData / SPATIAL_MMA8451Q_BITS_PER_G; + //analog accel + else + phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = (double)accelData / SPATIAL_KXR94_2050_w_AD7689_BITS_PER_G; + } + + updateLatestDataTime(phid, i); + + phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime; + + phid->bufferWritePtr++; + if(phid->bufferWritePtr >= SPATIAL_DATA_BUFFER_SIZE) + phid->bufferWritePtr = 0; + } + break; + } case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042: case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044: { @@ -653,8 +653,7 @@ CPHIDGETDATA(Spatial) phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = PUNK_DBL; } - phid->latestDataTime.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) / 1000000; - phid->latestDataTime.microseconds = (phid->timestamp.microseconds + (i + 1) * phid->dataRateMax * 1000) % 1000000; + updateLatestDataTime(phid, i); phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime; |