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;
}
}
