uwrov-module.cc

Go to the documentation of this file.

//
// Copyright (c) 2017 Regents of the SIGNET lab, University of Padova.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Padova (SIGNET lab) nor the
// names of its contributors may be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
#include "uwrov-module.h"
#include <iostream>
#include <rng.h>
#include <stdint.h>
extern packet_t PT_UWCBR;
extern packet_t PT_UWROV;
extern packet_t PT_UWROV_CTR;
int hdr_uwROV_monitoring::offset_; 
int hdr_uwROV_ctr::offset_; 
static class UwROVModuleClass : public TclClass {
public:
 
    UwROVModuleClass() : TclClass("Module/UW/ROV") {
    }
 
    TclObject* create(int, const char*const*) {
        return (new UwROVModule());
    }
} class_module_uwROV;
 
void UwROVSendAckTimer::expire(Event *e) {
    module->sendAck();
}
 
UwROVModule::UwROVModule() 
    : UwCbrModule()
    , last_sn_confirmed(0)
    , ack(0)
    , ackPolicy(ACK_PIGGYBACK)
    , ackTimeout(10)
    , ackTimer_(this)
    , ackPriority(0)
    , ackNotPgbk(0)
    , drop_old_waypoints(0)
    , log_flag(0)
    , out_file_stats(0)
{
    UWSMPosition p = UWSMPosition();
    posit=&p;
    bind("ackTimeout_", (int*) &ackTimeout);
    bind("ackPriority_", (int*) &ackPriority);
    bind("drop_old_waypoints_", (int*) &drop_old_waypoints);
    bind("log_flag_", (int*) &log_flag );
    if (ackTimeout < 0) {
        cerr << NOW << " Invalide ACK timout < 0, timeout set to 10 by defaults"
            << std::endl;
        ackTimeout = 10;
    }
 
}
 
UwROVModule::UwROVModule(UWSMPosition* p) 
    : UwCbrModule()
    , last_sn_confirmed(0)
    , ack(0)
    , ackPolicy(ACK_PIGGYBACK)
    , ackTimeout(10)
    , ackTimer_(this)
    , ackPriority(0)
    , ackNotPgbk(0)
    , drop_old_waypoints(0)
    , log_flag(0)
    , out_file_stats(0)
{
    posit = p;
    bind("ackTimeout_", (int*) &ackTimeout);
    bind("ackPriority_", (int*) &ackPriority);
    bind("drop_old_waypoints_", (int*) &drop_old_waypoints);
    bind("log_flag_", (int*) &log_flag );
    if (ackTimeout < 0) {
        cerr << NOW << " Invalide ACK timout < 0, timeout set to 10 by defaults"
            << std::endl;
        ackTimeout = 10;
    }
 
 
}
 
UwROVModule::~UwROVModule() {}
 
void UwROVModule::setPosition(UWSMPosition* p){
    posit = p;
}
 
int UwROVModule::command(int argc, const char*const* argv) {
    Tcl& tcl = Tcl::instance();
    if(argc == 2){
        if (strcasecmp(argv[1], "getROVMonheadersize") == 0) {
            tcl.resultf("%d", getROVMonHeaderSize());
            return TCL_OK;
        }
        else if(strcasecmp(argv[1], "getROVctrheadersize") == 0) {
            tcl.resultf("%d", getROVCTRHeaderSize());
            return TCL_OK;
        }
        else if(strcasecmp(argv[1], "getX") == 0) {
            tcl.resultf("%f", posit->getX());
            return TCL_OK;
        }
        else if(strcasecmp(argv[1], "getY") == 0) {
            tcl.resultf("%f", posit->getY());
            return TCL_OK;
        }
        else if(strcasecmp(argv[1], "getZ") == 0) {
            tcl.resultf("%f", posit->getZ());
            return TCL_OK;
        }
        else if(strcasecmp(argv[1], "getAckNotPgbk") == 0) {
            tcl.resultf("%d", ackNotPgbk);
            return TCL_OK;
        }
    }
    else if(argc == 3){
        if (strcasecmp(argv[1], "setPosition") == 0) {
            UWSMPosition* p = dynamic_cast<UWSMPosition*> (tcl.lookup(argv[2]));
            posit=p;
            tcl.resultf("%s", "position Setted\n");
            return TCL_OK;
        }
        if (strcasecmp(argv[1], "setAckPolicy") == 0) {
            if (atof(argv[2]) == 1) {
                ackPolicy = ACK_PIGGYBACK;
                return TCL_OK;
            }
            if (atof(argv[2]) == 2) {
                ackPolicy = ACK_IMMEDIATELY;
                return TCL_OK;
            }
            if (atof(argv[2]) == 3) {
                ackPolicy = ACK_PGBK_OR_TO;
                return TCL_OK;
            }
        }
        if (strcasecmp(argv[1], "setAckTimeout") == 0) {
            ackTimeout = atof(argv[2]);
            return TCL_OK;
        }
        if (strcasecmp(argv[1], "setAckPriority") == 0) {
            ackPriority = atof(argv[2]);
            return TCL_OK;
        }
    }
    else if(argc == 5){
        if (strcasecmp(argv[1], "setdest") == 0) {
            posit->setdest(atof(argv[2]),atof(argv[3]),atof(argv[4]));
            return TCL_OK;
        }
    }
    else if(argc == 6){
    if (strcasecmp(argv[1], "setdest") == 0) {
        posit->setdest(atof(argv[2]),atof(argv[3]),atof(argv[4]),atof(argv[5]));
        return TCL_OK;
        }
    }
    return UwCbrModule::command(argc,argv);
}
 
void UwROVModule::initPkt(Packet* p) {
    hdr_uwROV_monitoring* uwROVh = HDR_UWROV_MONITORING(p);
    hdr_uwcbr *uwcbrh = HDR_UWCBR(p);
    uwROVh->x() = posit->getX();
    uwROVh->y() = posit->getY();
    uwROVh->z() = posit->getZ();
    uwROVh->ack() = ack;
    
    ack=0;
 
    if (debug_)
        std::cout << NOW << " UwROVModule::initPkt(Packet *p) ROV current "
            << "position: X = " << uwROVh->x() << ", Y = " << uwROVh->y() 
            << ", Z = " << uwROVh->z()<< std::endl;
    ackTimer_.force_cancel();
    UwCbrModule::initPkt(p);
    if (debug_) {
        std::cout << NOW << " UwROVModule::sending packet with priority " 
            << (int)uwcbrh->priority() << std::endl;
    }
    priority_ = 0;
}
 
void UwROVModule::recv(Packet* p, Handler* h) {
    recv(p);
}
 
void UwROVModule::recv(Packet* p) {
 
    hdr_uwROV_ctr* uwROVh = HDR_UWROV_CTR(p);
 
    if (drop_old_waypoints == 1 && uwROVh->sn() <= last_sn_confirmed) { //obsolete packets
        if (debug_) {
            std::cout << NOW << " UwROVModule::old waypoint with sn " 
                << uwROVh->sn() << " dropped " << std::endl;
        }
 
    } else { //packet in order
        posit->setdest(uwROVh->x(),uwROVh->y(),uwROVh->z(),uwROVh->speed());
        last_sn_confirmed = uwROVh->sn();
    }
 
    ack = last_sn_confirmed+1;
    priority_ = (char) ackPriority;
 
    if (log_flag == 1) {
        out_file_stats.open("my_log_file.csv",std::ios_base::app);
        out_file_stats << left << "time: " << NOW << ", positions ROV: x = " 
            << posit->getX() << ", y = " << posit->getY() 
            << ", z = " << posit->getZ() << std::endl;
        out_file_stats.close();
    }
 
 
    if (debug_) {
        std::cout << NOW << " UwROVModule::recv(Packet *p) ROV received new "
            "way point: X = " << uwROVh->x() << ", Y = " << uwROVh->y() 
            << ", Z = " << uwROVh->z()<< std::endl;
    }
    UwCbrModule::recv(p);
    if (ackPolicy == ACK_IMMEDIATELY) {
            
        if (ackPriority == 0) {
            UwCbrModule::sendPkt();
            if (debug_)
                cout << NOW << " ACK sent immediately with standard priority " 
                    << std::endl;
        } else {
            UwCbrModule::sendPktHighPriority();
            if (debug_)
                cout << NOW << " ACK sent immediately with high priority " 
                    << std::endl;
        }
    }
 
    if (ackPolicy == ACK_PGBK_OR_TO) {
        ackTimer_.resched(ackTimeout);
    }
}
 
void UwROVModule::sendAck() {
    ackNotPgbk++;
    if (ackPriority == 0) {
        UwCbrModule::sendPkt();
        if (debug_)
            cout << NOW << " ACK timeout expired, ACK sent with standard "
                << "priority " << std::endl;
    } else {
        UwCbrModule::sendPktHighPriority();
        if (debug_)
            cout << NOW << " ACK timeout expired, ACK sent with high priority " 
                << std::endl;
    }
}