public RMPMonitor2Msg(BinaryReader br)
{
pitchAngle = Endian.BigToLittle(br.ReadInt16());
pitchRate = Endian.BigToLittle(br.ReadInt16());
rollAngle = Endian.BigToLittle(br.ReadInt16());
rollRate = Endian.BigToLittle(br.ReadInt16());
}
public RMPMonitor7Msg(BinaryReader br)
{
operationalMode = Endian.BigToLittle(br.ReadInt16());
gainSchedule = Endian.BigToLittle(br.ReadInt16());
uiBatteryVoltage = Endian.BigToLittle(br.ReadInt16());
baseBatteryVoltage = Endian.BigToLittle(br.ReadInt16());
}
public RMPMonitor3Msg(BinaryReader br)
{
leftWheelVel = Endian.BigToLittle(br.ReadInt16());
rightWheelVel = Endian.BigToLittle(br.ReadInt16());
yawRate = Endian.BigToLittle(br.ReadInt16());
servoFrames = Endian.BigToLittle(br.ReadUInt16());
}
public RMPHeartbeatMsg(BinaryReader br)
{
br.ReadByte();
canChannel = br.ReadByte();
batteryStatus = Endian.BigToLittle(br.ReadInt16());
batteryVoltage = Endian.BigToLittle(br.ReadInt16());
}
private void ReceiveCallback(IAsyncResult ar)
{
if (this.sock == null)
{
return;
}
int bytesReceived = this.sock.EndReceive(ar);
if (bytesReceived > 0)
{
MemoryStream ms = new MemoryStream(buf, 0, bytesReceived, false);
BinaryReader br = new BinaryReader(ms);
byte septID = br.ReadByte();
UInt16 secs = Endian.BigToLittle(br.ReadUInt16());
UInt32 ticks = Endian.BigToLittle(br.ReadUInt32());
double ts = secs + ticks / 10000.0;
br.ReadByte(); br.ReadByte();
UInt16 CRC = br.ReadUInt16();
UInt16 temp = br.ReadUInt16();
UInt16 curMsgID = (UInt16)(temp & 0x1FFF);
Byte curMsgIDRev = (Byte)(temp >> 13);
curMsgLen = br.ReadUInt16();
br.BaseStream.Position = 7;
byte[] msg = br.ReadBytes(curMsgLen);
//if (septID == ID)
ProcessPacket(septID, curMsgID, 0, msg, ts);
}
this.sock.BeginReceive(this.buf, 0, this.buf.Length, SocketFlags.None, ReceiveCallback, null);
}
//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);
}
}
private void ParsePacket(BinaryReader br, int bytesReceived)
{
UInt16 secs = Endian.BigToLittle(br.ReadUInt16());
UInt32 ticks = Endian.BigToLittle(br.ReadUInt32());
double ts = secs + ticks / 10000.0;
byte packetid = br.ReadByte();
UInt16 len = Endian.BigToLittle(br.ReadUInt16());
switch (packetid)
{
case 0xFE:
ProcessTSIPPacket(br, bytesReceived - 9, ts);
break;
case 0xFD:
ProcessTSIPErrorPacket(br, bytesReceived - 9, ts);
break;
}
}
private void ProcessPacket(ushort id, byte idRev, byte[] msg, double ts)
{
Console.WriteLine("Got: " + id + " or " + Endian.BigToLittle(id) + " " + i++);
switch (id)
{
case 4007:
ParsePVTGeodetic(idRev, msg, ts); break;
case 5906:
ParsePosCovGeodetic(idRev, msg, ts); break;
case 5908:
ParseVelCovGeodetic(idRev, msg, ts); break;
case 4001:
ParseDOP(idRev, msg, ts); break;
case 5921:
ParseEndOfPVT(idRev, msg, ts); break;
}
}
public RMPMonitor8Msg(BinaryReader br)
{
velocityCommand = Endian.BigToLittle(br.ReadInt16());
turnCommand = Endian.BigToLittle(br.ReadInt16());
}
public RMPMonitor6Msg(BinaryReader br)
{
leftMotorTorque = Endian.BigToLittle(br.ReadInt16());
rightMotorTorque = Endian.BigToLittle(br.ReadInt16());
}
private void ParseSegwayPacket(BinaryReader br)
{
//figure out what it is
UInt16 secs = Endian.BigToLittle(br.ReadUInt16());
UInt32 ticks = Endian.BigToLittle(br.ReadUInt32());
double ts = secs + ticks / 10000.0;
byte id = br.ReadByte();
if (id != 0xFF)
{
return;
}
UInt16 len = Endian.BigToLittle(br.ReadUInt16());
ushort msgID = Endian.BigToLittle(br.ReadUInt16());
switch (msgID)
{
case RMPHeartbeatMsg.id: HandleHeartbeat(br, ts);
break;
case RMPMonitor1Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 1));
}
HandleMonitor1(br, ts);
break;
case RMPMonitor2Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 2));
}
HandleMonitor2(br, ts);
break;
case RMPMonitor3Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 3));
}
HandleMonitor3(br, ts);
break;
case RMPMonitor4Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 4));
}
HandleMonitor4(br, ts);
break;
case RMPMonitor5Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 5));
}
HandleMonitor5(br, ts);
break;
case RMPMonitor6Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 6));
}
HandleMonitor6(br, ts);
break;
case RMPMonitor7Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 7));
}
HandleMonitor7(br, ts);
break;
case RMPMonitor8Msg.id: if (Msg1 != null)
{
Msg1(this, new TimestampedEventArgs <int>(ts, 8));
}
HandleMonitor8(br, ts);
break;
case RMPControlMsg.id: HandleControl(br);
break;
case RMPShutdownMsg.id: HandleShutdown(br);
break;
case RMPStatusMsg.id: HandleStatus(br);
break;
default:
Console.WriteLine("Unknown msg id: " + msgID); break;
}
}
