c++ - C-Mex 函数和共享内存的多个实例

Question

我有一个 C-Mex S-function，它实现了 Tutsin - Eulor PECE 算法。

如果模型中仅存在一个 block ，则该函数可以完美运行。如果我放置该 block 的多个实例来过滤多个信号，则输出开始为空。

我认为函数的不同实例共享相同的内存，这会扰乱我的状态 vector 。我没有使用 MatLab 提供的离散状态函数，而是使用我自己声明的 double 组 (real_T )，它们不是外部的。

任何有关此问题的线索将不胜感激。

代码可以在这里找到:

https://www.dropbox.com/s/d5nfdnio6qqrizq/te_pece.c?dl=0

或下:

/*  File    : toto.c
*  Abstract:
*
*      Implements a Tutsin-Euler PECE algorithm.
*
*      This block implements a time transfer function discretisation
*      using Tutsin as a predictor and Euler as a corrector.
*
*      This block is capable of receiving multiple inputs at once (bus / vector)
*      and will treat them as separate signals to be processed by the same TF.
*
*      Use in real time, fixed step, environment.
*
*
*/

#define S_FUNCTION_NAME toto
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"

#include "matrix.h"
#include "mex.h"

#define NUMERATOR_IDX 0
#define NUMERATOR_PARAM(S) ssGetSFcnParam(S,NUMERATOR_IDX)

#define DENOMINATOR_IDX 1
#define DENOMINATOR_PARAM(S) ssGetSFcnParam(S,DENOMINATOR_IDX)

#define SAMPLE_TIME_IDX 2
#define SAMPLE_TIME(S) ssGetSFcnParam(S,SAMPLE_TIME_IDX)

#define NPARAMS 3

/*====================*
* Private members    *
*====================*/
#define NSAMPLES 3
typedef enum {
    eN = 0,
    eNplusOne = 1,
    eNplusTwo = 2}
rankEnum;

typedef enum {
    eNcd = 0,
    eStatesProcessed = 1,
    eOutputProcessed = 2}
rankProcessState;
rankProcessState mProcessState;

int_T mTotalNumberOfDiscreteStates;
int_T mSignalNumberOfDiscreteStates;
int_T mNumberOfInputSignals;

int_T mLoopIndex;

int_T mInputWidth;

real_T* mNumerator;
real_T* mDenominator;

real_T** mPredictedStateVector;
real_T** mCorrectedStateVector;
real_T** mPredictedFunction;
real_T** mCorrectedFunction;
/*====================*
* Private methods *
*====================*/
void mInitializeRecursion();

void mComputePredictedStates(real_T iSampleTime, rankEnum iComputedRank, rankEnum iPreviousRank);
void mComputeCorrectedStates(real_T iSampleTime, rankEnum iComputedRank, rankEnum iPreviousRank);
void mComputePredictedFunction(real_T iInput, rankEnum iComputedRank);
void mComputeCorrectedFunction(real_T iInput, rankEnum iComputedRank);

void mUpdateStateVectors();

/* Function: mEmitWarning ===============================================
* Abstract:
*    Display iMessage in matlab console
*    
*/
void mEmitWarning(const char* iMessage)
{
    mxArray *cell_array_ptr;

    cell_array_ptr = mxCreateCellMatrix((mwSize)1,1);
    mxSetCell(cell_array_ptr,(mwIndex)0,mxCreateString(iMessage));
    mexCallMATLAB(0,NULL,1,&cell_array_ptr,"disp");
}

/*====================*
* S-function methods *
*====================*/

/* Function: mdlInitializeSizes ===============================================
* Abstract:
*    The sizes information is used by Simulink to determine the S-function
*    block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NPARAMS);  /* Number of expected parameters */

    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);   
    ssSetInputPortDirectFeedThrough(S,0,true);

    if (!ssSetNumOutputPorts(S, 1)) return; 
    ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);

    ssSetNumDiscStates(S, 1);
}

/* Function: mdlInitializeSampleTimes =========================================
* Abstract:
*    Specifiy that we inherit our sample time from the driving block.
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, *mxGetPr(SAMPLE_TIME(S)));
    ssSetOffsetTime(S, 0, 0.0);
}

#define MDL_INITIALIZE_CONDITIONS
/* Function: mdlInitializeConditions ========================================
* Abstract:
*    Initialize the discrete states to zero and
*    allocate for states and function vectors
*/
static void mdlInitializeConditions(SimStruct *S)
{   
    int_T wDenominatorLength;
    int_T wNumeratorLength;
    real_T* wNumerator;

    mInputWidth = ssGetInputPortWidth(S,0);
    ssSetOutputPortWidth(S, 0, mInputWidth);

    //Avoid repetitive use of mxGetPr/mxGetNumberOfElements by fetching datas once.
    wNumeratorLength    = mxGetNumberOfElements(NUMERATOR_PARAM(S));
    wDenominatorLength  = mxGetNumberOfElements(DENOMINATOR_PARAM(S));

    wNumerator   = mxGetPr(NUMERATOR_PARAM(S));
    mDenominator = mxGetPr(DENOMINATOR_PARAM(S));

    mNumberOfInputSignals = ssGetInputPortWidth(S,0);
    mSignalNumberOfDiscreteStates = wDenominatorLength - 1;
    mTotalNumberOfDiscreteStates = mNumberOfInputSignals*mSignalNumberOfDiscreteStates;

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, mTotalNumberOfDiscreteStates);        

    //Ensure identical sizes for numerator and number of states, for matrix computations
    mNumerator = calloc(wDenominatorLength,sizeof(real_T));
    if(wNumeratorLength < mSignalNumberOfDiscreteStates)
    {   
        memcpy(mNumerator+(mSignalNumberOfDiscreteStates-wNumeratorLength),wNumerator,wNumeratorLength*sizeof(real_T));
    }
    else
    {
        memcpy(mNumerator,wNumerator,mSignalNumberOfDiscreteStates*sizeof(real_T));
    }

    //Allocating for keeping in memory from n to n+2
    mPredictedStateVector = calloc(mTotalNumberOfDiscreteStates, sizeof(real_T));
    mCorrectedStateVector = calloc(mTotalNumberOfDiscreteStates, sizeof(real_T));
    mPredictedFunction = calloc(mTotalNumberOfDiscreteStates, sizeof(real_T));
    mCorrectedFunction = calloc(mTotalNumberOfDiscreteStates, sizeof(real_T));
    for (mLoopIndex = 0; mLoopIndex < mTotalNumberOfDiscreteStates; mLoopIndex++)
    {
        mPredictedStateVector[mLoopIndex] = calloc(NSAMPLES, sizeof(real_T));
        mCorrectedStateVector[mLoopIndex] = calloc(NSAMPLES, sizeof(real_T));
        mPredictedFunction[mLoopIndex] = calloc(NSAMPLES, sizeof(real_T));
        mCorrectedFunction[mLoopIndex] = calloc(NSAMPLES, sizeof(real_T));
    }

    mInitializeRecursion(S);
}

/* Function: mdlOutputs =======================================================
* Abstract:
*      y = Cx + Du 
*
*      The discrete system is evaluated using a controllable state space
*      representation of the transfer function.  
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    int_T wStateIndex;
    real_T *wOutput = ssGetOutputPortRealSignal(S,0);   

    if (eStatesProcessed == mProcessState  && ssIsSampleHit(S, 0, tid))
    {
        for (mLoopIndex = 0; mLoopIndex < mNumberOfInputSignals; mLoopIndex++)
        {
            if(NULL != wOutput)
            {
                *wOutput = 0;
                for (wStateIndex = 0; wStateIndex < mSignalNumberOfDiscreteStates; wStateIndex++)
                {
                    *wOutput  += mNumerator[wStateIndex]* mCorrectedStateVector[mLoopIndex*mSignalNumberOfDiscreteStates+wStateIndex][eNplusTwo];           
                }               
                *wOutput++;
            }
            else
            {
                break;
            }

        }
        mUpdateStateVectors();
        mProcessState = eOutputProcessed;
    }
}

#define MDL_UPDATE
/* Function: mdlUpdate ======================================================
* Abstract:
*      dx = Ax + Bu 
*      The discrete system is evaluated using a controllable state space
*      representation of the transfer function.  
*
*/

static void mdlUpdate(SimStruct *S, int_T tid)
{
    InputRealPtrsType wInput; 
    real_T wSampleTime; 

    if ((eOutputProcessed == mProcessState || eNcd == mProcessState) && ssIsSampleHit(S, 0, tid))
    {
        wInput = (InputRealPtrsType)ssGetInputPortSignalPtrs(S,0);
        wSampleTime = *mxGetPr(SAMPLE_TIME(S));

        if(wInput != NULL)
        {
            mComputePredictedStates(wSampleTime,eNplusTwo,eNplusOne);
            mComputePredictedFunction(*wInput[0],eNplusTwo);

            mComputeCorrectedStates(wSampleTime,eNplusTwo,eNplusOne);
            mComputeCorrectedFunction(*wInput[0],eNplusTwo);

            mProcessState = eStatesProcessed;
        }
    }
}

/* Function: mdlTerminate =====================================================
* Abstract:
*    Free memory
*/
static void mdlTerminate(SimStruct *S)
{
    UNUSED_ARG(S); /* unused input argument */

    free(mNumerator) ;

    free(mPredictedStateVector) ;
    free(mCorrectedStateVector) ;
    free(mPredictedFunction) ;
    free(mCorrectedFunction) ;
}

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif

/*====================*
* Private methods    *
*====================*/

/* Function: mInitializeRecursion =======================================================
* Abstract:
*      
*
*/
void mInitializeRecursion()
{
    mProcessState = eNcd;

    if (mCorrectedFunction !=NULL)
    {
        mCorrectedFunction[mTotalNumberOfDiscreteStates-1][eNplusOne] = 1;
    }
    else
    {
        mEmitWarning("Error in mdlInitializeConditions: Vectors are null");
    }
}

/* Function: mComputePredictedStates =======================================================
* Abstract:
*      
*
*/
void mComputePredictedStates(real_T iSampleTime, rankEnum iComputedRank, rankEnum iPreviousRank)
{
    if (mPredictedStateVector != NULL && mCorrectedStateVector !=NULL && mCorrectedFunction !=NULL)
    {
        for (mLoopIndex = 0; mLoopIndex < mTotalNumberOfDiscreteStates; mLoopIndex++)
        {
            mPredictedStateVector[mLoopIndex][iComputedRank]  = mCorrectedStateVector[mLoopIndex][iPreviousRank] + 1/(double)2*iSampleTime*(3*mCorrectedFunction[mLoopIndex][iPreviousRank] - mCorrectedFunction[mLoopIndex][eN]);                  
        }
    }
    else
    {
        mEmitWarning("Error in mComputePredictedStates: Vectors are null");
    }
}

/* Function: mComputeCorrectedStates =======================================================
* Abstract:
*      
*
*/
void mComputeCorrectedStates(real_T iSampleTime, rankEnum iComputedRank, rankEnum iPreviousRank)
{
    if (mCorrectedStateVector != NULL && mCorrectedStateVector !=NULL && mPredictedFunction !=NULL)
    {
        for (mLoopIndex = 0; mLoopIndex < mTotalNumberOfDiscreteStates; mLoopIndex++)
        {
            mCorrectedStateVector[mLoopIndex][iComputedRank]  = mCorrectedStateVector[mLoopIndex][iPreviousRank] + 1/(double)2*iSampleTime*(mPredictedFunction[mLoopIndex][iComputedRank] + mCorrectedFunction[mLoopIndex][iPreviousRank]);         
        }
    }
    else
    {
        mEmitWarning("Error in mComputeCorrectedStates: Vectors are null");
    }
}

/* Function: mComputePredictedFunction =======================================================
* Abstract:
*      
*
*/
void mComputePredictedFunction(real_T iInput, rankEnum iComputedRank)
{
    int_T wStateIndex;
    if (mPredictedStateVector != NULL && mPredictedFunction !=NULL)
    {
        for (mLoopIndex = 0; mLoopIndex < mTotalNumberOfDiscreteStates; mLoopIndex++)
        {
            if(0 == (mLoopIndex+1)%mSignalNumberOfDiscreteStates)
            {
                mPredictedFunction[mLoopIndex][iComputedRank] = iInput;
                for (wStateIndex = 0; wStateIndex < mSignalNumberOfDiscreteStates; wStateIndex++)
                {
                    mPredictedFunction[mLoopIndex][iComputedRank]  += -1*mDenominator[mSignalNumberOfDiscreteStates-wStateIndex]* mPredictedStateVector[wStateIndex][iComputedRank];            
                }
            }
            else
            {
                mPredictedFunction[mLoopIndex][iComputedRank]  = mPredictedStateVector[mLoopIndex+1][iComputedRank];    
            }       
        }
    }
    else
    {
        mEmitWarning("Error in mComputePredictedFunction: Vectors are null");
    }
}

/* Function: mComputeCorrectedFunction =======================================================
* Abstract:
*      
*
*/
void mComputeCorrectedFunction(real_T iInput, rankEnum iComputedRank)
{
    int_T wStateIndex;
    if (mCorrectedStateVector != NULL && mCorrectedFunction !=NULL)
    {
        for (mLoopIndex = 0; mLoopIndex < mTotalNumberOfDiscreteStates; mLoopIndex++)
        {
            if(0 == (mLoopIndex+1)%mSignalNumberOfDiscreteStates)
            {
                mCorrectedFunction[mLoopIndex][iComputedRank] = iInput;
                for (wStateIndex = 0; wStateIndex < mSignalNumberOfDiscreteStates; wStateIndex++)
                {
                    mCorrectedFunction[mLoopIndex][iComputedRank]  += -1*mDenominator[mSignalNumberOfDiscreteStates-wStateIndex]* mCorrectedStateVector[wStateIndex][iComputedRank];            
                }
            }
            else
            {
                mCorrectedFunction[mLoopIndex][iComputedRank]  = mCorrectedStateVector[mLoopIndex+1][iComputedRank];    
            }
        }
    }
    else
    {
        mEmitWarning("Error in mComputeCorrectedFunction: Vectors are null");
    }
}

/* Function: mUpdateStateVectors =======================================================
* Abstract:
*      
*
*/
void mUpdateStateVectors()
{
    if (mPredictedStateVector != NULL && mCorrectedStateVector !=NULL && mPredictedFunction != NULL && mCorrectedFunction !=NULL)
    {
        for (mLoopIndex = 0; mLoopIndex < mTotalNumberOfDiscreteStates; mLoopIndex++)
        {
            mPredictedStateVector[mLoopIndex][eN]  = mPredictedStateVector[mLoopIndex][eNplusOne];
            mCorrectedStateVector[mLoopIndex][eN]  = mCorrectedStateVector[mLoopIndex][eNplusOne];
            mPredictedFunction[mLoopIndex][eN]  = mPredictedFunction[mLoopIndex][eNplusOne];
            mCorrectedFunction[mLoopIndex][eN]  = mCorrectedFunction[mLoopIndex][eNplusOne];

            mPredictedStateVector[mLoopIndex][eNplusOne]  = mPredictedStateVector[mLoopIndex][eNplusTwo];
            mCorrectedStateVector[mLoopIndex][eNplusOne]  = mCorrectedStateVector[mLoopIndex][eNplusTwo];
            mPredictedFunction[mLoopIndex][eNplusOne]  = mPredictedFunction[mLoopIndex][eNplusTwo];
            mCorrectedFunction[mLoopIndex][eNplusOne]  = mCorrectedFunction[mLoopIndex][eNplusTwo];
        }
    }
    else
    {
        mEmitWarning("Error in mUpdateStateVectors: Vectors are null");
    }

}

Answer 1

谢谢，我使用 DWork vector 解决了这个问题:)