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;
}
/// <summary>
/// Writes calibration values in the registry
/// </summary>
/// <param name="cal">Calibration response PDU</param>
public static void WRITE_CALIBRATION(CAL_RSP cal)
{
ThreadPool.QueueUserWorkItem(func =>
{
kernelLock.WaitOne();
try
{
RegistryKey rk = Registry.LocalMachine.CreateSubKey(Core.REGISTRY_SENSORS_PATH + "\\" + cal._SensorID.ToString("0"));
rk.SetValue("_X1G", cal._X1G, RegistryValueKind.String);
rk.SetValue("_XN1G", cal._XN1G, RegistryValueKind.String);
rk.SetValue("_Y1G", cal._Y1G, RegistryValueKind.String);
rk.SetValue("_YN1G", cal._YN1G, RegistryValueKind.String);
rk.SetValue("_Z1G", cal._Z1G, RegistryValueKind.String);
rk.SetValue("_ZN1G", cal._ZN1G, RegistryValueKind.String);
rk.Close();
}
catch (Exception e)
{
Logger.Error("Core.cs:WRITE_CALIBRATION:" + e.ToString());
}
kernelLock.Release();
});
}