From 0b624384cd52be20e61284551d832b499d7b7707 Mon Sep 17 00:00:00 2001 From: Jonathan McCrohan Date: Sat, 14 Apr 2012 12:56:48 +0100 Subject: Imported Upstream version 2.1.8.20120216 --- cphidgetaccelerometer.c | 258 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 258 insertions(+) create mode 100644 cphidgetaccelerometer.c (limited to 'cphidgetaccelerometer.c') diff --git a/cphidgetaccelerometer.c b/cphidgetaccelerometer.c new file mode 100644 index 0000000..4977de2 --- /dev/null +++ b/cphidgetaccelerometer.c @@ -0,0 +1,258 @@ +#include "stdafx.h" +#include "cphidgetaccelerometer.h" +#include "cusb.h" +#include "math.h" +#include "csocket.h" +#include "cthread.h" + +// === Internal Functions === // + +//clearVars - sets all device variables to unknown state +CPHIDGETCLEARVARS(Accelerometer) + int i = 0; + + phid->accelerationMax = PUNI_DBL; + phid->accelerationMin = PUNI_DBL; + + for (i = 0; iaxis[i] = PUNI_DBL; + phid->axisLastTrigger[i] = PUNK_DBL; + phid->axisChangeTrigger[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(Accelerometer) + int i = 0; + + TESTPTR(phid); + + //Setup max/min values + switch(phid->phid.deviceIDSpec) + { + case PHIDID_ACCELEROMETER_2AXIS: + if (phid->phid.deviceVersion < 200) + { + phid->accelerationMax = 2.1; + phid->accelerationMin = -2.1; + } + else if ((phid->phid.deviceVersion >= 200) && (phid->phid.deviceVersion < 300)) + { + phid->accelerationMax = 10.1; + phid->accelerationMin = -10.1; + } + else if ((phid->phid.deviceVersion >= 300) && (phid->phid.deviceVersion < 400)) + { + phid->accelerationMax = 5.1; + phid->accelerationMin = -5.1; + } + else + return EPHIDGET_BADVERSION; + break; + case PHIDID_ACCELEROMETER_3AXIS: + if ((phid->phid.deviceVersion >= 400) && (phid->phid.deviceVersion < 500)) + { + phid->accelerationMax = 3.1; + phid->accelerationMin = -3.1; + } + else + return EPHIDGET_BADVERSION; + break; + default: + return EPHIDGET_UNEXPECTED; + } + + //initialize triggers, set data arrays to unknown + for (i = 0; iphid.attr.accelerometer.numAxis; i++) + { + phid->axis[i] = PUNK_DBL; + phid->axisLastTrigger[i] = PUNK_DBL; + phid->axisChangeTrigger[i] = 0.001; + } + + //issue one read + CPhidget_read((CPhidgetHandle)phid); + + return EPHIDGET_OK; +} + +//dataInput - parses device packets +CPHIDGETDATA(Accelerometer) + int i = 0; + double axis[ACCEL_MAXAXES]; + + if (length<0) return EPHIDGET_INVALIDARG; + TESTPTR(phid); + TESTPTR(buffer); + + ZEROMEM(axis, sizeof(axis)); + + //Parse device packets - store data locally + switch(phidG->deviceIDSpec) + { + case PHIDID_ACCELEROMETER_2AXIS: + if (phidG->deviceVersion < 200) + { + int data = 0; + data = (signed short)((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8)); + axis[0] = round_double((((double)data-16384) / 2000), 4); + data = (signed short)((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8)); + axis[1] = round_double((((double)data-16384) / 2000), 4); + } + else if ((phidG->deviceVersion >= 200) && (phidG->deviceVersion < 300)) + { + int data = 0; + data = (signed short)((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8)); + axis[0] = round_double((((double)data-16384) / 650), 4); + data = (signed short)((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8)); + axis[1] = round_double((((double)data-16384) / 650), 4); + } + else if ((phidG->deviceVersion >= 300) && (phidG->deviceVersion < 400)) + { + int data = 0; + data = ((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8)); + axis[0] = round_double((((double)(data-32768)) / 4000), 5); + data = ((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8)); + axis[1] = round_double((((double)(data-32768)) / 4000), 5); + } + else + return EPHIDGET_UNEXPECTED; + break; + case PHIDID_ACCELEROMETER_3AXIS: + if ((phidG->deviceVersion >= 400) && (phidG->deviceVersion < 500)) + { + int data = 0; + data = ((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8)); + axis[0] = round_double((((double)(data-32768)) / 6553.6), 5); + data = ((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8)); + axis[1] = round_double((((double)(data-32768)) / 6553.6), 5); + data = ((unsigned short)buffer[4]+((unsigned short)buffer[5]<<8)); + axis[2] = round_double((((double)(data-32768)) / 6553.6), 5); + } + else + return EPHIDGET_UNEXPECTED; + break; + default: + return EPHIDGET_UNEXPECTED; + } + + //Make sure values are within defined range, and store to structure + for (i = 0; iphid.attr.accelerometer.numAxis; i++) + { + if(axis[i] > phid->accelerationMax) axis[i] = phid->accelerationMax; + if(axis[i] < phid->accelerationMin) axis[i] = phid->accelerationMin; + phid->axis[i] = axis[i]; + } + + //send out any events that exceed or match the trigger + for (i = 0; iphid.attr.accelerometer.numAxis; i++) + { + if (fabs(phid->axis[i] - phid->axisLastTrigger[i]) >= phid->axisChangeTrigger[i] + || phid->axisLastTrigger[i] == PUNK_DBL) + { + FIRE(AccelerationChange, i, phid->axis[i]); + phid->axisLastTrigger[i] = phid->axis[i]; + } + } + + return EPHIDGET_OK; +} + +//eventsAfterOpen - sends out an event for all valid data, used during attach initialization +CPHIDGETINITEVENTS(Accelerometer) + + for (i = 0; iphid.attr.accelerometer.numAxis; i++) + { + if(phid->axis[i] != PUNK_DBL) + { + FIRE(AccelerationChange, i, phid->axis[i]); + phid->axisLastTrigger[i] = phid->axis[i]; + } + } + + return EPHIDGET_OK; +} + +//getPacket - not used for accelerometer +CGETPACKET(Accelerometer) + return EPHIDGET_UNEXPECTED; +} + +// === Exported Functions === // + +//create and initialize a device structure +CCREATE(Accelerometer, PHIDCLASS_ACCELEROMETER) + +//event setup functions +CFHANDLE(Accelerometer, AccelerationChange, int, double) + +CGET(Accelerometer,AxisCount,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER) + TESTATTACHED + + MASGN(phid.attr.accelerometer.numAxis) +} + +CGETINDEX(Accelerometer,Acceleration,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER) + TESTATTACHED + TESTINDEX(phid.attr.accelerometer.numAxis) + TESTMASGN(axis[Index], PUNK_DBL) + + MASGN(axis[Index]) +} + +CGETINDEX(Accelerometer,AccelerationMax,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER) + TESTATTACHED + TESTINDEX(phid.attr.accelerometer.numAxis) + TESTMASGN(accelerationMax, PUNK_DBL) + + MASGN(accelerationMax) +} + +CGETINDEX(Accelerometer,AccelerationMin,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER) + TESTATTACHED + TESTINDEX(phid.attr.accelerometer.numAxis) + TESTMASGN(accelerationMin, PUNK_DBL) + + MASGN(accelerationMin) +} + +CGETINDEX(Accelerometer,AccelerationChangeTrigger,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER) + TESTATTACHED + TESTINDEX(phid.attr.accelerometer.numAxis) + TESTMASGN(axisChangeTrigger[Index], PUNK_DBL) + + MASGN(axisChangeTrigger[Index]) +} +CSETINDEX(Accelerometer,AccelerationChangeTrigger,double) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER) + TESTATTACHED + TESTINDEX(phid.attr.accelerometer.numAxis) + TESTRANGE(0, phid->accelerationMax - phid->accelerationMin) + + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(Trigger, "%lE", axisChangeTrigger); + else + phid->axisChangeTrigger[Index] = newVal; + + return EPHIDGET_OK; +} + +// === Deprecated Functions === // + +CGET(Accelerometer,NumAxis,int) + return CPhidgetAccelerometer_getAxisCount(phid, pVal); +} -- cgit v1.2.3