public HTCDiamondReceiver()
: base(BUFFER_SIZE)
{
//this.bufferIndex = 0;
this.type = ReceiverTypes.HTCDiamond;
this.lastTS = WocketsTimer.GetUnixTime();
this.sampleTimeSpace = 1000 / MAXIMUM_SAMPLING_RATE;
// HTCSensorSetSensitivity(myHandle, HTCSensitivity.TWOG);
//HTCSensorSetPollingInterval(myHandle,200);
//HTCSensorGetPollingInterval(myHandle, out ttt);
//HTCSensorClose(myHandle);
//ECS_PowerDown(1);
//ECS_PowerUp();
//myHandle=HTCSensorOpen(HTCSensor.GSensor);
//HTCSensorGetPollingInterval(myHandle, out ttt);
/*for (uint x = 0; (x < uint.MaxValue); x++)
* {
*
* ttt = HTCSensorSetPollingInterval(myHandle, x);
* if (ttt == 0)
* Console.WriteLine();
* Thread.Sleep(10);
* }*/
}
private void WriteTimeStampPLFormat(double unixTime, ByteWriter byteWriter)
{
if (isActive)
{
WocketsTimer.GetUnixTimeBytes(unixTime, retBytes);
byteWriter.WriteBytes(retBytes, 6);
}
}
/// <summary>
/// A static method that opens a connection to a Bluetooth serial service and spawns a processing thread
/// </summary>
/// <param name="buffer">Handle to a circular receive buffer</param>
/// <param name="sbuffer">Handle to a circular send buffer</param>
/// <param name="address">A machine independent (Little or Big Endian) byte array of the bluetooth address</param>
/// <param name="pin">A pin for the remote bluetooth device</param>
/// <returns>A BluetoothStream object on success, otherwise a null</returns>
public static BluetoothStream Open(CircularBuffer buffer, CircularBuffer sbuffer, byte[] address, string pin)
{
//Initialize the Bluetooth stack
if (!NetworkStacks._BluetoothStack.Initialize())
{
return(null);
}
try
{
// Initialize the microsoft bluetooth stream
MicrosoftBluetoothStream btStream = new MicrosoftBluetoothStream(buffer, sbuffer, address, pin);
btStream._Status = BluetoothStatus.Reconnecting;
// Critical section: Allow one connection at a time
lock (mylock)
{
btStream.socket = new Socket(BluetoothStream._AddressFamily, SocketType.Stream, BluetoothStream._ProtocolType);
//btStream.socket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReceiveTimeout,10000);
btStream.socket.Blocking = true;
btStream._ConnectionTime = WocketsTimer.GetUnixTime(DateTime.Now);
btStream.socket.Connect(btStream._RemoteEP);
btStream._CurrentConnectionUnixTime = WocketsTimer.GetUnixTime(DateTime.Now);
btStream._ConnectionTime = btStream._CurrentConnectionUnixTime - btStream._ConnectionTime;
btStream.nstream = new NetworkStream(btStream.socket, true);
}
// Spawn the processing thread
btStream.processingThread = new Thread(new ThreadStart(btStream.Process));
btStream.processingThread.Priority = ThreadPriority.AboveNormal;
btStream.processingThread.Start();
//if (CurrentWockets._Configuration._SoftwareMode == SoftwareConfiguration.DEBUG)
// Logger.Debug("MicrosoftBluetoothStream: Open: Successful connection to" + btStream._HexAddress + ".");
return(btStream);
}
catch (Exception e)
{
// Exception failed to connect
CurrentWockets._LastError = ErrorCodes.CONNECTION_FAILED_TO_OPEN;
string hex = "";
for (int i = 0; i < address.Length; i++)
{
hex += address[i].ToString("X2");
}
CurrentWockets._LastErrorMessage = "Failed to open bluetooth connection to " + hex + ". " + e.ToString();
Logger.Error("MicrosoftBluetoothStream: Open: <" + ErrorCodes.CONNECTION_FAILED_TO_OPEN + ">: Failed to open connection to " + hex + ". " + e.ToString());
return(null);
}
}
public override void Update()
{
lock (this)
{
/// If a disconnection is detected for the bluetooth stream, update the status of the
/// receiver and flush the send buffer and delete references to the bluetooth stream
if ((this.bluetoothStream != null) && (this.bluetoothStream._Status == BluetoothStatus.Disconnected))
{
this.bluetoothStream = null;
this.status = ReceiverStatus.Disconnected;
this._SBuffer._Head = 0;
this.ndisc++;
this.disconnectionTime = WocketsTimer.GetUnixTime();
}
// If the bluetooth stream is null or the receiver is not reconnecting
// then instantiate a thread to reconnect
if ((this.bluetoothStream == null) && (this.status != ReceiverStatus.Reconnecting))
{
this.status = ReceiverStatus.Reconnecting;
reconnectionThread = new Thread(new ThreadStart(this.Reconnect));
reconnectionThread.Start();
//if (CurrentWockets._Configuration._SoftwareMode == SoftwareConfiguration.DEBUG)
// Logger.Debug("RFCOMMReceiver: Update: Spawning a reconnection thread for "+ this._Address);
}
if ((this.status != ReceiverStatus.Connected) && (this.bluetoothStream != null) && (this.bluetoothStream._Status == BluetoothStatus.Connected))
{
if (this.status == ReceiverStatus.Reconnecting)
{
reconnectionThread.Join();
reconnectionThread.Abort();
reconnectionThread = null;
}
//if (CurrentWockets._Configuration._SoftwareMode == SoftwareConfiguration.DEBUG)
// Logger.Debug("RFCOMMReceiver: Update: Reconnection successful for "+ this._Address);
this.status = ReceiverStatus.Connected;
if (this.disconnectionTime != 0)
{
this.disconTime += (int)((WocketsTimer.GetUnixTime() - this.disconnectionTime) / 1000);
}
}
}
}
public override int Decode(int sensorID, byte[] data, int length)
{
int rawDataIndex = 0;
int numDecodedPackets = 0;
if ((length != 0) && (data[0] == 0xff) && (data[15] == 0xff) && (length == HTCDiamondTouchAccelerationData.NUM_RAW_BYTES)) // Have some data
{
// if (decodedDataIndex >= this._Data.Length)
// throw new Exception("HTC Diamond Touch buffer too small " + this._Data.Length);
HTCDiamondTouchAccelerationData datum = ((HTCDiamondTouchAccelerationData)this._Data[this.head]);
datum.Reset();
//copy raw bytes
for (int i = 0; (i < HTCDiamondTouchAccelerationData.NUM_RAW_BYTES); i++)
{
datum.RawBytes[i] = data[i];
}
datum._Type = SensorDataType.UNCOMPRESSED_DATA_PDU;
datum._SensorID = (byte)sensorID;
datum._X = (short)(BitConverter.ToInt16(data, 1) + 1024);
datum._Y = (short)(BitConverter.ToInt16(data, 3) + 1024);
datum._Z = (short)(BitConverter.ToInt16(data, 5) + 1024);
//Set time stamps
datum.UnixTimeStamp = WocketsTimer.GetUnixTime();
//if (IsValid(datum))
if (this.head >= (BUFFER_SIZE - 1))
{
this.head = 0;
}
else
{
this.head++;
}
numDecodedPackets++;
this.headerSeen = false;
}
return(numDecodedPackets);
}
/// <summary>
/// Saves data to a binary file
/// </summary>
public override void Save()
{
#if (PocketPC)
if (_Saving)
{
#region Determine the head of the data buffer
int currentHead = -1;
currentHead = this._Decoder._Head;
#endregion Determine the head of the data buffer
#region Check if a new binary file need to be created
DateTime now = DateTime.Now;
if (presentHour != now.Hour) //((bw==null)||(presentHour != DateTime.Now.Hour)|| (presentMinute != DateTime.Now.Minute) || (presentSecond!= DateTime.Now.Second))
{
if (bw != null)
{
bw.CloseFile();
}
presentHour = now.Hour;
presentMinute = now.Minute;
presentSecond = now.Second;
// Need to create a new directory and switch the file name
dayPath = DirectoryStructure.DayDirectoryToUse(this._RootStorageDirectory);
// Make sure hour directory exists
currentDataFile = dayPath + "\\" + presentHour + "\\";
if (!System.IO.Directory.Exists(currentDataFile))
{
System.IO.Directory.CreateDirectory(currentDataFile);
}
currentDataFile = currentDataFile + FILE_TYPE_PREFIX + "." +
DirectoryStructure.GetDate() + "." + this._ID + "." + FILE_EXT;
bw = new ByteWriter(currentDataFile, true);
bw.OpenFile(32768);
// Ensure that the first data point in the new file will start
// with the full, rather than differential, timecode info.
isForceTimestampSave = true;
}
#endregion Check if a new binary file need to be created
// Write data as long as the tail is not equal to the head
while (tail != currentHead)
{
#region Populate the acceleration data that need to be written
data = ((AccelerationData)this._Decoder._Data[tail]);
#endregion Populate the acceleration data that need to be written
#region Check for timestamp errors
aUnixTime = data.UnixTimeStamp;
if (aUnixTime < lastUnixTime)
{
lastUnixTime = aUnixTime;
Logger.Error("Accelerometer: Save: Data overwritten without saving Accelerometer.cs Save " + this._ID + " " + aUnixTime + " " + lastUnixTime);
}
#endregion Check for timestamp errors
#region Write Data
if (bw != null)
{
#region Write Timestamp
diffMS = (int)(aUnixTime - lastUnixTime);
if (isForceTimestampSave || (diffMS > 254) || (timeSaveCount == TIMESTAMP_AFTER_SAMPLES))
{
bw.WriteByte((byte)255);
WocketsTimer.GetUnixTimeBytes(aUnixTime, retBytes);
bw.WriteBytes(retBytes, 6);
timeSaveCount = 0;
isForceTimestampSave = false;
}
else
{
bw.WriteByte((byte)diffMS);
timeSaveCount++;
}
#endregion Write Timestamp
#region Write Raw Data
for (int j = 0; j < data._Length; j++)
{
bw.WriteByte(data.RawBytes[j]);
}
#endregion Write Raw Data
}
#endregion Write Data
#region Update Pointers, statistics and time stamps
lastUnixTime = aUnixTime;
this.tailUnixTimestamp = aUnixTime;
if (tail >= this._Decoder._BufferSize - 1)
{
tail = 0;
}
else
{
tail++;
}
this._SavedPackets++;
#endregion Update Pointers, statistics and time stamps
}
if ((bw != null) && (this._Flush))
{
bw.Flush();
}
}
#endif
}
/// <summary>
/// Returns a vector that desribes the direction of gravity/acceleration in relation to the device screen.
/// When the device is face up on a flat surface, this method would return 0, 0, -9.8.
/// The Z value of -9.8 would mean that the acceleration in the opposite direction of the orientation of the screen.
/// When the device is held up, this method would return 0, -9.8, 0.
/// The Y value of -9.8 would mean that the device is accelerating in the direction of the bottom of the screen.
/// Conversely, if the device is held upside down, this method would return 0, 9.8, 0.
/// </summary>
/// <returns>
/// The vector returned will have a length measured in the unit meters per second square.
/// Ideally the when the device is in a motionless state, the vector would be of length 9.8.
/// However, the sensor is not extremely accurate, so this almost never the case.
/// </returns>
public int Read()
{
kkk++;
/* if (kkk == 200)
* ECS_PowerUp();
* else if (kkk == 400)
* {
* ECS_PowerDown(1);
* kkk = 0;
* }*/
#if (PocketPC)
//Check for the 20Hz
double now = WocketsTimer.GetUnixTime();
if ((now - lastTS) < this.sampleTimeSpace)
{
return(0);
}
lastTS = now;
HTCGSensorData data;
HTCSensorGetDataOutput(myHandle, out data);
int mytail = 0;
mytail = this._Buffer._Tail;
this._Buffer._Bytes[mytail++] = 0xff;
mytail = mytail % this._Buffer._Bytes.Length;
byte[] bytes = BitConverter.GetBytes(data.TiltX);
for (int i = 0; (i < bytes.Length); i++)
{
this._Buffer._Bytes[mytail++] = bytes[i];
mytail = mytail % this._Buffer._Bytes.Length;
}
bytes = BitConverter.GetBytes(data.TiltY);
for (int i = 0; (i < bytes.Length); i++)
{
this._Buffer._Bytes[mytail++] = bytes[i];
mytail = mytail % this._Buffer._Bytes.Length;
}
bytes = BitConverter.GetBytes(data.TiltZ);
for (int i = 0; (i < bytes.Length); i++)
{
this._Buffer._Bytes[mytail++] = bytes[i];
mytail = mytail % this._Buffer._Bytes.Length;
}
bytes = BitConverter.GetBytes(data.AngleX);
for (int i = 0; (i < bytes.Length); i++)
{
this._Buffer._Bytes[mytail++] = bytes[i];
mytail = mytail % this._Buffer._Bytes.Length;
}
bytes = BitConverter.GetBytes(data.AngleY);
for (int i = 0; (i < bytes.Length); i++)
{
this._Buffer._Bytes[mytail++] = bytes[i];
mytail = mytail % this._Buffer._Bytes.Length;
}
this._Buffer._Bytes[mytail++] = 0xff;
mytail = mytail % this._Buffer._Bytes.Length;
this._Buffer._Tail = mytail;
#endif
return(BUFFER_SIZE);
}
public void FromXML(string xml)
{
XmlDocument dom = new XmlDocument();
dom.LoadXml(xml);
XmlNode iNode = dom.DocumentElement;
int index = 0;
if (iNode.Name == ANNOTATION_ELEMENT)
{
//parsing category attributes
foreach (XmlAttribute iAttribute in iNode.Attributes)
{
if (iAttribute.Name == DATE_ATTRIBUTE)
{
//parse date
string p = @"(\d+)/(\d+)/(\d+)";
Match m = Regex.Match(iAttribute.Value, p);
if (m.Groups.Count == 4)
{
this.start_month = this.end_month = Convert.ToInt32(m.Groups[1].Value);
this.start_day = this.end_day = Convert.ToInt32(m.Groups[2].Value);
this.start_year = this.end_year = Convert.ToInt32(m.Groups[3].Value);
}
else
{
throw new Exception("Error parsing " + DATE_ATTRIBUTE + ". " + iAttribute.Value);
}
}
else if (iAttribute.Name == STARTTIME_ATTRIBUTE)
{
//parse date
string p = @"(\d+):(\d+):(\d+)([.](\d+))?";
Match m = Regex.Match(iAttribute.Value, p);
this.start_hour = Convert.ToInt32(m.Groups[1].Value);
this.start_minute = Convert.ToInt32(m.Groups[2].Value);
this.start_second = Convert.ToInt32(m.Groups[3].Value);
if (m.Groups[5].Value.Length > 0)
{
this.start_millisecond = Convert.ToInt32(m.Groups[5].Value);
}
}
else if (iAttribute.Name == ENDTIME_ATTRIBUTE)
{
//parse date
string p = @"(\d+):(\d+):(\d+)([.](\d+))?";
Match m = Regex.Match(iAttribute.Value, p);
this.end_hour = Convert.ToInt32(m.Groups[1].Value);
this.end_minute = Convert.ToInt32(m.Groups[2].Value);
this.end_second = Convert.ToInt32(m.Groups[3].Value);
this.end_millisecond = 0;
if (m.Groups[5].Value.Length > 0)
{
this.end_millisecond = Convert.ToInt32(m.Groups[5].Value);
}
}
else if (iAttribute.Name == STARTDATE_ATTRIBUTE)
{
//parse date
//2008-06-17 14:03:42-07:00
string p = @"^(\d+)-(\d+)-(\d+)\s+(\d+):(\d+):(\d+)";
Match m = Regex.Match(iAttribute.Value, p);
if (m.Groups.Count == 7)
{
this.start_date = m.Groups[0].Value.Substring(0, 10);
this.start_month = Convert.ToInt32(m.Groups[2].Value);
this.start_day = Convert.ToInt32(m.Groups[3].Value);
this.start_year = Convert.ToInt32(m.Groups[1].Value);
this.start_hour = Convert.ToInt32(m.Groups[4].Value);
this.start_minute = Convert.ToInt32(m.Groups[5].Value);
this.start_second = Convert.ToInt32(m.Groups[6].Value);
this.start_millisecond = 0;
}
else
{
throw new Exception("Error parsing " + STARTDATE_ATTRIBUTE + ". " + iAttribute.Value);
}
}
else if (iAttribute.Name == ENDDATE_ATTRIBUTE)
{
//parse date
string p = @"^(\d+)-(\d+)-(\d+)\s+(\d+):(\d+):(\d+)";
Match m = Regex.Match(iAttribute.Value, p);
if (m.Groups.Count == 7)
{
//added
this.end_date = m.Groups[0].Value.Substring(0, 10);
this.end_month = Convert.ToInt32(m.Groups[2].Value);
this.end_day = Convert.ToInt32(m.Groups[3].Value);
this.end_year = Convert.ToInt32(m.Groups[1].Value);
this.end_hour = Convert.ToInt32(m.Groups[4].Value);
this.end_minute = Convert.ToInt32(m.Groups[5].Value);
this.end_second = Convert.ToInt32(m.Groups[6].Value);
this.end_millisecond = 0;
}
else
{
throw new Exception("Error parsing " + ENDDATE_ATTRIBUTE + ". " + iAttribute.Value);
}
}
if ((this.start_unix < 0) && (this.start_year > 0) && (this.start_hour > 0) && (iAttribute.Name == STARTTIME_ATTRIBUTE))
{
DateTime dt = new DateTime(this.start_year, this.start_month, this.start_day, this.start_hour, this.start_minute, this.start_second, this.start_millisecond);
this.start_unix = WocketsTimer.GetUnixTime(dt);
}
if ((this.end_unix < 0) && (this.end_year > 0) && (this.end_hour > 0) && (iAttribute.Name == ENDTIME_ATTRIBUTE))
{
DateTime dt = new DateTime(this.end_year, this.end_month, this.end_day, this.end_hour, this.end_minute, this.end_second, this.end_millisecond);
this.end_unix = WocketsTimer.GetUnixTime(dt);
}
}
this.activities = new ActivityList();
foreach (XmlNode jNode in iNode.ChildNodes)
{
Activity activity = new Activity();
foreach (XmlAttribute jAttribute in jNode.Attributes)
{
if (jAttribute.Name == LABEL_ATTRIBUTE)
{
activity._Name = jAttribute.Value.Replace("A - ", "").Replace("A- ", "").Replace(' ', '-').Replace(',', '_').ToLower();
}
else if (jAttribute.Name == CATEGORY_ATTRIBUTE)
{
activity._Category = jAttribute.Value;
}
}
this.activities.Insert(index++, activity);
}
}
}
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
}
void InterfaceActivityTracker()
{
int k = 0;
int[] dataSavedSeconds = new int[this._Sensors.Count];
int[] secondsCounter = new int[this._Sensors.Count];
int[] full = new int[this._Sensors.Count];
int[] partial = new int[this._Sensors.Count];
int[] empty = new int[this._Sensors.Count];
if (!Directory.Exists(this._StorageDirectory + "\\log\\"))
{
Directory.CreateDirectory(this._StorageDirectory + "\\log\\");
}
if (!Directory.Exists(this._StorageDirectory + "\\data\\summary\\"))
{
Directory.CreateDirectory(this._StorageDirectory + "\\data\\summary\\");
}
for (int i = 0; (i < this._Sensors.Count); i++)
{
dataSavedSeconds[i] = 0;
secondsCounter[i] = 0;
full[i] = 0;
partial[i] = 0;
this._Sensors[i]._ReceivedACs = 0;
this._Sensors[i]._TotalReceivedACs = 0;
this._Sensors[i]._SavedACs = 0;
this._Sensors[i]._TotalSavedACs = 0;
Core.WRITE_FULL_RECEIVED_COUNT(i, 0);
Core.WRITE_PARTIAL_RECEIVED_COUNT(i, 0);
Core.WRITE_EMPTY_RECEIVED_COUNT(i, 0);
Core.WRITE_RECEIVED_ACs(i, -1);
Core.WRITE_SAVED_ACs(i, -1);
}
while (true)
{
if (connecting)
{
SystemIdleTimerReset();
if ((this != null) && (this._Sensors.Count > 0))
{
//Check 2 things, num of connection failures
// check if data received is > 0
// if num connections >2, ready to pause
// if data received is >0, ready to pause within 2 seconds.
bool receiveFailed = true;
bool receivedFullData = true;
bool notimeoutData = true;
for (int i = 0; (i < this._Sensors.Count); i++)
{
receivedFullData &= (this._Sensors[i]._ReceivedPackets == ((WocketsDecoder)this._Decoders[i])._ExpectedBatchCount);
//halt, if either 1 successful connection was made
// or any data was received
// or 3 or more reconnections were made
if ((((RFCOMMReceiver)this._Receivers[i])._Reconnections < 3))
{
receiveFailed = false;
}
//if (((RFCOMMReceiver)this._Receivers[i])._SuccessfulConnections >= 1)
secondsCounter[i] = secondsCounter[i] + 1;
notimeoutData &= (secondsCounter[i] > 20);
}
if ((receivedFullData) || (receiveFailed) || (notimeoutData))
{
// if didnt get full data, sleep for 2 seconds
if (!receivedFullData)
{
Thread.Sleep(3000);
}
//save whatever we have so far then sleep
connecting = false;
SYSTEM_POWER_STATUS_EX2 bpower = Battery.GetSystemPowerStatus();
DateTime now = DateTime.Now;
double unixtime = WocketsTimer.GetUnixTime(now);
string currentTime = now.ToString("yyyy-MM-dd HH:mm:ss");
string log_line = ++k + "," + currentTime + "," + bpower.BatteryLifePercent + "," + bpower.BatteryVoltage + "," + bpower.BatteryCurrent + "," + bpower.BatteryTemperature;
string hourlyPath = now.ToString("yyyy-MM-dd") + "\\" + now.Hour;
DiskSpace space1 = Memory.GetDiskSpace(this._StorageDirectory);
DiskSpace space2 = Memory.GetDiskSpace("/");
int remainingStorage1 = (int)(space1.TotalNumberOfBytes / System.Math.Pow(2, 20));
int remainingStorage2 = (int)(space2.TotalNumberOfBytes / System.Math.Pow(2, 20));
string upload_log = currentTime + "," + bpower.BatteryLifePercent + "," + remainingStorage2 + "," + remainingStorage1;
for (int i = 0; (i < this._Sensors.Count); i++)
{
if ((this._Sensors[i]._ReceivedPackets > 4000) || (((WocketsDecoder)this._Decoders[i])._ExpectedBatchCount > 4000))
{
upload_log += "," + this._Sensors[i]._BatteryLevel + "," + 0 + "," + 0;
}
else
{
upload_log += "," + this._Sensors[i]._BatteryLevel + "," + ((WocketsDecoder)this._Decoders[i])._ExpectedBatchCount + "," + this._Sensors[i]._ReceivedPackets;
}
if (this._Sensors[i]._ReceivedPackets == ((WocketsDecoder)this._Decoders[i])._ExpectedBatchCount)
{
full[i] = full[i] + 1;
}
else if (this._Sensors[i]._ReceivedPackets == 0)
{
empty[i] = empty[i] + 1;
}
else
{
partial[i] = partial[i] + 1;
}
Core.WRITE_FULL_RECEIVED_COUNT(i, full[i]);
Core.WRITE_PARTIAL_RECEIVED_COUNT(i, partial[i]);
Core.WRITE_EMPTY_RECEIVED_COUNT(i, empty[i]);
Core.WRITE_RECEIVED_ACs(i, ((WocketsDecoder)this._Decoders[i])._ACIndex);
Core.WRITE_RECEIVED_COUNT(i, this._Sensors[i]._ReceivedPackets);
log_line += "," + this._Sensors[i]._ReceivedPackets + "," + ((WocketsDecoder)this._Decoders[i])._ExpectedBatchCount + "," + ((RFCOMMReceiver)this._Receivers[i])._SuccessfulConnections + "," + ((RFCOMMReceiver)this._Receivers[i])._Reconnections + "," + ((RFCOMMReceiver)this._Receivers[i])._ConnectionTime;
dataSavedSeconds[i] = 0;
countSeconds[i] = false;
secondsCounter[i] = 0;
((WocketsDecoder)this._Decoders[i])._ExpectedBatchCount = -1;
if (!Directory.Exists(this._StorageDirectory + "\\data\\summary\\" + hourlyPath))
{
Directory.CreateDirectory(this._StorageDirectory + "\\data\\summary\\" + hourlyPath);
}
TextWriter tw2 = new StreamWriter(this._StorageDirectory + "\\data\\summary\\" + hourlyPath + "\\SummaryAC-" + this._Sensors[i]._Location + "-" + i + ".csv", true);
int nextACIndex = LastACIndex[i] + 1;
if (nextACIndex == 960)
{
nextACIndex = 0;
}
int countsaved = 0;
for (int j = nextACIndex; (j != ((WocketsDecoder)this._Decoders[i])._ActivityCountIndex);)
{
DateTime ac_dt = new DateTime();
WocketsTimer.GetDateTime((long)((WocketsDecoder)this._Decoders[i])._ActivityCounts[j]._TimeStamp, out ac_dt);
string ac_currentTime = ac_dt.ToString("yyyy-MM-dd HH:mm:ss");
tw2.WriteLine(DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss") + "," + j + "," + ((WocketsDecoder)this._Decoders[i])._ActivityCounts[j]._SeqNum + "," + ac_currentTime + "," + ((WocketsDecoder)this._Decoders[i])._ActivityCounts[j]._TimeStamp + "," + ((WocketsDecoder)this._Decoders[i])._ActivityCounts[j]._Count);
SynchronizedLogger.Write(i + "," + ac_currentTime + "," + ((RFCOMMReceiver)this._Receivers[i])._Address + "," + ((WocketsDecoder)this._Decoders[i])._ActivityCounts[j]._Count);
LastSeqNum[i] = ((WocketsDecoder)this._Decoders[i])._ActivityCounts[j]._SeqNum;
LastACIndex[i] = j;
countsaved++;
j++;
if (j == 960)
{
j = 0;
}
}
if (countsaved > 10)
{
Core.WRITE_SAVED_ACs(i, 10);
}
else
{
Core.WRITE_SAVED_ACs(i, countsaved);
}
tw2.Close();
}
this._Polling = false;
//shutting down BT here causes a strange delay on wakeup.
int btshutdown_counter = 0;
while (true)
{
btshutdown_counter++;
try
{
if ((Wockets.Utils.network.NetworkStacks._BluetoothStack.Dispose()) || (btshutdown_counter > 5))
{
break;
}
}
catch
{
}
SystemIdleTimerReset();
Thread.Sleep(1000);
}
SynchronizedLogger.Write(upload_log);
//TextWriter tw = new StreamWriter(this._StorageDirectory + "\\data\\log\\" + hourlyPath + "\\stats.csv", true);
//tw.Close();
Logger.Log(log_line);
SystemIdleTimerReset();
for (int i = 0; (i < this._Sensors.Count); i++)
{
try
{
this._Sensors[i].Save();
}
catch (Exception e)
{
Logger.Error("Sensor " + i + ": failed to save:" + e.Message);
}
}
Thread.Sleep(1000);
if ((bpower.BatteryCurrent < 0) && (DateTime.Now.Subtract(lastActivity).TotalSeconds > 30))
{
SetSystemPowerState(null, POWER_STATE_SUSPEND, POWER_FORCE);
}
}
}
}
Thread.Sleep(1000);
}
}
//exception thrown only when there is an overflow
public override int Decode(int sensorID, byte[] data, int length)
{
int rawDataIndex = 0;
int decodedDataIndex = 0;
if (length != 0) // Have some data
{
while (rawDataIndex < length)
{
v = data[rawDataIndex] & 0xff;
// First check if got a reset and start of packet
if ((this.packetPosition == NO_HEADER_SEEN) && (v == PACKET_MARKER))
{
this.packetPosition = FIRST_HEADER_SEEN;
}
// Any two markers in a row is *always* a packet. This will occasionally cause an error.
else if ((v == PACKET_MARKER) && (lV == PACKET_MARKER))
{
this.packetPosition = 0;
}
else if (this.packetPosition >= 0) // Must have seen header
{
this.packet[this.packetPosition] = data[rawDataIndex];
this.packetPosition++;
if (this.packetPosition == MITesAccelerationData.NUM_RAW_BYTES)
{
if (decodedDataIndex >= this._Data.Length)
{
throw new Exception("MITesDecoder buffer too small " + this._Data.Length);
}
// completely read a whole packet
MITesAccelerationData datum = ((MITesAccelerationData)this._Data[decodedDataIndex]);
//Reset the data element
datum.Reset();
//copy raw bytes
for (int i = 0; (i < MITesAccelerationData.NUM_RAW_BYTES); i++)
{
datum.RawBytes[i] = this.packet[i];
}
//Decode the MITes axes data
datum._Type = SensorDataType.UNCOMPRESSED_DATA_PDU;
datum._SensorID = (byte)sensorID;
datum._X = (short)(this.packet[1] | ((this.packet[4] & 0xC0) << 2));
datum._Y = (short)(this.packet[2] | ((this.packet[4] & 0x30) << 4));
datum._Z = (short)(this.packet[3] | ((this.packet[4] & 0x0C) << 6));
//Set time stamps
datum.UnixTimeStamp = WocketsTimer.GetUnixTime();
//if (IsValid(datum))
decodedDataIndex++;
//Reset for a new packet
this.packetPosition = 0;
}
}
else
{
if (v != PACKET_MARKER)
{
packetPosition = NO_HEADER_SEEN;
}
else
{
packetPosition = FIRST_HEADER_SEEN;
}
}
lV = v;
rawDataIndex++;
}
}
this._Size = decodedDataIndex;
return(decodedDataIndex);
}
/*
* public override bool IsValid(SensorData data)
* {
* //Check for valid HTC Channels
* if (data.Channel != HTCDIAMOND_CHANNEL)
* return false;
* return true;
* }
*/
public override int Decode(int sourceSensor, CircularBuffer data, int start, int end)
{
int rawDataIndex = start;
int numDecodedPackets = 0;
//count number of packets to decode
int dataLength = end - start; //((RFCOMMReceiver)currentReceiver).bluetoothStream._Tail - currentReceiver._Head;
if (dataLength < 0)
{
dataLength = data._Bytes.Length - start + end;//((RFCOMMReceiver)currentReceiver).bluetoothStream._Buffer.Length - currentReceiver._Head + ((RFCOMMReceiver)currentReceiver).bluetoothStream._Tail;
}
int maxDecodedPackets = dataLength / HTCDiamondTouchAccelerationData.NUM_RAW_BYTES;
while ((maxDecodedPackets > 0) && (rawDataIndex != end))
{
//lock (data._Bytes)
//{
int firstByte = rawDataIndex;
int lastByte = (rawDataIndex + HTCDiamondTouchAccelerationData.NUM_RAW_BYTES - 1) % data._Bytes.Length;
if ((data._Bytes[firstByte] == 0xff) && ((data._Bytes[lastByte] == 0xff)))
{
HTCDiamondTouchAccelerationData datum = ((HTCDiamondTouchAccelerationData)this._Data[this.head]);
datum.Reset();
//copy raw bytes
for (int i = 0; (i < HTCDiamondTouchAccelerationData.NUM_RAW_BYTES); i++)
{
datum.RawBytes[i] = data._Bytes[rawDataIndex];
rawDataIndex = (rawDataIndex + 1) % data._Bytes.Length;
}
datum._Type = SensorDataType.UNCOMPRESSED_DATA_PDU;
datum._SensorID = (byte)sourceSensor;
datum._X = (short)(BitConverter.ToInt16(datum.RawBytes, 1) + 1024);
datum._Y = (short)(BitConverter.ToInt16(datum.RawBytes, 3) + 1024);
datum._Z = (short)(BitConverter.ToInt16(datum.RawBytes, 5) + 1024);
datum.UnixTimeStamp = WocketsTimer.GetUnixTime();
if (this.head >= (BUFFER_SIZE - 1))
{
this.head = 0;
}
else
{
this.head++;
}
numDecodedPackets++;
data._Head = (data._Head + HTCDiamondTouchAccelerationData.NUM_RAW_BYTES) % data._Bytes.Length;
if (numDecodedPackets == maxDecodedPackets)
{
break;
}
}
else
{
break;
}
//}
}
return(numDecodedPackets);
}
public override void Load(ByteReader br)
{
#region Read Timestamp
if (!(br.ReadByte(b)))
{
throw new Exception("Error: reading first byte in PLFormat file");
}
//read a complete timestamp
if (b[0] == ((int)255))
{
if (!(br.ReadBytes(b6)))
{
throw new Exception("Error: reading full timestamp in PLFormat file");
}
lastUnixTime = WocketsTimer.DecodeUnixTimeCodeBytesFixed(b6);
}
else //read a differential timestamp
{
lastUnixTime += (int)b[0];
}
#endregion Read Timestamp
DateTime dt = new DateTime();
WocketsTimer.GetDateTime((long)lastUnixTime, out dt);
if (!br.ReadByte(header))
{
throw new Exception("Error: reading data in PLFormat file");
}
pdu[0] = header[0];
if (!(br.ReadBytes(pdu, 1, 16)))
{
throw new Exception("Error: reading data in PLFormat file");
}
int lastDecodedIndex = 0;
//Successfully decoded a packet
if (this.Decode(this._ID, pdu, 16) == 1)
{
if (this._Head == 0)
{
lastDecodedIndex = this._Data.Length - 1;
}
else
{
lastDecodedIndex = this._Head - 1;
}
this._Data[lastDecodedIndex].UnixTimeStamp = lastUnixTime;
return;
}
else
{
throw new Exception("Failed to decode data");
}
}
public override int Decode(int sourceSensor, byte[] data, int length)
{
int rawDataIndex = 0;
int decodedDataIndex = 0;
if (length != 0) // Have some data
{
while (rawDataIndex < length)
{
if ((data[rawDataIndex]) == '#') //grab the next 6 bytes
{
this.packetPosition = 0;
this.headerSeen = true;
}
if ((this.headerSeen == true) && (this.packetPosition < SparkfunAccelerationData.NUM_RAW_BYTES))
{
this.packet[this.packetPosition] = data[rawDataIndex];
}
this.packetPosition++;
rawDataIndex++;
if ((this.packetPosition == SparkfunAccelerationData.NUM_RAW_BYTES) && (packet[SparkfunAccelerationData.NUM_RAW_BYTES - 1] == '$')) //a full packet was received
{
//DecodeSparkfunFrame(someData[dataIndex], packet3);
if (decodedDataIndex >= this._Data.Length)
{
throw new Exception("SparkfunDecoder buffer too small " + this._Data.Length);
}
// completely read a whole packet
SparkfunAccelerationData datum = ((SparkfunAccelerationData)this._Data[decodedDataIndex]);
//Reset the data element
datum.Reset();
//copy raw bytes
for (int i = 0; (i < SparkfunAccelerationData.NUM_RAW_BYTES); i++)
{
datum.RawBytes[i] = this.packet[i];
}
//decode sparkfun packet
datum._Type = SensorDataType.UNCOMPRESSED_DATA_PDU;
datum._SensorID = (byte)sourceSensor;
datum._X = (short)((((short)this.packet[4]) << 8) | ((short)this.packet[5]));
datum._Y = (short)((((short)this.packet[6]) << 8) | ((short)this.packet[7]));
datum._Z = (short)((((short)this.packet[8]) << 8) | ((short)this.packet[9]));
//Set time stamps
datum.UnixTimeStamp = WocketsTimer.GetUnixTime();
//if (IsValid(datum))
decodedDataIndex++;
this.packetPosition = 0;
this.headerSeen = false;
}
}
}
this._Size = decodedDataIndex;
return(decodedDataIndex);
}
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);
}
