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; } }