void GroundStation::processPendingDatagrams()
{
while(recvUdpSocket.hasPendingDatagrams())
{
QTextStream(stdout) << "Processing started" << endl;
QByteArray datagram;
datagram.resize(recvUdpSocket.pendingDatagramSize());
// Read from the udpSocket while there is a datagram and store into datagram.
recvUdpSocket.readDatagram(datagram.data(), datagram.size());
//do {
//datagram.resize(udpSocket.pendingDatagramSize());
//udpSocket.readDatagram(datagram.data(), datagram.size());
//} while (udpSocket.hasPendingDatagrams());
//QDataStream in(&datagram, QIODevice::ReadOnly);
//in.setVersion(QDataStream::Qt_4_3);
// Validates check sum first and then convert Datagram into proper packet.
Protocol::Packet* packet = Protocol::Packet::Parse((uint8_t*)datagram.data(), datagram.size());
Protocol::PacketType packet_type = packet->get_type();
// Depending on the type call the proper method to extract packet's information and print
if(packet != nullptr)
{
QTextStream(stdout) << "Packet number " << GroundStation::NUM_RECV_PACKETS + 1 << endl;
switch(packet_type)
{
case Protocol::PacketType::Action:
print_action_packet(*dynamic_cast<Protocol::ActionPacket*>(packet));
break;
case Protocol::PacketType::Ack:
print_ack_packet(*dynamic_cast<Protocol::AckPacket*>(packet));
break;
case Protocol::PacketType::Info:
print_info_packet(*dynamic_cast<Protocol::InfoPacket*>(packet));
break;
case Protocol::PacketType::Telem:
print_telemetry_packet(*dynamic_cast<Protocol::TelemetryPacket*>(packet));
break;
default:
break;
}
QTextStream(stdout) << ""<< endl;
++GroundStation::NUM_RECV_PACKETS; }
else
{
QTextStream(stdout) << "ERROR: Packet is invalid" << endl;
}
}
}
void UAV::processPendingDatagrams()
{
QTextStream(stdout) << "data recieved!" << endl;
while(recvUdpSocket.hasPendingDatagrams())
{
QTextStream(stdout) << "Processing started" << endl;
QByteArray datagram;
datagram.resize(recvUdpSocket.pendingDatagramSize());
// Read from the udpSocket while there is a datagram and store into datagram.
recvUdpSocket.readDatagram(datagram.data(), datagram.size());
// Validates check sum first and then convert Datagram into proper packet.
Protocol::Packet* packet = Protocol::Packet::Parse((uint8_t*)datagram.data(), datagram.size());
Protocol::PacketType packet_type = packet->get_type();
// Depending on the type call the proper method to extract packet's information and print
if(packet != nullptr)
{
QTextStream(stdout) << "Packet number " << UAV::NUM_RECV_PACKETS + 1 << endl;
switch(packet_type)
{
case Protocol::PacketType::Action:
print_action_packet(*dynamic_cast<Protocol::ActionPacket*>(packet));
respond_to_action_packet(*dynamic_cast<Protocol::ActionPacket*>(packet));
sendAckPacket(packet, "Action Packet");
break;
case Protocol::PacketType::Ack:
print_ack_packet(*dynamic_cast<Protocol::AckPacket*>(packet));
break;
case Protocol::PacketType::Info:
print_info_packet(*dynamic_cast<Protocol::InfoPacket*>(packet));
sendAckPacket(packet, "Info Packet");
break;
case Protocol::PacketType::Telem:
print_telemetry_packet(*dynamic_cast<Protocol::TelemetryPacket*>(packet));
sendAckPacket(packet, "Telemetry Packet");
break;
default:
break;
}
QTextStream(stdout) << ""<< endl;
++UAV::NUM_RECV_PACKETS;
}
else
{
QTextStream(stdout) << "ERROR: Packet is invalid" << endl;
}
}
}
void NetworkListener::processPendingDatagrams() {
static int pack_number = 1;
QByteArray datagram;
datagram.resize(udpSocket.pendingDatagramSize());
udpSocket.readDatagram(datagram.data(), datagram.size());
Protocol::Packet* incPack = Protocol::Packet::Parse((uint8_t*)datagram.data(), datagram.size());
Protocol::PacketType type = incPack->get_type();
if (type == Protocol::PacketType::Ack) {
std::cout<< "AckPacket Recieved" << std::endl;
Protocol::AckPacket *ackPacket = (Protocol::AckPacket*)incPack;
myMessageBox->addAckPacket(*ackPacket);
} else if (type == Protocol::PacketType::Action) {
std::cout<< "ActionPacket Recieved" << std::endl;
Protocol::ActionPacket *actionPacket = (Protocol::ActionPacket*)incPack;
Protocol::Waypoint test_wp;
test_wp = actionPacket->GetWaypoint();
std::cout << pack_number << " Latitude: " << test_wp.lat << " Longitude: " << test_wp.lon << std::endl;
++pack_number;
if (actionPacket->GetAction() == Protocol::ActionType::AddWaypoint) {
Protocol::TelemetryPacket telem;
telem.SetLocation(test_wp.lat,test_wp.lon,200);
myMessageBox->addTelemetryPacket(telem);
}
myMessageBox->addActionPacket(*actionPacket);
} else if (type == Protocol::PacketType::Telem) {
std::cout<< "TelemPacket Recieved" << std::endl;
Protocol::TelemetryPacket *telemPacket = (Protocol::TelemetryPacket*)incPack;
//std::cout << pack_number << " Latitude: " << telemPacket << " Longitude: " << telemPacket->lon << std::endl;
++pack_number;
myMessageBox->addTelemetryPacket(*telemPacket);
} else if (type == Protocol::PacketType::Info) {
std::cout<< "InfoPacket Recieved" << std::endl;
Protocol::InfoPacket *infoPacket = (Protocol::InfoPacket*)incPack;
myMessageBox->addInfoPacket(*infoPacket);
} else {
std::cout<< "UNKNOWN PACKET TYPE RECIEVED!" << std::endl;
}
return;
}
void ServerQueue::enqueue(Protocol::Packet *packet, unsigned int priority) {
QueueEntry *newEntry = (QueueEntry*)malloc(sizeof(QueueEntry));
unsigned char str[1024];
size_t length = packet->GetBytes(str,1024);
Protocol::Packet *readPacket = Protocol::Packet::Parse(str,length);
Protocol::Packet *newPacket;
switch(readPacket->get_type()) {
case Protocol::PacketType::Ack:
newPacket = new Protocol::AckPacket(str,length);
break;
case Protocol::PacketType::Action:
newPacket = new Protocol::ActionPacket(str,length);
break;
case Protocol::PacketType::Info:
newPacket = new Protocol::InfoPacket(str,length);
break;
case Protocol::PacketType::Telem:
newPacket = new Protocol::TelemetryPacket(str,length);
break;
default:
return;
}
newEntry->packet = newPacket;
newEntry->priority = priority;
newEntry->waitingForAck = true;
QLinkedList<QueueEntry*>::iterator i;
for(i = pendingPackets.begin(); i!=pendingPackets.end(); ++i) {
if((*i)->priority > newEntry->priority) {
break;
}
}
pendingPackets.insert(i,newEntry);
}