// add item
public void addMessage(IMUMessage msg)
{
// locking queue acess vi the sepamphore
QueueAccessLock.WaitOne();
Messages.Add(msg);
if (Messages.Count() > MaxLength)
Messages.RemoveAt(0);
// releasing access
QueueAccessLock.Release();
}
// add item
public void addMessage(IMUMessage msg)
{
// locking queue acess vi the sepamphore
QueueAccessLock.WaitOne();
Messages.Add(msg);
if (Messages.Count() > MaxLength)
{
Messages.RemoveAt(0);
}
// releasing access
QueueAccessLock.Release();
}
// remove an item
public IMUMessage removeMessage()
{
IMUMessage msg = null;
// locking queue
QueueAccessLock.WaitOne();
if (!isEmpty())
{
msg = Messages[0];
Messages.Remove(msg);
}
// releasing queue
QueueAccessLock.Release();
return(msg);
}
// "unwrap" a raw message
public static IMUMessage unwrapRawMessage(byte[] rawMsg)
{
// creating a new IMU Message object
IMUMessage msg = new IMUMessage();
// 1. Header
msg.Header = (ushort)((ushort)(rawMsg[0] << 8) + rawMsg[1]);
msg.Count = (ushort)((ushort)(rawMsg[2] << 8) + rawMsg[3]);
// 2. axis 1 rate
msg.Axis1_Rate = getFloatFromBytes(rawMsg[4], rawMsg[5], rawMsg[6], rawMsg[7]);
// 3. axis 1 quad
msg.Axis1_Quad = getFloatFromBytes(rawMsg[8], rawMsg[9], rawMsg[10], rawMsg[11]);
// 4. axis 1 frequency
msg.Axis1_Frequency = getFloatFromBytes(rawMsg[12], rawMsg[13], rawMsg[14], rawMsg[15]);
// 5. axis 1 accelerometer
msg.Axis1_Accelerometer = getFloatFromBytes(rawMsg[16], rawMsg[17], rawMsg[18], rawMsg[19]);
// 6. axis 1 accelerometer
msg.Axis1_AccelerometerTemp = getFloatFromBytes(rawMsg[20], rawMsg[21], rawMsg[22], rawMsg[23]);
// 7. axis 2 rate
msg.Axis2_Rate = getFloatFromBytes(rawMsg[24], rawMsg[25], rawMsg[26], rawMsg[27]);
// 8. axis 2 quad
msg.Axis2_Quad = getFloatFromBytes(rawMsg[28], rawMsg[29], rawMsg[30], rawMsg[31]);
// 9. axis 2 frequency
msg.Axis2_Frequency = getFloatFromBytes(rawMsg[32], rawMsg[33], rawMsg[34], rawMsg[35]);
// 10. axis 2 accelerometer
msg.Axis2_Accelerometer = getFloatFromBytes(rawMsg[36], rawMsg[37], rawMsg[38], rawMsg[39]);
// 11. axis 2 accelerometer temperature
msg.Axis2_AccelerometerTemp = getFloatFromBytes(rawMsg[40], rawMsg[41], rawMsg[42], rawMsg[43]);
// 12. axis 3 rate
msg.Axis3_Rate = getFloatFromBytes(rawMsg[44], rawMsg[45], rawMsg[46], rawMsg[47]);
// 13. axis 3 quad
msg.Axis3_Quad = getFloatFromBytes(rawMsg[48], rawMsg[49], rawMsg[50], rawMsg[51]);
// 14. axis 3 frequency
msg.Axis3_Frequency = getFloatFromBytes(rawMsg[52], rawMsg[53], rawMsg[54], rawMsg[55]);
// 15. axis 3 accelrometer
msg.Axis3_Accelerometer = getFloatFromBytes(rawMsg[56], rawMsg[57], rawMsg[58], rawMsg[59]);
// 16. axis 3 accelrometer temperature
msg.Axis3_AccelerometerTemp = getFloatFromBytes(rawMsg[60], rawMsg[61], rawMsg[62], rawMsg[63]);
// 17. Built In Mode and Test Results
msg.Status = new IMUStatusFlags((ushort)((rawMsg[64] << 8) + rawMsg[65]));
// 18. The checksum
msg.Checksum = (ushort)((rawMsg[66] << 8) + rawMsg[67]);
return(msg);
}
// The function that assembles incoming serial data and puts in a queue
private void processData()
{
int i;
// retrieving the number of available bytes in the serial port buffer
int numBytesToRead = CommPort.BytesToRead;
if (numBytesToRead > 0)
//do
{
if (Data_RemainingBytes == 0)
{ // i.e., the previous message is complete (or we are commencing reception now)
if (numBytesToRead >= 2)
{ // we have -at least-the message header in the buffer.
// Her's the CORRECT ORDER (i.e., the oldest byte is always read first from the buffer):
byte Header_High = (byte)CommPort.ReadByte();
byte Header_Low = (byte)CommPort.ReadByte();
// allocating space for the incoming message in raw form (i.e., byte array)
Data_Rawmessage = new byte[IMUMessage.c_MessageLength];
ushort head = (ushort)(Header_High * 256 + Header_Low);
if (head == IMUMessage.c_Header) // the buffer contains gibberish. Empty it.
//CommPort.DiscardInBuffer();
//else
{ // the two bytes were equal to a header and therefore the remaining bytes for this message should be 34 - 2 = 32
Data_RemainingBytes = IMUMessage.c_MessageLength - 2; // the size of the message MINUS the 2 bytes of the header
Data_Rawmessage[0] = Header_High; // storing in a MSB-comes-first-order
Data_Rawmessage[1] = Header_Low;
}
}
}
else if (numBytesToRead >= Data_RemainingBytes)
{ // the buffer contains the rest of the message for sure...
// reading the rest of the message into the allocated raw message buffer
CommPort.Read(Data_Rawmessage, 2, Data_RemainingBytes);
//for (i = 2; i < IMUMessage.c_MessageLength; i++)
// Data_Rawmessage[i] = (byte)CommPort.ReadByte();
// resetting the remaining bytes to 0
Data_RemainingBytes = 0;
// unwrapping the message (hopefully correctly...)
IMUMessage msg = IMUMessage.unwrapRawMessage(Data_Rawmessage);
// ************* checking for cheksum errors ********************
ushort estimatedChecksum = 0;
UInt32 sum = 0;
for (i = 0; i < IMUMessage.c_MessageLength / 2 - 1; i++)
{
ushort u_word = (ushort)((ushort)(Data_Rawmessage[i * 2] << 8) + (ushort)Data_Rawmessage[2 * i + 1]);
sum += u_word;
}
int temp = (int)(0 - (sum & 0xffff));
estimatedChecksum = (ushort)temp;
// ****************************************************************
// adding the message to the queue
//MsgQueueSemaphore.WaitOne(); // blocking the queue from external thread access
//MessageQueue.addMessage(msg);
//MsgQueueSemaphore.Release(); // releasing the structure for access
// and updating the last known reading
Last_Acc1 = msg.Axis1_Accelerometer; Last_Acc2 = msg.Axis2_Accelerometer; Last_Acc3 = msg.Axis3_Accelerometer;
Last_Angvel1 = msg.Axis1_Rate; Last_Angvel2 = msg.Axis2_Rate; Last_Angvel3 = msg.Axis3_Rate;
// for demonstration
/*Console.Write("Estimated checksum : ");
* Console.Write(estimatedChecksum);
* Console.Write(" and actual checksum : ");
* Console.WriteLine( msg.Checksum);
*
* Console.Write("Axis 1 rate :");
* Console.WriteLine(msg.Axis1_Rate);
*
*
* Console.Write("Axis 1 Temperature :");
* Console.WriteLine(msg.Axis1_AccelerometerTemp);
*
* Console.Write("Axis 1 Frequency :");
* Console.WriteLine(msg.Axis2_Frequency);
*
* Console.Write("Axis 2 Temperature :");
* Console.WriteLine(msg.Axis2_AccelerometerTemp);
*
* Console.Write("Axis 1 Quad:");
* Console.WriteLine(msg.Axis1_Quad);
*
* Console.Write("Axis 1 Acceleration: ");
* Console.WriteLine(msg.Axis1_Accelerometer);
*
* Console.Write("Axis 2 Acceleration: ");
* Console.WriteLine(msg.Axis2_Accelerometer);
*
* Console.Write("Axis 3 Acceleration: ");
* Console.WriteLine(msg.Axis3_Accelerometer);
*/
}
// updating the number of bytes left in the buffer
//numBytesToRead = CommPort.BytesToRead;
} //while ((numBytesToRead >= Data_RemainingBytes) && (Data_RemainingBytes > 0));
}
// "unwrap" a raw message
public static IMUMessage unwrapRawMessage(byte[] rawMsg)
{
// creating a new IMU Message object
IMUMessage msg = new IMUMessage();
// 1. Header
msg.Header = (ushort)((ushort)(rawMsg[0] << 8) + rawMsg[1]);
msg.Count = (ushort)((ushort)(rawMsg[2] << 8) + rawMsg[3]);
// 2. axis 1 rate
msg.Axis1_Rate = getFloatFromBytes(rawMsg[4], rawMsg[5], rawMsg[6], rawMsg[7]);
// 3. axis 1 quad
msg.Axis1_Quad = getFloatFromBytes(rawMsg[8], rawMsg[9], rawMsg[10], rawMsg[11]);
// 4. axis 1 frequency
msg.Axis1_Frequency = getFloatFromBytes(rawMsg[12], rawMsg[13], rawMsg[14], rawMsg[15]);
// 5. axis 1 accelerometer
msg.Axis1_Accelerometer = getFloatFromBytes(rawMsg[16], rawMsg[17], rawMsg[18], rawMsg[19]);
// 6. axis 1 accelerometer
msg.Axis1_AccelerometerTemp = getFloatFromBytes(rawMsg[20], rawMsg[21], rawMsg[22], rawMsg[23]);
// 7. axis 2 rate
msg.Axis2_Rate = getFloatFromBytes(rawMsg[24], rawMsg[25], rawMsg[26], rawMsg[27]);
// 8. axis 2 quad
msg.Axis2_Quad = getFloatFromBytes(rawMsg[28], rawMsg[29], rawMsg[30], rawMsg[31]);
// 9. axis 2 frequency
msg.Axis2_Frequency = getFloatFromBytes(rawMsg[32], rawMsg[33], rawMsg[34], rawMsg[35]);
// 10. axis 2 accelerometer
msg.Axis2_Accelerometer = getFloatFromBytes(rawMsg[36], rawMsg[37], rawMsg[38], rawMsg[39]);
// 11. axis 2 accelerometer temperature
msg.Axis2_AccelerometerTemp = getFloatFromBytes(rawMsg[40], rawMsg[41], rawMsg[42], rawMsg[43]);
// 12. axis 3 rate
msg.Axis3_Rate = getFloatFromBytes(rawMsg[44], rawMsg[45], rawMsg[46], rawMsg[47]);
// 13. axis 3 quad
msg.Axis3_Quad = getFloatFromBytes(rawMsg[48], rawMsg[49], rawMsg[50], rawMsg[51]);
// 14. axis 3 frequency
msg.Axis3_Frequency = getFloatFromBytes(rawMsg[52], rawMsg[53], rawMsg[54], rawMsg[55]);
// 15. axis 3 accelrometer
msg.Axis3_Accelerometer = getFloatFromBytes(rawMsg[56], rawMsg[57], rawMsg[58], rawMsg[59]);
// 16. axis 3 accelrometer temperature
msg.Axis3_AccelerometerTemp = getFloatFromBytes(rawMsg[60], rawMsg[61], rawMsg[62], rawMsg[63]);
// 17. Built In Mode and Test Results
msg.Status = new IMUStatusFlags((ushort)((rawMsg[64] << 8) + rawMsg[65]));
// 18. The checksum
msg.Checksum = (ushort)((rawMsg[66] << 8) + rawMsg[67]);
return msg;
}