public override int Decode(int sourceSensor, byte[] data, int length)
{
int rawDataIndex = 0;
int numDecodedPackets = 0;
if (length != 0) // Have some data
{
while (rawDataIndex < length)
{
if ((data[rawDataIndex] & 0x80) != 0)
{
this.packetPosition = 0;
this.headerSeen = true;
packetType = (SensorDataType)((int)((byte)(((byte)data[rawDataIndex]) << 1) >> 6));
switch (packetType)
{
case SensorDataType.UNCOMPRESSED_DATA_PDU:
bytesToRead = 5;
break;
case SensorDataType.COMPRESSED_DATA_PDU:
bytesToRead = 3;
break;
case SensorDataType.RESPONSE_PDU:
responseType = (ResponseTypes)((int)(((byte)data[rawDataIndex]) & 0x1f));
switch (responseType)
{
case ResponseTypes.BP_RSP:
case ResponseTypes.SENS_RSP:
case ResponseTypes.SR_RSP:
case ResponseTypes.ALT_RSP:
case ResponseTypes.PDT_RSP:
case ResponseTypes.TM_RSP:
case ResponseTypes.HV_RSP:
case ResponseTypes.FV_RSP:
bytesToRead = 2;
break;
case ResponseTypes.BL_RSP:
case ResponseTypes.BC_RSP:
bytesToRead = 3;
break;
case ResponseTypes.PC_RSP:
bytesToRead = 6;
break;
case ResponseTypes.CAL_RSP:
case ResponseTypes.BTCAL_RSP:
bytesToRead = 10;
break;
default:
break;
}
break;
default:
break;
}
}
if ((this.headerSeen == true) && (this.packetPosition < bytesToRead))
{
this.packet[this.packetPosition] = data[rawDataIndex];
}
this.packetPosition++;
rawDataIndex++;
if ((this.packetPosition == bytesToRead)) //a full packet was received
{
WocketsAccelerationData datum = ((WocketsAccelerationData)this._Data[this.head]);
datum.Reset();
//copy raw bytes
for (int i = 0; (i < bytesToRead); i++)
{
datum.RawBytes[i] = this.packet[i];
}
datum.UnixTimeStamp = lastUnixTime;
if ((packetType == SensorDataType.UNCOMPRESSED_DATA_PDU) || (packetType == SensorDataType.COMPRESSED_DATA_PDU))
{
short x = 0;
short y = 0;
short z = 0;
if (packetType == SensorDataType.UNCOMPRESSED_DATA_PDU)
{
datum._Type = SensorDataType.UNCOMPRESSED_DATA_PDU;
x = (short)((((short)(this.packet[0] & 0x03)) << 8) | (((short)(this.packet[1] & 0x7f)) << 1) | (((short)(this.packet[2] & 0x40)) >> 6));
y = (short)((((short)(this.packet[2] & 0x3f)) << 4) | (((short)(this.packet[3] & 0x78)) >> 3));
z = (short)((((short)(this.packet[3] & 0x07)) << 7) | ((short)(this.packet[4] & 0x7f)));
_UncompressedPDUCount++;
}
else
{
datum._Type = SensorDataType.COMPRESSED_DATA_PDU;
x = (short)(((this.packet[0] & 0x0f) << 1) | ((this.packet[1] & 0x40) >> 6));
x = ((((short)((this.packet[0] >> 4) & 0x01)) == 1) ? ((short)(prevx + x)) : ((short)(prevx - x)));
y = (short)(this.packet[1] & 0x1f);
y = ((((short)((this.packet[1] >> 5) & 0x01)) == 1) ? ((short)(prevy + y)) : ((short)(prevy - y)));
z = (short)((this.packet[2] >> 1) & 0x1f);
z = ((((short)((this.packet[2] >> 6) & 0x01)) == 1) ? ((short)(prevz + z)) : ((short)(prevz - z)));
_CompressedPDUCount++;
}
prevx = x;
prevy = y;
prevz = z;
datum._X = (short)x;
datum._Y = (short)y;
datum._Z = (short)z;
if (this.head >= (BUFFER_SIZE - 1))
{
this.head = 0;
}
else
{
this.head++;
}
numDecodedPackets++;
this.packetPosition = 0;
this.headerSeen = false;
}
}
}
}
//this._Size = decodedDataIndex;
return(numDecodedPackets);
}
private void Poll()
{
#region Poll All Wockets and MITes and Decode Data
if ((this._Mode == MemoryMode.BluetoothToLocal) || (this._Mode == MemoryMode.BluetoothToShared))
{
bool allWocketsDisconnected = true;
bool bluetoothIsReset = false;
Receiver currentReceiver = null;
Sensor sensor = null;
int[] batteryPoll = new int[this._Sensors.Count];
int[] alive = new int[this._Sensors.Count];
GET_BT GET_BT_CMD = new GET_BT();
ALIVE ALIVE_CMD = new ALIVE();
int pollCounter = 0;
System.Reflection.Assembly a = System.Reflection.Assembly.GetExecutingAssembly();
System.Reflection.AssemblyName an = a.GetName();
Logger.Warn("Version " + an.Version.ToString() + " Date:" + DateTime.Now.ToString("f"));
this.StartTime = WocketsTimer.GetUnixTime();
while (true)
{
if (!polling)
{
this.waitToPollEvent.Set();
for (int i = 0; (i < this._Sensors.Count); i++)
{
this._Sensors[i]._ReceivedPackets = 0;
this._Sensors[i]._SavedPackets = 0;
this._Receivers[i].Dispose();
}
this.pollingEvent.WaitOne();
}
allWocketsDisconnected = true;
pollCounter++;
for (int i = 0; (i < this._Sensors.Count); i++)
{
sensor = this._Sensors[i];
if (sensor._Loaded)
{
currentReceiver = sensor._Receiver;
try
{
if (this._TMode == TransmissionMode.Bursty60)
{
int expectedPackets = ((Wockets.Decoders.Accelerometers.WocketsDecoder)sensor._Decoder)._ExpectedBatchCount;
//skip if got everything
if ((expectedPackets > 0) && (sensor._ReceivedPackets == expectedPackets))
{
continue;
}
else
{
currentReceiver.Update();
}
}
else
{
currentReceiver.Update();
}
if (currentReceiver._Status == ReceiverStatus.Connected)
{
Decoder decoder = sensor._Decoder;
int numDecodedPackets = 0;
int tail = currentReceiver._Buffer._Tail;
int head = currentReceiver._Buffer._Head;
int dataLength = tail - head; //((RFCOMMReceiver)currentReceiver).bluetoothStream._Tail - currentReceiver._Head;
if (dataLength < 0)
{
dataLength = currentReceiver._Buffer._Bytes.Length - head + tail;//((RFCOMMReceiver)currentReceiver).bluetoothStream._Buffer.Length - currentReceiver._Head + ((RFCOMMReceiver)currentReceiver).bluetoothStream._Tail;
}
//test if all wockets are disconnected
if (sensor._Class == SensorClasses.Wockets)
{
if (bluetoothIsReset)
{
bluetoothIsReset = false;
}
if (allWocketsDisconnected)
{
allWocketsDisconnected = false;
}
}
if (dataLength > 0)
{
if (currentReceiver._Type == ReceiverTypes.HTCDiamond)
{
numDecodedPackets = decoder.Decode(sensor._ID, currentReceiver._Buffer, head, tail);
sensor._ReceivedPackets += numDecodedPackets;
}
else if (sensor._Class == SensorClasses.Wockets)
{
#region Write Data
#region Battery Query
batteryPoll[i] -= 1;
if (batteryPoll[i] <= 0)
{
((SerialReceiver)currentReceiver).Write(GET_BT_CMD._Bytes);
batteryPoll[i] = 6000 + i * 200;
}
#endregion Battery Query
#region Alive
alive[i] -= 1;
if (alive[i] <= 0)
{
((SerialReceiver)currentReceiver).Write(ALIVE_CMD._Bytes);
alive[i] = 200;
}
#endregion Alive
#endregion Write Data
#region Read Data
numDecodedPackets = decoder.Decode(sensor._ID, currentReceiver._Buffer, head, tail); //((RFCOMMReceiver)currentReceiver).bluetoothStream._Buffer, head, tail);
currentReceiver._Buffer._Head = tail;
sensor._ReceivedPackets += numDecodedPackets;
#endregion Read Data
}
}
if (pollCounter > 2000)
{
//Logger.Warn("Receiver " + sensor._Receiver._ID + " decoded:" + sensor._ReceivedPackets + ",saved:" + sensor._SavedPackets + ", tail=" + tail + ",head=" + head);
pollCounter = 0;
}
}
}
catch (Exception ex)
{
alive[i] = 200;//10 in sniff//200 in continuous worked well
Logger.Error(ex.Message + " \nTrace:" + ex.StackTrace);
currentReceiver.Dispose();
}
}
}
//reset bluetooth stack once if all wockets are disconnected
if ((this._TMode == TransmissionMode.Continuous) && (!bluetoothIsReset) && (allWocketsDisconnected))
{
try
{
//if (CurrentWockets._Configuration._SoftwareMode == Wockets.Data.Configuration.SoftwareConfiguration.DEBUG)
// Logger.Debug("All Wockets Disconnected. BT Reset.");
NetworkStacks._BluetoothStack.Dispose();
NetworkStacks._BluetoothStack.Initialize();
bluetoothIsReset = true;
}
catch
{
}
}
Thread.Sleep(10);
}
}
#if (PocketPC)
//Read data from shared memory and populate the decoder
else if (this._Mode == MemoryMode.SharedToLocal)
{
MemoryMappedFileStream[] sdata = null;
MemoryMappedFileStream[] shead = null;
byte[] head = new byte[4];
int sdataSize = 0;
int numSensors = CurrentWockets._Controller._Sensors.Count;
int[] decoderTails;
byte[] timestamp = new byte[sizeof(double)];
byte[] acc = new byte[sizeof(short)];
sdata = new MemoryMappedFileStream[numSensors];
shead = new MemoryMappedFileStream[numSensors];
sdataSize = (int)Decoder._DUSize * Wockets.Decoders.Accelerometers.WocketsDecoder.BUFFER_SIZE;
decoderTails = new int[numSensors];
for (int i = 0; (i < numSensors); i++)
{
sdata[i] = new MemoryMappedFileStream("\\Temp\\wocket" + i + ".dat", "wocket" + i, (uint)sdataSize, MemoryProtection.PageReadWrite);
shead[i] = new MemoryMappedFileStream("\\Temp\\whead" + i + ".dat", "whead" + i, sizeof(int), MemoryProtection.PageReadWrite);
sdata[i].MapViewToProcessMemory(0, sdataSize);
shead[i].MapViewToProcessMemory(0, sizeof(int));
shead[i].Read(head, 0, 4);
int currentHead = BitConverter.ToInt32(head, 0);
decoderTails[i] = currentHead;
shead[i].Seek(0, System.IO.SeekOrigin.Begin);
sdata[i].Seek((currentHead * (sizeof(double) + 3 * sizeof(short))), System.IO.SeekOrigin.Begin);
}
while (true)
{
try
{
for (int i = 0; (i < CurrentWockets._Controller._Sensors.Count); i++)
{
int tail = decoderTails[i];
int currentHead = tail;
shead[i].Read(head, 0, 4);
currentHead = BitConverter.ToInt32(head, 0);
shead[i].Seek(0, System.IO.SeekOrigin.Begin);
while (tail != currentHead)
{
#if (PocketPC)
int bufferHead = CurrentWockets._Controller._Decoders[i]._Head;
WocketsAccelerationData datum = ((WocketsAccelerationData)CurrentWockets._Controller._Decoders[i]._Data[bufferHead]);
datum.Reset();
datum._SensorID = (byte)i;
sdata[i].Read(timestamp, 0, sizeof(double));
datum.UnixTimeStamp = BitConverter.ToDouble(timestamp, 0);
sdata[i].Read(acc, 0, sizeof(short));
datum._X = BitConverter.ToInt16(acc, 0);
sdata[i].Read(acc, 0, sizeof(short));
datum._Y = BitConverter.ToInt16(acc, 0);
sdata[i].Read(acc, 0, sizeof(short));
datum._Z = BitConverter.ToInt16(acc, 0);
datum._Type = Data.SensorDataType.UNCOMPRESSED_DATA_PDU;
CurrentWockets._Controller._Decoders[i].TotalSamples++;
//copy raw bytes
//for (int i = 0; (i < bytesToRead); i++)
// datum.RawBytes[i] = this.packet[i];
//datum.RawBytes[0] = (byte)(((datum.RawBytes[0])&0xc7)|(sourceSensor<<3));
if (bufferHead >= (CurrentWockets._Controller._Decoders[i]._BufferSize - 1))
{
bufferHead = 0;
}
else
{
bufferHead++;
}
CurrentWockets._Controller._Decoders[i]._Head = bufferHead;
#endif
if (tail >= (Wockets.Decoders.Accelerometers.WocketsDecoder.BUFFER_SIZE - 1))
{
tail = 0;
#if (PocketPC)
sdata[i].Seek(0, System.IO.SeekOrigin.Begin);
#endif
}
else
{
tail++;
}
}
decoderTails[i] = currentHead;
}
}
catch
{
}
Thread.Sleep(100);
}
}
#endif
#endregion Poll All Wockets and MITes and Decode Data
}
public override int Decode(int sourceSensor, CircularBuffer data, int start, int end)
{
int rawDataIndex = start;
int numDecodedPackets = 0;
//int bufferHead = this.head;
while (rawDataIndex != end)
{
//If a PDU first byte
if ((data._Bytes[rawDataIndex] & 0x80) != 0)
{
this.packetPosition = 0;
this.headerSeen = true;
packetType = (SensorDataType)((int)((byte)(((byte)data._Bytes[rawDataIndex]) << 1) >> 6));
this.timestamp = data._Timestamp[rawDataIndex];
switch (packetType)
{
case SensorDataType.UNCOMPRESSED_DATA_PDU:
bytesToRead = 5;
break;
case SensorDataType.COMPRESSED_DATA_PDU:
bytesToRead = 3;
break;
case SensorDataType.RESPONSE_PDU:
responseType = (ResponseTypes)((int)(((byte)data._Bytes[rawDataIndex]) & 0x1f));
switch (responseType)
{
case ResponseTypes.BP_RSP:
case ResponseTypes.SENS_RSP:
case ResponseTypes.SR_RSP:
case ResponseTypes.ALT_RSP:
case ResponseTypes.PDT_RSP:
case ResponseTypes.TM_RSP:
case ResponseTypes.HV_RSP:
case ResponseTypes.FV_RSP:
bytesToRead = 2;
break;
case ResponseTypes.BL_RSP:
case ResponseTypes.BC_RSP:
case ResponseTypes.ACC_RSP:
case ResponseTypes.OFT_RSP:
bytesToRead = 3;
break;
case ResponseTypes.TCT_RSP:
bytesToRead = 5;
break;
case ResponseTypes.AC_RSP:
case ResponseTypes.PC_RSP:
bytesToRead = 6;
break;
case ResponseTypes.CAL_RSP:
case ResponseTypes.BTCAL_RSP:
bytesToRead = 10;
break;
default:
break;
}
break;
default:
break;
}
}
if ((this.headerSeen == true) && (this.packetPosition < bytesToRead))
{
this.packet[this.packetPosition] = data._Bytes[rawDataIndex];
}
this.packetPosition++;
rawDataIndex = (rawDataIndex + 1) % data._Bytes.Length;
if ((this.packetPosition == bytesToRead)) //a full packet was received
{
if ((packetType == SensorDataType.UNCOMPRESSED_DATA_PDU) || (packetType == SensorDataType.COMPRESSED_DATA_PDU))
{
short x = 0;
short y = 0;
short z = 0;
//for each batch reinitialize the activity count sum and offset
if (this._ActivityCountOffset < 0)
{
acc_count = -1;
this._ActivityCountOffset = -1;
}
if (packetType == SensorDataType.UNCOMPRESSED_DATA_PDU)
{
x = (short)((((short)(this.packet[0] & 0x03)) << 8) | (((short)(this.packet[1] & 0x7f)) << 1) | (((short)(this.packet[2] & 0x40)) >> 6));
y = (short)((((short)(this.packet[2] & 0x3f)) << 4) | (((short)(this.packet[3] & 0x78)) >> 3));
z = (short)((((short)(this.packet[3] & 0x07)) << 7) | ((short)(this.packet[4] & 0x7f)));
_UncompressedPDUCount++;
}
else
{
x = (short)(((this.packet[0] & 0x0f) << 1) | ((this.packet[1] & 0x40) >> 6));
x = ((((short)((this.packet[0] >> 4) & 0x01)) == 1) ? ((short)(prevx + x)) : ((short)(prevx - x)));
y = (short)(this.packet[1] & 0x1f);
y = ((((short)((this.packet[1] >> 5) & 0x01)) == 1) ? ((short)(prevy + y)) : ((short)(prevy - y)));
z = (short)((this.packet[2] >> 1) & 0x1f);
z = ((((short)((this.packet[2] >> 6) & 0x01)) == 1) ? ((short)(prevz + z)) : ((short)(prevz - z)));
_CompressedPDUCount++;
}
prevx = x;
prevy = y;
prevz = z;
double ts = 0;
//Use the high precision timer
if (CurrentWockets._Controller._TMode == TransmissionMode.Continuous)
{
ts = WocketsTimer.GetUnixTime();
}
else // use date time now assuming suspension is possible
{
ts = batchCurrentTime;
batchCurrentTime += batchDeltaTime;
}
this.TotalSamples++;
//if (CurrentWockets._Configuration._MemoryMode == Wockets.Data.Configuration.MemoryConfiguration.NON_SHARED)
// if (CurrentWockets._Controller._Mode== MemoryMode.BluetoothToLocal)
//{
int bufferHead = this.head;
WocketsAccelerationData datum = ((WocketsAccelerationData)this._Data[bufferHead]);
datum.Reset();
datum.UnixTimeStamp = ts;
//copy raw bytes
for (int i = 0; (i < bytesToRead); i++)
{
datum.RawBytes[i] = this.packet[i];
}
if (bytesToRead == 3)
{
datum._Type = SensorDataType.COMPRESSED_DATA_PDU;
}
else
{
datum._Type = SensorDataType.UNCOMPRESSED_DATA_PDU;
}
datum._Length = bytesToRead;
//datum.RawBytes[0] = (byte)(((datum.RawBytes[0])&0xc7)|(sourceSensor<<3));
datum._SensorID = (byte)sourceSensor;
datum._X = x;
datum._Y = y;
datum._Z = z;
#if (PocketPC)
if ((CurrentWockets._Controller._TMode == TransmissionMode.Continuous) && (CurrentWockets._Controller._Mode == MemoryMode.BluetoothToShared)) //if (CurrentWockets._Configuration._MemoryMode == Wockets.Data.Configuration.MemoryConfiguration.SHARED)
{
this.sdata.Write(BitConverter.GetBytes(ts), 0, sizeof(double));
//this.head+=sizeof(double);
this.sdata.Write(BitConverter.GetBytes(x), 0, sizeof(short));
//this.head+=sizeof(short);
this.sdata.Write(BitConverter.GetBytes(y), 0, sizeof(short));
//this.head+=sizeof(short);
this.sdata.Write(BitConverter.GetBytes(z), 0, sizeof(short));
}
#endif
if (bufferHead >= (BUFFER_SIZE - 1))
{
bufferHead = 0;
#if (PocketPC)
if (CurrentWockets._Controller._TMode == TransmissionMode.Continuous)
{
this.sdata.Seek(0, System.IO.SeekOrigin.Begin);
}
#endif
}
else
{
bufferHead++;
}
this.head = bufferHead;
#if (PocketPC)
if (CurrentWockets._Controller._TMode == TransmissionMode.Continuous)
{
this.shead.Seek(0, System.IO.SeekOrigin.Begin);
this.shead.Write(BitConverter.GetBytes(this.head), 0, sizeof(int));
}
#endif
numDecodedPackets++;
}
else if (packetType == SensorDataType.RESPONSE_PDU)
{
switch (responseType)
{
case ResponseTypes.BL_RSP:
BL_RSP br = new BL_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
br.RawBytes[i] = this.packet[i];
}
br._BatteryLevel = (((int)this.packet[1]) << 3) | ((this.packet[2] >> 4) & 0x07);
#if (PocketPC)
Core.WRITE_BATTERY_LEVEL(br);
#endif
FireEvent(br);
break;
case ResponseTypes.CAL_RSP:
CAL_RSP cal = new CAL_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
cal.RawBytes[i] = this.packet[i];
}
cal._X1G = ((this.packet[1] & 0x7f) << 3) | ((this.packet[2] & 0x70) >> 4);
cal._XN1G = ((this.packet[2] & 0x0f) << 6) | ((this.packet[3] & 0x7e) >> 1);
cal._Y1G = ((this.packet[3] & 0x01) << 9) | ((this.packet[4] & 0x7f) << 2) | ((this.packet[5] & 0x60) >> 5);
cal._YN1G = ((this.packet[5] & 0x1f) << 5) | ((this.packet[6] & 0x7c) >> 2);
cal._Z1G = ((this.packet[6] & 0x03) << 8) | ((this.packet[7] & 0x7f) << 1) | ((this.packet[8] & 0x40) >> 6);
cal._ZN1G = ((this.packet[8] & 0x3f) << 4) | ((this.packet[9] & 0x78) >> 3);
#if (PocketPC)
Core.WRITE_CALIBRATION(cal);
#endif
FireEvent(cal);
break;
case ResponseTypes.BTCAL_RSP:
BTCAL_RSP btcal = new BTCAL_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
btcal.RawBytes[i] = this.packet[i];
}
btcal._CAL100 = ((this.packet[1] & 0x7f) << 3) | ((this.packet[2] & 0x70) >> 4);
btcal._CAL80 = ((this.packet[2] & 0x0f) << 6) | ((this.packet[3] & 0x7e) >> 1);
btcal._CAL60 = ((this.packet[3] & 0x01) << 9) | ((this.packet[4] & 0x7f) << 2) | ((this.packet[5] & 0x60) >> 5);
btcal._CAL40 = ((this.packet[5] & 0x1f) << 5) | ((this.packet[6] & 0x7c) >> 2);
btcal._CAL20 = ((this.packet[6] & 0x03) << 8) | ((this.packet[7] & 0x7f) << 1) | ((this.packet[8] & 0x40) >> 6);
btcal._CAL10 = ((this.packet[8] & 0x3f) << 4) | ((this.packet[9] & 0x78) >> 3);
FireEvent(btcal);
break;
case ResponseTypes.SR_RSP:
SR_RSP sr = new SR_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
sr.RawBytes[i] = this.packet[i];
}
sr._SamplingRate = (this.packet[1] & 0x7f);
this._ExpectedSamplingRate = sr._SamplingRate;
#if (PocketPC)
Core.WRITE_SAMPLING_RATE(sr);
#endif
FireEvent(sr);
break;
case ResponseTypes.BP_RSP:
BP_RSP bp = new BP_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
bp.RawBytes[i] = this.packet[i];
}
bp._Percent = (this.packet[1] & 0x7f);
#if (PocketPC)
Core.WRITE_BATTERY_PERCENT(bp);
#endif
FireEvent(bp);
break;
case ResponseTypes.TM_RSP:
TM_RSP tm = new TM_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
tm.RawBytes[i] = this.packet[i];
}
tm._TransmissionMode = (TransmissionMode)((this.packet[1] >> 4) & 0x07);
#if (PocketPC)
Core.WRITE_TRANSMISSION_MODE(tm);
#endif
FireEvent(tm);
break;
case ResponseTypes.SENS_RSP:
SENS_RSP sen = new SENS_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
sen.RawBytes[i] = this.packet[i];
}
sen._Sensitivity = (Sensitivity)((this.packet[1] >> 4) & 0x07);
#if (PocketPC)
Core.WRITE_SENSITIVITY(sen);
#endif
FireEvent(sen);
break;
case ResponseTypes.PC_RSP:
PC_RSP pc = new PC_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
pc.RawBytes[i] = this.packet[i];
}
pc._Count = ((this.packet[1] & 0x7f) << 25) | ((this.packet[2] & 0x7f) << 18) | ((this.packet[3] & 0x7f) << 11) | ((this.packet[4] & 0x7f) << 4) | ((this.packet[5] & 0x7f) >> 3);
#if (PocketPC)
Core.WRITE_PDU_COUNT(pc);
#endif
FireEvent(pc);
break;
case ResponseTypes.PDT_RSP:
PDT_RSP pdt = new PDT_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
pdt.RawBytes[i] = this.packet[i];
}
pdt._Timeout = (this.packet[1] & 0x7f);
FireEvent(pdt);
break;
case ResponseTypes.HV_RSP:
HV_RSP hv = new HV_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
hv.RawBytes[i] = this.packet[i];
}
hv._Version = (this.packet[1] & 0x7f);
FireEvent(hv);
break;
case ResponseTypes.FV_RSP:
FV_RSP fv = new FV_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
fv.RawBytes[i] = this.packet[i];
}
fv._Version = (this.packet[1] & 0x7f);
Logger.Debug(this._ID + "," + fv._Version);
FireEvent(fv);
break;
case ResponseTypes.BC_RSP:
BC_RSP bc = new BC_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
bc.RawBytes[i] = this.packet[i];
}
bc._Count = ((this.packet[1] & 0x7f) << 7) | (this.packet[2] & 0x7f);
this._ExpectedBatchCount = bc._Count;
//Compute the start time and delta for timestamping the data
double calculated_startTime = ((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime;
//attempt correcting using the sampling rate, check if the
// first sample has a timestamp greater than the last timestamped
// sample... if that is the case continue
// if it is not the case then temporarily alter the delta value
// to fit within the start time and end time for the samples
// this is necessary to avoid overspreading the samples when disconnections
// occur
calculated_startTime -= ((1000.0 / this._ExpectedSamplingRate) * bc._Count);
// Only use the ideal sampling rate to spread out the signal if
// there is a huge gap with the previous transmission
if ((calculated_startTime > lastDecodedSampleTime) && ((calculated_startTime - lastDecodedSampleTime) > 60000))
{
batchCurrentTime = calculated_startTime;
batchDeltaTime = 1000.0 / this._ExpectedSamplingRate;
}
else
{
batchDeltaTime = (((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime - lastDecodedSampleTime) / bc._Count;
batchCurrentTime = lastDecodedSampleTime + batchDeltaTime;
}
lastDecodedSampleTime = ((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime;
FireEvent(bc);
break;
case ResponseTypes.AC_RSP:
AC_RSP ac = new AC_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
ac.RawBytes[i] = this.packet[i];
}
ac._SeqNum = ((this.packet[1] & 0x7f) << 9) | ((this.packet[2] & 0x7f) << 2) | ((this.packet[3] >> 5) & 0x03);
ac._Count = ((this.packet[3] & 0x1f) << 11) | ((this.packet[4] & 0x7f) << 4) | ((this.packet[5] >> 2) & 0x0f);
//to recover from resets
if ((this._LastActivityCountIndex != -1) && (ac._SeqNum == 0) && (((this._ActivityCounts[this._LastActivityCountIndex]._SeqNum) - ac._SeqNum) > 20))
{
this._LastActivityCountIndex = -1;
}
/*if ((this._LastActivityCountIndex==-1) //First time base it on the sampling rate
|| (acc_count<this._ActivityCounts[this._LastActivityCountIndex]._SeqNum)) //accidental reset
||
|| {
|| ac_unixtime=((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime - ((this._ActivityCountOffset * 1000.0)/this._ExpectedSamplingRate) - (acc_count* 60000.0);
|| ac_delta=60000.0;
|| ac_refseq=0;
|| }
|| else if (ac_delta == 0) //First sample after ACC, handles overflow as well
|| {
|| ac_unixtime = this._ActivityCounts[this._LastActivityCountIndex]._TimeStamp;
|| ac_refseq = this._ActivityCounts[this._LastActivityCountIndex]._SeqNum;
|| ac_delta = (((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime - ((this._ActivityCountOffset * 1000.0) / this._ExpectedSamplingRate) - this._ActivityCounts[this._LastActivityCountIndex]._TimeStamp) / (acc_count - ac_refseq);
|| }*/
ac._TimeStamp = (double)timestamps[ac._SeqNum];
//Has to stay here to protect against situations when a batch is sent
//with some ACs that were received and others that were not
//ac._TimeStamp = ac_unixtime + (++_ACIndex * ac_delta);
++_ACIndex;
//ac._TimeStamp = ac_unixtime + (((ac._SeqNum-ac_refseq)+1) * ac_delta);
#if (PocketPC)
if (_ACIndex == 10)
{
((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID]).Write(new Wockets.Data.Commands.ACK()._Bytes);
_ACIndex = 0;
}
#endif
//Only insert new sequence numbers
// if this is the first AC or it is not equal to the previous sequence number
if ((this._ActivityCountOffset >= 0) && (acc_count >= 0))
{
if ((this._LastActivityCountIndex == -1) || (ac._SeqNum == (this._ActivityCounts[this._LastActivityCountIndex]._SeqNum + 1)) || ((ac._SeqNum - this._ActivityCounts[this._LastActivityCountIndex]._SeqNum + 1) > 960))
{
this._LastActivityCountIndex = this._ActivityCountIndex;
this._ActivityCounts[this._ActivityCountIndex++] = ac;
if (this._ActivityCountIndex == 960)
{
this._ActivityCountIndex = 0;
}
#if (PocketPC)
Core.WRITE_ACTIVITY_COUNT(ac);
#endif
}
}
Logger.Warn("ACC" + this._ID + "," + acc_count + "," + this._ActivityCounts[this._LastActivityCountIndex]._SeqNum + "," + ac._SeqNum + "," + ac._Count);
FireEvent(ac);
break;
case ResponseTypes.TCT_RSP:
TCT_RSP tct = new TCT_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
tct.RawBytes[i] = this.packet[i];
}
tct._TCT = (((this.packet[1] & 0x7f) << 1) | ((this.packet[2] >> 6) & 0x01));
tct._REPS = (((this.packet[2] & 0x3f) << 2) | ((this.packet[3] >> 5) & 0x03));
tct._LAST = (((this.packet[3] & 0x1f) << 3) | ((this.packet[4] >> 4) & 0x07));
#if (PocketPC)
Core.WRITE_TCT(tct);
#endif
FireEvent(tct);
break;
case ResponseTypes.ACC_RSP:
ACC_RSP acc = new ACC_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
acc.RawBytes[i] = this.packet[i];
}
acc._Count = ((this.packet[1] & 0x7f) << 7) | (this.packet[2] & 0x7f);
ac_unixtime = 0;
ac_delta = 0;
_ACIndex = 0;
acc_count = acc._Count;
// this._ActivityCounts[this._LastActivityCountIndex]._TimeStamp;
if ((this._LastActivityCountIndex == -1) || //First time base it on the sampling rate
(acc_count < this._ActivityCounts[this._LastActivityCountIndex]._SeqNum)) //accidental reset
{
ac_unixtime = ((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime - ((this._ActivityCountOffset * 1000.0) / this._ExpectedSamplingRate);
ac_delta = 60000.0;
ac_refseq = 0;
timestamps = new Hashtable();
}
else if (ac_delta == 0) //First sample after ACC, handles overflow as well
{
ac_unixtime = ((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime - ((this._ActivityCountOffset * 1000.0) / this._ExpectedSamplingRate);
ac_delta = (((RFCOMMReceiver)CurrentWockets._Controller._Receivers[this._ID])._CurrentConnectionUnixTime - ((this._ActivityCountOffset * 1000.0) / this._ExpectedSamplingRate) - this._ActivityCounts[this._LastActivityCountIndex]._TimeStamp) / (acc_count - this._ActivityCounts[this._LastActivityCountIndex]._SeqNum);
}
for (int i = acc._Count; (i >= 0); i--)
{
if (timestamps.ContainsKey(i))
{
break;
}
else
{
timestamps.Add(i, ac_unixtime - ((acc._Count - i) * ac_delta));
}
}
FireEvent(acc);
break;
case ResponseTypes.OFT_RSP:
OFT_RSP offset = new OFT_RSP(this._ID);
for (int i = 0; (i < bytesToRead); i++)
{
offset.RawBytes[i] = this.packet[i];
}
offset._Offset = ((this.packet[1] & 0x7f) << 7) | (this.packet[2] & 0x7f);
this._ActivityCountOffset = offset._Offset;
FireEvent(offset);
break;
default:
break;
}
}
this.packetPosition = 0;
this.headerSeen = false;
}
}
return(numDecodedPackets);
}
