public virtual bool Initialize()
{
//if ( (CurrentWockets._Controller._Mode== MemoryMode.BluetoothToLocal) || (CurrentWockets._Controller._Mode== MemoryMode.SharedToLocal))
this._Data = new SensorData[this._BufferSize];
#if (PocketPC)
// else
if (CurrentWockets._Controller._Mode == MemoryMode.BluetoothToShared)
{
this.sdata = new MemoryMappedFileStream("\\Temp\\wocket" + this._ID + ".dat", "wocket" + this._ID, (_DUSize * (uint)this._BufferSize), MemoryProtection.PageReadWrite);
this.shead = new MemoryMappedFileStream("\\Temp\\whead" + this._ID + ".dat", "whead" + this._ID, sizeof(int), MemoryProtection.PageReadWrite);
this.sdataSize = (int)(_DUSize * this._BufferSize);
this.sdata.MapViewToProcessMemory(0, this.sdataSize);
this.shead.MapViewToProcessMemory(0, sizeof(int));
this.shead.Write(BitConverter.GetBytes(this.head), 0, sizeof(int));
return(true);
}
#endif
return(false);
}
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
}
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 virtual bool Initialize()
{
//if ( (CurrentWockets._Controller._Mode== MemoryMode.BluetoothToLocal) || (CurrentWockets._Controller._Mode== MemoryMode.SharedToLocal))
this._Data = new SensorData[this._BufferSize];
#if (PocketPC)
// else
if (CurrentWockets._Controller._Mode == MemoryMode.BluetoothToShared)
{
this.sdata = new MemoryMappedFileStream("\\Temp\\wocket" + this._ID + ".dat", "wocket" + this._ID, (_DUSize * (uint)this._BufferSize), MemoryProtection.PageReadWrite);
this.shead = new MemoryMappedFileStream("\\Temp\\whead" + this._ID + ".dat", "whead" + this._ID, sizeof(int), MemoryProtection.PageReadWrite);
this.sdataSize = (int)(_DUSize * this._BufferSize);
this.sdata.MapViewToProcessMemory(0, this.sdataSize);
this.shead.MapViewToProcessMemory(0, sizeof(int));
this.shead.Write(BitConverter.GetBytes(this.head), 0, sizeof(int));
return true;
}
#endif
return false;
}
