caBasicService.cpp

Go to the documentation of this file.

#include "caBasicService.h"

#include <QTimer>

static bool firstCAM = true;
#define T_GenCamMin 100
#define T_GenCamMax 1000
#define MSG_LENGTH  500

caBasicService::caBasicService(GeoNet* geonet, FakeGPSProvider* paramGPS, ParameterConfiguration* paramConf,
                               FakeApplicationProvider* paramAppli, FakeCANProvider* paramCAN)
{
    setN_GenCam(DEFAULT_N_GEN_CAM);
    setT_CheckGenCam(T_GenCamMax);
    firstCAM = true;
    m_geonet = geonet;
    m_paramGPS = paramGPS;
    m_paramConf = paramConf;
    m_paramAppli = paramAppli;
    m_paramCAN = paramCAN;
    udpSocket = new QUdpSocket;
    udpSocket->bind(QHostAddress::LocalHost);
    header.messageID = messageID_cam;
    header.protocolVersion = protocolVersion_currentVersion;
    header.stationID = m_paramAppli->data().stationID();
    m_T_GenCam = T_GenCamMax;
    m_T_GenCam_DCC = T_GenCamMax;

    QObject::connect(m_paramGPS, SIGNAL(positionChanged()), this, SLOT(checkToSendCAM()));
    QObject::connect(m_paramCAN, SIGNAL(canChanged()), this, SLOT(checkToSendCAM()));
}

void caBasicService::execute()
{
    qDebug() << "caBasicService start!\n";

    /* 1) Set T_CheckGenCam and start the timer. */
    m_quick = false;
    m_elapsed.start();
    m_timer.singleShot(T_GenCamMin, this, SLOT(checkToSendCAM()));
}

void caBasicService::utInitialize()
{
    firstCAM = true;
    m_timer.stop();
}

void caBasicService::reset()
{
    for(int i = 0 ; i < MAX_REFERENCEPOSITION ; i++)
        memset(&m_listPathPoint[i], 0, sizeof m_listPathPoint[i]);
    m_listReferencePosition.clear();
    m_listGenerationDeltaTime.clear();
    m_quick = false;
    m_T_GenCam = T_GenCamMax;
}

void caBasicService::checkToSendCAM()
{
    CAM_t cam;
    asn_enc_rval_t rval;
    char msg[MSG_LENGTH];
    int msgLength;

    m_timer.stop();

    /* 2) When the timer T_CheckGenCam expires, check the CAM generation conditions */
    if(checkCamGeneration())
    {
        qDebug() << "true";
        /*
         * 2)a. If any of the condition is satisfied, continues the operation
         *
         * 3) Collect data for mandatory containers.
         */
//        qDebug() << "Collect mandatory data";
        cam = collectMandatoryData();
        /* 4) Check if optional containers are to be added for CAM generation. */
//        qDebug() << "Check for optional containers";
        if(checkForOptionalContainers())
        {
            /*
             * 4)a. If yes, check the ITS-S type and ITS-S role and collect data for optional
             * containers.
             */
//            qDebug() << "Collect optional data";
            checkAndCollectDataForOptionalContainers(&cam);
            setLastLF(QTime::currentTime());
        }
        /*
         * 4)b. If no, continue the operation;
         * 5) Encode CAM.
         */
//        qDebug() << "Encode CAM";
        memset(&msg, '\0', MSG_LENGTH);
        rval = uper_encode_to_buffer(&asn_DEF_CAM, &cam, &msg, MSG_LENGTH);
        if(rval.encoded < 1)
        {
            qCritical("Failed to encode %s\n", rval.failed_type->name);
        }
        else
        {
            /* 6). Pass CAM to the ITS networking & transport layer. */
//            qDebug() << "Pass CAM to the ITS networking & transport layer";
            msgLength = (rval.encoded + 7) / 8;
            PassCamToNetworkAndTransportLayer(msg, msgLength);
            firstCAM = false;
            m_previousCAM = cpy_CAM(cam);
            setPreviousTime(QTime::currentTime());
            m_elapsed.start();
            m_timer.singleShot(T_GenCamMin, this, SLOT(checkToSendCAM()));
        }
    }
    else
    {
        m_timer.singleShot(T_GenCamMin, this, SLOT(checkToSendCAM()));
    }
}

void caBasicService::PassCamToNetworkAndTransportLayer(char *cam, int camLength)
{
    QByteArray payload(cam, camLength);

    LifeTime* lt = new LifeTime(1, LifeTime::base_1_s);

    gnDataRequest_t packet;
    packet.upperProtocolEntity = CommonHeader::nextHeaderType_BTP_B;
    packet.packetTransportType = CommonHeader::headerType_topologicallyScopedBroadcast;
    packet.packetTransportSubType = CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop;
    packet.destinationPort = btpPort_CAM;
    packet.destinationPortInfo = 0;
    packet.destinationAddress.present = destinationAddress_PR_UNKNOWN;
    packet.gnCommunicationProfile = gnCommunicationProfile_ITS_G5A;
    packet.maximumPacketLifeTime = lt;
    packet.repetitionInterval = 0;
    packet.maximumRepetitionTime = 0;
    packet.maximumHopLimit = 0;
    packet.trafficClass = itsGnDefaultTrafficClass;
    packet.payload = payload;

    if(m_geonet != 0)
        m_geonet->send(packet);
    m_elapsed.start();
    qDebug() << "Message sent to the ITS networking & transport layer:";
    //print_hexa((const unsigned char*)cam, camLength);
    qDebug() << "Of size" << camLength;
}

bool caBasicService::checkCamGeneration()
{

    /* First CAM to send. */
    if(firstCAM)
    {
        reset();
        return true;
    }
    /*
     * The CAM generation trigger conditions shall be as follow:
     * 1) The time elapsed since the last CAM generation is equal to or greater than T_GenCam_DCC…
     */
    quint64 elapTime = m_elapsed.elapsed();
    if(elapTime >= T_GenCamMin)
    {
        if(!m_paramGPS)
            return false;

        int prevH = previousCAM().cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue,
                currentH = m_paramGPS->data().heading().headingValue;
        int minH = MIN(prevH, currentH), maxH = minH == prevH ? currentH : prevH,
         offsetH = MIN((maxH - minH), (3600 - maxH + minH));
        /*
         * … and one of the following ITS-S dynamics related conditions is given:
         * - the absolute difference between the current heading of the originating ITS-S and
         *   the heading including in the CAM previously transmitted by the originating ITS-S
         *   exceeds 4°;
         */
        if(offsetH > 40)
        {
            m_T_GenCam = elapTime;
            m_generedCam = 1;
            m_quick = true;
            return true;
        }
        /*
         * - the distance between the current position of the originating ITS-S and the position
         *   included in the CAM previously transmitted by the originating ITS-S exceed 4 m;
         */
        if(dist(m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude,
                previousCAM().cam.camParameters.basicContainer.referencePosition.latitude,
                previousCAM().cam.camParameters.basicContainer.referencePosition.longitude) > 4)
        {
            m_T_GenCam = elapTime;
            m_generedCam = 1;
            m_quick = true;
            return true;
        }
        /*
         * - the absolute difference between the current speed of the originating ITS-S and the speed
         *   included in the CAM previously transmitted by the originating ITS-S exceed 0,5 m/s.
         */
        if(abs(m_paramGPS->data().speed().speedValue - previousCAM().cam.camParameters.highFrequencyContainer.choice.
               basicVehicleContainerHighFrequency.speed.speedValue) > 50)
        {
            m_T_GenCam = elapTime;
            m_generedCam = 1;
            m_quick = true;
            return true;
        }

        if(m_quick && elapTime >= m_T_GenCam)
        {
            m_generedCam++;
            if(m_generedCam > m_N_GenCam - 1)
            {
                m_generedCam = 0;
                m_T_GenCam = T_GenCamMax;
                m_quick = false;
            }
            return true;
        }
    }

    /*
     * 2) The time elapsed since the last CAM generation is equal to or greater than T_GenCam and
     *    equal to or greater than T_GenCam_DCC.
     */
    // TODO: rechercher tgencamdcc dans paramcam
    if(elapTime >= m_T_GenCam && elapTime >= T_GenCam_DCC())
        return true;

    return false;
}

CAM_t caBasicService::collectMandatoryData()
{
    CAM_t c = make_CAM();

    if(m_paramAppli)
        c.header.stationID = m_paramAppli->data().stationID();
    if(m_paramGPS)
        c.cam.generationDeltaTime = m_paramGPS->timestamp() % 65536;

    if(!firstCAM)
    {
        if(m_listGenerationDeltaTime.count() >= MAX_REFERENCEPOSITION)
            m_listGenerationDeltaTime.removeLast();
        m_listGenerationDeltaTime.prepend(c.cam.generationDeltaTime);
    }

    c.cam.camParameters.basicContainer = collectMandatoryData_BasicContainer();
    switch (c.cam.camParameters.basicContainer.stationType)
    {
    case StationType_passengerCar:
        c.cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
        c.cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency = collectMandatoryData_BasicVehicleContainerHighFrequency();
        break;
    case StationType_roadSideUnit:
        c.cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
        c.cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency = collectMandatoryData_RSUContainerHighFrequency();
        break;
    case StationType_pedestrian:
    case StationType_cyclist:
    case StationType_moped:
    case StationType_motorcycle:
    case StationType_bus:
    case StationType_lightTruck:
    case StationType_heavyTruck:
    case StationType_trailer:
    case StationType_specialVehicles:
    case StationType_tram:
    default:
        c.cam.camParameters.highFrequencyContainer = make_HighFrequencyContainer(HighFrequencyContainer_PR_NOTHING);
        break;
    }
    c.cam.camParameters.lowFrequencyContainer = 0;
    c.cam.camParameters.specialVehicleContainer = 0;

    return c;
}

BasicContainer_t caBasicService::collectMandatoryData_BasicContainer()
{
    BasicContainer_t b = make_BasicContainer();

    if(m_paramAppli)
        b.stationType = m_paramAppli->data().stationType();
    if(m_paramGPS)
        b.referencePosition = m_paramGPS->data().referencePosition();

    if(!firstCAM)
    {
        if(m_listReferencePosition.count() >= MAX_REFERENCEPOSITION)
            m_listReferencePosition.removeLast();
        m_listReferencePosition.prepend(b.referencePosition);
    }

    return b;
}

BasicVehicleContainerHighFrequency_t caBasicService::collectMandatoryData_BasicVehicleContainerHighFrequency()
{
    BasicVehicleContainerHighFrequency_t b = make_BasicVehicleContainerHighFrequency();

    if(m_paramGPS)
    {
        b.heading = m_paramGPS->data().heading();
        b.speed = m_paramGPS->data().speed();
    }
    if(m_paramCAN)
    {
        b.driveDirection = m_paramCAN->data().driveDirection();
        b.vehicleLength = m_paramConf->vehicleLength();
        b.vehicleWidth = m_paramConf->vehicleWidth();
        b.longitudinalAcceleration = m_paramCAN->data().longitudinalAcceleration();
        b.curvature = m_paramCAN->data().curvature();
        b.yawRate = m_paramCAN->data().yawRate();
        b.curvatureCalculationMode = m_paramCAN->data().curvatureCalculationMode();

        if(m_paramCAN->data().accelerationControl() != 0x0)
        {
            b.accelerationControl = (AccelerationControl_t*)calloc(1, sizeof *b.accelerationControl);
            *b.accelerationControl = cpy_AccelerationControl(*m_paramCAN->data().accelerationControl());
        }

        b.steeringWheelAngle = m_paramCAN->data().steeringWheelAngle();

        if(m_paramCAN->data().lateralAcceleration()!= 0x0)
        {
            b.lateralAcceleration = (LateralAcceleration_t*)calloc(1, sizeof *b.lateralAcceleration);
            *b.lateralAcceleration = cpy_LateralAcceleration(*m_paramCAN->data().lateralAcceleration());
        }

        if(m_paramCAN->data().verticalAcceleration()!= 0x0)
        {
            b.verticalAcceleration = (VerticalAcceleration_t*)calloc(1, sizeof *b.verticalAcceleration);
            *b.verticalAcceleration = cpy_VerticalAcceleration(*m_paramCAN->data().verticalAcceleration());
        }
    }

    b.lanePosition = 0;

    b.performanceClass = 0;
    b.cenDsrcTollingZone = 0;

    return b;
}

RSUContainerHighFrequency_t caBasicService::collectMandatoryData_RSUContainerHighFrequency()
{
    RSUContainerHighFrequency_t r;

    memset (&r, 0, sizeof r);

    if(m_paramAppli->data().protectedCommunicationZones())
        r.protectedCommunicationZonesRSU = m_paramAppli->data().protectedCommunicationZones();

    return r;
}

bool caBasicService::checkForOptionalContainers()
{
    if(m_paramAppli->data().stationType() != StationType_roadSideUnit)
    {
        if(firstCAM)
            return true;
        if(abs(QTime::currentTime().msecsTo(lastLF())) >= 500)
            return true;
    }
    return false;
}

void caBasicService::checkAndCollectDataForOptionalContainers(CAM_t *cam)
{
    cam->cam.camParameters.lowFrequencyContainer =
            (LowFrequencyContainer_t*)calloc(1, sizeof *cam->cam.camParameters.lowFrequencyContainer);
    switch(cam->cam.camParameters.basicContainer.stationType)
    {
    case StationType_passengerCar:
        cam->cam.camParameters.lowFrequencyContainer->present =
                LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
        cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency =
                collectOptionalData_BasicVehicleContainerLowFrequency();
        break;
    case StationType_pedestrian:
    case StationType_cyclist:
    case StationType_moped:
    case StationType_motorcycle:
    case StationType_bus:
    case StationType_lightTruck:
    case StationType_heavyTruck:
    case StationType_trailer:
    case StationType_specialVehicles:
    case StationType_tram:
    case StationType_roadSideUnit:
    default:
        cam->cam.camParameters.lowFrequencyContainer = 0;
        break;
    }

    if(cam->cam.camParameters.lowFrequencyContainer)
    {
        if(needSpecialVehicleContainer(
                    cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole))
        {
            cam->cam.camParameters.specialVehicleContainer = (SpecialVehicleContainer_t*)calloc(1,
                                                             sizeof *cam->cam.camParameters.specialVehicleContainer);
            *cam->cam.camParameters.specialVehicleContainer = collectOptionalData_SpecialVehicleContainer(
                        cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole);
        }
        else
            cam->cam.camParameters.specialVehicleContainer = 0;
    }
}

bool caBasicService::needSpecialVehicleContainer(VehicleRole_t role)
{
    switch (role)
    {
    case VehicleRole_publicTransport:
    case VehicleRole_specialTransport:
    case VehicleRole_dangerousGoods:
    case VehicleRole_roadWork:
    case VehicleRole_rescue:
    case VehicleRole_emergency:
    case VehicleRole_safetyCar:
        return true;
        break;
    case VehicleRole_default:
    case VehicleRole_agriculture:
    case VehicleRole_commercial:
    case VehicleRole_military:
    case VehicleRole_roadOperator:
    case VehicleRole_taxi:
    case VehicleRole_reserved1:
    case VehicleRole_reserved2:
    case VehicleRole_reserved3:
    default:
        break;
    }
    return false;
}

BasicVehicleContainerLowFrequency_t caBasicService::collectOptionalData_BasicVehicleContainerLowFrequency()
{
    BasicVehicleContainerLowFrequency_t b = make_BasicVehicleContainerLowFrequency();

    if(m_paramAppli)
        b.vehicleRole = m_paramAppli->data().vehicleRole();
    if(m_paramCAN)
        b.exteriorLights = m_paramCAN->data().exteriorLights();
    b.pathHistory = getPathHistory();

    return b;
}

SpecialVehicleContainer_t caBasicService::collectOptionalData_SpecialVehicleContainer(
        VehicleRole_t v)
{
    SpecialVehicleContainer_t s;

    switch (v)
    {
    case VehicleRole_publicTransport:
        s.present = SpecialVehicleContainer_PR_publicTransportContainer;
        s.choice.publicTransportContainer = collectOptionalData_PublicTransportContainer();
        break;
    case VehicleRole_specialTransport:
        s.present = SpecialVehicleContainer_PR_specialTransportContainer;
        s.choice.specialTransportContainer = collectOptionalData_SpecialTransportContainer();
        break;
    case VehicleRole_dangerousGoods:
        s.present = SpecialVehicleContainer_PR_dangerousGoodsContainer;
        s.choice.dangerousGoodsContainer = collectOptionalData_DangerousGoodsContainer();
        break;
    case VehicleRole_roadWork:
        s.present = SpecialVehicleContainer_PR_roadWorksContainerBasic;
        s.choice.roadWorksContainerBasic = collectOptionalData_RoadWorksContainerbasic();
        break;
    case VehicleRole_rescue:
        s.present = SpecialVehicleContainer_PR_rescueContainer;
        s.choice.rescueContainer = collectOptionalData_RescueContainer();
        break;
    case VehicleRole_emergency:
        s.present = SpecialVehicleContainer_PR_emergencyContainer;
        s.choice.emergencyContainer = collectOptionalData_EmergencyContainer();
        break;
    case VehicleRole_safetyCar:
        s.present = SpecialVehicleContainer_PR_safetyCarContainer;
        s.choice.safetyCarContainer = collectOptionalData_SafetyCarContainer();
        break;
    default:
        break;
    }

    return s;
}

PublicTransportContainer_t caBasicService::collectOptionalData_PublicTransportContainer()
{
    PublicTransportContainer_t p = make_PublicTransportContainer();

    if(m_paramAppli)
    {
        p.embarkationStatus = m_paramAppli->data().embarkationStatus();
        p.ptActivation = m_paramAppli->data().ptActivation();
    }

    return p;
}

SpecialTransportContainer_t caBasicService::collectOptionalData_SpecialTransportContainer()
{
    SpecialTransportContainer_t s = make_SpecialTransportContainer();

    if(m_paramAppli)
        s.specialTransportType = m_paramAppli->data().specialTransportType();
    if(m_paramCAN)
        s.lightBarSirenInUse = m_paramCAN->data().lightBarSirenInUse();

    return s;
}

DangerousGoodsContainer_t caBasicService::collectOptionalData_DangerousGoodsContainer()
{
    DangerousGoodsContainer_t d = make_DangerousGoodsContainer();

    if(m_paramAppli)
        d.dangerousGoodsBasic = m_paramAppli->data().dangerousGoodsBasic();

    return d;
}

RoadWorksContainerBasic_t caBasicService::collectOptionalData_RoadWorksContainerbasic()
{
    RoadWorksContainerBasic_t r = make_RoadWorksContainerBasic();

    if(m_paramAppli)
    {
        r.roadworksSubCauseCode = m_paramAppli->data().roadworksSubCauseCode();
        r.closedLanes = m_paramAppli->data().closedLanes();
    }
    if(m_paramCAN)
        r.lightBarSirenInUse = m_paramCAN->data().lightBarSirenInUse();

    return r;
}

RescueContainer_t caBasicService::collectOptionalData_RescueContainer()
{
    RescueContainer_t r = make_RescueContainer();

    if(m_paramCAN)
        r.lightBarSirenInUse = m_paramCAN->data().lightBarSirenInUse();

    return r;
}

EmergencyContainer_t caBasicService::collectOptionalData_EmergencyContainer()
{
    EmergencyContainer_t e = make_EmergencyContainer();

    if(m_paramCAN)
        e.lightBarSirenInUse = m_paramCAN->data().lightBarSirenInUse();
    if(m_paramAppli)
    {
        e.incidentIndication = m_paramAppli->data().causeCodeEmergency();
        e.emergencyPriority = m_paramAppli->data().emergencyPriority();
    }

    return e;
}

SafetyCarContainer_t caBasicService::collectOptionalData_SafetyCarContainer()
{
    SafetyCarContainer_t s = make_SafetyCarContainer();

    if(m_paramCAN)
        s.lightBarSirenInUse = m_paramCAN->data().lightBarSirenInUse();
    if(m_paramAppli)
    {
        s.incidentIndication = m_paramAppli->data().causeCodeSafety();
        s.trafficRule = m_paramAppli->data().trafficRule();
        s.speedLimit = m_paramAppli->data().speedLimit();
    }

    return s;
}

PathHistory_t caBasicService::getPathHistory()
{
    int i;
    PathHistory_t ret;

    ret.list.array = (PathPoint_t**)calloc(1, sizeof *ret.list.array);
    ret.list.count = 0;
    ret.list.free = 0;
    ret.list.size = 0;
    for(i = 0 ; i < m_listReferencePosition.count() ; i++)
    {
        m_listPathPoint[i] = getPathPoint(i);
        checkPathPoint(i);
        ret.list.array[i] = (PathPoint_t*)calloc(1, sizeof *ret.list.array[i]);
        asn_sequence_add(&ret, &m_listPathPoint[i]);
    }

    return ret;
}

PathPoint_t caBasicService::getPathPoint(int i)
{
    PathPoint_t p;

    p.pathDeltaTime = (PathDeltaTime_t*)calloc(1, sizeof *p.pathDeltaTime);
    if(i)
    {
        *p.pathDeltaTime = abs(m_listGenerationDeltaTime.at(i) - m_listGenerationDeltaTime.at(i - 1)) / 10;
        *p.pathDeltaTime %= 65536;
        if(*p.pathDeltaTime == 0)
            *p.pathDeltaTime = 1;
        p.pathPosition.deltaLongitude = m_listReferencePosition.at(i).longitude -
                                       m_listReferencePosition.at(i - 1).longitude;
        p.pathPosition.deltaLatitude = m_listReferencePosition.at(i).latitude -
                                       m_listReferencePosition.at(i - 1).latitude;
        p.pathPosition.deltaAltitude = m_listReferencePosition.at(i).altitude.altitudeValue -
                                       m_listReferencePosition.at(i - 1).altitude.altitudeValue;
    }
    else
    {
        *p.pathDeltaTime = abs(m_listGenerationDeltaTime.at(i) - previousCAM().cam.generationDeltaTime) / 10;
        *p.pathDeltaTime %= 65536;
        if(*p.pathDeltaTime == 0)
            *p.pathDeltaTime = 1;
        p.pathPosition.deltaLongitude = m_listReferencePosition.at(i).longitude -
                previousCAM().cam.camParameters.basicContainer.referencePosition.longitude;
        p.pathPosition.deltaLatitude = m_listReferencePosition.at(i).latitude -
                previousCAM().cam.camParameters.basicContainer.referencePosition.latitude;
        p.pathPosition.deltaAltitude = m_listReferencePosition.at(i).altitude.altitudeValue -
                previousCAM().cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue;
    }

    return p;
}

void caBasicService::checkPathPoint(int i)
{
    if(m_listPathPoint[i].pathPosition.deltaLatitude < -131071 ||
       m_listPathPoint[i].pathPosition.deltaLatitude > 131072)
        m_listPathPoint[i].pathPosition.deltaLatitude = DeltaLatitude_unavailable;
    if(m_listPathPoint[i].pathPosition.deltaLongitude < -131071 ||
       m_listPathPoint[i].pathPosition.deltaLongitude > 131072)
        m_listPathPoint[i].pathPosition.deltaLongitude = DeltaLongitude_unavailable;
    if(m_listPathPoint[i].pathPosition.deltaAltitude < -12700 ||
       m_listPathPoint[i].pathPosition.deltaAltitude > 12800)
        m_listPathPoint[i].pathPosition.deltaAltitude = DeltaAltitude_unavailable;
}