sil/task.cpp

#include "task.h"
#include <qlogging.h>
#include <QLibrary>
#include <QDateTime>
#include <QTextStream>

extern "C"{
#include "SIL_capi.h"
}

void Task::setup_param(const QVector<QString> &head, const QVector<double> &onecase)
{
    const rtwCAPI_ModelMappingStaticInfo *sm = (*getmap_func)();
    if (sm)
    {
        //unsigned int n = rtwCAPI_GetNumBlockParametersFromStaticMap(sm);
        unsigned int m = rtwCAPI_GetNumModelParametersFromStaticMap(sm);
        rtwCAPI_ModelParameters const *prm = rtwCAPI_GetModelParametersFromStaticMap(sm);
        rtwCAPI_DataTypeMap const *dtm = rtwCAPI_GetDataTypeMapFromStaticMap(sm);
        rtwCAPI_DimensionMap const *dmm = rtwCAPI_GetDimensionMapFromStaticMap(sm);
        uint_T const *dam = rtwCAPI_GetDimensionArrayFromStaticMap(sm);
        rtwCAPI_ModelMappingInfo *MMI = &(rtmGetDataMapInfo(this->m).mmi);
        void **da = rtwCAPI_GetDataAddressMap(MMI);
        for (unsigned int i = 0; i < m; ++i)
        {
            const char *name = rtwCAPI_GetModelParameterName(prm, i);
            int idx = rtwCAPI_GetModelParameterAddrIdx(prm, i);
            uint16_t dti = rtwCAPI_GetModelParameterDataTypeIdx(prm, i);
            uint16_t dmi = rtwCAPI_GetModelParameterDimensionIdx(prm, i);
            uint8_t ss = rtwCAPI_GetDataTypeSLId(dtm, dti);
            uint16_t dai = rtwCAPI_GetDimArrayIndex(dmm, dmi);
            uint8_t nd = rtwCAPI_GetNumDims(dmm, dmi);
            uint16_t n = 1;
            for (int j = 0; j < nd; ++j)
            {
                n *= dam[dai + j];
            }
            int j = 0;
            for (auto i = head.begin(); i != head.end(); ++i, ++j)
            {
                QStringList s = i->split(QRegExp("[()]"));
                if (*i == name || ((s.size() >= 2) && (s.value(0) == name)))
                {
                    int target_idx = 0;
                    double target_value = onecase[j];
                    if (s.size() >= 2)
                    {
                        target_idx = s.value(1).toInt();
                    }
                    switch (ss)
                    {
                    case SS_DOUBLE:
                    {
                        double *d = (double *)da[idx];
                        d[target_idx] = target_value;
                    }
                    break;
                    case SS_SINGLE:
                    {
                        float *s = (float *)da[idx];
                        s[target_idx] = target_value;
                    }
                    break;
                    case SS_INT8:
                    {
                        int8_T *c = (int8_T *)da[idx];
                        c[target_idx] = target_value;
                    }
                    break;
                    case SS_INT16:
                    {
                        int16_T *h = (int16_T *)da[idx];
                        h[target_idx] = target_value;
                    }
                    break;
                    case SS_INT32:
                    {
                        int32_T *I = (int32_T *)da[idx];
                        I[target_idx] = target_value;
                    }
                    break;
                    case SS_UINT8:
                    {
                        uint8_T *c = (uint8_T *)da[idx];
                        c[target_idx] = target_value;
                    }
                    break;
                    case SS_BOOLEAN:
                    {
                        boolean_T *c = (boolean_T *)da[idx];
                        c[target_idx] = target_value > 0.5 ?true: false;
                    }
                    break;
                    case SS_UINT16:
                    {
                        uint16_T *h = (uint16_T *)da[idx];
                        h[target_idx] = target_value;
                    }
                    break;
                    case SS_UINT32:
                    {
                        uint32_T *I = (uint32_T *)da[idx];
                        I[target_idx] = target_value;
                    }
                    break;
                    }
                }
            }
        }
    }
}

Task::Task(QObject *parent, QString fn, int index) : QObject(parent),filename(fn),idx(index)
{
    if (idx < 1)
    {
        idx = 1;
    }
}

void Task::run()
{
    QLibrary *mdl = new QLibrary("SIL_win64.dll");
    if (mdl->isLoaded())
    {
        init_func = static_cast<SIL_initialize_ptr>(mdl->resolve("SIL_initialize"));
        step_func = static_cast<SIL_step_ptr>(mdl->resolve("SIL_step"));
        getmap_func = (SIL_GetCAPIStaticMap_ptr)(mdl->resolve("SIL_GetCAPIStaticMap"));
        u = (ExtU_SIL_T *)mdl->resolve("SIL_U");
        y = (ExtY_SIL_T *)mdl->resolve("SIL_Y");
        m = (RT_MODEL_SIL_T *)mdl->resolve("SIL_M_");
    }
    else
    {
        init_func = &SIL_initialize;
        step_func = &SIL_step;
        getmap_func = &SIL_GetCAPIStaticMap;
        u = &SIL_U;
        y = &SIL_Y;
        m = (RT_MODEL_SIL_T *)SIL_M;
    }

    if (init_func && step_func && u && y && getmap_func)
    {
        SIL_InitializeDataMapInfo();

        QFile data(filename);
        if (data.open(QFile::ReadOnly)) {
            QVector<QString> head;
            QVector<double> onecase;
            head.push_back("linked_default");
            onecase.push_back(1);
            QTextStream stream(&data);
            QString line;
            while (stream.readLineInto(&line)) {
              QStringList items = line.split(QString(","));
              head.push_back(items[0]);
              onecase.push_back(items[idx].toDouble());
            }
            setup_param(head, onecase);
        }

        (*init_func)();

        QString filename = QString("sil")
                +QDateTime::currentDateTime().toString("yyyyMMddhhmmss")
                +".DAT";
        rec_file = new QFile(filename, this);
        rec_file->open(QFile::WriteOnly);

        u->launch_trigger = 0.0;
        u->fault = 0x0;

        uint8_t seq_rec = 0;
        int ticks = 0;
        for (float t=0;;t+=0.005f)
        {
            u->data_len = 0u;
            if (t > 30 && u->launch_trigger < 1)
            {
                u->launch_trigger = 1;
            }
            if (t > 35 && y->C2out.recovery > 1)
            {
                break;
            }
            for (int i = 0; i < 1; ++i)
            {
                (*step_func)();
            }
            if (y->rec_seq != seq_rec)
            {
                seq_rec = y->rec_seq;
                rec_file->write((const char *)y->rec_buff, y->rec_len);
            }
            if (++ticks >= 1000)
            {
                qInfo("%.0f ", t);
                ticks = 0;
            }
        }
    }
    emit finished();

}