diff options
| author | Jonathan McCrohan <jmccrohan@gmail.com> | 2012-04-14 12:56:48 +0100 |
|---|---|---|
| committer | Jonathan McCrohan <jmccrohan@gmail.com> | 2012-04-14 12:56:48 +0100 |
| commit | 0b624384cd52be20e61284551d832b499d7b7707 (patch) | |
| tree | 6f95a4bbef47abc9720b96c0722e8f632aef228a /cphidgetspatial.c | |
| download | libphidget21-0b624384cd52be20e61284551d832b499d7b7707.tar.gz | |
Imported Upstream version 2.1.8.20120216upstream/2.1.8.20120216
Diffstat (limited to 'cphidgetspatial.c')
| -rw-r--r-- | cphidgetspatial.c | 1382 |
1 files changed, 1382 insertions, 0 deletions
diff --git a/cphidgetspatial.c b/cphidgetspatial.c new file mode 100644 index 0000000..41a8048 --- /dev/null +++ b/cphidgetspatial.c @@ -0,0 +1,1382 @@ +#include "stdafx.h"
+#include "cphidgetspatial.h"
+#include "cusb.h"
+#include "math.h"
+#include "csocket.h"
+#include "cthread.h"
+
+// === Internal Functions === //
+static double getCorrectedField(CPhidgetSpatialHandle phid, double fields[], int axis);
+
+//clearVars - sets all device variables to unknown state
+CPHIDGETCLEARVARS(Spatial)
+ int i = 0;
+
+ phid->dataRateMin = PUNI_INT;
+ phid->dataRate = PUNI_INT;
+ phid->dataRateMax = PUNI_INT;
+
+ phid->accelerationMax = PUNI_DBL;
+ phid->accelerationMin = PUNI_DBL;
+ phid->angularRateMax = PUNI_DBL;
+ phid->angularRateMin = PUNI_DBL;
+ phid->magneticFieldMax = PUNI_DBL;
+ phid->magneticFieldMin = PUNI_DBL;
+ phid->interruptRate = PUNI_INT;
+
+ phid->spatialDataNetwork = PUNI_BOOL;
+
+ for (i = 0; i<SPATIAL_MAX_ACCELAXES; i++)
+ {
+ phid->accelAxis[i] = PUNI_DBL;
+ }
+ for (i = 0; i<SPATIAL_MAX_GYROAXES; i++)
+ {
+ phid->gyroAxis[i] = PUNI_DBL;
+ }
+ for (i = 0; i<SPATIAL_MAX_COMPASSAXES; i++)
+ {
+ phid->compassAxis[i] = PUNI_DBL;
+ }
+ return EPHIDGET_OK;
+}
+
+//initAfterOpen - sets up the initial state of an object, reading in packets from the device if needed
+// used during attach initialization - on every attach
+CPHIDGETINIT(Spatial)
+ int i = 0;
+
+ TESTPTR(phid);
+
+ //Setup max/min values
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ if (phid->phid.deviceVersion < 200)
+ {
+ phid->accelerationMax = 5.1;
+ phid->accelerationMin = -5.1;
+ phid->interruptRate = 8;
+ phid->dataRateMin = SPATIAL_MIN_DATA_RATE;
+ phid->dataRate = phid->interruptRate;
+ phid->dataRateMax = 1; //actual data rate
+ phid->angularRateMax = 0;
+ phid->angularRateMin = 0;
+ phid->magneticFieldMax = 0;
+ phid->magneticFieldMin = 0;
+ phid->calDataValid = PFALSE;
+ }
+ else
+ return EPHIDGET_BADVERSION;
+ break;
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ if (phid->phid.deviceVersion < 300)
+ {
+ phid->accelerationMax = 5.1;
+ phid->accelerationMin = -5.1;
+ phid->interruptRate = 8;
+ phid->dataRateMin = SPATIAL_MIN_DATA_RATE;
+ phid->dataRate = phid->interruptRate;
+ phid->dataRateMax = 4; //actual data rate
+ phid->angularRateMax = 400.1;
+ phid->angularRateMin = -400.1;
+ phid->magneticFieldMax = 4.1;
+ phid->magneticFieldMin = -4.1;
+ phid->userMagField = 1.0;
+ phid->calDataValid = PFALSE;
+ }
+ // 1042 (Digital 3/3/3)
+ else if (phid->phid.deviceVersion >= 300 && phid->phid.deviceVersion < 400)
+ {
+ phid->accelerationMax = 5.1;
+ phid->accelerationMin = -5.1;
+ phid->interruptRate = 8;
+ phid->dataRateMin = SPATIAL_MIN_DATA_RATE;
+ phid->dataRate = phid->interruptRate;
+ phid->dataRateMax = 4; //actual data rate
+ phid->angularRateMax = 400.1;
+ phid->angularRateMin = -400.1;
+ phid->magneticFieldMax = 4.1;
+ phid->magneticFieldMin = -4.1;
+ phid->userMagField = 1.0;
+ phid->calDataValid = PFALSE;
+ }
+ // 1044 (1056 Replacement)
+ else if (phid->phid.deviceVersion >= 400 && phid->phid.deviceVersion < 500)
+ {
+ phid->accelerationMax = 5.1;
+ phid->accelerationMin = -5.1;
+ phid->interruptRate = 4;
+ phid->dataRateMin = SPATIAL_MIN_DATA_RATE;
+ phid->dataRate = 8;//phid->interruptRate;
+ phid->dataRateMax = 4; //actual data rate
+ phid->angularRateMax = 400.1;
+ phid->angularRateMin = -400.1;
+ phid->magneticFieldMax = 10.1;
+ phid->magneticFieldMin = -10.1;
+ phid->userMagField = 1.0;
+ phid->calDataValid = PFALSE;
+ }
+ else
+ return EPHIDGET_BADVERSION;
+ break;
+ default:
+ return EPHIDGET_UNEXPECTED;
+ }
+
+ //initialize triggers, set data arrays to unknown
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ phid->accelAxis[i] = PUNK_DBL;
+ phid->accelGain1[i] = PUNK_DBL;
+ phid->accelGain2[i] = PUNK_DBL;
+ phid->accelOffset[i] = PUNK_INT;
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ phid->gyroAxis[i] = PUNK_DBL;
+ phid->gryoCorrection[i] = 0;
+ phid->gyroGain1[i] = PUNK_DBL;
+ phid->gyroGain2[i] = PUNK_DBL;
+ phid->gyroOffset[i] = PUNK_INT;
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ phid->compassAxis[i] = PUNK_DBL;
+ phid->userCompassGain[i] = 1.0;
+ }
+ phid->bufferReadPtr = 0;
+ phid->bufferWritePtr = 0;
+ phid->timestamp.seconds = 0;
+ phid->timestamp.microseconds = 0;
+ phid->lastEventTime.seconds = 0;
+ phid->lastEventTime.microseconds = 0;
+ phid->latestDataTime.seconds = 0;
+ phid->latestDataTime.microseconds = 0;
+
+ phid->lastTimeCounterValid = PFALSE;
+ phid->doZeroGyro = PFALSE;
+
+ //get calibration values
+ switch(phid->phid.deviceIDSpec) {
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ // 1041, 1043 - no streamed calibration values
+ if (phid->phid.deviceVersion >= 200)
+ break;
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ // 1042, 1044 - no streamed calibration values
+ if (phid->phid.deviceVersion >= 300)
+ break;
+ else
+ {
+ unsigned char buffer[8] = { 0 };
+ int result;
+ int readCount = 125; // up to 1 second of data - should be PLENTY
+ //ask for calibration values
+ buffer[0] = SPATIAL_READCALIB;
+ if ((result = CUSBSendPacket((CPhidgetHandle)phid, buffer)) != EPHIDGET_OK)
+ return result;
+ while(phid->calDataValid == PFALSE && readCount--)
+ {
+ //note that Windows queues up to 32 packets, so we need to read at least this many to get the calibration packet
+ CPhidget_read((CPhidgetHandle)phid);
+ }
+ if(!phid->calDataValid)
+ return EPHIDGET_UNEXPECTED;
+ }
+ break;
+ default:
+ break;
+ }
+
+ //issue one read
+ //this should fill in the data because the dataRate is the interrupt rate
+ CPhidget_read((CPhidgetHandle)phid);
+
+ return EPHIDGET_OK;
+}
+
+//dataInput - parses device packets
+CPHIDGETDATA(Spatial)
+ int i = 0, j = 0, count = 0, dataRate = phid->dataRate, cal;
+ unsigned char doneGyroZero = PFALSE;
+ double accelAvg[SPATIAL_MAX_ACCELAXES], angularRateAvg[SPATIAL_MAX_ACCELAXES], magneticFieldAvg[SPATIAL_MAX_ACCELAXES], magneticFieldCorr[SPATIAL_MAX_ACCELAXES];
+ CPhidgetSpatial_SpatialEventDataHandle *eventData;
+
+ ZEROMEM(accelAvg, sizeof(accelAvg));
+ ZEROMEM(angularRateAvg, sizeof(angularRateAvg));
+ ZEROMEM(magneticFieldAvg, sizeof(magneticFieldAvg));
+
+ if (length<0) return EPHIDGET_INVALIDARG;
+ TESTPTR(phid);
+ TESTPTR(buffer);
+
+ //Parse device packets - store data locally
+ switch(phidG->deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ if (phid->phid.deviceVersion < 200)
+ {
+ int data;
+ double accelUncalib[3] = {0,0,0};
+ int time;
+
+ //top 2 bits in buffer[0] are packet type
+ switch(buffer[0] & 0xc0)
+ {
+ case SPATIAL_PACKET_DATA:
+ if(phid->calDataValid)
+ {
+ count = buffer[0] / 3;
+ if(count == 0)
+ goto done;
+
+ //this timestamp is for the latest data
+ time = ((unsigned short)buffer[1]<<8) + (unsigned short)buffer[2];
+ if(phid->lastTimeCounterValid)
+ {
+ //0-255 ms
+ int timechange = (unsigned short)((unsigned short)time - (unsigned short)phid->lastTimeCounter);
+ timechange *= 1000; //us
+
+ phid->timestamp.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + timechange) / 1000000;
+ phid->timestamp.microseconds = (phid->timestamp.microseconds + timechange) % 1000000;
+ }
+ else
+ {
+ phid->lastTimeCounterValid = PTRUE;
+ }
+ phid->lastTimeCounter = time;
+
+ //add data to data buffer
+ for(i=0;i<count;i++)
+ {
+ //LIS344ALH - Vdd/15 V/g - 0x1fff/15 = 0x222 (546.06666666666666666666666666667)
+ for(j=0;j<3;j++)
+ {
+ data = ((unsigned short)buffer[3 + j * 2 + i * 6]<<8) + (unsigned short)buffer[4 + j * 2 + i * 6];
+ accelUncalib[j] = ((double)data - 0x0fff) / 546.066667;
+ }
+ accelUncalib[1] = -accelUncalib[1]; //reverse Y-axis
+ //Apply offsets
+ for(j=0;j<3;j++)
+ {
+ accelUncalib[j] -= phid->accelOffset[j];
+ }
+ //X
+ if(accelUncalib[0] > 0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[0] = accelUncalib[0] * phid->accelGain1[0] + accelUncalib[1] * phid->accelFactor1[0] + accelUncalib[2] * phid->accelFactor2[0];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[0] = accelUncalib[0] * phid->accelGain2[0] + accelUncalib[1] * phid->accelFactor1[0] + accelUncalib[2] * phid->accelFactor2[0];
+ //Y
+ if(accelUncalib[1] > 0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[1] = accelUncalib[1] * phid->accelGain1[1] + accelUncalib[0] * phid->accelFactor1[1] + accelUncalib[2] * phid->accelFactor2[1];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[1] = accelUncalib[1] * phid->accelGain2[1] + accelUncalib[0] * phid->accelFactor1[1] + accelUncalib[2] * phid->accelFactor2[1];
+ //Z
+ if(accelUncalib[2] > 0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[2] = accelUncalib[2] * phid->accelGain1[2] + accelUncalib[0] * phid->accelFactor1[2] + accelUncalib[1] * phid->accelFactor2[2];
+ 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;
+
+ phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime;
+
+ phid->bufferWritePtr++;
+ if(phid->bufferWritePtr >= SPATIAL_DATA_BUFFER_SIZE)
+ phid->bufferWritePtr = 0;
+ }
+ }
+ break;
+ case SPATIAL_PACKET_CALIB:
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ cal = ((unsigned short)buffer[i*7 + 1]<<4) + ((unsigned short)buffer[i*7 + 2]>>4);
+ phid->accelGain1[i] = cal / (4096/0.4) + 0.8;
+ cal = (((unsigned short)buffer[i*7 + 2]<<8) & 0x0F00) | ((unsigned short)buffer[i*7 + 3]);
+ phid->accelGain2[i] = cal / (4096/0.4) + 0.8;
+ cal = (unsigned short)((unsigned short)buffer[i*7 + 4]<<8) + (unsigned short)buffer[i*7 + 5];
+ phid->accelOffset[i] = cal / (65535 / 1.0) - 0.5;
+ cal = (unsigned char)buffer[i*7 + 6];
+ phid->accelFactor1[i] = cal / (256 / 0.2) - 0.1;
+ cal = (unsigned char)buffer[i*7 + 7];
+ phid->accelFactor2[i] = cal / (256 / 0.2) - 0.1;
+ //LOG(PHIDGET_LOG_INFO, "Accel(%d) Calib: %1.4lf, %1.4lf, %1.4lf, %1.4lf, %1.4lf", i,
+ // phid->accelGain1[i], phid->accelGain2[i], phid->accelOffset[i], phid->accelFactor1[i], phid->accelFactor2[i]);
+ }
+ phid->calDataValid = PTRUE;
+ break;
+ }
+ }
+ else
+ return EPHIDGET_UNEXPECTED;
+ break;
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ if (phidG->deviceVersion < 300)
+ {
+ //top 2 bits in buffer[0] are packet type
+ switch(buffer[0])
+ {
+ case SPATIAL_PACKET_DATA:
+ if(phid->calDataValid)
+ {
+ int data;
+ double accelUncalib[3] = {0,0,0};
+ double gyroUncalib[3] = {0,0,0};
+ int time;
+
+ count = (buffer[1] & 0x1f) / 9;
+ if(count == 0)
+ goto done;
+
+ //this timestamp is for the latest data
+ time = ((unsigned short)buffer[2]<<8) + (unsigned short)buffer[3];
+ if(phid->lastTimeCounterValid)
+ {
+ //0-255 ms
+ int timechange = (unsigned short)((unsigned short)time - (unsigned short)phid->lastTimeCounter);
+ timechange *= 1000; //us
+
+ phid->timestamp.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + timechange) / 1000000;
+ phid->timestamp.microseconds = (phid->timestamp.microseconds + timechange) % 1000000;
+ }
+ else
+ {
+ phid->lastTimeCounterValid = PTRUE;
+ }
+ phid->lastTimeCounter = time;
+
+ //add data to data buffer
+ for(i=0;i<count;i++)
+ {
+ //LIS344ALH - Vdd/15 V/g - 0xffff/15 = 0x1111 (4369.0)
+ for(j=0;j<3;j++)
+ {
+ data = ((unsigned short)buffer[4 + j * 2 + i * 18]<<8) + (unsigned short)buffer[5 + j * 2 + i * 18];
+ accelUncalib[j] = ((double)data - 0x7fff) / 4369.0;
+ }
+ accelUncalib[1] = -accelUncalib[1]; //reverse Y-axis
+ //Apply offsets
+ for(j=0;j<3;j++)
+ {
+ accelUncalib[j] -= phid->accelOffset[j];
+ }
+ //X
+ if(accelUncalib[0] > 0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[0] = accelUncalib[0] * phid->accelGain1[0] + accelUncalib[1] * phid->accelFactor1[0] + accelUncalib[2] * phid->accelFactor2[0];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[0] = accelUncalib[0] * phid->accelGain2[0] + accelUncalib[1] * phid->accelFactor1[0] + accelUncalib[2] * phid->accelFactor2[0];
+ //Y
+ if(accelUncalib[1] > 0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[1] = accelUncalib[1] * phid->accelGain1[1] + accelUncalib[0] * phid->accelFactor1[1] + accelUncalib[2] * phid->accelFactor2[1];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[1] = accelUncalib[1] * phid->accelGain2[1] + accelUncalib[0] * phid->accelFactor1[1] + accelUncalib[2] * phid->accelFactor2[1];
+ //Z
+ if(accelUncalib[2] > 0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[2] = accelUncalib[2] * phid->accelGain1[2] + accelUncalib[0] * phid->accelFactor1[2] + accelUncalib[1] * phid->accelFactor2[2];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[2] = accelUncalib[2] * phid->accelGain2[2] + accelUncalib[0] * phid->accelFactor1[2] + accelUncalib[1] * phid->accelFactor2[2];
+
+ //ADC ref is 0-3.3V - 50.355uV/bit, gyro zero rate is 1.23V, 2.5mV/deg/s - these voltages are fixed, non-ratiometric to Vref
+ // 1 / 0.000050355 = 19859 (1V)
+ // 1.23 * 19859 = 24427
+ // 0.0025 * 19859 = 49.6477bits/deg/s
+ for(j=0;j<3;j++)
+ {
+ data = ((unsigned short)buffer[10 + j * 2 + i * 18]<<8) + (unsigned short)buffer[11 + j * 2 + i * 18];
+ if(j==1)
+ gyroUncalib[j] = ((double)(data-24427) + phid->gyroOffset[j]) / 49.6477;
+ else
+ gyroUncalib[j] = ((double)-(data-24427) + phid->gyroOffset[j]) / 49.6477; //reverse X/Z-axis
+ }
+ //0 - we multiply these by their gains so revered axes will still appear positive and get the correct gain
+ if((gyroUncalib[0] * phid->gyroGain1[0]) > 0)
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[0] = gyroUncalib[0] * phid->gyroGain1[0] - gyroUncalib[1] * phid->gyroFactor1[0] - gyroUncalib[2] * phid->gyroFactor2[0];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[0] = gyroUncalib[0] * phid->gyroGain2[0] - gyroUncalib[1] * phid->gyroFactor1[0] - gyroUncalib[2] * phid->gyroFactor2[0];
+ //1
+ if((gyroUncalib[1] * phid->gyroGain1[1]) > 0)
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[1] = gyroUncalib[1] * phid->gyroGain1[1] - gyroUncalib[0] * phid->gyroFactor1[1] - gyroUncalib[2] * phid->gyroFactor2[1];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[1] = gyroUncalib[1] * phid->gyroGain2[1] - gyroUncalib[0] * phid->gyroFactor1[1] - gyroUncalib[2] * phid->gyroFactor2[1];
+ //2
+ if((gyroUncalib[2] * phid->gyroGain1[2]) > 0)
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[2] = gyroUncalib[2] * phid->gyroGain1[2] - gyroUncalib[0] * phid->gyroFactor1[2] - gyroUncalib[1] * phid->gyroFactor2[2];
+ else
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[2] = gyroUncalib[2] * phid->gyroGain2[2] - gyroUncalib[0] * phid->gyroFactor1[2] - gyroUncalib[1] * phid->gyroFactor2[2];
+
+ //checks if compass data is valid
+ //Note: we miss ~7 samples (28ms) every second while the compass is callibrating
+ if(buffer[1] & (0x80 >> i))
+ {
+ //ADC 50.355uV/bit (0-3.3V)
+ //ideal compass midpoint is 0x7FFF (32767) (1.65V)
+ //valid range for zero field offset is: 0.825V - 2.475V (16384-49151) (+-16384)
+ // Note that this may be less (~3x) because the Gain is less, but I'm not sure. (+-5460)
+ //valid output voltage range is defined as 0.165V - 3.135V (3277-62258),
+ // so we can't really trust values outside of this, though we do seem to get valid data...
+ //ideal sensitivity is 250mV/gauss (ext. resistor), valid range is 195 - 305
+ // 1 / 0.000050355 = 19859 (1Volt)
+ // 0.250 * 19859 = 4964.75 bits/gauss (1.0 gain) (ideal) - valid range is (3861-6068) (+-1103)
+ //We have defined the compass gain multiplier to be based on 6500bits/gauss to keep the math resonable,
+ // so we must use that value here. Implications?
+ //The largest range we can guarantee is:
+ // 16384-3277/6068 = +-2.16 gauss or, more likely: +-3.96 gauss
+ // Ideal is: 32767-3277/4964.75 = +-5.94 gauss
+ // we can tell from the incoming data whether it's valid or not,
+ // we'll probably have more range in one dirrection then the other because of offset.
+ for(j=0;j<phid->phid.attr.spatial.numCompassAxes; j++)
+ {
+ data = ((unsigned short)buffer[16 + i * 18 + j * 2]<<8) + (unsigned short)buffer[17 + i * 18 + j * 2];
+ //if we are not within (3277-62258), data is not valid
+ if(data < 3277)
+ {
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMin;
+ break;
+ }
+ if(data > 62258)
+ {
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMax;
+ break;
+ }
+
+ //if gain or offset don't make sense, throw out data
+ //if(phid->compassGain[j] > 6068 || phid->compassGain[j] < 3861 ||
+ if(phid->compassGain[j] > 6068 || phid->compassGain[j] < 2500 || //lower gains seem to be common
+ phid->compassOffset[j] > 5460 || phid->compassOffset[j] < -5460)
+ {
+ if(data > 32767)
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMax;
+ else
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMin;
+ break;
+ }
+
+ //Convert ADC to Gauss
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] =
+ -((double)data - 0x7fff - phid->compassOffset[j]) / phid->compassGain[j];
+
+ //constrain to max/min
+ //ie if field is 4.02 and max is 4.1, make it 4.1, since real max is 4.0
+ if(phid->dataBuffer[phid->bufferWritePtr].magneticField[j] > (phid->magneticFieldMax - 0.1))
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMax;
+ if(phid->dataBuffer[phid->bufferWritePtr].magneticField[j] < (phid->magneticFieldMin + 0.1))
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMin;
+ }
+
+ }
+ else
+ {
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[0] = PUNK_DBL;
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[1] = PUNK_DBL;
+ 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;
+
+ phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime;
+
+ phid->bufferWritePtr++;
+ if(phid->bufferWritePtr >= SPATIAL_DATA_BUFFER_SIZE)
+ phid->bufferWritePtr = 0;
+ }
+ }
+ break;
+ case SPATIAL_PACKET_CALIB:
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ cal = ((unsigned short)buffer[i*7 + 1]<<4) + ((unsigned short)buffer[i*7 + 2]>>4);
+ phid->accelGain1[i] = cal / (4096/0.4) + 0.8;
+ cal = (((unsigned short)buffer[i*7 + 2]<<8) & 0x0F00) | ((unsigned short)buffer[i*7 + 3]);
+ phid->accelGain2[i] = cal / (4096/0.4) + 0.8;
+ cal = (unsigned short)((unsigned short)buffer[i*7 + 4]<<8) + (unsigned short)buffer[i*7 + 5];
+ phid->accelOffset[i] = cal / (65535 / 1.0) - 0.5;
+ cal = (unsigned char)buffer[i*7 + 6];
+ phid->accelFactor1[i] = cal / (256 / 0.2) - 0.1;
+ cal = (unsigned char)buffer[i*7 + 7];
+ phid->accelFactor2[i] = cal / (256 / 0.2) - 0.1;
+ //LOG(PHIDGET_LOG_INFO, "Accel(%d) Calib: %1.4lf, %1.4lf, %1.4lf, %1.4lf, %1.4lf", i,
+ // phid->accelGain1[i], phid->accelGain2[i], phid->accelOffset[i], phid->accelFactor1[i], phid->accelFactor2[i]);
+ }
+ for (j=0; j<phid->phid.attr.spatial.numGyroAxes; i++,j++)
+ {
+ if (phidG->deviceVersion < 200)
+ {
+ cal = ((unsigned short)buffer[i*7 + 1]<<4) + ((unsigned short)buffer[i*7 + 2]>>4);
+ phid->gyroGain1[j] = cal / (4096/0.4) + 0.8;
+ cal = (((unsigned short)buffer[i*7 + 2]<<8) & 0x0F00) | ((unsigned short)buffer[i*7 + 3]);
+ phid->gyroGain2[j] = cal / (4096/0.4) + 0.8;
+ }
+ //Allow for negative gains
+ else if (phidG->deviceVersion >= 200 && phidG->deviceVersion < 300)
+ {
+ cal = ((signed short)((signed char)buffer[i*7 + 1])<<4) + ((unsigned short)buffer[i*7 + 2]>>4);
+ phid->gyroGain1[j] = cal / (4096/0.4) + (cal > 0 ? 0.9 : -0.9);
+ cal = ((((unsigned short)buffer[i*7 + 2]<<8) & 0x0F00) | ((unsigned short)buffer[i*7 + 3]) ) << 4;
+ cal = (signed short)cal >> 4;
+ phid->gyroGain2[j] = cal / (4096/0.4) + (cal > 0 ? 0.9 : -0.9);
+ }
+ cal = (signed short)((unsigned short)buffer[i*7 + 4]<<8) + (unsigned short)buffer[i*7 + 5];
+ phid->gyroOffset[j] = cal;
+ cal = (unsigned char)buffer[i*7 + 6];
+ phid->gyroFactor1[j] = cal / (256 / 0.1) - 0.05;
+ cal = (unsigned char)buffer[i*7 + 7];
+ phid->gyroFactor2[j] = cal / (256 / 0.1) - 0.05;
+
+//Zero out calibrations
+#if 0
+ phid->gyroGain1[j] = 1;
+ phid->gyroGain2[j] = 1;
+ phid->gyroOffset[j] = 0;
+ phid->gyroFactor1[j] = 0;
+ phid->gyroFactor2[j] = 0;
+#endif
+
+ LOG(PHIDGET_LOG_VERBOSE, "Gyro(%d) Calib: %1.4lf, %1.4lf, %1.4lf, %1.4lf, %1.4lf", j,
+ phid->gyroGain1[j], phid->gyroGain2[j], phid->gyroOffset[j], phid->gyroFactor1[j], phid->gyroFactor2[j]);
+ }
+ for(j=0;j<phid->phid.attr.spatial.numCompassAxes; j++)
+ {
+ phid->compassOffset[j] = (signed short)((unsigned short)buffer[j*4 + 49]<<8) + (unsigned short)buffer[j*4 + 50];
+ phid->compassGain[j] = ((unsigned short)buffer[j*4 + 51]<<8) + (unsigned short)buffer[j*4 + 52];
+ //phid->compassGain[j] = 4964;
+ }
+ //LOG(PHIDGET_LOG_INFO, "Compass Gain: %d, %d, %d", phid->compassGain[0], phid->compassGain[1], phid->compassGain[2]);
+ //LOG(PHIDGET_LOG_INFO, "Compass Offset: %d, %d, %d", phid->compassOffset[0], phid->compassOffset[1], phid->compassOffset[2]);
+ phid->calDataValid = PTRUE;
+ break;
+ }
+ }
+ // 1044 (1056 Replacement) and 1042 (low-cost 3/3/3)
+ else if (phidG->deviceVersion >= 300 && phidG->deviceVersion < 500)
+ { //top 2 bits in buffer[0] are packet type
+ switch(buffer[0])
+ {
+ case SPATIAL_PACKET_DATA:
+ {
+ int data;
+ int time;
+
+ count = buffer[1] & 0x03;
+ if(count == 0)
+ goto done;
+
+ //this timestamp is for the latest data
+ time = ((unsigned short)buffer[2]<<8) + (unsigned short)buffer[3];
+ //LOG(PHIDGET_LOG_DEBUG, "TimeStamp: %6d",time);
+ if(phid->lastTimeCounterValid)
+ {
+ //1-255 ms
+ int timechange = (unsigned short)((unsigned short)time - (unsigned short)phid->lastTimeCounter);
+ timechange *= 1000; //us
+
+ if(timechange > 500000)
+ LOG(PHIDGET_LOG_DEBUG, "Timechange: %d",timechange);
+
+ phid->timestamp.seconds = phid->timestamp.seconds + (phid->timestamp.microseconds + timechange) / 1000000;
+ phid->timestamp.microseconds = (phid->timestamp.microseconds + timechange) % 1000000;
+ }
+ else
+ {
+ phid->lastTimeCounterValid = PTRUE;
+ }
+ phid->lastTimeCounter = time;
+
+ //add data to data buffer
+ for(i=0;i<count;i++)
+ {
+ //digital accel
+ if(buffer[1] & (0x08 >> i))
+ {
+ LOG(PHIDGET_LOG_DEBUG, "digital accel");
+ //+-8g resolution is 1g/1024/bit
+ for(j=0;j<3;j++)
+ {
+ short accelData =
+ (signed short)((unsigned short)buffer[4 + j * 2 + i * 18]<<8) + (unsigned short)buffer[5 + j * 2 + i * 18];
+
+ if(j==0)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = -((double)accelData / 1024.0);
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = (double)accelData / 1024.0;
+ }
+ }
+ //analog accel
+ else
+ {
+ //KXR94-2050 - 660 mV/g (VCC/5) - 0xffff/5 = 0x3333 (13107.0)
+ for(j=0;j<3;j++)
+ {
+ data = ((unsigned short)buffer[4 + j * 2 + i * 18]<<8) + (unsigned short)buffer[5 + j * 2 + i * 18];
+ if(j==2)
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = ((double)data - 0x7fff) / 13107;
+ else
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[j] = -((double)data - 0x7fff) / 13107;
+ }
+ phid->dataBuffer[phid->bufferWritePtr].acceleration[1] = -phid->dataBuffer[phid->bufferWritePtr].acceleration[1]; //reverse Y-axis
+ }
+
+ //digital gyro
+ if(buffer[1] & (0x20 >> i))
+ {
+ LOG(PHIDGET_LOG_DEBUG, "digital gyro");
+ //2000dps
+ // 0.07 dps/digit
+ for(j=0;j<3;j++)
+ {
+ short gyroData =
+ (signed short)((unsigned short)buffer[10 + j * 2 + i * 18]<<8) + (unsigned short)buffer[11 + j * 2 + i * 18];
+
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = (double)gyroData * 0.07;
+ }
+ }
+ //analog gyro
+ else
+ {
+ //ADC ref is 0-3.3V - 50.355uV/bit, gyro zero rate is 1.5V, X/Y: 2.5mV/deg/s Z: 3.572mV/deg/s
+ // these voltages are fixed, non-ratiometric to Vref
+ // 1 / 0.000050355 = 19859 (1V)
+ // 1.5 * 19859 = 29789
+ // 0.0025 * 19859 = 49.6477bits/deg/s
+ // 0.003572 * 19859 = 70.936348bits/deg/s
+ for(j=0;j<3;j++)
+ {
+ data = ((unsigned short)buffer[10 + j * 2 + i * 18]<<8) + (unsigned short)buffer[11 + j * 2 + i * 18];
+ if(j==0)
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = ((double)-(data-29789)) / 49.6477; //reverse X-axis
+ else if(j==2)
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = ((double)-(data-29789)) / 70.936348; //reverse Z-axis
+ else
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = ((double)(data-29789)) / 49.6477;
+ }
+ }
+
+ //LOG(PHIDGET_LOG_DEBUG, "ADC Values: %1.3lf, %1.3lf, %1.3lf, %1.3lf, %1.3lf, %1.3lf", dataTrack[0], dataTrack[1], dataTrack[2], dataTrack[3], dataTrack[4], dataTrack[5]);
+
+ //checks if compass data is valid
+ if(buffer[1] & (0x80 >> i))
+ {
+ for(j=0;j<phid->phid.attr.spatial.numCompassAxes; j++)
+ {
+ short magData =
+ (signed short)((unsigned short)buffer[16 + i * 18 + j * 2]<<8) + (unsigned short)buffer[17 + i * 18 + j * 2];
+
+ //each bit is 0.1 uT
+
+
+ //Convert to Gauss (divide by 1000)
+ if(j==1)
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = -(magData / 1000.0);
+ else
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = magData / 1000.0;
+
+ //constrain to max/min
+ //ie if field is 4.02 and max is 4.1, make it 4.1, since real max is 4.0
+ if(phid->dataBuffer[phid->bufferWritePtr].magneticField[j] > (phid->magneticFieldMax - 0.1))
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMax;
+ if(phid->dataBuffer[phid->bufferWritePtr].magneticField[j] < (phid->magneticFieldMin + 0.1))
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = phid->magneticFieldMin;
+ }
+
+ }
+ else
+ {
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[0] = PUNK_DBL;
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[1] = PUNK_DBL;
+ 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;
+
+ //LOG(PHIDGET_LOG_DEBUG, "Time: %d:%d",phid->latestDataTime.seconds,phid->latestDataTime.microseconds);
+
+ phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime;
+
+ phid->bufferWritePtr++;
+ if(phid->bufferWritePtr >= SPATIAL_DATA_BUFFER_SIZE)
+ phid->bufferWritePtr = 0;
+ }
+
+ break;
+ }
+ }
+ }
+ else
+ return EPHIDGET_UNEXPECTED;
+ break;
+ default:
+ return EPHIDGET_UNEXPECTED;
+ }
+
+ if(phid->doZeroGyro)
+ {
+ //done
+ if(timestampdiff(phid->latestDataTime, phid->dataBuffer[phid->gyroZeroReadPtr].timestamp) >= SPATIAL_ZERO_GYRO_TIME)
+ {
+ double gryoCorrectionTemp[SPATIAL_MAX_GYROAXES] = {0,0,0};
+ int gryoCorrectionCount = 0;
+
+ while(phid->gyroZeroReadPtr != phid->bufferWritePtr)
+ {
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ gryoCorrectionTemp[i] += phid->dataBuffer[phid->gyroZeroReadPtr].angularRate[i];
+ }
+
+ phid->gyroZeroReadPtr++;
+ if(phid->gyroZeroReadPtr >= SPATIAL_DATA_BUFFER_SIZE)
+ phid->gyroZeroReadPtr = 0;
+
+ gryoCorrectionCount++;
+ }
+
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ phid->gryoCorrection[i] = gryoCorrectionTemp[i] / (double)gryoCorrectionCount;
+ }
+
+ doneGyroZero = PTRUE;
+ }
+ }
+
+ //see if it's time for an event
+ if(timestampdiff(phid->latestDataTime, phid->lastEventTime) >= dataRate)
+ {
+ CPhidget_Timestamp tempTime;
+ //int lastPtr;
+ int accelCounter[SPATIAL_MAX_ACCELAXES], angularRateCounter[SPATIAL_MAX_ACCELAXES], magneticFieldCounter[SPATIAL_MAX_ACCELAXES];
+
+ int dataPerEvent = 0;
+
+ int multipleDataPerEvent = PFALSE;
+
+ if(dataRate < phid->interruptRate)
+ multipleDataPerEvent = PTRUE;
+
+ //max of 16 data per event
+ eventData = malloc(16 * sizeof(CPhidgetSpatial_SpatialEventDataHandle));
+
+ for(j=0;;j++)
+ {
+ //makes sure we read all data
+ if(phid->bufferReadPtr == phid->bufferWritePtr || j>=16)
+ {
+ dataPerEvent = j;
+ break;
+ }
+
+ eventData[j] = malloc(sizeof(CPhidgetSpatial_SpatialEventData));
+ ZEROMEM(accelCounter, sizeof(accelCounter));
+ ZEROMEM(angularRateCounter, sizeof(angularRateCounter));
+ ZEROMEM(magneticFieldCounter, sizeof(magneticFieldCounter));
+
+ tempTime = phid->dataBuffer[phid->bufferReadPtr].timestamp;
+
+ //average data for each stage
+ while(phid->bufferReadPtr != phid->bufferWritePtr &&
+ (!multipleDataPerEvent || timestampdiff(phid->dataBuffer[phid->bufferReadPtr].timestamp, tempTime) < dataRate))
+ {
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ if(phid->dataBuffer[phid->bufferReadPtr].acceleration[i] != PUNK_DBL)
+ {
+ if(phid->dataBuffer[phid->bufferReadPtr].acceleration[i] > phid->accelerationMax)
+ phid->dataBuffer[phid->bufferReadPtr].acceleration[i] = phid->accelerationMax;
+ if(phid->dataBuffer[phid->bufferReadPtr].acceleration[i] < phid->accelerationMin)
+ phid->dataBuffer[phid->bufferReadPtr].acceleration[i] = phid->accelerationMin;
+ accelAvg[i] += phid->dataBuffer[phid->bufferReadPtr].acceleration[i];
+ accelCounter[i]++;
+ }
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ if(phid->dataBuffer[phid->bufferReadPtr].angularRate[i] != PUNK_DBL)
+ {
+ double rate = phid->dataBuffer[phid->bufferReadPtr].angularRate[i] - phid->gryoCorrection[i];
+
+ if(rate > phid->angularRateMax)
+ angularRateAvg[i] += phid->angularRateMax;
+ else if(rate < phid->angularRateMin)
+ angularRateAvg[i] += phid->angularRateMin;
+ else
+ angularRateAvg[i] += rate;
+ angularRateCounter[i]++;
+ }
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ if(phid->dataBuffer[phid->bufferReadPtr].magneticField[i] != PUNK_DBL)
+ {
+ if(phid->dataBuffer[phid->bufferReadPtr].magneticField[i] > phid->magneticFieldMax)
+ phid->dataBuffer[phid->bufferReadPtr].magneticField[i] = phid->magneticFieldMax;
+ if(phid->dataBuffer[phid->bufferReadPtr].magneticField[i] < phid->magneticFieldMin)
+ phid->dataBuffer[phid->bufferReadPtr].magneticField[i] = phid->magneticFieldMin;
+ magneticFieldAvg[i] += phid->dataBuffer[phid->bufferReadPtr].magneticField[i];
+ magneticFieldCounter[i]++;
+ }
+ }
+
+ //lastPtr = phid->bufferReadPtr;
+
+ phid->bufferReadPtr++;
+ if(phid->bufferReadPtr >= SPATIAL_DATA_BUFFER_SIZE)
+ phid->bufferReadPtr = 0;
+ }
+
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ if(accelCounter[i] > 0)
+ eventData[j]->acceleration[i] = round_double(accelAvg[i] / (double)accelCounter[i], 5);
+ else
+ eventData[j]->acceleration[i] = PUNK_DBL;
+ accelAvg[i] = 0;
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ if(angularRateCounter[i] > 0)
+ {
+ if(phid->doZeroGyro && !doneGyroZero)
+ eventData[j]->angularRate[i] = 0;
+ else
+ eventData[j]->angularRate[i] = round_double(angularRateAvg[i] / (double)angularRateCounter[i], 5);
+ }
+ else
+ eventData[j]->angularRate[i] = PUNK_DBL;
+ angularRateAvg[i] = 0;
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ if(magneticFieldCounter[i] > 0)
+ eventData[j]->magneticField[i] = round_double(magneticFieldAvg[i] / (double)magneticFieldCounter[i], 5);
+ else
+ eventData[j]->magneticField[i] = PUNK_DBL;
+ magneticFieldAvg[i] = 0;
+ }
+ eventData[j]->timestamp = tempTime;
+ }
+
+ //correct magnetic field data in the event structure
+ for( j = 0; j < dataPerEvent; j++)
+ {
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ magneticFieldCorr[i] = eventData[j]->magneticField[i];
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ if(eventData[j]->magneticField[i] != PUNK_DBL)
+ {
+ eventData[j]->magneticField[i] = getCorrectedField(phid, magneticFieldCorr, i);
+ }
+ }
+ }
+
+ //store to local structure
+ ZEROMEM(accelCounter, sizeof(accelCounter));
+ ZEROMEM(angularRateCounter, sizeof(angularRateCounter));
+ ZEROMEM(magneticFieldCounter, sizeof(magneticFieldCounter));
+ for( j = 0; j < dataPerEvent; j++)
+ {
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ if(eventData[j]->acceleration[i] != PUNK_DBL)
+ {
+ accelAvg[i] += eventData[j]->acceleration[i];
+ accelCounter[i]++;
+ }
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ if(eventData[j]->angularRate[i] != PUNK_DBL)
+ {
+ angularRateAvg[i] += eventData[j]->angularRate[i];
+ angularRateCounter[i]++;
+ }
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ if(eventData[j]->magneticField[i] != PUNK_DBL)
+ {
+ magneticFieldAvg[i] += eventData[j]->magneticField[i];
+ magneticFieldCounter[i]++;
+ }
+ }
+ }
+
+ //Set local get data to averages
+ for (i = 0; i<phid->phid.attr.spatial.numAccelAxes; i++)
+ {
+ if(accelCounter[i] > 0)
+ phid->accelAxis[i] = round_double(accelAvg[i] / (double)accelCounter[i], 5);
+ else
+ phid->accelAxis[i] = PUNK_DBL;
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numGyroAxes; i++)
+ {
+ if(angularRateCounter[i] > 0)
+ {
+ if(phid->doZeroGyro && !doneGyroZero)
+ phid->gyroAxis[i] = 0;
+ else
+ phid->gyroAxis[i] = round_double(angularRateAvg[i] / (double)angularRateCounter[i], 5);
+ }
+ else
+ phid->gyroAxis[i] = PUNK_DBL;
+ }
+ for (i = 0; i<phid->phid.attr.spatial.numCompassAxes; i++)
+ {
+ if(magneticFieldCounter[i] > 0)
+ phid->compassAxis[i] = round_double(magneticFieldAvg[i] / (double)magneticFieldCounter[i], 5);
+ else
+ phid->compassAxis[i] = PUNK_DBL;
+ }
+
+ //send out any events
+ FIRE(SpatialData, eventData, dataPerEvent);
+
+ phid->lastEventTime = phid->latestDataTime;
+
+ for(i=0;i<dataPerEvent;i++)
+ free(eventData[i]);
+ free(eventData);
+ }
+done:
+
+ //this will signal the zero function to return;
+ if(doneGyroZero)
+ phid->doZeroGyro = PFALSE;
+
+ return EPHIDGET_OK;
+}
+
+//eventsAfterOpen - sends out an event for all valid data, used during attach initialization
+CPHIDGETINITEVENTS(Spatial)
+ TESTPTR(phid);
+ //don't need to worry, because the interrupts come at a set rate
+ return EPHIDGET_OK;
+}
+
+//getPacket - not used for spatial
+CGETPACKET(Spatial)
+ return EPHIDGET_UNEXPECTED;
+}
+
+static double getCorrectedField(CPhidgetSpatialHandle phid, double fields[], int axis)
+{
+ switch(axis)
+ {
+ case 0:
+ return phid->userMagField *
+ (phid->userCompassGain[0] * (fields[0] - phid->userCompassOffset[0])
+ + phid->userCompassTransform[0] * (fields[1] - phid->userCompassOffset[1])
+ + phid->userCompassTransform[1] * (fields[2] - phid->userCompassOffset[2]));
+ case 1:
+ return phid->userMagField *
+ (phid->userCompassGain[1] * (fields[1] - phid->userCompassOffset[1])
+ + phid->userCompassTransform[2] * (fields[0] - phid->userCompassOffset[0])
+ + phid->userCompassTransform[3] * (fields[2] - phid->userCompassOffset[2]));
+ case 2:
+ return phid->userMagField *
+ (phid->userCompassGain[2] * (fields[2] - phid->userCompassOffset[2])
+ + phid->userCompassTransform[4] * (fields[0] - phid->userCompassOffset[0])
+ + phid->userCompassTransform[5] * (fields[1] - phid->userCompassOffset[1]));
+ default:
+ return 0;
+ }
+}
+
+// === Exported Functions === //
+
+//create and initialize a device structure
+CCREATE(Spatial, PHIDCLASS_SPATIAL)
+
+//event setup functions
+CFHANDLE(Spatial, SpatialData, CPhidgetSpatial_SpatialEventDataHandle *, int)
+
+CGET(Spatial,AccelerationAxisCount,int)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ MASGN(phid.attr.spatial.numAccelAxes)
+}
+CGET(Spatial,GyroAxisCount,int)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ MASGN(phid.attr.spatial.numGyroAxes)
+}
+CGET(Spatial,CompassAxisCount,int)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ MASGN(phid.attr.spatial.numCompassAxes)
+}
+
+CGETINDEX(Spatial,Acceleration,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTINDEX(phid.attr.spatial.numAccelAxes)
+ TESTMASGN(accelAxis[Index], PUNK_DBL)
+
+ MASGN(accelAxis[Index])
+}
+
+CGETINDEX(Spatial,AccelerationMax,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTINDEX(phid.attr.spatial.numAccelAxes)
+ TESTMASGN(accelerationMax, PUNK_DBL)
+
+ MASGN(accelerationMax)
+}
+
+CGETINDEX(Spatial,AccelerationMin,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTINDEX(phid.attr.spatial.numAccelAxes)
+ TESTMASGN(accelerationMin, PUNK_DBL)
+
+ MASGN(accelerationMin)
+}
+
+CGETINDEX(Spatial,AngularRate,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ TESTINDEX(phid.attr.spatial.numGyroAxes)
+ TESTMASGN(gyroAxis[Index], PUNK_DBL)
+ MASGN(gyroAxis[Index])
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+CGETINDEX(Spatial,AngularRateMax,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ TESTINDEX(phid.attr.spatial.numGyroAxes)
+ TESTMASGN(angularRateMax, PUNK_DBL)
+ MASGN(angularRateMax)
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+CGETINDEX(Spatial,AngularRateMin,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ TESTINDEX(phid.attr.spatial.numGyroAxes)
+ TESTMASGN(angularRateMin, PUNK_DBL)
+ MASGN(angularRateMin)
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+CGETINDEX(Spatial,MagneticField,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ TESTINDEX(phid.attr.spatial.numCompassAxes)
+ TESTMASGN(compassAxis[Index], PUNK_DBL)
+ MASGN(compassAxis[Index])
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+CGETINDEX(Spatial,MagneticFieldMax,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ TESTINDEX(phid.attr.spatial.numCompassAxes)
+ TESTMASGN(magneticFieldMax, PUNK_DBL)
+ MASGN(magneticFieldMax)
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+CGETINDEX(Spatial,MagneticFieldMin,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ TESTINDEX(phid.attr.spatial.numCompassAxes)
+ TESTMASGN(magneticFieldMin, PUNK_DBL)
+ MASGN(magneticFieldMin)
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+CSET(Spatial,DataRate,int)
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTRANGE(phid->dataRateMax, phid->dataRateMin)
+
+ //make sure it's a power of 2, or 1
+ if(newVal < phid->interruptRate)
+ {
+ int temp = phid->dataRateMax;
+ unsigned char good = FALSE;
+ while(temp <= newVal)
+ {
+ if(temp == newVal)
+ {
+ good = TRUE;
+ break;
+ }
+ temp *= 2;
+ }
+ if(!good)
+ return EPHIDGET_INVALIDARG;
+ }
+ //make sure it's divisible by interruptRate
+ else
+ {
+ if(newVal%phid->interruptRate)
+ return EPHIDGET_INVALIDARG;
+ }
+
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ ADDNETWORKKEY(DataRate, "%d", dataRate);
+ else
+ phid->dataRate = newVal;
+
+ return EPHIDGET_OK;
+}
+CGET(Spatial,DataRate,int)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTMASGN(dataRate, PUNK_INT)
+
+ MASGN(dataRate)
+}
+
+CGET(Spatial,DataRateMax,int)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTMASGN(dataRateMax, PUNK_INT)
+
+ MASGN(dataRateMax)
+}
+
+CGET(Spatial,DataRateMin,int)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTMASGN(dataRateMin, PUNK_INT)
+
+ MASGN(dataRateMin)
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_zeroGyro(CPhidgetSpatialHandle phid)
+{
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ if(phid->phid.attr.spatial.numGyroAxes==0)
+ return EPHIDGET_UNSUPPORTED;
+
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ {
+ int newVal = phid->flip^1;
+ ADDNETWORKKEY(ZeroGyro, "%d", flip);
+ }
+ else
+ {
+ if(!phid->doZeroGyro)
+ {
+ phid->gyroZeroReadPtr = phid->bufferReadPtr;
+ phid->doZeroGyro = PTRUE;
+ }
+ }
+
+ return EPHIDGET_OK;
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_resetCompassCorrectionParameters(
+ CPhidgetSpatialHandle phid)
+{
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ {
+ char newVal[1024];
+ sprintf(newVal, "1,0,0,0,1,1,1,0,0,0,0,0,0");
+ ADDNETWORKKEY(CompassCorrectionParams, "%s", compassCorrectionParamsString);
+ }
+ else
+ {
+ phid->userMagField = 1;
+
+ phid->userCompassOffset[0] = 0;
+ phid->userCompassOffset[1] = 0;
+ phid->userCompassOffset[2] = 0;
+
+ phid->userCompassGain[0] = 1;
+ phid->userCompassGain[1] = 1;
+ phid->userCompassGain[2] = 1;
+
+ phid->userCompassTransform[0] = 0;
+ phid->userCompassTransform[1] = 0;
+ phid->userCompassTransform[2] = 0;
+ phid->userCompassTransform[3] = 0;
+ phid->userCompassTransform[4] = 0;
+ phid->userCompassTransform[5] = 0;
+ }
+ return EPHIDGET_OK;
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+PHIDGET21_API int CCONV CPhidgetSpatial_setCompassCorrectionParameters(
+ CPhidgetSpatialHandle phid,
+ double magField,
+ double offset0, double offset1, double offset2,
+ double gain0, double gain1, double gain2,
+ double T0, double T1, double T2, double T3, double T4, double T5)
+{
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceIDSpec)
+ {
+ case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
+ //Magnetic Field 0.1-1000
+ if(magField < 0.1 || magField > 1000)
+ return EPHIDGET_INVALIDARG;
+ //Offsets need to be 0+-5.0
+ if(offset0 < -5 || offset0 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(offset1 < -5 || offset1 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(offset2 < -5 || offset2 > 5)
+ return EPHIDGET_INVALIDARG;
+ //Gains need to be 0-15.0
+ if(gain0 < 0 || gain0 > 15)
+ return EPHIDGET_INVALIDARG;
+ if(gain1 < 0 || gain1 > 15)
+ return EPHIDGET_INVALIDARG;
+ if(gain2 < 0 || gain2 > 15)
+ return EPHIDGET_INVALIDARG;
+ //T params 0+-5.0
+ if(T0 < -5 || T0 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(T1 < -5 || T1 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(T2 < -5 || T2 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(T3 < -5 || T3 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(T4 < -5 || T4 > 5)
+ return EPHIDGET_INVALIDARG;
+ if(T5 < -5 || T5 > 5)
+ return EPHIDGET_INVALIDARG;
+
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ {
+ char newVal[1024];
+ sprintf(newVal, "%lE,%lE,%lE,%lE,%lE,%lE,%lE,%lE,%lE,%lE,%lE,%lE,%lE",
+ magField, offset0, offset1, offset2, gain0, gain1, gain2, T0, T1, T2, T3, T4, T5);
+ ADDNETWORKKEY(CompassCorrectionParams, "%s", compassCorrectionParamsString);
+ }
+ else
+ {
+ phid->userMagField = magField;
+
+ phid->userCompassOffset[0] = offset0;
+ phid->userCompassOffset[1] = offset1;
+ phid->userCompassOffset[2] = offset2;
+
+ phid->userCompassGain[0] = gain0;
+ phid->userCompassGain[1] = gain1;
+ phid->userCompassGain[2] = gain2;
+
+ phid->userCompassTransform[0] = T0;
+ phid->userCompassTransform[1] = T1;
+ phid->userCompassTransform[2] = T2;
+ phid->userCompassTransform[3] = T3;
+ phid->userCompassTransform[4] = T4;
+ phid->userCompassTransform[5] = T5;
+ }
+
+ return EPHIDGET_OK;
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+//Maybe add these later
+/*
+CGET(Spatial,GyroHeading,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTMASGN(gyroHeading, PUNK_DBL)
+
+ MASGN(gyroHeading)
+}
+
+CGET(Spatial,CompassHeading,double)
+ TESTPTRS(phid,pVal)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTMASGN(compassHeading, PUNK_DBL)
+
+ MASGN(compassHeading)
+}*/
|