protected void HandleFrame(int dataType, int bufferId, int packetSeqId, byte[] data)
{
//If the drone is pinging us -> Send back pong
if (bufferId == CommandSet.ARNETWORK_MANAGER_INTERNAL_BUFFER_ID_PING)
{
SendPong(data);
Logger.Debug("Pong");
}
switch (dataType)
{
//Drone is asking for us to acknowledge the retrieval of the packet
case CommandSet.ARNETWORKAL_FRAME_TYPE_ACK:
int ackSeqNumber = data[0];
CommandReceiver.SetCommandReceived("SEND_WITH_ACK", ackSeqNumber, true);
AckPacket(bufferId, ackSeqNumber);
Logger.Debug("Send Ack");
break;
// Drone just sent us sensor data
case CommandSet.ARNETWORKAL_FRAME_TYPE_DATA:
if (bufferId == CommandSet.BD_NET_DC_NAVDATA_ID || bufferId == CommandSet.BD_NET_DC_EVENT_ID)
{
UpdateSensorData(dataType, bufferId, packetSeqId, data, false);
}
break;
case CommandSet.ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY:
Logger.Debug("Handle low latency data?");
break;
case CommandSet.ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK:
if (bufferId == CommandSet.BD_NET_DC_NAVDATA_ID || bufferId == CommandSet.BD_NET_DC_EVENT_ID)
{
UpdateSensorData(dataType, bufferId, packetSeqId, data, true);
}
break;
case CommandSet.ARNETWORKAL_FRAME_TYPE_MAX:
Logger.Debug("Received a maxframe(Technically unknown?)");
break;
case CommandSet.ARNETWORKAL_FRAME_TYPE_UNINITIALIZED:
Logger.Debug("Received an uninitialized frame");
break;
default:
Logger.Fatal("Unknown data type received from drone!");
break;
}
}
protected bool SendCommandAck(byte[] packet, int sequenceId)
{
int tryNum = 0;
CommandReceiver.SetCommandReceived("SEND_WITH_ACK", sequenceId, false);
while (tryNum < MaxPacketRetries && !CommandReceiver.IsCommandReceived("SEND_WITH_ACK", sequenceId))
{
SafeSend(packet);
tryNum++;
SmartSleep(250);
}
return(CommandReceiver.IsCommandReceived("SEND_WITH_ACK", sequenceId));
}