I have an C-Mex S-function that implements a Tutsin - Eulor PECE algorithm.
The function is worky perfectly fine, if only one block is present in the model. If I put several instances of the block, to filter several signals, then the outputs starts to be null.
I think that the difference instances of the function share the same memory, and this mess with my states vectors. I am not using the discrete state functions provided by MatLab, instead I use my own declared arrays of doubles (real_T ) which are not extern.
Any lead about this would be appreciated.
The code can be found here:
https://www.dropbox.com/s/d5nfdnio6qqrizq/te_pece.c?dl=0
Or under:
/* 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");
}
}
