void UAV::send_info_packet()
{
Protocol::InfoPacket ip;
ip.SetBattery(battery);
ip.SetStorable(pointsStorable);
sendAPacket(dynamic_cast<Protocol::Packet*>(&ip));
}
void GroundStation::sendAllPackets(std::vector<Protocol::Packet*> packets)
{
//QTextStream(stdout) << "The size of the vector is " << packets.size() << endl;
for(auto i = packets.begin(); i != packets.end(); ++i)
{
sendAPacket(*i);
}
}
void UAV::sendAckPacket(Protocol::Packet* p, const QString& pt)
{
Protocol::AckPacket ack;
ack.set_timestamp(p->get_timestamp());
QTextStream(stdout) << "Sending ack packet " << pt << " and time: " << p->get_timestamp() << endl;
sendAPacket(&ack);
}
void GroundStation::sendAllPackets(std::queue<Protocol::Packet*> packets)
{
//QTextStream(stdout) << "The size of the vector is " << packets.size() << endl;
int size = packets.size();
for(int i = 0; i < size; ++i)
{
sendAPacket(packets.front());
packets.pop();
}
}
void UAV::sendCurrentTelem()
{
updateUavLatLng();
Protocol::TelemetryPacket tp(telemSeqNumber++);
double velocity_x = qrand() % 30 + 80;
double velocity_y = qrand() % 30 + 80;
double velocity_z = qrand() % 30 + 80;
double altitude = qrand() % 75 + 150;
tp.SetVelocity(velocity_x, velocity_y, velocity_z);
tp.SetOrientation(4,5,6);
tp.SetLocation(uavLat, uavLng, altitude);
tp.SetHeading(10);
sendAPacket(&tp);
}
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;
}
}