aboutsummaryrefslogtreecommitdiffstats
path: root/cphidgetspatial.c
diff options
context:
space:
mode:
Diffstat (limited to 'cphidgetspatial.c')
-rw-r--r--cphidgetspatial.c99
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;