void GroundStation::print_action_packet(Protocol::ActionPacket& packet){
double lat, lon;
float alt, speed;
QTextStream(stdout) << "Type: ActionPacket" << endl;
Protocol::ActionType type = packet.GetAction();
switch(type)
{
case Protocol::ActionType::Start : QTextStream(stdout) << "Start: " << (uint8_t)type << endl; break;
case Protocol::ActionType::RequestInfo : QTextStream(stdout) << "Request Info: " << (uint8_t)type << endl; break;
case Protocol::ActionType::AddWaypoint : QTextStream(stdout) << "Add Waypoint: " << (uint8_t)type << endl; break;
case Protocol::ActionType::SetHome : QTextStream(stdout) << "Set Home: " << (uint8_t)type << endl; break;
case Protocol::ActionType::RemoveWaypoint : QTextStream(stdout) << "Remove Waypoint: " << (uint8_t)type << endl; break;
case Protocol::ActionType::Stop : QTextStream(stdout) << "Stop: " << (uint8_t)type << endl; break;
case Protocol::ActionType::Shutdown : QTextStream(stdout) << "Shutdown: " << (uint8_t)type << endl; break;
default : QTextStream(stdout) << "Unknown Type: " << (uint8_t)type << endl; break;
}
Protocol::Waypoint waypoint = packet.GetWaypoint();
lat = waypoint.lat;
lon = waypoint.lon;
alt = waypoint.alt;
speed = waypoint.speed;
QTextStream(stdout) << "Latitude: " << lat << endl;
QTextStream(stdout) << "Longitude: " << lon << endl;
QTextStream(stdout) << "Altitude: " << alt << endl;
QTextStream(stdout) << "Speed: " << speed << endl;
}
int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);
QTextStream(stdout) << "run this";
GroundStation station;
// messagebox mb;
// mb.load_action_packet(1,2,3,4);
// mb.load_action_packet(5,6,7,8);
// mb.load_action_packet(9,10,11,12);
// std::vector<Protocol::ActionPacket> actionPacket = mb.get_action_packets();
std::queue<Protocol::Packet*> packets_to_send;
// for (auto& action : actionPacket)
// packets_to_send.push(&action);
// Protocol::InfoPacket info;
// info.SetBattery(150);
// info.SetOther("Im");
// info.SetStorable(300);
// packets_to_send.push(&info);
Protocol::ActionPacket initialInfoReq;
initialInfoReq.SetAction(Protocol::ActionType::RequestInfo);
packets_to_send.push(&initialInfoReq);
// Protocol::AckPacket ack;
// packets_to_send.push(&ack);
// station.sendAllActionPackets(actionPacket);
// Protocol::TelemetryPacket tel;
// tel.SetVelocity(1,2,3);
// tel.SetOrientation(4,5,6);
// tel.SetLocation(7,8,9);
// tel.SetHeading(10);
// packets_to_send.push(&tel);
Protocol::ActionPacket waypoint1, waypoint2;
waypoint1.SetAction(Protocol::ActionType::AddWaypoint);
Protocol::Waypoint wp;
wp.lon = -117;
wp.lat = 33.6;
waypoint1.SetWaypoint(wp);
packets_to_send.push(&waypoint1);
wp.lon = -115;
wp.lat = 37;
waypoint2.SetAction(Protocol::ActionType::AddWaypoint);
waypoint2.SetWaypoint(wp);
packets_to_send.push(&waypoint2);
Protocol::ActionPacket start;
start.SetAction(Protocol::ActionType::Start);
packets_to_send.push(&start);
station.sendAllPackets(packets_to_send);
// return 0;
return a.exec();
}
void UAV::respond_to_action_packet(Protocol::ActionPacket ap)
{
Protocol::ActionType ap_type = ap.GetAction();
// Simple one packet send first.
switch(ap_type)
{
case Protocol::ActionType::RequestInfo:
if(uavOn)
send_info_packet();
break;
case Protocol::ActionType::AddWaypoint:
if(uavOn && can_add_waypoint())
{
QTextStream(stdout) << "Adding new waypoint" << endl;
uavWaypointsReady = true;
++currentNumOfPoints;
addWaypoint(ap);
}
else
QTextStream(stdout) << "Cannot add waypoint." << endl;
break;
case Protocol::ActionType::SetHome:
{
Protocol::Waypoint newHome = ap.GetWaypoint();
QTextStream(stdout) << "Setting Home: (" << newHome.lat << "," << newHome.lon << ")" << endl;
uavHomeLat = newHome.lat;
uavHomeLng = newHome.lon;
}
break;
case Protocol::ActionType::Stop:
QTextStream(stdout) << "STOPPING" << endl;
uavFlying = false;
stopAction = true;
break;
case Protocol::ActionType::Start:
//if(uavOn && !receivedInfoPacketReq && uavWaypointsReady)
if(!uavOn)
{
QTextStream(stdout) << "STARTING... "<< endl;
uavOn = true;
uavFlying = true;
receivedInfoPacketReq = true;
timer->start(200);
}
break;
case Protocol::ActionType::Shutdown:
shutdownAction = true;
uavOn = false;
timer->stop();
break;
default:
break;
}
}
void UAV::addWaypoint(Protocol::ActionPacket ap)
{
//QTextStream(stdout) << "Waypoint added" << endl;
Protocol::Waypoint wp = ap.GetWaypoint();
pointOfInterest.push(wp);
}
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 UAV::updateUavLatLng()
{
if(pointOfInterest.empty()){
return;
}
Protocol::Waypoint nextPoint = pointOfInterest.front();
if(uavLat < nextPoint.lat && (uavLat + latLngSpd) < nextPoint.lat)
uavLat += latLngSpd;
else if(uavLat > nextPoint.lat && (uavLat - latLngSpd) > nextPoint.lat)
uavLat -= latLngSpd;
else if(uavLat < nextPoint.lat && (uavLat + latLngSpd) >= nextPoint.lat)
uavLat = nextPoint.lat;
else if(uavLat > nextPoint.lat && (uavLat - latLngSpd) <= nextPoint.lat)
uavLat = nextPoint.lat;
if(uavLng < nextPoint.lon && (uavLng + latLngSpd) < nextPoint.lon)
uavLng += latLngSpd;
else if(uavLng > nextPoint.lon && (uavLng - latLngSpd) > nextPoint.lon)
uavLng -= latLngSpd;
else if(uavLng < nextPoint.lon && (uavLng + latLngSpd) >= nextPoint.lon)
uavLng = nextPoint.lon;
else if(uavLng > nextPoint.lon && (uavLng -latLngSpd) <= nextPoint.lon)
uavLng = nextPoint.lon;
if(!uavFlyingHome && uavLng == nextPoint.lon && uavLat == nextPoint.lat && pointOfInterest.size() > 0)
{
Protocol::ActionPacket waypointPacket;
waypointPacket.SetAction(Protocol::ActionType::AddWaypoint);
waypointPacket.SetWaypoint(nextPoint);
sendAPacket(&waypointPacket);
QTextStream(stdout) << "Destination reached: (" << nextPoint.lat << ", " << nextPoint.lon << "). Sent waypoint packet" << endl;
pointOfInterest.pop();
}
else if(!uavFlyingHome && pointOfInterest.size() == 0)
{
QTextStream(stdout) << "Nowhere else to go... Heading Home." << endl;
uavFlyingHome = true;
Protocol::Waypoint homePoint;
homePoint.lat = uavHomeLat;
homePoint.lon = uavHomeLng;
pointOfInterest.push(homePoint);
}
else if(uavFlyingHome && uavLng == nextPoint.lon && uavLat == nextPoint.lat && pointOfInterest.size() > 0)
{
Protocol::ActionPacket waypointPacket;
waypointPacket.SetAction(Protocol::ActionType::AddWaypoint);
waypointPacket.SetWaypoint(nextPoint);
sendAPacket(&waypointPacket);
QTextStream(stdout) << "Home reached. Sent waypoint packet" << endl;
pointOfInterest.pop();
}
else if(uavFlyingHome && pointOfInterest.size() == 0)
{
timer->stop();
uavFlying = false;
uavWaypointsReady = false;
}
}