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;


}
Example #2
0
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();
}
Example #3
0
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;

    }
}
Example #4
0
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;
}
Example #6
0
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;
    }

}