/* check the supplied matlab function and its Jacobian. Returns 1 on error, 0 otherwise */
static int checkFuncAndJacobian(double *p, int m, int n, int havejac, struct mexdata *dat)
{
mxArray *lhs[1];
register int i;
int ret=0;
double *mp;
mexSetTrapFlag(1); /* handle errors in the MEX-file */
mp=mxGetPr(dat->rhs[0]);
for(i=0; i<m; ++i)
mp[i]=p[i];
/* attempt to call the supplied func */
i=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->fname);
if(i){
fprintf(stderr, "levmar: error calling '%s'.\n", dat->fname);
ret=1;
}
else if(!mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || !(mxGetM(lhs[0])==1 || mxGetN(lhs[0])==1) ||
__MAX__(mxGetM(lhs[0]), mxGetN(lhs[0]))!=n){
fprintf(stderr, "levmar: '%s' should produce a real vector with %d elements (got %d).\n",
dat->fname, n, __MAX__(mxGetM(lhs[0]), mxGetN(lhs[0])));
ret=1;
}
/* delete the matrix created by matlab */
mxDestroyArray(lhs[0]);
if(havejac){
/* attempt to call the supplied jac */
i=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->jacname);
if(i){
fprintf(stderr, "levmar: error calling '%s'.\n", dat->jacname);
ret=1;
}
else if(!mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || mxGetM(lhs[0])!=n || mxGetN(lhs[0])!=m){
fprintf(stderr, "levmar: '%s' should produce a real %dx%d matrix (got %dx%d).\n",
dat->jacname, n, m, mxGetM(lhs[0]), mxGetN(lhs[0]));
ret=1;
}
else if(mxIsSparse(lhs[0])){
fprintf(stderr, "levmar: '%s' should produce a real dense matrix (got a sparse one).\n", dat->jacname);
ret=1;
}
/* delete the matrix created by matlab */
mxDestroyArray(lhs[0]);
}
mexSetTrapFlag(0); /* on error terminate the MEX-file and return control to the MATLAB prompt */
return ret;
}
void TT_RUNKERNEL_ERROR(const char *error_msg) {
mxArray *rhs[1];
rhs[0] = mxCreateString(error_msg);
mexSetTrapFlag(1);
mexCallMATLAB(0, NULL, 1, rhs, "error");
}
static int_T GetRTWEnvironmentMode(SimStruct *S)
{
int_T status;
mxArray *plhs[1];
mxArray *prhs[1];
int_T err;
/*
* Get the name of the Simulink block diagram
*/
prhs[0] = mxCreateString(ssGetModelName(ssGetRootSS(S)));
plhs[0] = NULL;
/*
* Call "isSimulationTarget = rtwenvironmentmode(modelName)" in MATLAB
*/
mexSetTrapFlag(1);
err = mexCallMATLAB(1, plhs, 1, prhs, "rtwenvironmentmode");
mexSetTrapFlag(0);
mxDestroyArray(prhs[0]);
/*
* Set the error status if an error occurred
*/
if (err) {
if (plhs[0]) {
mxDestroyArray(plhs[0]);
plhs[0] = NULL;
}
ssSetErrorStatus(S, "Unknow error during call to 'rtwenvironmentmode'.");
return -1;
}
/*
* Get the value returned by rtwenvironmentmode(modelName)
*/
if (plhs[0]) {
status = (int_T) (mxGetScalar(plhs[0]) != 0);
mxDestroyArray(plhs[0]);
plhs[0] = NULL;
}
return (status);
}
static Matrix *
SetStr (
Matrix * mat
)
{
Matrix * result = NULL;
int status;
if (mxIsNumeric(mat)) {
mexSetTrapFlag(1);
status = mexCallMATLAB(1, & result, 1, & mat, "setstr");
if (status == 1) {
result = NULL;
}
mexSetTrapFlag(0);
}
return (result);
}
static Matrix *
SetNum (
Matrix * mat
)
{
Matrix * result = NULL;
int status;
if (mxIsString(mat)) {
mexSetTrapFlag(1);
status = mexCallMATLAB(1, & result, 1, & mat, "abs");
if (status == 1) {
result = NULL;
}
mexSetTrapFlag(0);
}
return (result);
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/*-- input --*/
const mwSize *dim = mxGetDimensions(prhs[0]);
int is_3D = (dim[1] == 3);
double *d_dims = mxGetPr(prhs[0]);
mexSetTrapFlag(1); /* bail to Matlab prompt on mexCallMATLAB error control MATLAB's response to errors in mexCallMATLAB, 1=>On error, control returns to your MEX-file
/*-- check command arguments --*/
if (check_usage(nlhs, plhs, nrhs, prhs) == 0) //check that
return;
uint dims[3] = { d_dims[0], d_dims[1], (is_3D? d_dims[2] : 1) };
mwSize dims_m[3] = { d_dims[0], d_dims[1], (is_3D? d_dims[2] : 1) };
uint N = mxGetScalar(prhs[1]);
uint label_cnt = mxGetScalar(prhs[2]);
m_R_fn = (mxArray *)prhs[3];
m_B_fn = (mxArray *)prhs[4];
V = mxGetPr(prhs[5]);
/*-- weight calculation call backs --*/
m_label = mxCreateDoubleScalar(-1);
label = mxGetPr(m_label);
MALLOC_SET(mxArray *, m_R_w, label_cnt);
/* compute neighbor weights */
m_e = mxCreateNumericMatrix(3, 1, mxDOUBLE_CLASS, mxREAL);
/*-- graph cut --*/
uint *map = graphchop(dims, N, label_cnt, R_fn, B_fn, V);
/*-- output --*/
plhs[0] = mxCreateNumericArray(3, dims_m, mxUINT32_CLASS, mxREAL);
memcpy(mxGetData(plhs[0]), map, dims[0]*dims[1]*dims[2]*sizeof(*map));
/*-- clean up --*/
for (uint i = 0; i < label_cnt; i++)
mxDestroyArray(m_R_w[i]);
JM_FREE(m_R_w);
mxDestroyArray(m_B_w);
mxDestroyArray(m_label);
mxDestroyArray(m_e);
JM_FREE(map);
}
/* check the supplied matlab projection function and its Jacobian. Returns 1 on error, 0 otherwise */
static int checkFuncAndJacobianMATLAB(int j, int i, double *aj, double *bi, int chkproj, int chkjac, int mintype, struct mexdata *dat)
{
mxArray *lhs[2]={NULL, NULL};
register int k;
int nlhs, ret=0;
double *mp;
mexSetTrapFlag(1); /* handle errors in the MEX-file */
mp=mxGetPr(dat->rhs[0]); *mp=j;
mp=mxGetPr(dat->rhs[1]); *mp=i;
mp=mxGetPr(dat->rhs[2]);
for(k=0; k<dat->cnp; ++k)
mp[k]=aj[k];
mp=mxGetPr(dat->rhs[3]);
for(k=0; k<dat->pnp; ++k)
mp[k]=bi[k];
if(chkproj){
/* attempt to call the supplied proj */
k=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->projname);
if(k){
fprintf(stderr, "sba: error calling '%s'.\n", dat->projname);
ret=1;
}
else if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || !(mxGetM(lhs[0])==1 || mxGetN(lhs[0])==1) ||
_MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp){
fprintf(stderr, "sba: '%s' should produce a real vector with %d elements (got %d).\n",
dat->projname, dat->mnp, _MAX_(mxGetM(lhs[0]), mxGetN(lhs[0])));
ret=1;
}
/* delete the vector created by matlab */
mxDestroyArray(lhs[0]);
}
if(chkjac){
lhs[0]=lhs[1]=NULL;
nlhs=(mintype==BA_MOTSTRUCT)? 2 : 1;
/* attempt to call the supplied jac */
k=mexCallMATLAB(nlhs, lhs, dat->nrhs, dat->rhs, dat->projacname);
if(k){
fprintf(stderr, "sba: error calling '%s'.\n", dat->projacname);
ret=1;
}
else if(mintype==BA_MOTSTRUCT || mintype==BA_MOT){
if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) ||
_MIN_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=1 ||
_MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp*dat->cnp){
fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its first output arg (got %dx%d).\n",
dat->projacname, dat->mnp*dat->cnp, mxGetM(lhs[0]), mxGetN(lhs[0]));
ret=1;
}
}
else{ /* BA_STRUCT */
if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) ||
_MIN_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=1 ||
_MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp*dat->pnp){
fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its first output arg (got %dx%d).\n",
dat->projacname, dat->mnp*dat->pnp, mxGetM(lhs[0]), mxGetN(lhs[0]));
ret=1;
}
}
if(lhs[0] && mxIsSparse(lhs[0])){
fprintf(stderr, "sba: '%s' should produce a real dense vector as its first output arg, not a sparse one.\n");
ret=1;
}
if(nlhs==2){ /* BA_MOTSTRUCT */
if(!lhs[1] || !mxIsDouble(lhs[1]) || mxIsComplex(lhs[1]) ||
_MIN_(mxGetM(lhs[1]), mxGetN(lhs[1]))!=1 ||
_MAX_(mxGetM(lhs[1]), mxGetN(lhs[1]))!=dat->mnp*dat->pnp){
fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its second output arg (got %dx%d).\n",
dat->projacname, dat->mnp*dat->pnp, mxGetM(lhs[1]), mxGetN(lhs[1]));
ret=1;
}
else if(lhs[1] && mxIsSparse(lhs[1])){
fprintf(stderr, "sba: '%s' should produce a real dense vector as its second output arg, not a sparse one.\n");
ret=1;
}
}
/* delete the vectors created by matlab */
for(k=0; k<nlhs; ++k)
mxDestroyArray(lhs[k]);
}
mexSetTrapFlag(0); /* on error terminate the MEX-file and return control to the MATLAB prompt */
return ret;
}
static void mdlInitializeSizes(SimStruct *S)
{
static int printed = 0;
if (!printed) {
printed = 1;
printf("-------------------------------------------------------\n");
printf(" TrueTime, Version 1.5\n");
printf(" Copyright (c) 2007\n");
printf(" Martin Ohlin, Dan Henriksson and Anton Cervin\n");
printf(" Department of Automatic Control LTH\n");
printf(" Lund University, Sweden\n");
printf("-------------------------------------------------------\n");
}
#ifdef KERNEL_MATLAB
char initfun[100];
static mxArray *lhs[1]; // warning: used multiple times
static mxArray *rhs[3]; // warning: used multiple times
segArray = mxCreateDoubleScalar(0.0);
destroyed = false;
mexMakeArrayPersistent(segArray);
#endif
ssSetNumSFcnParams(S, 4); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
#ifdef KERNEL_MATLAB
mxGetString(ssGetSFcnParam(S, 0), initfun, 100);
#endif
rtsys = new RTsys;
/* Assign function pointers */
rtsys->contextSwitchCode = contextSwitchCode;
rtsys->periodicTaskHandlerCode = periodicTaskHandlerCode;
rtsys->timeSort = timeSort;
rtsys->prioSort = prioSort;
rtsys->default_arrival = default_arrival;
rtsys->default_release = default_release;
rtsys->default_start = default_start;
rtsys->default_suspend = default_suspend;
rtsys->default_resume = default_resume;
rtsys->default_finish = default_finish;
rtsys->prioFP = prioFP;
rtsys->prioRM = prioRM;
rtsys->prioEDF = prioEDF;
rtsys->prioDM = prioDM;
#ifdef KERNEL_MATLAB
mexSetTrapFlag(1); // return control to the MEX file after an error
/* Write rtsys pointer to global workspace */
char rtsysbuf[100];
sprintf(rtsysbuf, "%p", rtsys);
mxArray* var = mxCreateDoubleScalar(0.0);
mexMakeArrayPersistent(var);
*((long *)mxGetPr(var)) = (long)rtsys;
mexPutVariable("global", "_rtsys", var);
rtsys->rtsysptr = (mxArray*)mexGetVariablePtr("global", "_rtsys");
/* Evaluating user-defined init function (MATLAB) */
rhs[0] = mxCreateString(initfun);
if (mexCallMATLAB(1, lhs, 1, rhs, "nargin") != 0) {
printf("Call to init function failed!\n");
ssSetErrorStatus(S, "Call to init function failed!");
return;
} else {
if (*mxGetPr(lhs[0]) == 0) {
if (mexCallMATLAB(0, NULL, 0, NULL, initfun) != 0) {
printf("Call to init function failed!\n");
ssSetErrorStatus(S, "Call to init function failed!");
return;
} else {
rtsys->init_phase = false;
}
} else if (*mxGetPr(lhs[0]) == 1) {
rhs[0] = (mxArray *)ssGetSFcnParam(S, 1);
if (mexCallMATLAB(0, NULL, 1, rhs, initfun) != 0) {
printf("Call to init function failed!\n");
ssSetErrorStatus(S, "Call to init function failed!");
return;
} else {
rtsys->init_phase = false;
}
} else {
printf("Init function takes wrong number (> 1) of arguments!\n");
ssSetErrorStatus(S, "Init function takes wrong number (> 1) of arguments!");
return;
}
}
#else
/* Save pointer to init args */
rtsys->initarg = (mxArray *)ssGetSFcnParam(S, 1);
/* Evaluating user-defined init function (C++) */
init();
rtsys->init_phase = false;
#endif
if (!rtsys->initialized) {
printf("ttInitKernel was not called in init function!\n");
ssSetErrorStatus(S, "ttInitKernel was not called in init function!");
return;
}
// Clock drift parameters
const mxArray *arg;
arg = ssGetSFcnParam(S, 2);
if (mxIsDoubleScalar(arg)) {
rtsys->clockDrift = *mxGetPr(arg) + 1;
}
arg = ssGetSFcnParam(S, 3);
if (mxIsDoubleScalar(arg)) {
rtsys->clockOffset = *mxGetPr(arg);
}
//printf("drift: %f, offset:%f\n", rtsys->clockDrift, rtsys->clockOffset);
if (!ssSetNumInputPorts(S, 4)) return;
ssSetInputPortDirectFeedThrough(S, 0, 0);
ssSetInputPortDirectFeedThrough(S, 1, 0);
ssSetInputPortDirectFeedThrough(S, 2, 0);
ssSetInputPortDirectFeedThrough(S, 3, 0);
if (!ssSetNumOutputPorts(S, 5)) return;
/* Input Ports */
if (rtsys->nbrOfInputs > 0)
ssSetInputPortWidth(S, 0, rtsys->nbrOfInputs);
else
ssSetInputPortWidth(S, 0, 1);
if (rtsys->nbrOfTriggers > 0)
ssSetInputPortWidth(S, 1, rtsys->nbrOfTriggers);
else
ssSetInputPortWidth(S, 1, 1);
if (rtsys->nbrOfNetworks > 0)
ssSetInputPortWidth(S, 2, rtsys->nbrOfNetworks); /* Network receive */
else
ssSetInputPortWidth(S, 2, 1);
ssSetInputPortWidth(S, 3, 1); //battery
/* Output Ports */
if (rtsys->nbrOfOutputs > 0)
ssSetOutputPortWidth(S, 0, rtsys->nbrOfOutputs);
else
ssSetOutputPortWidth(S, 0, 1);
if (rtsys->nbrOfNetworks > 0)
ssSetOutputPortWidth(S, 1, (rtsys->nbrOfNetworks)); /* Network send */
else
ssSetOutputPortWidth(S, 1, 1);
if (rtsys->nbrOfSchedTasks+rtsys->nbrOfSchedHandlers > 0)
ssSetOutputPortWidth(S, 2, rtsys->nbrOfSchedTasks+rtsys->nbrOfSchedHandlers);
else
ssSetOutputPortWidth(S, 2, 1);
if (rtsys->nbrOfSchedMonitors > 0)
ssSetOutputPortWidth(S, 3, rtsys->nbrOfSchedMonitors*rtsys->nbrOfTasks);
else
ssSetOutputPortWidth(S, 3, 1);
ssSetOutputPortWidth(S, 4, 1); //Energy consumption
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 1);
ssSetUserData(S, rtsys);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_CALL_TERMINATE_ON_EXIT);
}
double executeCode(char *codeName, int seg, Task *task) {
double retval;
mxArray *lhs[2];
mxArray *rhs[2];
*mxGetPr(segArray) = (double) seg;
rhs[0] = segArray;
if (task->dataMATLAB) {
//rhs[1] = mxDuplicateArray(task->dataMATLAB);
rhs[1] = task->dataMATLAB;
} else {
rhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
*mxGetPr(rhs[1]) = 0.0;
}
mexSetTrapFlag(1); // return control to the MEX file after an error
lhs[0] = NULL; // needed not to crash Matlab after an error
lhs[1] = NULL; // needed not to crash Matlab after an error
if (mexCallMATLAB(2, lhs, 2, rhs, codeName) != 0) {
rtsys->error = true;
return 0.0;
}
if (mxGetClassID(lhs[0]) == mxUNKNOWN_CLASS) {
printf("??? executeCode: execution time not assigned\n\n");
printf("Error in ==> code function '%s', segment %d\n", codeName, seg);
rtsys->error = true;
return 0.0;
}
if (!mxIsDoubleScalar(lhs[0])) {
printf("??? executeCode: illegal execution time\n\n");
printf("Error in ==> code function '%s', segment %d\n", codeName, seg);
rtsys->error = true;
return 0.0;
}
if (mxGetClassID(lhs[1]) == mxUNKNOWN_CLASS) {
printf("??? executeCode: data not assigned\n\n");
printf("Error in ==> code function '%s', segment %d\n", codeName, seg);
rtsys->error = true;
return 0.0;
}
//if ( task->dataMATLAB ) {
if ( task->dataMATLAB != lhs[1] ) {
mxDestroyArray(task->dataMATLAB);
//task->dataMATLAB = mxDuplicateArray(lhs[1]);
task->dataMATLAB = lhs[1];
mexMakeArrayPersistent(task->dataMATLAB);
}
retval = *mxGetPr(lhs[0]);
//mxDestroyArray(rhs[1]);
mxDestroyArray(lhs[0]);
//mxDestroyArray(lhs[1]);
return retval;
}
/* the input problem could be reduced to a conventional 2D matrix product..
so here we use the fast MATLAB version */
mxArray* MATLAB_mm(MxInfo zinfo, const MxInfo xinfo, const MxInfo yinfo,
const MxInfo xrest, const MxInfo yrest,
const MxInfo xmacc, const MxInfo ymacc){
mxArray *Xmx, *Ymx, *Zmx, *args[2];
/* call matlab to do the real work -- more reliable than dgemm */
/* first create a new matrix with the right size to use in the matlab call*/
/* create and empty array */
Xmx= mxCreateNumericMatrix(0,0,xinfo.dtype,(xinfo.ip==0)?mxREAL:mxCOMPLEX);
Ymx= mxCreateNumericMatrix(0,0,yinfo.dtype,(yinfo.ip==0)?mxREAL:mxCOMPLEX);
/* and populate it with data */
mxSetPr(Xmx,xinfo.rp); if ( xinfo.ip ) mxSetPi(Xmx,xinfo.ip);
mxSetPr(Ymx,yinfo.rp); if ( yinfo.ip ) mxSetPi(Ymx,yinfo.ip);
/* Set trap so errors return here so we can clean up correctly */
mexSetTrapFlag(1);
/* now do the calls to matlab to get the result */
args[0] = Xmx; args[1]= Ymx;
Zmx=0;
/* no accumulated dims -- just outer product, or just inner product */
if ( xmacc.nd == 1 && xmacc.sz[0]==1 ) {
/* set X as its vector version, implicitly transpose Y and then prod */
mxSetM(Xmx,MAX(sz(xrest,0),1)); mxSetN(Xmx,1);
mxSetM(Ymx,1); mxSetN(Ymx,MAX(sz(yrest,1),1));
mexCallMATLAB(1, &Zmx, 2, args, "*");
} else if ( xmacc.stride[0] == 1 && ymacc.stride[0] == 1 ){
/* | x | == macc/rest * macc/rest*/
mxSetM(Xmx,xmacc.sz[0]); mxSetN(Xmx,xinfo.numel/xmacc.sz[0]);
mxSetM(Ymx,ymacc.sz[0]); mxSetN(Ymx,yinfo.numel/ymacc.sz[0]);
/* transpose X and call matlab */
if( yinfo.numel+zinfo.numel>xinfo.numel*.75 ){/*cheaper to transpose X*/
mxArray *XmxT;
mexCallMATLAB(1, &XmxT, 1, &Xmx, ".\'");/* N.B. this copies X!!! */
args[0]=XmxT;
mexCallMATLAB(1, &Zmx, 2, args, "*");
mxDestroyArray(XmxT);
} else { /*cheaper to transpose y and z */
mxArray *YmxT;
mexCallMATLAB(1, &YmxT, 1, &Ymx, ".\'");/* N.B. this copies Y!!! */
args[0]=YmxT; args[1]=Xmx;
mexCallMATLAB(1, &Zmx, 2, args, "*");
mxDestroyArray(YmxT);
if( ymacc.sz[0]!=yinfo.numel ) {/* transpose result if necessary */
mxArray *ZmxT;
mexCallMATLAB(1, &ZmxT, 1, &Zmx, ".\'");/* N.B. this copies Z!!! */
mxDestroyArray(Zmx);
Zmx=ZmxT;
}
}
} else if ( xmacc.stride[0] == 1 && ymacc.stride[0] > 1 ){
/* | x _ == macc/rest * rest/macc */
mxSetM(Xmx,xmacc.sz[0]);
mxSetN(Xmx,xinfo.numel/xmacc.sz[0]);
mxSetM(Ymx,ymacc.stride[0]);
mxSetN(Ymx,yinfo.numel/ymacc.stride[0]);
if( yinfo.numel+xinfo.numel<zinfo.numel*.75 ){/* cheaper to transpose Z */
/* reverse order of multiply, call matlab and transpose Z */
mxArray *ZmxT;
args[0]=Ymx; args[1]=Xmx;
mexCallMATLAB(1, &Zmx, 2, args, "*");
mexCallMATLAB(1, &ZmxT, 1, &Zmx, ".\'");/* N.B. this copies Z!!! */
mxDestroyArray(Zmx);
Zmx=ZmxT;
} else { /* cheaper to transpose twice */
mxArray *XmxT, *YmxT;
mexCallMATLAB(1, &XmxT, 1, &Xmx, ".\'");/* N.B. this copies X!!! */
mexCallMATLAB(1, &YmxT, 1, &Ymx, ".\'");/* N.B. this copies Y!!! */
args[0]=XmxT; args[1]=YmxT;
mexCallMATLAB(1, &Zmx, 2, args, "*");
mxDestroyArray(XmxT); mxDestroyArray(YmxT);
}
} else if ( xmacc.stride[0] > 1 && ymacc.stride[0] == 1 ){
/* _ x | == rest/macc * macc/rest */
mxSetM(Xmx,xmacc.stride[0]);
mxSetN(Xmx,xinfo.numel/xmacc.stride[0]);
mxSetM(Ymx,ymacc.sz[0]);
mxSetN(Ymx,yinfo.numel/ymacc.sz[0]);
mexCallMATLAB(1, &Zmx, 2, args, "*");
} else if ( xmacc.stride[0] > 1 && ymacc.stride[0] > 1 ){
/* _ x _ == rest/macc * rest/macc */
mxSetM(Xmx,xmacc.stride[0]);
mxSetN(Xmx,xinfo.numel/xmacc.stride[0]);
mxSetM(Ymx,ymacc.stride[0]);
mxSetN(Ymx,yinfo.numel/ymacc.stride[0]);
if( xinfo.numel+zinfo.numel>yinfo.numel*.75 ){/* cheaper to transpose Y */
/* transpose Y and call matlab */
mxArray *YmxT;
mexCallMATLAB(1, &YmxT, 1, &Ymx, ".\'");/* N.B. this copies Y!!! */
args[1]=YmxT;
mexCallMATLAB(1, &Zmx, 2, args, "*");
mxDestroyArray(YmxT);
} else { /* cheaper to transpose X and Z */
mxArray *XmxT;
mexCallMATLAB(1, &XmxT, 1, &Xmx, ".\'");/* N.B. this copies X!!! */
args[0]=Ymx; args[1]=XmxT;
mexCallMATLAB(1, &Zmx, 2, args, "*");
mxDestroyArray(XmxT);
if( xmacc.sz[0]!=xinfo.numel ) {/* transpose result if necessary */
mxArray *ZmxT;
mexCallMATLAB(1, &ZmxT, 1, &Zmx, ".\'");/* N.B. this copies Z!!! */
mxDestroyArray(Zmx);
Zmx=ZmxT;
}
}
} else {
ERROR("tprod: somethings gone horibbly wrong!");
}
/* Set trap so errors return here so we can clean up correctly */
mexSetTrapFlag(0);
/* set the tempory X and Y matrices back to empty without ref to data to
stop matlab "helpfully" double freeing them? */
mxSetM(Xmx,0);mxSetN(Xmx,0);mxSetPr(Xmx,0);mxSetPi(Xmx,0);
mxDestroyArray(Xmx);
mxSetM(Ymx,0);mxSetN(Ymx,0);mxSetPr(Ymx,0);mxSetPi(Ymx,0);
mxDestroyArray(Ymx);
return Zmx;
}
static void mdlInitializeSizes(SimStruct *S)
{
debugPrintf("'%s': mdlInitializeSizes\n", S->path);
int i;
#ifdef KERNEL_MATLAB
char initfun[MAXCHARS];
static mxArray *lhs[1]; // warning: used multiple times
static mxArray *rhs[3]; // warning: used multiple times
mxArray *error_msg_array[1];
char *error_msg;
int nargin;
#endif
ssSetNumSFcnParams(S, 7); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
TT_CALLBACK_ERROR(S, "Wrong number of parameters to S-function!");
return; /* Parameter mismatch will be reported by Simulink */
}
rtsys = new RTsys;
strncpy((char*)rtsys->blockName, ssGetBlockName(S), MAXCHARS);
#ifdef KERNEL_MATLAB
rtsys->segArray = mxCreateScalarDouble(0.0);
mexMakeArrayPersistent(rtsys->segArray);
#endif
/* Assign various function pointers */
rtsys->contextSwitchCode = contextSwitchCode;
rtsys->timeCmp = timeCmp;
rtsys->prioCmp = prioCmp;
rtsys->default_arrival = default_arrival;
rtsys->default_release = default_release;
rtsys->default_start = default_start;
rtsys->default_suspend = default_suspend;
rtsys->default_resume = default_resume;
rtsys->default_finish = default_finish;
rtsys->default_runkernel = default_runkernel;
rtsys->prioFP = prioFP;
rtsys->prioEDF = prioEDF;
rtsys->prioDM = prioDM;
/* Create basic kernel data structures */
rtsys->taskList = new List("TaskList", NULL);
rtsys->handlerList = new List("HandlerList", NULL);
rtsys->timerList = new List("TimerList", NULL);
rtsys->monitorList = new List("MonitorList", NULL);
rtsys->eventList = new List("EventList", NULL);
rtsys->mailboxList = new List("MailboxList", NULL);
rtsys->semaphoreList = new List("SemaphoreList", NULL);
rtsys->logList = new List("LogList", NULL);
rtsys->cbsList = new List("CBSList", NULL);
/* Read number of inputs, outputs, and triggers from block mask */
const mxArray *arg;
arg = ssGetSFcnParam(S, 2);
int m = mxGetM(arg);
int n = mxGetN(arg);
if (n != 2 || m != 1) {
TT_CALLBACK_ERROR(S, "Illegal number of analog inputs/outputs!");
return;
}
int ninputs = (int)mxGetPr(arg)[0];
int noutputs = (int)mxGetPr(arg)[1];
if (ninputs < 0 || noutputs < 0) {
TT_CALLBACK_ERROR(S, "Illegal number of analog inputs/outputs!");
return;
}
rtsys->nbrOfInputs = ninputs;
rtsys->nbrOfOutputs = noutputs;
if (ninputs > 0) {
rtsys->inputs = new double[ninputs];
}
if (noutputs > 0) {
rtsys->outputs = new double[noutputs];
}
arg = ssGetSFcnParam(S, 3);
if (!mxIsDoubleScalar(arg)) {
TT_CALLBACK_ERROR(S, "Illegal number of triggers!");
return;
}
int ntriggers = (int)*mxGetPr(arg);
if (ntriggers < 0) {
TT_CALLBACK_ERROR(S, "Illegal number of triggers!");
return;
}
rtsys->nbrOfTriggers = ntriggers;
rtsys->triggers = new Trigger[ntriggers];
arg = ssGetSFcnParam(S, 4);
if (!mxIsDoubleScalar(arg)) {
TT_CALLBACK_ERROR(S, "Illegal trigger type!");
return;
}
int trigType = (int)*mxGetPr(arg);
rtsys->trigType = trigType;
/* Create network interfaces according to the block mask */
arg = ssGetSFcnParam(S, 5);
m = mxGetM(arg); // number of rows = number of network interfaces
n = mxGetN(arg); // number of cols should be 1 or 2
int networkNbr;
int nodeNbr;
char nwname[MAXCHARS];
if ((n == 1 && m != 1) || n > 2) {
TT_CALLBACK_ERROR(S, "Illegal network or node numbers!");
return;
}
if (m > 0) {
rtsys->networkInterfaces = new NetworkInterface[m];
rtsys->nbrOfNetworks = m;
for (i=0; i<m; i++) {
if (n == 1) {
networkNbr = 1;
nodeNbr = (int)mxGetPr(arg)[i];
} else {
networkNbr = (int)mxGetPr(arg)[i];
nodeNbr = (int)mxGetPr(arg)[i+m];
}
NetworkInterface *nwi = &(rtsys->networkInterfaces[i]);
nwi->networkNbr = networkNbr;
nwi->nodeNbr = nodeNbr-1;
nwi->portNbr = i;
sprintf(nwname, "network_%d", networkNbr);
}
}
/* Read clock offset and drift parameters from the block mask */
arg = ssGetSFcnParam(S, 6);
if (!mxIsEmpty(arg)) {
if (mxGetM(arg) == 1 && mxGetN(arg) == 2) {
rtsys->clockOffset = mxGetPr(arg)[0];
rtsys->clockDrift = mxGetPr(arg)[1] + 1.0;
} else {
TT_CALLBACK_ERROR(S, "Illegal offset/drift parameters!");
return;
}
}
#ifdef KERNEL_MATLAB
mexSetTrapFlag(1); // return control to the MEX file after an error
/* Write rtsys pointer to global workspace */
if (mexGetVariablePtr("global", "_rtsys") == 0) {
// pointer variable does not exist - let's create one
debugPrintf("Creating global _rtsys variable\n");
mxArray* var = mxCreateScalarDouble(0.0);
mexMakeArrayPersistent(var);
mexPutVariable("global", "_rtsys", var);
}
rtsys->rtsysptr = (mxArray*)mexGetVariablePtr("global", "_rtsys");
*((long *)mxGetPr(rtsys->rtsysptr)) = (long)rtsys;
/* Evaluating user-defined init function (MATLAB) */
mxGetString(ssGetSFcnParam(S, 0), initfun, MAXCHARS);
rhs[0] = mxCreateString(initfun);
if (mexCallMATLAB(1, lhs, 1, rhs, "nargin") != 0) {
goto error;
}
nargin = (int)*mxGetPr(lhs[0]);
if (nargin == 0) {
if (mexCallMATLAB(0, NULL, 0, NULL, initfun) != 0) {
goto error;
} else {
rtsys->init_phase = false;
}
} else if (nargin == 1) {
rhs[0] = (mxArray *)ssGetSFcnParam(S, 1);
if (mexCallMATLAB(0, NULL, 1, rhs, initfun) != 0) {
goto error;
} else {
rtsys->init_phase = false;
}
} else {
TT_CALLBACK_ERROR(S, "Init function takes wrong number (> 1) of arguments!");
return;
}
if (rtsys->error) {
error:
mexCallMATLAB(1 ,error_msg_array, 0, NULL, "lasterr");
error_msg = mxArrayToString(error_msg_array[0]);
snprintf(errbuf, MAXERRBUF, "Error in init function '%s'\n%s", initfun, error_msg);
mxFree(error_msg);
TT_CALLBACK_ERROR(S, errbuf);
return;
}
#else
/* Save pointer to init args */
mxArray *initArg = (mxArray *)ssGetSFcnParam(S, 1);
rtsys->initArg = initArg;
/* Evaluating user-defined init function (C++) */
init();
if (rtsys->error) {
TT_RUNKERNEL_ERROR(errbuf);
mexPrintf("??? Error in init() function\n%s\n\n", errbuf);
mexPrintf("In block ==> '%s'\nSimulation aborted!\n", ssGetBlockName(S));
ssSetErrorStatus(S, "");
return;
}
rtsys->init_phase = false;
#endif
if (!rtsys->initialized) {
TT_CALLBACK_ERROR(S, "ttInitKernel was not called in init function");
return;
}
if (!ssSetNumInputPorts(S, 4)) return;
ssSetInputPortDirectFeedThrough(S, 0, 0);
ssSetInputPortDirectFeedThrough(S, 1, 0);
ssSetInputPortDirectFeedThrough(S, 2, 0);
ssSetInputPortDirectFeedThrough(S, 3, 0);
if (!ssSetNumOutputPorts(S, 4)) return;
/* Input Ports */
if (rtsys->nbrOfInputs > 0)
ssSetInputPortWidth(S, 0, rtsys->nbrOfInputs);
else
ssSetInputPortWidth(S, 0, 1);
if (rtsys->nbrOfTriggers > 0)
ssSetInputPortWidth(S, 1, rtsys->nbrOfTriggers);
else
ssSetInputPortWidth(S, 1, 1);
if (rtsys->nbrOfNetworks > 0) {
ssSetInputPortWidth(S, 2, rtsys->nbrOfNetworks); /* Network receive */
}
else
ssSetInputPortWidth(S, 2, 1);
ssSetInputPortWidth(S, 3, 1); //battery
/* Output Ports */
if (rtsys->nbrOfOutputs > 0)
ssSetOutputPortWidth(S, 0, rtsys->nbrOfOutputs);
else
ssSetOutputPortWidth(S, 0, 1);
if (rtsys->nbrOfNetworks > 0)
ssSetOutputPortWidth(S, 1, (rtsys->nbrOfNetworks)); /* Network send */
else
ssSetOutputPortWidth(S, 1, 1);
if (rtsys->nbrOfSchedTasks > 0)
ssSetOutputPortWidth(S, 2, rtsys->nbrOfSchedTasks * rtsys->nbrOfCPUs);
else
ssSetOutputPortWidth(S, 2, 1);
ssSetOutputPortWidth(S, 3, 1); //Energy consumption
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 1);
ssSetUserData(S, rtsys);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_CALL_TERMINATE_ON_EXIT);
}
double runKernel(double externalTime) {
Task *task, *temp, *newrunning;
UserTask *usertask;
InterruptHandler *handler;
DataNode* dn;
// If no energy, then we can not run
if (rtsys->energyLevel <= 0) {
debugPrintf("'%s': Energy is out at time: %f\n", rtsys->blockName, rtsys->time);
return TT_MAX_TIMESTEP;
}
double timeElapsed = externalTime - rtsys->prevHit; // time since last invocation
rtsys->prevHit = externalTime; // update previous invocation time
debugPrintf("'%s': runkernel at %.16f\n", rtsys->blockName, rtsys->time);
#ifdef KERNEL_MATLAB
// Write rtsys pointer to global workspace so that MATLAB kernel
// primitives can access it
*((long *)mxGetPr(rtsys->rtsysptr)) = (long)rtsys;
#endif
double timestep = 0.0;
int niter = 0;
while (timestep < TT_TIME_RESOLUTION) {
if (++niter == TT_MAX_ITER) {
mexPrintf("??? Fatal kernel error: maximum number of iterations reached!\n");
rtsys->error = 1;
return 0.0;
}
// For each CPU, count down execution time for the current task
for (int i=0; i<rtsys->nbrOfCPUs; i++) {
debugPrintf("running core %d\n", i);
rtsys->currentCPU = i;
rtsys->running = rtsys->runnings[i];
task = rtsys->running;
if (task != NULL) {
if (task->state == RUNNING) {
task->state = READY;
}
double duration = timeElapsed * rtsys->cpuScaling;
// Decrease remaining execution time for current segment and increase total CPU time
task->execTime -= duration;
task->CPUTime += duration;
// If user task, call runkernel hook (to e.g. update budgets)
if (task->isUserTask()) {
usertask = (UserTask*)task;
usertask->runkernel_hook(usertask,duration);
}
// Check if task has finished current segment or not yet started
if (task->execTime / rtsys->cpuScaling < TT_TIME_RESOLUTION) {
// Execute next segment
task->segment = task->nextSegment;
task->nextSegment++; // default, can later be changed by ttSetNextSegment
#ifndef KERNEL_MATLAB
debugPrintf("'%s': executing code segment %d of task '%s'\n", rtsys->blockName, task->segment, task->name);
task->execTime = task->codeFcn(task->segment, task->data);
if (rtsys->error) {
TT_RUNKERNEL_ERROR(errbuf);
mexPrintf("In task ==> '%s', code segment %d\n", task->name, task->segment);
return 0.0;
}
#else
if (task->codeFcnMATLAB == NULL) {
task->execTime = task->codeFcn(task->segment, task->data);
} else {
mxArray *lhs[2];
mxArray *rhs[2];
debugPrintf("'%s': executing code function '%s'\n", rtsys->blockName, task->codeFcnMATLAB);
*mxGetPr(rtsys->segArray) = (double)task->segment;
rhs[0] = rtsys->segArray;
if (task->dataMATLAB) {
rhs[1] = task->dataMATLAB;
} else {
rhs[1] = mxCreateDoubleMatrix(0, 0, mxREAL);
}
mexSetTrapFlag(1); // return control to the MEX file after an error
lhs[0] = NULL; // needed not to crash Matlab after an error
lhs[1] = NULL; // needed not to crash Matlab after an error
if (mexCallMATLAB(2, lhs, 2, rhs, task->codeFcnMATLAB) != 0) {
rtsys->error = true;
return 0.0;
}
if (mxGetClassID(lhs[0]) == mxUNKNOWN_CLASS) {
snprintf(errbuf, MAXERRBUF, "Execution time not assigned in code function '%s'", task->codeFcnMATLAB);
TT_RUNKERNEL_ERROR(errbuf);
rtsys->error = true;
return 0.0;
}
if (!mxIsDoubleScalar(lhs[0])) {
snprintf(errbuf, MAXERRBUF, "Illegal execution time returned by code function '%s'", task->codeFcnMATLAB);
TT_RUNKERNEL_ERROR(errbuf);
rtsys->error = true;
return 0.0;
}
if (mxGetClassID(lhs[1]) == mxUNKNOWN_CLASS) {
snprintf(errbuf, MAXERRBUF, "Data not assigned in code function '%s'", task->codeFcnMATLAB);
TT_RUNKERNEL_ERROR(errbuf);
rtsys->error = true;
return 0.0;
}
//if ( task->dataMATLAB ) {
if ( task->dataMATLAB != lhs[1] ) {
mxDestroyArray(task->dataMATLAB);
//task->dataMATLAB = mxDuplicateArray(lhs[1]);
task->dataMATLAB = lhs[1];
mexMakeArrayPersistent(task->dataMATLAB);
}
task->execTime = *mxGetPr(lhs[0]);
//mxDestroyArray(rhs[1]);
mxDestroyArray(lhs[0]);
//mxDestroyArray(lhs[1]);
}
#endif
if (task->execTime < 0.0) {
// Negative execution time = task is finished
debugPrintf("'%s': task '%s' finished\n", rtsys->blockName, task->name);
task->execTime = 0.0;
task->segment = 0;
task->nextSegment = 1;
// Remove task from readyQ and set running to NULL
if (task->state == READY) {
task->remove();
task->state = SLEEPING;
} else {
snprintf(errbuf, MAXERRBUF, "Finished task '%s' not in ReadyQ.", task->name);
TT_RUNKERNEL_ERROR(errbuf);
rtsys->error = true;
return 0.0;
}
rtsys->runnings[i] = NULL;
rtsys->running = NULL;
if (task->isUserTask()) {
// Execute finish-hook
usertask = (UserTask*)task;
usertask->finish_hook(usertask);
rtsys->runningUserTasks[i] = NULL;
}
task->nbrInvocations--;
if (task->nbrInvocations > 0) {
// There are queued invocations, release the next one
dn = (DataNode*) task->pending->getFirst();
TaskInvocation *ti = (TaskInvocation *)dn->data;
if (task->isUserTask()) {
usertask = (UserTask*)task;
usertask->arrival = ti->timestamp;
usertask->release = rtsys->time;
usertask->release_hook(usertask); // could affect task prio
}
debugPrintf("'%s': releasing task '%s'\n", rtsys->blockName, task->name);
task->moveToList(rtsys->readyQs[task->affinity]); // re-insert task into readyQ
task->state = READY;
if (task->isHandler()) {
handler = (InterruptHandler*)task;
strncpy(handler->invoker, ti->invoker, MAXCHARS);
handler->timestamp = ti->timestamp;
}
task->pending->deleteNode(dn);
delete ti;
}
}
}
}
}
// Check time queue for possible releases and expired timers
task = (Task*) rtsys->timeQ->getFirst();
while (task != NULL) {
if ((task->wakeupTime() - rtsys->time) >= TT_TIME_RESOLUTION) {
break; // timeQ is sorted by time, no use to go further
}
// Task to be released
temp = task;
task = (Task*) task->getNext();
if (temp->isTimer()) {
Timer *timer = (Timer*)temp;
debugPrintf("'%s': timer '%s' expired at %f\n", rtsys->blockName, timer->name, rtsys->time);
invoke_task(timer->task, timer->name);
if (timer->isPeriodic) {
// if periodic timer put back in timeQ
timer->time += timer->period;
timer->moveToList(rtsys->timeQ);
} else {
timer->remove(); // remove timer from timeQ
if (!timer->isOverrunTimer) {
// delete the timer
dn = getNode(timer->name, rtsys->timerList);
rtsys->timerList->deleteNode(dn);
delete timer;
}
}
}
else if (temp->isUserTask()) {
usertask = (UserTask*)temp;
debugPrintf("'%s': releasing task '%s'\n", rtsys->blockName, usertask->name);
usertask->moveToList(rtsys->readyQs[usertask->affinity]);
usertask->state = READY;
}
else if (temp->isHandler()) {
mexPrintf("??? Fatal kernel error: interrupt handler in TimeQ!\n");
rtsys->error = 1;
return 0.0;
}
} // end: checking timeQ for releases
// For each core, determine the task with highest priority and make it running task
for (int i=0; i<rtsys->nbrOfCPUs; i++) {
debugPrintf("scheduling core %d\n", i);
rtsys->currentCPU = i;
newrunning = (Task*) rtsys->readyQs[i]->getFirst();
// If old running has been preempted, execute suspend_hook
if (rtsys->runnings[i] != NULL && rtsys->runnings[i] != newrunning) {
if (rtsys->runnings[i]->isUserTask()) {
usertask = (UserTask*)rtsys->runnings[i];
usertask->suspend_hook(usertask);
}
}
// If new running != old running, execute start_hook or resume_hook
if (newrunning != NULL && newrunning != rtsys->runnings[i]) {
if (newrunning->isUserTask()) {
usertask = (UserTask*)newrunning;
if (usertask->segment == 0) {
usertask->segment = 1;
usertask->start_hook(usertask);
} else {
usertask->resume_hook(usertask);
}
rtsys->runningUserTasks[i] = usertask;
}
}
rtsys->runnings[i] = (Task*) rtsys->readyQs[i]->getFirst(); // hooks may have released handlers
if (rtsys->runnings[i] != NULL) {
rtsys->runnings[i]->state = RUNNING;
}
}
// Determine next invocation of kernel
double compTime;
timestep = TT_MAX_TIMESTEP;
// Next release from timeQ (user task or timer)
if (rtsys->timeQ->getFirst() != NULL) {
Task* t = (Task*) rtsys->timeQ->getFirst();
timestep = t->wakeupTime() - rtsys->time;
}
// Remaining execution time of running tasks
for (int i=0; i<rtsys->nbrOfCPUs; i++) {
if (rtsys->runnings[i] != NULL) {
compTime = rtsys->runnings[i]->execTime / rtsys->cpuScaling;
timestep = (timestep < compTime) ? timestep : compTime;
}
}
timeElapsed = 0.0;
} // end: loop while timestep < TT_TIME_RESOLUTION
return timestep;
}
