aboutsummaryrefslogtreecommitdiffstats
path: root/cphidgetspatial.c
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--cphidgetspatial.c3047
1 files changed, 1665 insertions, 1382 deletions
diff --git a/cphidgetspatial.c b/cphidgetspatial.c
index 41a8048..2feccce 100644
--- a/cphidgetspatial.c
+++ b/cphidgetspatial.c
@@ -1,1382 +1,1665 @@
-#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)
-}*/
+#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.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1049:
+ 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;
+ break;
+
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1041:
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1043:
+ phid->accelerationMax = 8.1;
+ phid->accelerationMin = -8.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;
+ break;
+
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN:
+ 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;
+ break;
+
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ phid->accelerationMax = 8.1;
+ phid->accelerationMin = -8.1;
+ phid->interruptRate = 4;
+ phid->dataRateMin = SPATIAL_MIN_DATA_RATE;
+ phid->dataRate = 8;
+ phid->dataRateMax = 4; //actual data rate
+ phid->angularRateMax = 2000.1;
+ phid->angularRateMin = -2000.1;
+ phid->magneticFieldMax = 5.6;
+ phid->magneticFieldMin = -5.6;
+ phid->userMagField = 1.0;
+ phid->calDataValid = PFALSE;
+ 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.deviceUID) {
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1049:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN:
+ {
+ 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;
+ // No streamed Calibration
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1041:
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1043:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ 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;
+}
+
+static void updateTimestamp(CPhidgetSpatialHandle phid, int 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;
+}
+
+//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->deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1049:
+ {
+ 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;
+ }
+ 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
+ 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->deviceUID == PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056)
+ {
+ 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->deviceUID == PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN)
+ {
+ 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;
+ }
+ break;
+
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ {
+ int time;
+ int flags = buffer[1];
+ count = buffer[0];
+
+ if(count == 0)
+ goto done;
+
+ //this timestamp is for the latest data
+ time = ((unsigned short)buffer[2]<<8) + (unsigned short)buffer[3];
+ updateTimestamp(phid, time);
+
+ //add data to data buffer
+ for(i=0;i<count;i++)
+ {
+ int countOffset = i * 18; //Each set of samples is 18 bytes
+ for(j=0;j<3;j++)
+ {
+ int indexOffset = j * 2; //Each value is 2 bytes
+ short accelData = (signed short)((unsigned short)buffer[ 4 + indexOffset + countOffset] << 8) + (unsigned short)buffer[ 5 + indexOffset + countOffset];
+ short gyroData = (signed short)((unsigned short)buffer[10 + indexOffset + countOffset] << 8) + (unsigned short)buffer[11 + indexOffset + countOffset];
+ short magData = (signed short)((unsigned short)buffer[16 + indexOffset + countOffset] << 8) + (unsigned short)buffer[17 + indexOffset + countOffset];
+
+ //digital accel
+ if(flags & (0x02 >> 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;
+
+ //digital gyro
+ if(flags & (0x08 >> i))
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = (double)gyroData / SPATIAL_L3GD20_BITS_PER_DPS;
+ //analog gyro
+ else
+ if(j==2)
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = (double)(gyroData) / SPATIAL_LY330ALH_w_AD7689_BITS_PER_DPS;
+ else
+ phid->dataBuffer[phid->bufferWritePtr].angularRate[j] = (double)(gyroData) / SPATIAL_LRP410AL_w_AD7689_BITS_PER_DPS;
+
+ //compass valid
+ if(flags & (0x20 >> i))
+ phid->dataBuffer[phid->bufferWritePtr].magneticField[j] = magData / SPATIAL_HMC5883L_BITS_PER_GAUSS;
+ //no compass data
+ else
+ 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;
+
+ phid->dataBuffer[phid->bufferWritePtr].timestamp = phid->latestDataTime;
+
+ phid->bufferWritePtr++;
+ if(phid->bufferWritePtr >= SPATIAL_DATA_BUFFER_SIZE)
+ phid->bufferWritePtr = 0;
+ }
+ 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
+ // But only on devices that don't do this in Firmware!
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN:
+ 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);
+ }
+ }
+ }
+ break;
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ default:
+ break;
+ }
+
+ //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;
+ }
+}
+
+// Accel and Gyro tables are the same structure - just different table IDs.
+static int setCalibrationValues_inFirmware(CPhidgetSpatialHandle phid, int tableID, int index,
+ double gainPositive[3], double gainNegative[3], double offset[3], double factor1[3], double factor2[3])
+{
+ unsigned char buffer[SPATIAL_ACCEL_GYRO_CALIB_TABLE_LENGTH] = {0};
+ int i;
+ double offsetMultipliers[3];
+ unsigned int header;
+
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ if(!deviceSupportsGeneralUSBProtocol((CPhidgetHandle)phid))
+ return EPHIDGET_UNSUPPORTED;
+
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1043:
+ if(index == SPATIAL_ANALOG_ACCEL_CALIB_TABLE_INDEX)
+ {
+ offsetMultipliers[0] = offsetMultipliers[1] = offsetMultipliers[2] = SPATIAL_KXR94_2050_w_AD7689_BITS_PER_G;
+ break;
+ }
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1041:
+ if(index == SPATIAL_DIGITAL_ACCEL_CALIB_TABLE_INDEX)
+ {
+ offsetMultipliers[0] = offsetMultipliers[1] = offsetMultipliers[2] = SPATIAL_MMA8451Q_BITS_PER_G;
+ break;
+ }
+ else
+ return EPHIDGET_UNSUPPORTED;
+
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ if(index == SPATIAL_ANALOG_GYRO_CALIB_TABLE_INDEX)
+ {
+ offsetMultipliers[0] = offsetMultipliers[1] = SPATIAL_LRP410AL_w_AD7689_BITS_PER_DPS;
+ offsetMultipliers[2] = SPATIAL_LY330ALH_w_AD7689_BITS_PER_DPS;
+ break;
+ }
+ if(index == SPATIAL_ANALOG_ACCEL_CALIB_TABLE_INDEX)
+ {
+ offsetMultipliers[0] = offsetMultipliers[1] = offsetMultipliers[2] = SPATIAL_KXR94_2050_w_AD7689_BITS_PER_G;
+ break;
+ }
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ if(index == SPATIAL_DIGITAL_GYRO_CALIB_TABLE_INDEX)
+ {
+ offsetMultipliers[0] = offsetMultipliers[1] = offsetMultipliers[2] = SPATIAL_L3GD20_BITS_PER_DPS;
+ break;
+ }
+ if(index == SPATIAL_DIGITAL_ACCEL_CALIB_TABLE_INDEX)
+ {
+ offsetMultipliers[0] = offsetMultipliers[1] = offsetMultipliers[2] = SPATIAL_MMA8451Q_BITS_PER_G;
+ break;
+ }
+ else
+ return EPHIDGET_UNSUPPORTED;
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+
+ header = (((unsigned int)tableID) << 20) | SPATIAL_ACCEL_GYRO_CALIB_TABLE_LENGTH;
+
+ buffer[3] = header >> 24; //header high byte
+ buffer[2] = header >> 16;
+ buffer[1] = header >> 8;
+ buffer[0] = header >> 0; //header low byte
+
+ for(i=0;i<3;i++)
+ {
+ int int32Offset = i * 4;
+ int temp;
+
+ temp = round(gainPositive[i] * (double)0x10000);
+ buffer[int32Offset + 7] = temp >> 24;
+ buffer[int32Offset + 6] = temp >> 16;
+ buffer[int32Offset + 5] = temp >> 8;
+ buffer[int32Offset + 4] = temp >> 0;
+
+ temp = round(gainNegative[i] * (double)0x10000);
+ buffer[int32Offset + 19] = temp >> 24;
+ buffer[int32Offset + 18] = temp >> 16;
+ buffer[int32Offset + 17] = temp >> 8;
+ buffer[int32Offset + 16] = temp >> 0;
+
+ temp = round(offset[i] * offsetMultipliers[i]);
+ buffer[int32Offset + 31] = temp >> 24;
+ buffer[int32Offset + 30] = temp >> 16;
+ buffer[int32Offset + 29] = temp >> 8;
+ buffer[int32Offset + 28] = temp >> 0;
+
+ temp = round(factor1[i] * (double)0x10000);
+ buffer[int32Offset + 43] = temp >> 24;
+ buffer[int32Offset + 42] = temp >> 16;
+ buffer[int32Offset + 41] = temp >> 8;
+ buffer[int32Offset + 40] = temp >> 0;
+
+ temp = round(factor2[i] * (double)0x10000);
+ buffer[int32Offset + 55] = temp >> 24;
+ buffer[int32Offset + 54] = temp >> 16;
+ buffer[int32Offset + 53] = temp >> 8;
+ buffer[int32Offset + 52] = temp >> 0;
+ }
+
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ {
+ //TODO - maybe, maybe not.
+ return EPHIDGET_UNSUPPORTED;
+ }
+ else
+ return CPhidgetGPP_setDeviceSpecificConfigTable((CPhidgetHandle)phid, buffer, SPATIAL_ACCEL_GYRO_CALIB_TABLE_LENGTH, index);
+}
+
+static int setCompassCorrectionTable_inFimrware(
+ 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)
+{
+ unsigned char buffer[SPATIAL_COMPASS_CALIB_TABLE_LENGTH] = {0};
+ int i;
+ int gains[3], offsets[3], transforms[6], mag;
+ TESTPTR(phid)
+ if (!CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_ATTACHED_FLAG))
+ return EPHIDGET_NOTATTACHED;
+
+ if(!deviceSupportsGeneralUSBProtocol((CPhidgetHandle)phid))
+ return EPHIDGET_UNSUPPORTED;
+
+ //compass calibration table Header is: 0x3EA00038
+ buffer[3] = 0x3E; //header high byte
+ buffer[2] = 0xA0;
+ buffer[1] = 0x00;
+ buffer[0] = SPATIAL_COMPASS_CALIB_TABLE_LENGTH; //header low byte
+
+ //Mag Field (x0x10000)
+ mag = round(magField * (double)0x10000);
+ buffer[7] = mag >> 24;
+ buffer[6] = mag >> 16;
+ buffer[5] = mag >> 8;
+ buffer[4] = mag >> 0;
+
+ //Gain (x0x10000)
+ gains[0] = round(gain0 * (double)0x10000);
+ gains[1] = round(gain1 * (double)0x10000);
+ gains[2] = round(gain2 * (double)0x10000);
+ for(i=0;i<3;i++)
+ {
+ buffer[i*4+11] = gains[i] >> 24;
+ buffer[i*4+10] = gains[i] >> 16;
+ buffer[i*4+9] = gains[i] >> 8;
+ buffer[i*4+8] = gains[i] >> 0;
+ }
+
+ //Offset
+ offsets[0] = round(offset0 * (double)SPATIAL_HMC5883L_BITS_PER_GAUSS);
+ offsets[1] = round(offset1 * (double)SPATIAL_HMC5883L_BITS_PER_GAUSS);
+ offsets[2] = round(offset2 * (double)SPATIAL_HMC5883L_BITS_PER_GAUSS);
+ for(i=0;i<3;i++)
+ {
+ buffer[i*4+23] = offsets[i] >> 24;
+ buffer[i*4+22] = offsets[i] >> 16;
+ buffer[i*4+21] = offsets[i] >> 8;
+ buffer[i*4+20] = offsets[i] >> 0;
+ }
+
+ //Transforms (x0x10000)
+ transforms[0] = round(T0 * (double)0x10000);
+ transforms[1] = round(T1 * (double)0x10000);
+ transforms[2] = round(T2 * (double)0x10000);
+ transforms[3] = round(T3 * (double)0x10000);
+ transforms[4] = round(T4 * (double)0x10000);
+ transforms[5] = round(T5 * (double)0x10000);
+ for(i=0;i<6;i++)
+ {
+ buffer[i*4+35] = transforms[i] >> 24;
+ buffer[i*4+34] = transforms[i] >> 16;
+ buffer[i*4+33] = transforms[i] >> 8;
+ buffer[i*4+32] = transforms[i] >> 0;
+ }
+
+ //Label Table index is: 0
+ return CPhidgetGPP_setDeviceSpecificConfigTable((CPhidgetHandle)phid, buffer, SPATIAL_COMPASS_CALIB_TABLE_LENGTH, SPATIAL_COMPASS_CALIB_TABLE_INDEX);
+}
+
+// === 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)
+{
+ int result = EPHIDGET_OK;
+ 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
+ {
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ {
+ unsigned char buffer[8] = { 0 };
+ buffer[0] = SPATIAL_ZERO_GYRO;
+ result = CUSBSendPacket((CPhidgetHandle)phid, buffer);
+ break;
+ }
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN:
+ if(!phid->doZeroGyro)
+ {
+ phid->gyroZeroReadPtr = phid->bufferReadPtr;
+ phid->doZeroGyro = PTRUE;
+ }
+ break;
+ default:
+ return EPHIDGET_UNEXPECTED;
+ }
+ }
+
+ 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
+ {
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN:
+
+ 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;
+ break;
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ return setCompassCorrectionTable_inFimrware(phid,
+ 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0);
+ default:
+ return EPHIDGET_UNEXPECTED;
+ }
+ }
+ 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
+ {
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1056_NEG_GAIN:
+
+ 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;
+ break;
+
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ return setCompassCorrectionTable_inFimrware(phid,
+ magField, offset0, offset1, offset2,
+ gain0, gain1, gain2, T0, T1, T2, T3, T4, T5);
+ default:
+ return EPHIDGET_UNEXPECTED;
+ }
+ }
+ return EPHIDGET_OK;
+ case PHIDID_SPATIAL_ACCEL_3AXIS:
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+
+CSET(Spatial,AnalogDigitalMode,CPhidgetSpatial_AnalogDigitalMode)
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+ TESTRANGE(SPATIAL_ANALOG_AND_DIGITAL, SPATIAL_DIGITAL)
+
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_3AXIS_1043:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ {
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ {
+ //TODO
+ }
+ else
+ {
+ unsigned char buffer[8] = { 0 };
+ buffer[0] = SPATIAL_SET_POLLING_TYPE;
+ buffer[1] = newVal;
+ return CUSBSendPacket((CPhidgetHandle)phid, buffer);
+ }
+ }
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_unZeroGyro(CPhidgetSpatialHandle phid)
+{
+ TESTPTR(phid)
+ TESTDEVICETYPE(PHIDCLASS_SPATIAL)
+ TESTATTACHED
+
+ switch(phid->phid.deviceUID)
+ {
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1042:
+ case PHIDUID_SPATIAL_ACCEL_GYRO_COMPASS_1044:
+ {
+ if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
+ {
+ //TODO
+ }
+ else
+ {
+ unsigned char buffer[8] = { 0 };
+ buffer[0] = SPATIAL_UNZERO_GYRO;
+ return CUSBSendPacket((CPhidgetHandle)phid, buffer);
+ }
+ }
+ default:
+ return EPHIDGET_UNSUPPORTED;
+ }
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_setDigitalGyroCalibrationValues(CPhidgetSpatialHandle phid,
+ double gainPositive[3], double gainNegative[3], double offset[3], double factor1[3], double factor2[3])
+{
+ return setCalibrationValues_inFirmware(phid, SPATIAL_GyroCalibTable_ID, SPATIAL_DIGITAL_GYRO_CALIB_TABLE_INDEX,
+ gainPositive, gainNegative, offset, factor1, factor2);
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_setAnalogGyroCalibrationValues(CPhidgetSpatialHandle phid,
+ double gainPositive[3], double gainNegative[3], double offset[3], double factor1[3], double factor2[3])
+{
+ return setCalibrationValues_inFirmware(phid, SPATIAL_GyroCalibTable_ID, SPATIAL_ANALOG_GYRO_CALIB_TABLE_INDEX,
+ gainPositive, gainNegative, offset, factor1, factor2);
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_setDigitalAccelCalibrationValues(CPhidgetSpatialHandle phid,
+ double gainPositive[3], double gainNegative[3], double offset[3], double factor1[3], double factor2[3])
+{
+ return setCalibrationValues_inFirmware(phid, SPATIAL_AccelCalibTable_ID, SPATIAL_DIGITAL_ACCEL_CALIB_TABLE_INDEX,
+ gainPositive, gainNegative, offset, factor1, factor2);
+}
+
+PHIDGET21_API int CCONV CPhidgetSpatial_setAnalogAccelCalibrationValues(CPhidgetSpatialHandle phid,
+ double gainPositive[3], double gainNegative[3], double offset[3], double factor1[3], double factor2[3])
+{
+ return setCalibrationValues_inFirmware(phid, SPATIAL_AccelCalibTable_ID, SPATIAL_ANALOG_ACCEL_CALIB_TABLE_INDEX,
+ gainPositive, gainNegative, offset, factor1, factor2);
+}
+
+
+
+//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)
+}*/
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-18 07:22:59 +0000