// initiate transmission of a packet
public void sendPacket(EZPacket pack)
{
byte receiver_id = pack.ReceiverID;
CurrentPacket = packet2Bytes(pack);
// retrieving length of raw packet data
CurrentPacketLength = getRawPacketLength(CurrentPacket);
// computing number of frames required
TotalFrames = CurrentPacketLength / MAX_FRAME_PAYLOAD_LENGTH + 1;
// zeroing number of frames sent
FramesSent = 0;
// clearing timeout flag
PacketSendTimeout = false;
// marking the starting time for possible timeouts
TransmissionStartTime = DateTime.Now;
// setting Packet Receiver global
PacketReceiverID = receiver_id;
// setting state to sending...
state = stSending;
// now sending first frame
sendNextFrame();
}
// pushes a new packet into the outbound queue
public Boolean pushOutboundPacket(EZPacket pack)
{
Boolean success = false;
if (!OutboundQueueFull())
{
OutboundPacketsNum++;
OutboundPackets[OutboundPacketsNum - 1] = pack;
success = true;
}
else
{
success = false;
}
return(success);
}
// wraps a packet given a raw message (payload) for the avatar robot
private EZPacket createEZPacket(byte[] payload, int payloadsize)
{
EZPacket pack = new EZPacket();
pack.ReceiverID = AvatarNodeID;
pack.SenderID = NetDevice.NodeID;
pack.SenderNodeType = NetDevice.DevNodeType;
pack.MessageLength = (ushort)payloadsize;
int i;
pack.Message = new byte[pack.MessageLength];
for (i = 0; i < payloadsize; i++)
{
pack.Message[i] = payload[i];
}
return(pack);
}
// returns an euivalent array of bytes to a packet
byte[] packet2Bytes(EZPacket pack)
{
int packlen = 5 + pack.MessageLength; // 5 bytes in the header plus the message length
byte[] rawmsg = new byte[packlen];
rawmsg[0] = pack.SenderID;
rawmsg[1] = pack.ReceiverID;
rawmsg[2] = pack.SenderNodeType;
rawmsg[3] = (byte)(pack.MessageLength & 0x0FF);
rawmsg[4] = (byte)((pack.MessageLength >> 8) & 0x0FF);
int i;
for (i = 0; i < pack.MessageLength; i++)
{
rawmsg[5 + i] = pack.Message[i];
}
return(rawmsg);
}
// Event Handler for the Timer (SyncTimer) member
public void SyncTimerTick(object sender, EventArgs e)
{
processData(CommPortBuffer);
processEvents();
int packetsInQueue = packetsAvailable();
do
{
if (packetsInQueue > 0)
{
int i;
for (i = 0; i < packetsInQueue; i++)
{
EZPacket pack = getNextAvailablePacket();
int senderID = pack.SenderID;
int j = 0;
if (EntityNum > 0)
{
Boolean found = false;
while ((!found) & (j < EntityNum))
{
if (EntityList[j].AvatarNodeID == senderID)
{
found = true;
}
else
{
j++;
}
}
if (j < EntityNum)
{
EntityList[j].addIncomingPacket(pack);
}
}
}
}
packetsInQueue = packetsAvailable();
} while (packetsInQueue > 0);
}
// Convert bytes to a packet structure
public EZPacket bytes2Packet(byte[] rawpacket)
{
EZPacket packet = new EZPacket();
ushort messagelen = (ushort)(rawpacket[3] + 256 * rawpacket[4]);
packet.SenderID = rawpacket[0];
packet.ReceiverID = rawpacket[1];
packet.SenderNodeType = rawpacket[2];
packet.MessageLength = messagelen;
packet.Message = new byte[messagelen];
int i;
for (i = 0; i < messagelen; i++)
{
packet.Message[i] = rawpacket[5 + i];
}
return(packet);
}
// sends a command message to the avatar
public void sendCommandMessage(CommandMsg cmdmsg)
{
int paramLength = cmdmsg.ParamsLength;
int rawmsgLength = paramLength + 4;
byte[] rawcmdmsg = new byte[rawmsgLength];
rawcmdmsg[0] = (byte)cmdmsg.robot;
rawcmdmsg[1] = (byte)cmdmsg.Cmd;
rawcmdmsg[2] = (byte)(paramLength & 0x0FF);
rawcmdmsg[3] = (byte)((paramLength >> 8) & 0x0FF);
int i;
for (i = 0; i < paramLength; i++)
{
rawcmdmsg[4 + i] = cmdmsg.CmdParams[i];
}
// creating the packet now
EZPacket pack = createEZPacket(rawcmdmsg, rawmsgLength);
// sending
NetDevice.pushOutboundPacket(pack);
}
// adds an incoming packet to the PacketsReceived queue
public void addIncomingPacket(EZPacket pack)
{
ReceivedPacketsNum = (ReceivedPacketsNum < MAX_RECEIVED_PACKETS_NUM - 1) ? ReceivedPacketsNum + 1 : 1;
ReceivedPackets[ReceivedPacketsNum - 1] = pack;
}
// processEvents consults with the state and initiates packet handling
// additionally, if a timeout occurs, it re-initiates transmission
public void processEvents()
{
int i, j;
// 1. Checking Possible Timeouts while sending a packet
if (state == stWaitingAcknowledge)
{ // waiting for acknowledge
// checking time ellapsed
DateTime currentTime = DateTime.Now;
TimeSpan transmissionElapsed = currentTime - TransmissionStartTime;
if (transmissionElapsed.Seconds > PACKET_SENDING_TIMEOUT)
{ // transmission timedout
// raising error flag
PacketSendTimeout = true;
// quiting everything and resetting
PacketSenderID = 0;
PacketReceiverID = 0;
TotalFrames = 0;
FramesSent = 0;
FramesReceived = 0;
state = stIdle;
}
else
{
TimeSpan elapsed = currentTime - FrameTransmissionStartTime;
if (elapsed.Milliseconds > FRAME_ACKNOWLEDGE_TIMEOUT)
{ // timeout occured
// attempting to resend the frame
state = stSending; // sending again
sendNextFrame();
}
}
}
else // 2. Checking outbound packets queue to initiate next transmission
if (state == stIdle)
{
if (OutboundPacketsNum > 0)
{ // outbound queue is not empty. must initiate transmission
// removing a packet from the outbound queue
EZPacket outpacket = OutboundPackets[0];
OutboundPacketsNum--;
// shifting the Outbound queue to the left
for (i = 1; i <= OutboundPacketsNum; i++)
{
OutboundPackets[i - 1] = OutboundPackets[i];
}
// outbound queue shifted
// Now, sending packet
sendPacket(outpacket);
}
}
// 3. Checking for ready packets in the incomplete Packets Queue
for (i = 0; i < IncompletePacketsNum; i++)
{
if (IncompletePackets[i].state == stPacketReady)
{
// the packet is assembled and ready
// increase number of packets received
if (ReceivedPacketsNum < MAX_RECEIVED_PACKETS_NUM)
{
ReceivedPacketsNum++;
ReceivedPackets[ReceivedPacketsNum - 1] = bytes2Packet(IncompletePackets[i].CurrentPacket);
}
else // overflow
{
PacketQueueOverflow = true;
}
// decreasing number of incomplete packets
IncompletePacketsNum--;
// deleting the Incomplete Packet entry from the Queue
for (j = i; j < IncompletePacketsNum; j++)
{
IncompletePackets[j] = IncompletePackets[j + 1];
}
// clearing residual pointer
IncompletePackets[IncompletePacketsNum] = null;
}
}
// 4. Checking Possible Timeouts while receiving
for (i = 0; i < IncompletePacketsNum; i++)
{
if (IncompletePackets[i].state == stReceiving)
{ // receiving a packet
DateTime currentTime = DateTime.Now;
TimeSpan receptionElapsed = currentTime - IncompletePackets[i].ReceptionStartTime;
if (receptionElapsed.Seconds > PACKET_SENDING_TIMEOUT)
{ // packet's timedout
// raising timeout flag flag (must be handled shortly though...)
PacketReceiveTimeout = true;
PacketReceiveTimeoutSenderID = IncompletePackets[i].PacketSenderID;
// deleting the Packet Reception FSM
for (j = i; j < IncompletePacketsNum - 1; j++)
{
IncompletePackets[j] = IncompletePackets[j + 1];
}
// decreasing number of incomplete packets
IncompletePacketsNum--;
// clearing residual pointer
IncompletePackets[IncompletePacketsNum] = null;
}
else
{
// checking time elapsed since last acknowledgement
TimeSpan elapsed = currentTime - IncompletePackets[i].FrameReceptionStartTime;
if (elapsed.Milliseconds > FRAME_ACKNOWLEDGE_TIMEOUT)
{ // next frame timedout
// sending a new acknowledgement
sendAcknowledge(PacketReceiverID);
// reseting time
IncompletePackets[i].FrameReceptionStartTime = DateTime.Now;
IncompletePackets[i].state = stReceiving;
}
}
}
}
}