ParameterCAN.cpp

Go to the documentation of this file.

#include "ParameterCAN.h"

ParameterCAN::ParameterCAN()
{
    read();
    readDebug();
}

ParameterCAN::ParameterCAN(QMutex* mutex)
{
    mutex->lock();
    read();
    readDebug();
    mutex->unlock();
}

ParameterCAN::~ParameterCAN()
{
}

void ParameterCAN::read()
{
    cfg_opt_t curvature_opts[] =
    {
        CFG_INT("curvatureConfidence", CurvatureConfidence_unavailable, CFGF_NONE),
        CFG_INT("curvatureValue", CurvatureValue_unavailable, CFGF_NONE),
        CFG_END()
    };

    cfg_opt_t lateralAcceleration_opts[] =
    {
        CFG_INT("lateralAccelerationConfidence", NULL, CFGF_IGNORE_UNKNOWN),
        CFG_INT("lateralAccelerationValue", NULL, CFGF_IGNORE_UNKNOWN),
        CFG_END()
    };

    cfg_opt_t longitudinalAcceleration_opts[] =
    {
        CFG_INT("longitudinalAccelerationConfidence", AccelerationConfidence_unavailable, CFGF_NONE),
        CFG_INT("longitudinalAccelerationValue", LongitudinalAccelerationValue_unavailable, CFGF_NONE),
        CFG_END()
    };

    cfg_opt_t steeringWheelAngle_opts[] =
    {
        CFG_INT("steeringWheelAngleConfidence", NULL, CFGF_IGNORE_UNKNOWN),
        CFG_INT("steeringWheelAngleValue", NULL, CFGF_IGNORE_UNKNOWN),
        CFG_END()
    };

    cfg_opt_t verticalAcceleration_opts[] =
    {
        CFG_INT("verticalAccelerationConfidence", NULL, CFGF_IGNORE_UNKNOWN),
        CFG_INT("verticalAccelerationValue", NULL, CFGF_IGNORE_UNKNOWN),
        CFG_END()
    };

    cfg_opt_t yawRate_opts[] =
    {
        CFG_INT("yawRateConfidence", YawRateConfidence_unavailable, CFGF_NONE),
        CFG_INT("yawRateValue", YawRateValue_unavailable, CFGF_NONE),
        CFG_END()
    };

    struct cfg_opt_t opts[] =
    {
        CFG_BOOL_LIST("accelerationControl", "", CFGF_NODEFAULT | CFGF_IGNORE_UNKNOWN),

        CFG_SEC("curvature", curvature_opts, CFGF_NO_TITLE_DUPES),

        CFG_INT("driveDirection", DriveDirection_unavailable, CFGF_NONE),

        CFG_BOOL_LIST("exteriorLights", "{false, false, false, false, false, false, false, false}", CFGF_NONE),

        CFG_INT("externalTemperature", -10000, CFGF_IGNORE_UNKNOWN),

        CFG_SEC("lateralAcceleration", lateralAcceleration_opts, CFGF_NO_TITLE_DUPES),

        CFG_BOOL_LIST("lightBarSirenInUse", "{false, false}", CFGF_NONE),

        CFG_SEC("longitudinalAcceleration", longitudinalAcceleration_opts, CFGF_NO_TITLE_DUPES),

        CFG_INT("numberOfOccupants", -1, CFGF_IGNORE_UNKNOWN),

        CFG_INT("positioningSolutionType", -1, CFGF_IGNORE_UNKNOWN),

        CFG_BOOL_LIST("positionOfOccupants", "{false, false, false, true, false, false, false, false, true, false, false, false, false, true, false, false, false, false, true, false}", CFGF_NONE),

        CFG_SEC("steeringWheelAngle", steeringWheelAngle_opts, CFGF_MULTI),

        CFG_SEC("verticalAcceleration", verticalAcceleration_opts, CFGF_MULTI),

        CFG_SEC("yawRate", yawRate_opts, CFGF_NO_TITLE_DUPES),

        CFG_INT("speed", SpeedValue_unavailable, CFGF_NONE),

        CFG_END()
    };
    cfg_t *cfg;
    cfg_t *cfg_lateralAcceleration;
    cfg_t *cfg_longitudinalAcceleration;
    cfg_t *cfg_steeringWheelAngle;
    cfg_t *cfg_verticalAcceleration;
    cfg_t *cfg_yawRate;

    cfg = cfg_init(opts, CFGF_NONE);

    cfg_lateralAcceleration = cfg_getsec(cfg, "lateralAcceleration");

    cfg_longitudinalAcceleration = cfg_getsec(cfg, "longitudinalAcceleration");

    cfg_steeringWheelAngle = cfg_getsec(cfg, "steeringWheelAngle");

    cfg_verticalAcceleration = cfg_getsec(cfg, "verticalAcceleration");

    cfg_yawRate = cfg_getsec(cfg, "yawRate");

    if(cfg_lateralAcceleration)
        cfg_set_validate_func(cfg_lateralAcceleration, "lateralAccelerationConfidence", validate_unsigned_int);

    cfg_set_validate_func(cfg_longitudinalAcceleration, "longitudinalAccelerationConfidence", validate_unsigned_int);

    cfg_set_validate_func(cfg, "numberOfOccupants", validate_unsigned_int);

    cfg_set_validate_func(cfg, "positioningSolutionType", validate_unsigned_int);

    if(cfg_steeringWheelAngle)
        cfg_set_validate_func(cfg_steeringWheelAngle, "steeringWheelAngleConfidence", validate_unsigned_int);

    if(cfg_verticalAcceleration)
        cfg_set_validate_func(cfg_verticalAcceleration, "verticalAccelerationConfidence", validate_unsigned_int);

    cfg_set_validate_func(cfg_yawRate, "yawRateConfidence", validate_unsigned_int);

    cfg_add_searchpath(cfg, "/etc/its/config");
    if(cfg_parse(cfg, "can.conf") == CFG_PARSE_ERROR)
        printf("PARSE_ERROR\n");

    if(cfg_size(cfg, "accelerationControl"))
    {
        *m_accelerationControl = makeBitString(cfg_size(cfg,"accelerationControl"), cfg, "accelerationControl");
        //constr_AccelerationControl(m_accelerationControl);
    }
    else
        m_accelerationControl = 0;

    m_driveDirection = cfg_getint(cfg, "driveDirection");

    m_exteriorLights = makeBitString(cfg_size(cfg,"exteriorLights"), cfg, "exteriorLights");
    //constr_ExteriorLights(m_exteriorLights);

    if(cfg_getint(cfg, "externalTemperature") == -10000)
        m_externalTemperature = 0;
    else
    {
        m_externalTemperature = (Temperature_t*) calloc(1, sizeof(Temperature_t));
        *m_externalTemperature = cfg_getint(cfg, "externalTemperature");
        //constr_ExternalTemperature(m_externalTemperature);
    }

    if(cfg_size(cfg, "lateralAcceleration"))
    {
        m_lateralAcceleration = (LateralAcceleration*) calloc(1, sizeof(LateralAcceleration));
        m_lateralAcceleration->lateralAccelerationValue = cfg_getint(cfg_lateralAcceleration, "lateralAccelerationValue");
        m_lateralAcceleration->lateralAccelerationConfidence = cfg_getint(cfg_lateralAcceleration, "lateralAccelerationConfidence");
        //constr_LateralAcceleration(m_lateralAcceleration);
    }
    else
        m_lateralAcceleration = 0;

    m_lightBarSirenInUse = makeBitString(cfg_size(cfg,"lightBarSirenInUse"), cfg, "lightBarSirenInUse");
    //constr_LightBarSirenInUse(m_lightBarSirenInUse);

    m_longitudinalAcceleration.longitudinalAccelerationValue = cfg_getint(cfg_longitudinalAcceleration, "longitudinalAccelerationValue");
    m_longitudinalAcceleration.longitudinalAccelerationConfidence = cfg_getint(cfg_longitudinalAcceleration, "longitudinalAccelerationConfidence");
    //constr_LongitudinalAcceleration(m_longitudinalAcceleration);

    if(cfg_getint(cfg, "numberOfOccupants") == -1)
            m_numberOfOccupants = 0;
    else
    {
        m_numberOfOccupants = (NumberOfOccupants_t*) calloc(1, sizeof(NumberOfOccupants_t));
        *m_numberOfOccupants = cfg_getint(cfg, "numberOfOccupants");
        //constr_NumberOfOccupants(m_numberOfOccupants);
    }

    if(cfg_getint(cfg, "positioningSolutionType") == -1)
            m_positioningSolution = 0;
    else
    {
        m_positioningSolution = (PositioningSolutionType_t*) calloc(1, sizeof(PositioningSolutionType_t));
        *m_positioningSolution = cfg_getint(cfg, "positioningSolutionType");
        //constr_PositioningSolution(m_positioningSolution);
    }

    m_positionOfOccupants = makeBitString(cfg_size(cfg,"positionOfOccupants"), cfg, "positionOfOccupants");
    //constr_PositionOfOccupants(&m_positionOfOccupants);

    if(cfg_size(cfg, "steeringWheelAngle"))
    {
        m_steeringWheelAngle = (SteeringWheelAngle*) calloc(1, sizeof(SteeringWheelAngle));
        m_steeringWheelAngle->steeringWheelAngleValue = cfg_getint(cfg_steeringWheelAngle, "steeringWheelAngleValue");
        m_steeringWheelAngle->steeringWheelAngleConfidence = cfg_getint(cfg_steeringWheelAngle, "steeringWheelAngleConfidence");
        //constr_SteeringWheelAngle(m_steeringWheelAngle);
    }
    else
        m_steeringWheelAngle = 0;

    if(cfg_size(cfg, "verticalAcceleration"))
    {
        m_verticalAcceleration = (VerticalAcceleration*) calloc(1, sizeof(VerticalAcceleration));
        m_verticalAcceleration->verticalAccelerationValue = cfg_getint(cfg_verticalAcceleration, "verticalAccelerationValue");
        m_verticalAcceleration->verticalAccelerationConfidence = cfg_getint(cfg_verticalAcceleration, "verticalAccelerationConfidence");
        //constr_VerticalAcceleration(m_verticalAcceleration);
    }
    else
        m_verticalAcceleration = 0;

    m_yawRate.yawRateConfidence = cfg_getint(cfg_yawRate, "yawRateConfidence");
    m_yawRate.yawRateValue = cfg_getint(cfg_yawRate, "yawRateValue");
    //constr_yawRate(m_yawRate);

    m_speed = cfg_getint(cfg, "speed");

    if(m_yawRate.yawRateValue != YawRateValue_unavailable &&
       m_yawRate.yawRateConfidence != YawRateConfidence_outOfRange &&
       m_yawRate.yawRateConfidence != YawRateConfidence_unavailable)
    {
        m_curvatureCalculationMode = CurvatureCalculationMode_yawRateUsed;
        m_curvature = calculateCurvatureWithYawRate();
    }
    else
    {
        m_curvatureCalculationMode = CurvatureCalculationMode_unavailable;
        m_curvature = curvatureUnavailable();
    }

    cfg_free(cfg);
}

void ParameterCAN::readDebug()
{
    printf("-----------------------------------------------------------------\n\t\t\tReadCan\n\n");
    if(m_accelerationControl != 0)
    {
        printf("accelerationControl = ");
        printf("%s", printBitString(*m_accelerationControl).toStdString().c_str());
        printf("\n\n");
    }

    printf("curvature\n{\n\tcurvatureValue = %ld\n\n\tcurvatureConfidence = %ld\n}\n\n", m_curvature.curvatureValue, m_curvature.curvatureConfidence);

    printf("driveDirection = %ld\n\n", m_driveDirection);

    printf("exteriorLights = %s\n\n", printBitString(m_exteriorLights).toStdString().c_str());

    if(m_externalTemperature != 0)
    {
        printf("externalTemperature = %ld\n\n", *m_externalTemperature);
    }

    if(m_lateralAcceleration != 0)
        printf("lateralAcceleration\n{\n\tlateralAccelerationValue = %ld\n\n\tlateralAccelerationConfidence = %ld\n}\n\n",m_lateralAcceleration->lateralAccelerationValue, m_lateralAcceleration->lateralAccelerationConfidence);

    printf("lightBarSirenInUse = %s\n\n", printBitString(m_lightBarSirenInUse).toStdString().c_str());

    printf("longitudinalAcceleration\n{\n\tlongitudinalAccelerationValue = %ld\n\n\tlongitudinalAccelerationConfidence = %ld\n}\n\n",m_longitudinalAcceleration.longitudinalAccelerationValue, m_longitudinalAcceleration.longitudinalAccelerationConfidence);

    if(m_numberOfOccupants != 0)
    {
        printf("numberOfOccupants = %ld\n\n", *m_numberOfOccupants);
    }

    if(m_positioningSolution != 0)
    {
        printf("positioningSolutionType = %ld\n\n", *m_positioningSolution);
    }

    printf("positionOfOccupants = %s\n\n", printBitString(m_positionOfOccupants).toStdString().c_str());

    if(m_steeringWheelAngle != 0)
        printf("steeringWheelAngle\n{\n\tsteeringWheelAngleValue = %ld\n\n\tsteeringWheelAngleConfidence = %ld\n}\n\n",m_steeringWheelAngle->steeringWheelAngleValue, m_steeringWheelAngle->steeringWheelAngleConfidence);

    if(m_verticalAcceleration != 0)
        printf("verticalAcceleration\n{\n\tverticalAccelerationValue = %ld\n\n\tverticalAccelerationConfidence = %ld\n}\n\n",m_verticalAcceleration->verticalAccelerationValue, m_verticalAcceleration->verticalAccelerationConfidence);

    printf("yawRate\n{\n\tyawRateConfidence = %ld\n\n\tyawRateValue = %ld\n}\n\n", m_yawRate.yawRateConfidence, m_yawRate.yawRateValue);
}

Curvature_t ParameterCAN::calculateCurvatureWithYawRate()
{
    Curvature_t c;

    if(m_speed)
        c.curvatureValue = m_yawRate.yawRateValue / m_speed;
    else
        c.curvatureValue = CurvatureValue_unavailable;
    c.curvatureConfidence = CurvatureConfidence_unavailable;

    return c;
}

Curvature_t ParameterCAN::curvatureUnavailable()
{
    Curvature_t c;

    c.curvatureValue = CurvatureValue_unavailable;
    c.curvatureConfidence = CurvatureConfidence_unavailable;

    return c;
}

ParameterCAN& ParameterCAN::operator=(const ParameterCAN& other)
{
    this->m_accelerationControl = other.m_accelerationControl;
    this->m_curvature = other.m_curvature;
    this->m_curvatureCalculationMode = other.m_curvatureCalculationMode;
    this->m_driveDirection = other.m_driveDirection;
    this->m_exteriorLights = other.m_exteriorLights;
    this->m_externalTemperature = other.m_externalTemperature;
    this->m_lateralAcceleration = other.m_lateralAcceleration;
    this->m_lightBarSirenInUse = other.m_lightBarSirenInUse;
    this->m_longitudinalAcceleration = other.m_longitudinalAcceleration;
    this->m_numberOfOccupants = other.m_numberOfOccupants;
    this->m_positioningSolution = other.m_positioningSolution;
    this->m_positionOfOccupants = other.m_positionOfOccupants;
    this->m_speed = other.m_speed;
    this->m_steeringWheelAngle = other.m_steeringWheelAngle ;
    this->m_verticalAcceleration = other.m_verticalAcceleration;
    this->m_yawRate = other.m_yawRate;

    return *this;
}