UpperTester.cpp

Go to the documentation of this file.

#include "UpperTester.h"

static Manager* privateManager;
static GeoNet* privateGeoNet;
static Facilities* privateFac;
static FakeGPSProvider* privateGPS;
static UpperTester privateUT;
static QByteArray privateRequest;

UpperTester::UpperTester()
{

}

UpperTester::UpperTester(Manager* man, FakeCANProvider* can, FakeGPSProvider* gps, GeoNet* gn, Facilities *fac, QMutex* mutex)
{
    m_mutex = mutex;
    if(m_mutex != 0)
        m_mutex->lock();

    m_manager = man;
    m_gpsProvider = gps;
    m_canProvider = can;
    m_geonet = gn;
    m_facilities = fac;
    m_paramConf = (ParameterConfiguration*) man->hashMap["Configuration"];
    m_paramUpperTester = (ParameterUpperTester*) man->hashMap["UpperTester"];

    privateManager = man;
    privateGeoNet = gn;
    privateFac = fac;
    privateGPS = gps;

    memset(m_req, 0, NB_REQUEST);
    m_req[UpperTesterPrimitives::PRIMITIVE_utInitialize] = &UpperTester::utInitialize;
    m_req[UpperTesterPrimitives::PRIMITIVE_utChangePosition] = &UpperTester::utChangePosition;
    m_req[UpperTesterPrimitives::PRIMITIVE_utChangePseudonym] = &UpperTester::utChangePseudonym;

    m_req[UpperTesterPrimitives::PRIMITIVE_utDenmTrigger] = &UpperTester::utDenmTrigger;
    m_req[UpperTesterPrimitives::PRIMITIVE_utDenmUpdate] = &UpperTester::utDenmUpdate;
    m_req[UpperTesterPrimitives::PRIMITIVE_utDenmTerminate] = &UpperTester::utDenmTerminate;

    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_changeCurvature] = &UpperTester::utChangeCurvature;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_changeSpeed] = &UpperTester::utChangeSpeed;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setAccelerationControlStatus] = &UpperTester::utSetAccelerationControlStatus;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setExteriorLightsStatus] = &UpperTester::utSetExteriorLightsStatus;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_changeHeading] = &UpperTester::utChangeHeading;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setDriveDirection] = &UpperTester::utSetDriveDirection;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_changeYawRate] = &UpperTester::utChangeYawRate;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setStationType] = &UpperTester::utSetStationType;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setVehicleRole] = &UpperTester::utSetVehicleRole;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setEmbarkationStatus] = &UpperTester::utSetEmbarkationStatus;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setPtActivation] = &UpperTester::utSetPtActivation;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setDangerousGoods] = &UpperTester::utSetDangerousGoods;
    m_req[UpperTesterPrimitives::PRIMITIVE_utCamTrigger_setLightBarSiren] = &UpperTester::utSetLightBarSiren;

    m_req[UpperTesterPrimitives::PRIMITIVE_utGnTrigger_geoUnicast] = &UpperTester::sendGUC;
    m_req[UpperTesterPrimitives::PRIMITIVE_utGnTrigger_geoBroadcast] = &UpperTester::sendGBC;
    m_req[UpperTesterPrimitives::PRIMITIVE_utGnTrigger_geoAnycast] = &UpperTester::sendGAC;
    m_req[UpperTesterPrimitives::PRIMITIVE_utGnTrigger_shb] = &UpperTester::sendSHB;
    m_req[UpperTesterPrimitives::PRIMITIVE_utGnTrigger_tsb] = &UpperTester::sendTSB;

    m_writeSocket = new QUdpSocket(this);
    m_writeSocket->bind(m_paramUpperTester->upperTesterWritePort());
    m_readSocket = new QUdpSocket(this);
    m_readSocket->bind(m_paramUpperTester->upperTesterListenPort());
    m_readEventIndicationSocket = new QUdpSocket(this);
    m_readEventIndicationSocket->bind(m_paramUpperTester->upperTesterEventIndicationListenPort());
    QObject::connect(m_readSocket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
    QObject::connect(m_readEventIndicationSocket, SIGNAL(readyRead()), this, SLOT(readInternalDatagrams()));

    m_appSocket = new QUdpSocket(this);
    m_appSocket->bind();
    ((ParameterNetwork*)(man->hashMap["Network"]))->addPort("UpperTester", m_appSocket->localPort());
    m_appPort = ((ParameterNetwork*)(man->hashMap["Network"]))->internalPort("ApplicationUT");

    if(m_mutex != 0)
        m_mutex->unlock();
}

UpperTester::UpperTester(const UpperTester &other) : QObject()
{
    this->m_manager = other.m_manager;
    this->m_gpsProvider = other.m_gpsProvider;
    this->m_canProvider = other.m_canProvider;
    this->m_geonet = other.m_geonet;
    this->m_facilities = other.m_facilities;
}

void UpperTester::readPendingDatagrams()
{
    while(m_readSocket->hasPendingDatagrams())
    {
        QNetworkDatagram datagram = m_readSocket->receiveDatagram();
        unsigned char request = datagram.data().at(0);
        QByteArray req = datagram.data();
        printf("request : %X, senderAddr : %s, senderPort : %d\n", request,
               datagram.senderAddress().toString().toStdString().c_str(), datagram.senderPort());
        privateRequest = req;
        QByteArray res;

        if(m_req[request])
            res = (this->*m_req[request])();
        else
        {
            quint8 result = 0;
            QDataStream ds(&res, QIODevice::WriteOnly);
            ds << req << result;
        }
        printf("result : ");
        print_hexa((const unsigned char*)res.data(), res.length());
        m_lastSenderAddress = datagram.senderAddress();
        m_lastSenderPort = datagram.senderPort();
        m_writeSocket->writeDatagram(res, datagram.senderAddress(), datagram.senderPort());
    }
}

void UpperTester::readInternalDatagrams()
{
    while(m_readEventIndicationSocket->hasPendingDatagrams())
    {
        QNetworkDatagram datagram = m_readEventIndicationSocket->receiveDatagram();
        qDebug() << datagram.data().toHex() << "received in UT" << m_readEventIndicationSocket->localPort();
        m_writeSocket->writeDatagram(datagram.data(), m_lastSenderAddress, m_lastSenderPort);
        qDebug() << datagram.data().toHex() << "sent by UT to" << m_lastSenderPort;
    }
}

/* Common Upper Tester Primitives */

QByteArray UpperTester::utInitialize()
{
    printf("\n\n\ncalling utInit\n");

    if(privateGeoNet != 0)
        QTimer::singleShot(0, privateGeoNet, SLOT(utInitialize()));
    if(privateFac != 0)
        privateFac->utInitialize();

    uchar mid[6];
    foreach(QNetworkInterface netInterface, QNetworkInterface::allInterfaces())
    {
        // Return only the first non-loopback MAC Address
        if (!(netInterface.flags() & QNetworkInterface::IsLoopBack))
        {
            int last = -1;
            int rc = sscanf(netInterface.hardwareAddress().toStdString().c_str(), "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx%n",
                            mid + 0, mid + 1, mid + 2, mid + 3, mid + 4, mid + 5, &last);
            if(rc != 6 || netInterface.hardwareAddress().toStdString().size() > (unsigned int)last)
                fprintf(stderr, "invalid mac address format %s", netInterface.hardwareAddress().toStdString().c_str());
            privateManager->updateGnAddr(mid);
        }
    }

    *((ParameterConfiguration*) privateManager->hashMap["Configuration"]) = ParameterConfiguration();
    *((FakeGPSProvider*) privateManager->hashMap["GPS"]) = FakeGPSProvider();
    *((FakeCANProvider*) privateManager->hashMap["CAN"]) = FakeCANProvider();
    *((ParameterDENM*) privateManager->hashMap["DENM"]) = ParameterDENM();
    *((FakeApplicationProvider*) privateManager->hashMap["Application"]) = FakeApplicationProvider();
    *((ParameterUpperTester*) privateManager->hashMap["UpperTester"]) = ParameterUpperTester();

    sleep(1);
    printf("utInit called\n\n\n");
    return utInitializeResult(true);
}

QByteArray UpperTester::utInitializeResult(bool success)
{
    quint8 mt = UpperTesterPrimitives::PRIMITIVE_utInitializeResult;
    quint8 result = success;
    QByteArray res;
    QDataStream ds(&res, QIODevice::WriteOnly);
    ds << mt << result;
    return res;
}

QByteArray UpperTester::utChangePosition()
{
    uchar* data = (uchar*)privateRequest.data();
    quint64 offset = 1;
    qint32 deltaLatitude;
    offset += get_INT32(data + offset, &deltaLatitude);
    qint32 deltaLongitude;
    offset += get_INT32(data + offset, &deltaLongitude);
    qint8 deltaAltitude;
    offset += get_INT8(data + offset, &deltaAltitude);
    privateGPS->changePosition(deltaLatitude, deltaLongitude, deltaAltitude);
    return utChangePositionResult(true);
}

QByteArray UpperTester::utChangePseudonym()
{
    FakeApplicationProvider* paramAppli = (FakeApplicationProvider*) privateManager->hashMap["Application"];
    quint32 originatingStationID = paramAppli->data().stationID();
    quint32 stationID = rand() % 0x100000000;

    while(stationID == originatingStationID)
        stationID = rand() % 0x100000000;

    paramAppli->setStationID(stationID);

    return utChangePseudonymResult(true);
}

QByteArray UpperTester::utChangePseudonymResult(bool success)
{
    quint8 mt = UpperTesterPrimitives::PRIMITIVE_utChangePseudonymResult;
    quint8 result = success;
    QByteArray res;
    QDataStream ds(&res, QIODevice::WriteOnly);

    ds << mt << result;

    return res;
}

QByteArray UpperTester::utChangePositionResult(bool success)
{
    quint8 mt = UpperTesterPrimitives::PRIMITIVE_utChangePositionResult;
    quint8 result = success;
    QByteArray res;
    QDataStream ds(&res, QIODevice::WriteOnly);
    ds << mt << result;
    return res;
}

/* DENM Upper Tester Primitives */

QByteArray UpperTester::utDenmResult(UpperTesterPrimitives::e_DENMUpperTesterPrimitives messageType, bool success, ActionID_t actionID)
{
    quint8 mt = (quint8)messageType;
    quint8 result = success;
    QByteArray res;
    QDataStream ds(&res, QIODevice::WriteOnly);

    ds << mt << result << (quint32)actionID.originatingStationID << (quint16)actionID.sequenceNumber;

    return res;
}

QByteArray UpperTester::utDenmTriggerResult(bool success, ActionID_t actionID)
{
    return utDenmResult(UpperTesterPrimitives::PRIMITIVE_utDenmTriggerResult, success, actionID);
}

QByteArray UpperTester::utDenmTrigger()
{
    ParameterDENM* paramDENM = (ParameterDENM*) privateManager->hashMap["DENM"];
    ParameterUpperTester* paramUT = (ParameterUpperTester*) privateManager->hashMap["UpperTester"];
    AppDenmTriggerData trigger(paramDENM);

    char* data = privateRequest.data();

    int offset = 1;
    quint8 flags = extractUInt(data, 1, &offset);

    bool flagValidityDuration = 0, flagRepetitionDuration = 0, flagRelevanceTrafficDirection = 0,
            flagsTransmissionInterval = 0, flagRepetitionInterval = 0;

    flagValidityDuration |= flags &0x80;
    flagRepetitionDuration |= flags &0x40;
    flagRelevanceTrafficDirection |= flags &0x10;
    flagsTransmissionInterval |= flags &0x04;
    flagRepetitionInterval |= flags &0x02;

    qDebug() << "utDenmTrigger received :";
    qDebug() << "    flags :";
    qDebug() << "        V ::= " << flagValidityDuration;
    qDebug() << "        R ::= " << flagRepetitionDuration;
    qDebug() << "        T ::= " << flagRelevanceTrafficDirection;
    qDebug() << "        K ::= " << flagsTransmissionInterval;
    qDebug() << "        I ::= " << flagRepetitionInterval;

    trigger.setEventDetectionTime(extractUInt(data, 6, &offset));
    qDebug() << "    DetectionTime ::= " << trigger.eventDetectionTime();

    if(flagValidityDuration)
    {
        trigger.setValidityDuration(extractUInt(data, 3, &offset));
        qDebug() << "    ValidityDuration ::= " << *trigger.validityDuration();
    }
    else
    {
        offset += 3;
        qDebug() << "    ValidityDuration ::= omit";
    }
    if(flagRepetitionDuration)
    {
        trigger.setRepetitionDuration(extractUInt(data, 3, &offset));
        qDebug() << "    RepetitionDuration ::= " << *trigger.repetitionDuration();
    }
    else
    {
        offset += 3;
        qDebug() << "    RepetitionDuration ::= omit";
    }

    SituationContainer_t situationContainer;

    memset(&situationContainer, 0, sizeof situationContainer);
    situationContainer.informationQuality = extractUInt(data, 1, &offset);
    situationContainer.eventType.causeCode = extractUInt(data, 1, &offset);
    situationContainer.eventType.subCauseCode = extractUInt(data, 1, &offset);

    char situationContainer_uper[MSG_LENGTH] = {'\0'};
    asn_enc_rval_t rval = uper_encode_to_buffer(&asn_DEF_SituationContainer, &situationContainer, situationContainer_uper, MSG_LENGTH);
    asn_fprint(NULL, &asn_DEF_SituationContainer, &situationContainer);

    if(rval.encoded < 1)
    {
        qCritical() << "Unable to encode " << rval.failed_type->name;
        ActionID_t fail;
        fail.originatingStationID = fail.sequenceNumber = 0;
        return utDenmTriggerResult(false, fail);
    }

    trigger.setSituation(QByteArray(situationContainer_uper, (rval.encoded + 7) / 8));
    trigger.setRelevanceDistance(extractUInt(data, 1, &offset));

    if(flagRelevanceTrafficDirection)
    {
        trigger.setRelevanceTrafficDirection(extractUInt(data, 1, &offset));
        qDebug() << "    RelevanceTrafficDirection ::= " << trigger.relevanceTrafficDirection();
    }
    else
    {
        offset++;
        qDebug() << "    RelevanceTrafficDirection ::= omit";
    }
    if(flagsTransmissionInterval)
    {
        trigger.setTransmissionInterval(extractUInt(data, 2, &offset));
        qDebug() << "    TransmissionInterval ::= " << trigger.transmissionInterval();
    }
    else
    {
        offset += 2;
        qDebug() << "    TransmissionInterval ::= omit";
    }
    if(flagRepetitionInterval)
    {
        trigger.setRepetitionInterval(extractUInt(data, 2, &offset));
        qDebug() << "    RepetitionInterval ::= " << *trigger.repetitionInterval();
    }
    else
    {
        offset += 2;
        qDebug() << "    RepetitionInterval ::= omit";
    }

    if(data[offset])
    {
        qDebug() << "    Alacarte of length " << (quint8)data[offset];
        trigger.setAlacarte(QByteArray((char*)(data + offset + 1), data[offset]));
        AlacarteContainer_t *alacarte = 0;
        uper_decode_complete(NULL, &asn_DEF_AlacarteContainer, (void**)&alacarte, (data + offset + 1), data[offset]);
        asn_fprint(NULL, &asn_DEF_AlacarteContainer, alacarte);
        ASN_STRUCT_FREE(asn_DEF_AlacarteContainer, alacarte);
    }

    trigger.setEventPosition(ReferencePositionData(privateGPS->data().referencePosition()));
    trigger.setDestinationArea(paramUT->destinationArea());

    QByteArray msg = trigger.toBin();

    qDebug() << msg.toHex() << "sent from UT to" << m_appPort << "by" << m_appSocket->localPort();
    m_appSocket->writeDatagram(msg, QHostAddress::LocalHost, m_appPort);

    int msgLen = -1;
    char msgRep[MSG_LENGTH];
    QElapsedTimer timer;
    ActionID_t defaultAID;

    defaultAID.originatingStationID = 0;
    defaultAID.sequenceNumber = 0;

    timer.start();

    while(msgLen == -1 && timer.elapsed() < 10000)
        msgLen = m_appSocket->readDatagram(msgRep, MSG_LENGTH);

    if(msgLen != -1)
    {
        AppDenmResultData result(QByteArray(msgRep, msgLen));
        qDebug() << result.actionID().originatingStationID << result.actionID().sequenceNumber << QByteArray(msgRep, msgLen).toHex()
                 << "received in UT Trigger DENM";

        if(result.failureNotification())
            return utDenmTriggerResult(false, result.actionID());

        return utDenmTriggerResult(true, result.actionID());
    }
    return utDenmTriggerResult(false, defaultAID);
}

QByteArray UpperTester::utDenmUpdateResult(bool success, ActionID_t actionID)
{
    return utDenmResult(UpperTesterPrimitives::PRIMITIVE_utDenmUpdateResult, success, actionID);
}

QByteArray UpperTester::utDenmUpdate()
{
    ParameterDENM* paramDENM = (ParameterDENM*) privateManager->hashMap["DENM"];
    ParameterUpperTester* paramUT = (ParameterUpperTester*) privateManager->hashMap["UpperTester"];
    AppDenmUpdateData update(paramDENM);

    char* data = privateRequest.data();

    int offset = 1;
    quint8 flags = extractUInt(data, 1, &offset);

    bool flagValidityDuration = 0, flagRelevanceTrafficDirection = 0, flagsTransmissionInterval = 0,
            flagRepetitionInterval = 0, flagSituation = 0, flagRelevanceDistance = 0, flagTrafficClass = 0;

    flagValidityDuration |= flags & 0x80;
    flagSituation |= flags & 0x40;
    flagRelevanceDistance |= flags & 0x20;
    flagRelevanceTrafficDirection |= flags & 0x10;
    flagTrafficClass |= flags & 0x08;
    flagsTransmissionInterval |= flags & 0x04;
    flagRepetitionInterval |= flags & 0x02;


    qDebug() << "utDenmUpdate received :";
    qDebug() << "    flags :";
    qDebug() << "        V ::=" << flagValidityDuration;
    qDebug() << "        S ::=" << flagSituation;
    qDebug() << "        D ::=" << flagRelevanceDistance;
    qDebug() << "        T ::=" << flagRelevanceTrafficDirection;
    qDebug() << "        C ::=" << flagTrafficClass;
    qDebug() << "        K ::=" << flagsTransmissionInterval;
    qDebug() << "        I ::=" << flagRepetitionInterval;

    update.setActionID(ActionIDData(data, &offset));
    qDebug() << "    ActionID :";
    qDebug() << "        StationID ::=" << update.actionID().originatingStationID();
    qDebug() << "        SequenceNumber ::=" << update.actionID().sequenceNumber();

    update.setEventUpdateDetectionTime(extractUInt(data, 6, &offset));
    qDebug() << "    DetectionTime ::= " << update.eventUpdateDetectionTime();

    if(flagValidityDuration)
    {
        update.setValidityDuration(extractUInt(data, 3, &offset));
        qDebug() << "    ValidityDuration ::= " << *update.validityDuration();
    }
    else
    {
        offset += 3;
        qDebug() << "    ValidityDuraiton ::= omit";
    }
    if(flagSituation)
    {
        SituationContainer_t situation;
        asn_enc_rval_t rval;
        char situationUper[MSG_LENGTH] = {'\0'};

        memset(&situation, 0, sizeof situation);
        situation.informationQuality = extractUInt(data, 1, &offset);
        situation.eventType.causeCode = extractUInt(data, 1, &offset);
        situation.eventType.subCauseCode = extractUInt(data, 1, &offset);

        rval = uper_encode_to_buffer(&asn_DEF_SituationContainer, &situation, situationUper, MSG_LENGTH);
        update.setSituation(QByteArray(situationUper, (rval.encoded + 7) / 8));
        qDebug() << "    SituationContainer :";
        qDebug() << "        InformationQuality ::=" << situation.informationQuality;
        qDebug() << "        EventType :";
        qDebug() << "            CauseCode ::=" << situation.eventType.causeCode;
        qDebug() << "            SubCauseCode ::=" << situation.eventType.subCauseCode;
    }
    else
    {
        offset += 3;
        qDebug() << "    SituationContainer ::= omit";
    }
    if(flagRelevanceDistance)
    {
        update.setRelevanceDistance(extractUInt(data, 1, &offset));
        qDebug() << "    RelevanceDistance ::=" << update.relevanceDistance();
    }
    else
    {
        offset++;
        qDebug() << "    RelevanceDistance ::= omit";
    }
    if(flagRelevanceTrafficDirection)
    {
        update.setRelevanceTrafficDirection(extractUInt(data, 1, &offset));
        qDebug() << "    RelevanceTrafficDirection ::=" << update.relevanceTrafficDirection();
    }
    else
    {
        offset++;
        qDebug() << "    RelevanceTrafficDirection ::= omit";
    }
    if(flagTrafficClass)
    {
        update.setTrafficClass(TrafficClassData());
        qDebug() << "    TrafficClass ::=" << update.trafficClass()->toInt();
    }
    else
        qDebug() << "    TrafficClass ::= omit";
    if(flagsTransmissionInterval)
    {
        update.setTransmissionInterval(extractUInt(data, 2, &offset));
        qDebug() << "    TransmissionInterval ::=" << update.transmissionInterval();
    }
    else
    {
        offset += 2;
        qDebug() << "    TransmissionInterval ::= omit";
    }
    if(flagRepetitionInterval)
    {
        update.setRepetitionDuration(extractUInt(data, 2, &offset));
        qDebug() << "    RepetitionInterval ::=" << update.repetitionDuration();
    }
    else
    {
        offset += 2;
        qDebug() << "    RepetitionInterval ::= omit";
    }
    if(data[offset])
    {
        update.setAlacarte(QByteArray((char*)(data + offset + 1), data[offset]));
        AlacarteContainer_t *alacarte = 0;
        uper_decode_complete(NULL, &asn_DEF_AlacarteContainer, (void**)&alacarte, (data + offset + 1), data[offset]);
        qDebug() << "    AlacarteContainer :";
        asn_fprint(NULL, &asn_DEF_AlacarteContainer, alacarte);
        ASN_STRUCT_FREE(asn_DEF_AlacarteContainer, alacarte);
    }
    else
        qDebug() << "    AlacarteContainer ::= omit";

    update.setDestinationArea(paramUT->destinationArea());

    QByteArray request = update.toBin();

    qDebug() << "UT Update DENM sends" << request.toHex() << "to" << m_appPort;
    m_appSocket->writeDatagram(request, QHostAddress::LocalHost, m_appPort);

    int msgLen = -1;
    char msgRep[MSG_LENGTH];

    QElapsedTimer timer;
    ActionID_t defaultAID;

    defaultAID.originatingStationID = 0;
    defaultAID.sequenceNumber = 0;
    timer.start();

    while(msgLen == -1 && timer.elapsed() < 10000)
        msgLen = m_appSocket->readDatagram(msgRep, MSG_LENGTH);

    if(msgLen != -1)
    {
        AppDenmResultData result(QByteArray(msgRep, msgLen));
        qDebug() << result.actionID().originatingStationID << result.actionID().sequenceNumber << QByteArray(msgRep, msgLen).toHex()
                 << "received in UT Update DENM";

        if(result.failureNotification())
            return utDenmUpdateResult(false, result.actionID());
        return utDenmUpdateResult(true, result.actionID());
    }

    return utDenmUpdateResult(false, defaultAID);
}

QByteArray UpperTester::utDenmTerminate()
{
    ParameterDENM* paramDENM = (ParameterDENM*) privateManager->hashMap["DENM"];
    ParameterUpperTester* paramUT = (ParameterUpperTester*) privateManager->hashMap["UpperTester"];
    AppDenmTerminationData terminate(paramDENM);

    char* data = privateRequest.data();

    int offset = 1;
    ActionIDData actionID(data, &offset);
    ReferencePosition_t ref;

    ref.latitude = paramUT->destinationArea().geoAreaPosLatitude();
    ref.longitude = paramUT->destinationArea().geoAreaPosLongitude();
    ref.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
    ref.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
    ref.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
    ref.altitude.altitudeValue = AltitudeValue_unavailable;
    ref.altitude.altitudeConfidence = AltitudeConfidence_unavailable;

    terminate.setActionID(actionID);
    terminate.setDestinationArea(paramUT->destinationArea());
    terminate.setEventTerminationDetectionTime(privateGPS->data().timestamp());
    terminate.setEventPosition(ref);

    QByteArray request = terminate.toBin();

    qDebug() << "UT Terminate DENM sends" << request.toHex() << "to" << m_appPort;
    m_appSocket->writeDatagram(request, QHostAddress::LocalHost, m_appPort);

    int msgLen = -1;
    char msgRep[MSG_LENGTH];

    QElapsedTimer timer;
    ActionID_t defaultAID;

    defaultAID.originatingStationID = 0;
    defaultAID.sequenceNumber = 0;

    timer.start();

    while(msgLen == -1 && timer.elapsed() < 10000)
        msgLen = m_appSocket->readDatagram(msgRep, MSG_LENGTH);

    if(msgLen != -1)
    {
        AppDenmResultData result(QByteArray(msgRep, msgLen));

        if(result.failureNotification())
            return utDenmTerminateResult(false, result.actionID());
        return utDenmTerminateResult(true, result.actionID());
    }
    return utDenmTerminateResult(false, defaultAID);
}

QByteArray UpperTester::utDenmTerminateResult(bool success, ActionID_t actionID)
{
    return utDenmResult(UpperTesterPrimitives::PRIMITIVE_utDenmTerminateResult, success, actionID);
}

/* CAM Upper Tester Primitives */

QByteArray UpperTester::utCamTriggerResult(bool success)
{
    quint8 mt = UpperTesterPrimitives::PRIMITIVE_utCamTriggerResult;
    quint8 result = success;
    QByteArray res;
    QDataStream ds(&res, QIODevice::WriteOnly);
    ds << mt << result;
    return res;
}

QByteArray UpperTester::utChangeCurvature()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeCANProvider* paramCAN = (FakeCANProvider*) privateManager->hashMap["CAN"];
    qint16 curvature;
    get_INT16(data + 1, &curvature);
    paramCAN->changeCurvature(curvature);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utChangeSpeed()
{
    uchar* data = (uchar*)privateRequest.data();
    qint16 speed;
    get_INT16(data + 1, &speed);
    privateGPS->changeSpeed(speed);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetAccelerationControlStatus()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeCANProvider* paramCAN = (FakeCANProvider*) privateManager->hashMap["CAN"];
    quint8 accelerationControl = 0;
    get_UINT8(data + 1, &accelerationControl);
    paramCAN->setAccelerationControl(accelerationControl);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetExteriorLightsStatus()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeCANProvider* paramCAN = (FakeCANProvider*) privateManager->hashMap["CAN"];
    quint8 exteriorLights = 0;
    get_UINT8(data + 1, &exteriorLights);
    paramCAN->setExteriorLights(exteriorLights);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utChangeHeading()
{
    uchar* data = (uchar*)privateRequest.data();
    quint16 heading;
    get_UINT16(data + 1, &heading);
    privateGPS->changeHeading(heading);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetDriveDirection()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeCANProvider* paramCAN = (FakeCANProvider*) privateManager->hashMap["CAN"];
    quint8 driveDirection;
    get_UINT8(data + 1, &driveDirection);
    paramCAN->setDriveDirection(driveDirection);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utChangeYawRate()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeCANProvider* paramCAN = (FakeCANProvider*) privateManager->hashMap["CAN"];
    qint16 yawRate;
    get_INT16(data + 1, &yawRate);
    paramCAN->changeYawRate(yawRate);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetStationType()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeApplicationProvider* paramAppli = (FakeApplicationProvider*) privateManager->hashMap["Application"];
    quint8 stationType;
    get_UINT8(data + 1, &stationType);
    paramAppli->setStationType(stationType);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetVehicleRole()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeApplicationProvider* paramAppli = (FakeApplicationProvider*) privateManager->hashMap["Application"];
    quint8 vehicleRole;
    get_UINT8(data + 1, &vehicleRole);
    paramAppli->setVehicleRole(vehicleRole);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetEmbarkationStatus()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeApplicationProvider* paramAppli = (FakeApplicationProvider*) privateManager->hashMap["Application"];
    quint8 embarkationStatus;
    get_UINT8(data + 1, &embarkationStatus);
    embarkationStatus = embarkationStatus>1?1:0;
    paramAppli->setEmbarkationStatus(embarkationStatus);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetPtActivation()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeApplicationProvider* paramAppli = (FakeApplicationProvider*) privateManager->hashMap["Application"];
    quint64 offset = 1;
    quint8 ptActivationType;
    offset += get_UINT8(data + offset, &ptActivationType);
    quint8 ptActivationLength;
    offset += get_UINT8(data + offset, &ptActivationLength);
    uchar* ptActivationData = (uchar*)calloc(1,ptActivationLength);
    ptActivationData = data + offset;
    paramAppli->setPtActivation(ptActivationType, ptActivationLength, ptActivationData);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetDangerousGoods()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeApplicationProvider* paramAppli = (FakeApplicationProvider*) privateManager->hashMap["Application"];
    quint8 dangerousGoods;
    get_UINT8(data + 1, &dangerousGoods);
    paramAppli->setDangerousGoods(dangerousGoods);
    return utCamTriggerResult(true);
}

QByteArray UpperTester::utSetLightBarSiren()
{
    uchar* data = (uchar*)privateRequest.data();
    FakeCANProvider* paramCAN = (FakeCANProvider*) privateManager->hashMap["CAN"];
    quint8 lightBarSiren = 0;
    get_UINT8(data + 1, &lightBarSiren);
    paramCAN->setLightBarSirenInUse(lightBarSiren);
    return utCamTriggerResult(true);
}

/* GeoNetworking Upper Tester Primitives */

QByteArray UpperTester::utGnTriggerResult(bool success)
{
    quint8 mt = UpperTesterPrimitives::PRIMITIVE_utGnTriggerResult;
    quint8 result = success;
    QByteArray res;
    QDataStream ds(&res, QIODevice::WriteOnly);
    ds << mt << result;
    return res;
}

QByteArray UpperTester::sendGUC()
{
    printf("\n\n\n\n\ncalling sendGUC\n");
    gnDataRequest_t gnDR;
    uchar* data = (uchar*)privateRequest.data();
    quint64 offset = 1;

    gnDR.packetTransportType = CommonHeader::headerType_geoUnicast;
    gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoUnicast_unspecified;

    gnDR.destinationPort = 0;
    gnDR.destinationPortInfo = 0;
    gnDR.gnCommunicationProfile = gnCommunicationProfile_ITS_G5A;
    gnDR.repetitionInterval = 0;
    gnDR.maximumRepetitionTime = 0;
    gnDR.maximumHopLimit = 0;

    gnDR.destinationAddress.present = destinationAddress_PR_MID;

    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.gnAddr.stationType_StationCountryCode);

    for(int i=0; i<6; i++)
    {
        gnDR.destinationAddress.choice.gnAddr.mid[i] = data[offset];
        offset++;
    }

    quint16 totalLt;
    offset += get_UINT16(data + offset, &totalLt);

    LifeTime lt;

    if(totalLt%100000 == 0)
    {
        lt = LifeTime(totalLt / 100000, LifeTime::base_100_s);
    }
    else if(totalLt%10000 == 0)
    {
        lt = LifeTime(totalLt / 10000, LifeTime::base_10_s);
    }
    else if(totalLt/1000 == 0)
    {
        lt = LifeTime(totalLt % 1000, LifeTime::base_1_s);
    }
    else if(totalLt%50 == 0)
    {
        lt = LifeTime(totalLt / 50, LifeTime::base_50_ms);
    }
    else
    {
        printf("UpperTester : Fail to send GUC -> lifeTime error\n");
        return utGnTriggerResult(false);
    }

    gnDR.maximumPacketLifeTime = (LifeTime*)calloc(1, sizeof lt);
    *gnDR.maximumPacketLifeTime = lt;

    quint8 tmpTc;
    offset += get_UINT8(data + offset, &tmpTc);

    gnDR.trafficClass = TrafficClass(tmpTc);

    quint16 payloadLen;
    offset += get_UINT16(data + offset, &payloadLen);
    gnDR.payload = QByteArray((char*)data + offset, payloadLen);

    gnDR.upperProtocolEntity = CommonHeader::nextHeaderType_any;

    int res = privateGeoNet->send(gnDR);
    printf("sendGUC result : %s\n\n\n\n\n\n", privateGeoNet->geonetErrors[res].toStdString().c_str());
    return (res==GeoNet::networkError_success || res==GeoNet::geonetError_send_guc_bufferedPacket || res==GeoNet::geonetError_send_LSRequest_buffered)?utGnTriggerResult(true):utGnTriggerResult(false);
}

QByteArray UpperTester::sendGBC()
{
    printf("\n\n\n\n\ncalling sendGBC\n");
    gnDataRequest_t gnDR;
    uchar* data = (uchar*)privateRequest.data();
    quint64 offset = 1;

    gnDR.packetTransportType = CommonHeader::headerType_geoBroadcast;
    quint8 shape;
    offset += get_UINT8(data + offset, &shape);
    switch(shape)
    {
    case upperTester_circle:
        gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoBroadcast_circle;
        break;
    case upperTester_rectangle:
        gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoBroadcast_rectangle;
        break;
    case upperTester_ellipse:
        gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoBroadcast_ellipse;
        break;
    default:
        printf("UpperTester : Fail to send GBC -> headerSubType error\n");
        return utGnTriggerResult(false);
        break;
    }

    quint16 totalLt;
    offset += get_UINT16(data + offset, &totalLt);

    LifeTime lt;

    if(totalLt%100000 == 0)
    {
        lt = LifeTime(totalLt / 100000, LifeTime::base_100_s);
    }
    else if(totalLt%10000 == 0)
    {
        lt = LifeTime(totalLt / 10000, LifeTime::base_10_s);
    }
    else if(totalLt/1000 == 0)
    {
        lt = LifeTime(totalLt % 1000, LifeTime::base_1_s);
    }
    else if(totalLt%50 == 0)
    {
        lt = LifeTime(totalLt / 50, LifeTime::base_50_ms);
    }
    else
    {
        printf("UpperTester : Fail to send GBC -> lifeTime error\n");
        return utGnTriggerResult(false);
    }

    gnDR.maximumPacketLifeTime = (LifeTime*)calloc(1, sizeof lt);
    *gnDR.maximumPacketLifeTime = lt;

    quint8 tmpTc;
    offset += get_UINT8(data + offset, &tmpTc);

    gnDR.trafficClass = TrafficClass(tmpTc);
    offset += 3;

    gnDR.destinationPort = 0;
    gnDR.destinationPortInfo = 0;
    gnDR.gnCommunicationProfile = gnCommunicationProfile_ITS_G5A;
    gnDR.repetitionInterval = 0;
    gnDR.maximumRepetitionTime = 0;
    gnDR.maximumHopLimit = 0;

    gnDR.destinationAddress.present = destinationAddress_PR_DestinationArea;
    offset += get_INT32(data + offset, &gnDR.destinationAddress.choice.destinationArea.geoAreaPosLatitude);
    offset += get_INT32(data + offset, &gnDR.destinationAddress.choice.destinationArea.geoAreaPosLongitude);
    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.destinationArea.distanceA);
    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.destinationArea.distanceB);
    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.destinationArea.angle);
    quint16 payloadLen;
    offset += get_UINT16(data + offset, &payloadLen);
    gnDR.payload = QByteArray((char*)data + offset, payloadLen);
/*
    print_hexa((const unsigned char*)DestinationArea(gnDR.destinationAddress.choice.destinationArea).toHex().data(), DestinationArea(gnDR.destinationAddress.choice.destinationArea).toHex().length());
    printf("Lt :");
    print_hexa((const unsigned char*)gnDR.maximumPacketLifeTime->toHex().toStdString().c_str(), gnDR.maximumPacketLifeTime->toHex().length());
    printf("subType : %x\n", gnDR.packetTransportSubType);
    printf("TC : %s\n", gnDR.trafficClass.toHex().toStdString().c_str());
    printf("payload : %s\n", gnDR.payload.toHex().toStdString().c_str());
*/
    gnDR.upperProtocolEntity = CommonHeader::nextHeaderType_any;

    int res = privateGeoNet->send(gnDR);
    printf("sendGBC result : %s\n\n\n\n\n\n", privateGeoNet->geonetErrors[res].toStdString().c_str());
    return (res==GeoNet::networkError_success || res==GeoNet::geonetError_send_gbc_bufferedPacket)?utGnTriggerResult(true):utGnTriggerResult(false);
}

QByteArray UpperTester::sendGAC()
{
    printf("\n\n\n\n\ncalling sendGAC\n");
    gnDataRequest_t gnDR;
    uchar* data = (uchar*)privateRequest.data();
    quint64 offset = 1;

    gnDR.packetTransportType = CommonHeader::headerType_geoAnycast;
    quint8 shape;
    offset += get_UINT8(data + offset, &shape);
    switch(shape)
    {
    case upperTester_circle:
        gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoAnycast_circle;
        break;
    case upperTester_rectangle:
        gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoAnycast_rectangle;
        break;
    case upperTester_ellipse:
        gnDR.packetTransportSubType = CommonHeader::headerSubTypeGeoAnycast_ellipse;
        break;
    default:
        printf("UpperTester : Fail to send GAC -> headerSubType error\n");
        return utGnTriggerResult(false);
        break;
    }

    quint16 totalLt;
    offset += get_UINT16(data + offset, &totalLt);

    LifeTime lt;

    if(totalLt%100000 == 0)
    {
        lt = LifeTime(totalLt / 100000, LifeTime::base_100_s);
    }
    else if(totalLt%10000 == 0)
    {
        lt = LifeTime(totalLt / 10000, LifeTime::base_10_s);
    }
    else if(totalLt/1000 == 0)
    {
        lt = LifeTime(totalLt % 1000, LifeTime::base_1_s);
    }
    else if(totalLt%50 == 0)
    {
        lt = LifeTime(totalLt / 50, LifeTime::base_50_ms);
    }
    else
    {
        printf("UpperTester : Fail to send GAC -> lifeTime error\n");
        return utGnTriggerResult(false);
    }

    gnDR.maximumPacketLifeTime = (LifeTime*)calloc(1, sizeof lt);
    *gnDR.maximumPacketLifeTime = lt;

    quint8 tmpTc;
    offset += get_UINT8(data + offset, &tmpTc);

    gnDR.trafficClass = TrafficClass(tmpTc);
    offset += 3;

    gnDR.destinationPort = 0;
    gnDR.destinationPortInfo = 0;
    gnDR.gnCommunicationProfile = gnCommunicationProfile_ITS_G5A;
    gnDR.repetitionInterval = 0;
    gnDR.maximumRepetitionTime = 0;
    gnDR.maximumHopLimit = 0;

    gnDR.destinationAddress.present = destinationAddress_PR_DestinationArea;
    offset += get_INT32(data + offset, &gnDR.destinationAddress.choice.destinationArea.geoAreaPosLatitude);
    offset += get_INT32(data + offset, &gnDR.destinationAddress.choice.destinationArea.geoAreaPosLongitude);
    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.destinationArea.distanceA);
    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.destinationArea.distanceB);
    offset += get_UINT16(data + offset, &gnDR.destinationAddress.choice.destinationArea.angle);
    quint16 payloadLen;
    offset += get_UINT16(data + offset, &payloadLen);
    gnDR.payload = QByteArray((char*)data + offset, payloadLen);
/*
    print_hexa((const unsigned char*)DestinationArea(gnDR.destinationAddress.choice.destinationArea).toHex().data(), DestinationArea(gnDR.destinationAddress.choice.destinationArea).toHex().length());
    printf("Lt :");
    print_hexa((const unsigned char*)gnDR.maximumPacketLifeTime->toHex().toStdString().c_str(), gnDR.maximumPacketLifeTime->toHex().length());
    printf("subType : %x\n", gnDR.packetTransportSubType);
    printf("TC : %s\n", gnDR.trafficClass.toHex().toStdString().c_str());
    printf("payload : %s\n", gnDR.payload.toHex().toStdString().c_str());
*/
    gnDR.upperProtocolEntity = CommonHeader::nextHeaderType_any;

    int res = privateGeoNet->send(gnDR);
    printf("sendGAC result : %s\n\n\n\n\n\n", privateGeoNet->geonetErrors[res].toStdString().c_str());
    return (res==GeoNet::networkError_success || res==GeoNet::geonetError_send_gac_unicast_bufferedPacket || res==GeoNet::geonetError_send_gac_broadcast_bufferedPacket)?utGnTriggerResult(true):utGnTriggerResult(false);
}

QByteArray UpperTester::sendSHB()
{
    printf("\n\n\n\n\ncalling sendSHB\n");
    gnDataRequest_t gnDR;
    uchar* data = (uchar*)privateRequest.data();
    quint64 offset = 1;

    gnDR.packetTransportType = CommonHeader::headerType_topologicallyScopedBroadcast;
    gnDR.packetTransportSubType = CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop;

    gnDR.maximumPacketLifeTime = 0;

    quint8 tmpTc;
    offset += get_UINT8(data + offset, &tmpTc);
    gnDR.trafficClass = TrafficClass(tmpTc);

    gnDR.destinationPort = 0;
    gnDR.destinationPortInfo = 0;
    gnDR.gnCommunicationProfile = gnCommunicationProfile_ITS_G5A;
    gnDR.repetitionInterval = 0;
    gnDR.maximumRepetitionTime = 0;
    gnDR.maximumHopLimit = 0;

    gnDR.destinationAddress.present = destinationAddress_PR_UNKNOWN;

    quint16 payloadLen;
    offset += get_UINT16(data + offset, &payloadLen);
    gnDR.payload = QByteArray((char*)data + offset, payloadLen);

    gnDR.upperProtocolEntity = CommonHeader::nextHeaderType_any;

    int res = privateGeoNet->send(gnDR);
    printf("sendSHB result : %d\n\n\n\n\n\n", res);
    return res==GeoNet::networkError_success?utGnTriggerResult(true):utGnTriggerResult(false);
}

QByteArray UpperTester::sendTSB()
{
    printf("\n\n\n\n\ncalling sendTSB\n");
    gnDataRequest_t gnDR;
    uchar* data = (uchar*)privateRequest.data();
    quint64 offset = 1;

    gnDR.packetTransportType = CommonHeader::headerType_topologicallyScopedBroadcast;
    gnDR.packetTransportSubType = CommonHeader::headerSubTypeTopologicallyScopedBroadcast_multiHop;

    gnDR.maximumPacketLifeTime = 0;

    quint8 tmpNbHop;
    offset += get_UINT8(data + offset, &tmpNbHop);
    gnDR.maximumHopLimit = (quint8*)calloc(1, sizeof tmpNbHop);
    *gnDR.maximumHopLimit = tmpNbHop;

    quint16 totalLt;
    offset += get_UINT16(data + offset, &totalLt);

    LifeTime lt;

    if(totalLt%100000 == 0)
    {
        lt = LifeTime(totalLt / 100000, LifeTime::base_100_s);
    }
    else if(totalLt%10000 == 0)
    {
        lt = LifeTime(totalLt / 10000, LifeTime::base_10_s);
    }
    else if(totalLt/1000 == 0)
    {
        lt = LifeTime(totalLt % 1000, LifeTime::base_1_s);
    }
    else if(totalLt%50 == 0)
    {
        lt = LifeTime(totalLt / 50, LifeTime::base_50_ms);
    }
    else
    {
        printf("UpperTester : Fail to send GBC -> lifeTime error\n");
        return utGnTriggerResult(false);
    }

    gnDR.maximumPacketLifeTime = (LifeTime*)calloc(1, sizeof lt);
    *gnDR.maximumPacketLifeTime = lt;

    quint8 tmpTc;
    offset += get_UINT8(data + offset, &tmpTc);
    gnDR.trafficClass = TrafficClass(tmpTc);

    gnDR.destinationPort = 0;
    gnDR.destinationPortInfo = 0;
    gnDR.gnCommunicationProfile = gnCommunicationProfile_ITS_G5A;
    gnDR.repetitionInterval = 0;
    gnDR.maximumRepetitionTime = 0;
    gnDR.maximumHopLimit = 0;

    gnDR.destinationAddress.present = destinationAddress_PR_UNKNOWN;

    quint16 payloadLen;
    offset += get_UINT16(data + offset, &payloadLen);
    gnDR.payload = QByteArray((char*)data + offset, payloadLen);

    gnDR.upperProtocolEntity = CommonHeader::nextHeaderType_any;

    int res = privateGeoNet->send(gnDR);
    printf("sendTSB result : %d\n\n\n\n\n\n", res);
    return res==GeoNet::networkError_success?utGnTriggerResult(true):utGnTriggerResult(false);
}