From 260123716172d33f44bdc0e4e5422554d139215c Mon Sep 17 00:00:00 2001 From: Jonathan McCrohan Date: Wed, 9 May 2012 00:47:30 +0100 Subject: Imported Upstream version 2.1.8.20120507 --- cphidgetspatial.c | 3047 +++++++++++++++++++++++++++++------------------------ 1 file changed, 1665 insertions(+), 1382 deletions(-) (limited to 'cphidgetspatial.c') 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; iaccelAxis[i] = PUNI_DBL; - } - for (i = 0; igyroAxis[i] = PUNI_DBL; - } - for (i = 0; icompassAxis[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; iphid.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; iphid.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; iphid.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;iaccelOffset[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; iphid.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;iaccelOffset[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;jphid.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; iphid.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; jphid.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;jphid.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> 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;jphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.attr.spatial.numCompassAxes; i++) - { - magneticFieldCorr[i] = eventData[j]->magneticField[i]; - } - for (i = 0; iphid.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; iphid.attr.spatial.numAccelAxes; i++) - { - if(eventData[j]->acceleration[i] != PUNK_DBL) - { - accelAvg[i] += eventData[j]->acceleration[i]; - accelCounter[i]++; - } - } - for (i = 0; iphid.attr.spatial.numGyroAxes; i++) - { - if(eventData[j]->angularRate[i] != PUNK_DBL) - { - angularRateAvg[i] += eventData[j]->angularRate[i]; - angularRateCounter[i]++; - } - } - for (i = 0; iphid.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; iphid.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; iphid.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; iphid.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;idoZeroGyro = 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; iaccelAxis[i] = PUNI_DBL; + } + for (i = 0; igyroAxis[i] = PUNI_DBL; + } + for (i = 0; icompassAxis[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; iphid.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; iphid.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; iphid.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;iaccelOffset[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; iphid.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;idataBuffer[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;iaccelOffset[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;jphid.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; iphid.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; jphid.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;jphid.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> 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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.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; iphid.attr.spatial.numCompassAxes; i++) + { + magneticFieldCorr[i] = eventData[j]->magneticField[i]; + } + for (i = 0; iphid.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; iphid.attr.spatial.numAccelAxes; i++) + { + if(eventData[j]->acceleration[i] != PUNK_DBL) + { + accelAvg[i] += eventData[j]->acceleration[i]; + accelCounter[i]++; + } + } + for (i = 0; iphid.attr.spatial.numGyroAxes; i++) + { + if(eventData[j]->angularRate[i] != PUNK_DBL) + { + angularRateAvg[i] += eventData[j]->angularRate[i]; + angularRateCounter[i]++; + } + } + for (i = 0; iphid.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; iphid.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; iphid.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; iphid.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;idoZeroGyro = 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) +}*/ -- cgit v1.2.3