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); }