//Acceleration and Angular Rate (0xC2)
void ProcessIMUc2(BinaryReader br, double ts)
{
IMUData imuData = new IMUData();
addsum(br);
float accelX = BitConverter.ToSingle(temp, 0);
addsum(br);
float accelY = BitConverter.ToSingle(temp, 0);
addsum(br);
float accelZ = BitConverter.ToSingle(temp, 0);
addsum(br);
float angRateX = BitConverter.ToSingle(temp, 0);
addsum(br);
float angRateY = BitConverter.ToSingle(temp, 0);
addsum(br);
float angRateZ = BitConverter.ToSingle(temp, 0);
addsum(br);
//float timer = BitConverter.ToSingle(temp, 0); //time since system power-up
uint timer = BitConverter.ToUInt32(temp, 0); //time since system power-up
br.ReadBytes(2);
UInt16 checksum = Endian.BigToLittle(br.ReadUInt16());
if (sum == checksum)
{
imuData.xAccel = accelX;
imuData.yAccel = accelY;
imuData.zAccel = accelZ;
imuData.xRate = angRateX;
imuData.yRate = angRateY;
imuData.zRate = angRateZ;
imuData.DataType = "IMU";
imuData.timer = timer;
}
else //if (showDebugMessages)
{
Console.Write("IMU Msg: @" + ts.ToString("F4") + ":::didn't pass checksum.");
Console.WriteLine();
}
sum = 0;
if (IMUUpdate != null) IMUUpdate(this, new TimestampedEventArgs<IMUData>(ts, imuData));
if (showDebugMessages) Console.WriteLine("Accel: ( " + accelX.ToString("F8") + ", " + accelY.ToString("F8") + ", " + accelZ.ToString("F8") + " )" + "AngRate: ( " + angRateX.ToString("F8") + ", " + angRateY.ToString("F8") + ", " + angRateZ.ToString("F8") + " ) Timer: " + timer);
}
public IMUDataMessage(int robotID, IMUData data)
{
this.robotID = robotID; this.data = data;
}