Imported Upstream version 2.1.8.20120912upstream/2.1.8.20120912

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;