void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if(nrhs != 2) {
TT_MEX_ERROR("Error using ==> ttAnalogOut\nWrong number of input arguments.");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("Error using ==> ttAnalogOut\nIllegal output channel.");
return;
}
if (!mxIsDoubleScalar(prhs[1])) {
TT_MEX_ERROR("Error using ==> ttAnalogOut\nIllegal output value.");
return;
}
int outpChan = (int) *mxGetPr(prhs[0]);
double value = *mxGetPr(prhs[1]);
ttAnalogOut(outpChan, value);
}
static void mdlInitializeSizes(SimStruct *S)
{
const mxArray *arg;
ssSetNumSFcnParams(S, 3); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
if (!ssSetNumInputPorts(S, 0)) return;
if (!ssSetNumOutputPorts(S, 7)) return;
RMsys *rmsys = new RMsys;
rmsys->trigger = 0.0;
arg = ssGetSFcnParam(S, 0);
if (mxIsDoubleScalar(arg)) {
rmsys->networkNbr = (int)*mxGetPr(arg);
}
arg = ssGetSFcnParam(S, 1);
if (mxIsDoubleScalar(arg)) {
rmsys->receiver = (int)*mxGetPr(arg);
}
arg = ssGetSFcnParam(S, 2);
if (mxIsDoubleScalar(arg)) {
rmsys->outdim = (int)*mxGetPr(arg);
}
/* Output Ports */
ssSetOutputPortWidth(S, 0, rmsys->outdim); // data
ssSetOutputPortWidth(S, 1, 1); // receiver
ssSetOutputPortWidth(S, 2, 1); // length
ssSetOutputPortWidth(S, 3, 1); // prio
ssSetOutputPortWidth(S, 4, 1); // time stamp
ssSetOutputPortWidth(S, 5, 1); // signal power (wireless only)
ssSetOutputPortWidth(S, 6, 1); // message id
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetUserData(S, rmsys);
ssSetOptions(S, SS_OPTION_CALL_TERMINATE_ON_EXIT);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs > 2) {
TT_MEX_ERROR("ttCreateMailbox: Wrong number of input arguments!\nUsage: ttCreateMailbox(mailboxname [, maxSize])");
return;
}
if (mxIsChar(prhs[0]) != 1) {
TT_MEX_ERROR("ttCreateMailbox: mailboxname must be a string");
return;
}
char mailboxname[MAXCHARS];
mxGetString(prhs[0], mailboxname, MAXCHARS);
if (nrhs == 2) {
if (!mxIsDoubleScalar(prhs[1])) {
TT_MEX_ERROR("ttCreateMailbox: maxSize must be an integer scalar");
return;
}
int maxSize = (int) *mxGetPr(prhs[1]);
ttCreateMailbox(mailboxname, maxSize);
} else {
ttCreateMailbox(mailboxname);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 1) {
TT_MEX_ERROR("Wrong number of input arguments!\nUsage: ttAnalogIn(inpChan)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttAnalogIn: inpChan must be a number");
return;
}
int inpChan = (int) *mxGetPr(prhs[0]);
double retval = ttAnalogIn(inpChan);
plhs[0] = mxCreateDoubleScalar(retval);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 2) {
MEX_ERROR("ttCreateMonitor: Wrong number of input arguments!\nUsage: ttCreateMonitor(monitorname, display)");
return;
}
if (mxIsChar(prhs[0]) != 1) {
MEX_ERROR("ttCreateMonitor: monitorname must be a string");
return;
}
if (!mxIsDoubleScalar(prhs[1])) {
MEX_ERROR("ttCreateMonitor: display must be an integer scalar");
return;
}
char monitorname[100];
mxGetString(prhs[0], monitorname, 100);
int display = (int) *mxGetPr(prhs[1]);
if (display == 0) {
ttCreateMonitor(monitorname, false);
} else {
ttCreateMonitor(monitorname, true);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 1) {
TT_MEX_ERROR("ttSetNumberOfCPUs: Wrong number of input arguments!\nUsage: ttSetNumberOfCPUs(number)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttSetNumberOfCPUs: segment must be a positive integer");
return;
}
int nbr = (int) *mxGetPr(prhs[0]);
ttSetNumberOfCPUs(nbr);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 1) {
MEX_ERROR("ttLogStop: Wrong number of input arguments!\nUsage: ttLogStop(logID)");
return;
}
int ID = 0;
if (mxIsDoubleScalar(prhs[0])) {
ID = (int)*mxGetPr(prhs[0]);
}
if (ID <= 0) {
MEX_ERROR("ttLogStop: logID must be a positive number");
return;
}
ttLogStop(ID);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs < 2 || nrhs > 3 ) {
MEX_ERROR("ttCreateSemaphore: Wrong number of input arguments!\nUsage: ttCreateSemaphore(semaphorename, initval [, maxval])");
return;
}
if (mxIsChar(prhs[0]) != 1) {
MEX_ERROR("ttCreateSemaphore: semaphorename must be a string");
return;
}
if (!mxIsDoubleScalar(prhs[1])) {
MEX_ERROR("ttCreateSemaphore: initval must be an integer scalar");
return;
}
if (nrhs == 3) {
if (!mxIsDoubleScalar(prhs[2])) {
MEX_ERROR("ttCreateSemaphore: maxval must be an integer scalar");
return;
}
}
char semaphorename[100];
mxGetString(prhs[0], semaphorename, 100);
int initval = (int) *mxGetPr(prhs[1]);
if (nrhs == 2) {
ttCreateSemaphore(semaphorename, initval);
} else {
int maxval = (int) *mxGetPr(prhs[2]);
ttCreateSemaphore(semaphorename, initval, maxval);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 3) {
TT_MEX_ERROR("ttCallBlockSystem: Wrong number of input arguments!\nUsage: ttCallBlockSystem(nbrOutp, inpVec, blockname)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttCallBlockSystem: nbrOutp must be a number");
return;
}
if (!mxIsDouble(prhs[1])) {
TT_MEX_ERROR("ttCallBlockSystem: inpVec must contain numbers");
return;
}
if (mxIsChar(prhs[2]) != 1 || mxGetM(prhs[2]) != 1) {
TT_MEX_ERROR("ttCallBlockSystem: blockname must be a non-empty string");
return;
}
int nbrOutp = (int) *mxGetPr(prhs[0]);
double* outpVec = new double[nbrOutp];
int nbrInp = mxGetNumberOfElements(prhs[1]);
double* inpVec = mxGetPr(prhs[1]);
char blockname[MAXCHARS];
mxGetString(prhs[2], blockname, MAXCHARS);
ttCallBlockSystem(nbrOutp, outpVec, nbrInp, inpVec, blockname);
plhs[0] = mxCreateDoubleMatrix(1, nbrOutp, mxREAL);
// Copy values (will be zeros if call failed)
for (int i=0; i<nbrOutp; i++) {
mxGetPr(plhs[0])[i] = outpVec[i];
}
delete[] outpVec;
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
int networkNbr;
char nwhandler[MAXCHARS];
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys == NULL) return;
// Check number and type of arguments.
if (nrhs < 1 || nrhs > 2) {
mexErrMsgTxt("ttAttachNetworkHandler: Wrong number of input arguments!\nUsage: ttAttachNetworkHandler(network, nwhandler) or\n ttAttachNetworkHandler(nwhandler)");
}
if (nrhs == 2) {
if (!mxIsDoubleScalar(prhs[0])) {
mexErrMsgTxt("ttAttachNetworkHandler: network must be an integer");
}
if (mxIsChar(prhs[1]) != 1 || mxGetM(prhs[1]) != 1) {
mexErrMsgTxt("ttAttachNetworkHandler: nwhandler must be a string");
}
networkNbr = (int) *mxGetPr(prhs[0]);
mxGetString(prhs[1], nwhandler, MAXCHARS);
ttAttachNetworkHandler(networkNbr, nwhandler);
} else {
if (mxIsChar(prhs[0]) != 1 || mxGetM(prhs[0]) != 1) {
mexErrMsgTxt("ttAttachNetworkHandler: nwhandler must be a string");
}
mxGetString(prhs[0], nwhandler, MAXCHARS);
ttAttachNetworkHandler(nwhandler);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double value;
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs == 0 || nrhs > 2) {
MEX_ERROR("ttSetDeadline: Wrong number of input arguments!\nUsage: ttSetDeadline(value) or ttSetDeadline(value, taskname)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
MEX_ERROR("ttSetDeadline: value must be a number");
return;
}
if (nrhs == 1) {
value = *mxGetPr(prhs[0]);
ttSetDeadline(value);
} else {
if (mxIsChar(prhs[1]) != 1 || mxGetM(prhs[1]) != 1) {
MEX_ERROR("ttSetDeadline: taskname must be a string");
return;
}
value = *mxGetPr(prhs[0]);
char taskname[100];
mxGetString(prhs[1], taskname, 100);
ttSetDeadline(value, taskname);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
mxArray* data;
int network;
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs > 1) {
MEX_ERROR("ttGetMsg: Wrong number of input arguments!\nUsage: ttGetMsg or\n ttGetMsg(network)");
return;
}
if (nrhs == 1) {
if (!mxIsDoubleScalar(prhs[0])) {
MEX_ERROR("ttGetMsg: network must be an integer scalar");
return;
}
network = (int) *mxGetPr(prhs[0]);
} else {
network = 1;
}
double signalPower;
data = ttGetMsgMATLAB(network, &signalPower);
if ( data==NULL ) {
data = mxCreateDoubleMatrix(0,0,mxREAL); // Return empty matrix
}
plhs[0] = data;
if ( nlhs>=2 ) {
plhs[1] = mxCreateDoubleScalar(signalPower);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double prio;
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs == 0 || nrhs > 2) {
TT_MEX_ERROR("ttSetPriority: Wrong number of input argumenys!\nUsage: ttSetPriority(prio) or ttSetPriority(prio, taskname)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttSetPriority: prio must be a number");
return;
}
if (nrhs == 1) {
prio = *mxGetPr(prhs[0]);
ttSetPriority(prio);
} else {
if (mxIsChar(prhs[1]) != 1 || mxGetM(prhs[1]) != 1) {
TT_MEX_ERROR("ttSetPriority: taskname must be a string");
return;
}
prio = *mxGetPr(prhs[0]);
char taskname[MAXCHARS];
mxGetString(prhs[1], taskname, MAXCHARS);
ttSetPriority(prio, taskname);
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
int network;
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number of arguments.
if (nrhs > 1) {
TT_MEX_ERROR("ttDiscardUnsentMessages: Wrong number of input arguments!\nUsage: ttDiscardUnsentMessages() or\n ttDiscardUnsentMessages(network)");
return;
}
int nbr;
if (nrhs > 0) {
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttSendMsg: network must be a number");
return;
}
network = (int)*mxGetPr(prhs[0]);
nbr = ttDiscardUnsentMessages(network);
}
else
// default network (1)
nbr = ttDiscardUnsentMessages();
// if ( nlhs>=1 ){
plhs[0] = mxCreateDoubleScalar(nbr);
//}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs != 1) {
TT_MEX_ERROR("ttSleepUntil: Wrong number of input arguments!\nUsage: ttSleepUntil(time)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
TT_MEX_ERROR("ttSleepUntil: time must be a number");
return;
}
double time = *mxGetPr(prhs[0]);
ttSleepUntil(time);
}
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;
}
static void mdlInitializeSizes(SimStruct *S)
{
const mxArray *arg;
ssSetNumSFcnParams(S, 5); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
// Parse second argument only, to determine nbrOfNodes
// 2 - Number of nodes
int nbrOfNodes = 0;
arg = ssGetSFcnParam(S, 1);
if (mxIsDoubleScalar(arg)) {
nbrOfNodes = (int) *mxGetPr(arg);
}
if (nbrOfNodes <= 0) {
ssSetErrorStatus(S, "TrueTime Ultrasound Network: The number of nodes must be an integer > 0");
return;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumInputPorts(S, 3);
ssSetInputPortDirectFeedThrough(S, 0, 0);
ssSetInputPortWidth(S, 0, nbrOfNodes);
ssSetInputPortWidth(S, 1, nbrOfNodes);
ssSetInputPortWidth(S, 2, nbrOfNodes);
ssSetNumOutputPorts(S, 2);
ssSetOutputPortWidth(S, 0, nbrOfNodes);
ssSetOutputPortWidth(S, 1, nbrOfNodes);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 1);
// Make sure cleanup is performed even if errors occur
ssSetOptions(S, SS_OPTION_CALL_TERMINATE_ON_EXIT);
int i;
// Create new network struct
RTnetwork *nwsys = new RTnetwork;
ssSetUserData(S, nwsys); // save pointer in UserData
// Arg 1 - Network Number
nwsys->networkNbr = 0;
arg = ssGetSFcnParam(S, 0);
if (mxIsDoubleScalar(arg)) {
nwsys->networkNbr = (int) *mxGetPr(arg);
}
if (nwsys->networkNbr <= 0) {
ssSetErrorStatus(S, "TrueTime Ultrasound Network: The network number must be > 0");
return;
}
// Arg 2 - Number of nodes
arg = ssGetSFcnParam(S, 1);
nwsys->nbrOfNodes = (int) *mxGetPr(arg); // we know it's right
//mexPrintf("nbrOfNodes: %d\n", nwsys->nbrOfNodes);
// Arg 3 - Reach
nwsys->reach = 0.0;
arg = ssGetSFcnParam(S, 2);
if (mxIsDoubleScalar(arg)) {
nwsys->reach = *mxGetPr(arg);
}
if (nwsys->reach < 0.0) {
ssSetErrorStatus(S, "TrueTime Ultrasound Network: The reach must be >= 0");
return;
}
//mexPrintf("reach: %f\n", nwsys->reach);
// Arg 4 - Ping length
nwsys->pinglength = 0.0;
arg = ssGetSFcnParam(S, 3);
if (mxIsDoubleScalar(arg)) {
nwsys->pinglength = *mxGetPr(arg);
}
if (nwsys->pinglength < 0.0) {
ssSetErrorStatus(S, "TrueTime Ultrasound Network: The ping length must be >= 0");
return;
}
//mexPrintf("ping length: %f\n", nwsys->pinglength);
// Arg 5 - Speed of sound
nwsys->speedofsound = 0.0;
arg = ssGetSFcnParam(S, 4);
if (mxIsDoubleScalar(arg)) {
nwsys->speedofsound = *mxGetPr(arg);
}
if (nwsys->speedofsound <= 0.0) {
ssSetErrorStatus(S, "TrueTime Ultrasound Network: The speed of sound must be > 0");
return;
}
//mexPrintf("speedofsound: %f\n", nwsys->speedofsound);
/* Write pointer to Simulink block UserData */
/* mexCallMATLAB(1, lhs, 0, NULL, "gcbh");
sprintf(nwsysp,"%p",nwsys);
rhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(rhs[0]) = *mxGetPr(lhs[0]);
rhs[1] = mxCreateString("UserData");
rhs[2] = mxCreateString(nwsysp);
mexCallMATLAB(0, NULL, 3, rhs, "set_param"); */
/* Write pointer to MATLAB global workspace */
/* FIX: The code above is intended to be removed and replaced by this. */
/* Write rtsys pointer to global workspace */
mxArray* var = mxCreateDoubleScalar(0.0);
mexMakeArrayPersistent(var);
*((void **)mxGetData(var)) = nwsys;
char nwsysbuf[MAXCHARS];
sprintf(nwsysbuf, "_nwsys_%d", nwsys->networkNbr);
mexPutVariable("global", nwsysbuf, var);
nwsys->inputs = new double[nwsys->nbrOfNodes];
nwsys->oldinputs = new double[nwsys->nbrOfNodes];
nwsys->outputs = new double[nwsys->nbrOfNodes];
nwsys->sendschedule = new double[nwsys->nbrOfNodes];
for (i=0; i<nwsys->nbrOfNodes; i++) {
nwsys->inputs[i] = 0.0;
nwsys->oldinputs[i] = 0.0;
nwsys->outputs[i] = 0.0;
nwsys->sendschedule[i] = i+1;
}
nwsys->time = 0.0;
nwsys->prevHit = 0.0;
nwsys->nwnodes = new NWnode*[nwsys->nbrOfNodes];
for (i=0; i<nwsys->nbrOfNodes; i++) {
int j;
nwsys->nwnodes[i] = new NWnode();
//nwsys->nwnodes[i]->transmitPower = transmitPowerWatt;
nwsys->nwnodes[i]->signallevels = new double[nwsys->nbrOfNodes]; //802.11
for (j=0; j<nwsys->nbrOfNodes; j++) {
nwsys->nwnodes[i]->signallevels[j] = 0;
}
}
nwsys->waituntil = 0.0;
nwsys->sending = -1; // Note! -1 means nobody is sending
nwsys->rrturn = nwsys->nbrOfNodes - 1; // want to start at 0
nwsys->lasttime = -1.0;
nwsys->slotcount = nwsys->schedsize - 1; // want to start at 0
nwsys->currslottime = -nwsys->slottime; // same here
// rad, kolum, reella tal
//nwsys->nbrOfTransmissions = mxCreateDoubleMatrix(nwsys->nbrOfNodes, nwsys->nbrOfNodes, mxREAL);
//mexMakeArrayPersistent(nwsys->nbrOfTransmissions);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys(); // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs < 3 || nrhs > 4) {
MEX_ERROR("ttInitKernel: Wrong number of input arguments! \nUsage: ttInitKernel(nbrInputs, nbrOutputs, prioFcn) or\n ttInitKernel(nbrInputs, nbrOutputs, prioFcn, contextSwitchOH)");
return;
}
if (!mxIsDoubleScalar(prhs[0])) {
MEX_ERROR("ttInitKernel: nbrInputs must be an integer");
return;
}
if (!mxIsDoubleScalar(prhs[1])) {
MEX_ERROR("ttInitKernel: nbrOutputs must be an integer");
return;
}
if (mxIsChar(prhs[2]) != 1 || mxGetM(prhs[2]) != 1) {
MEX_ERROR("ttInitKernel: prioFcn must be a string");
return;
}
if (nrhs == 4) {
if (!mxIsDoubleScalar(prhs[3])) {
MEX_ERROR("ttInitKernel: contextSwitchOH must be a number");
return;
}
}
int nbrInp = (int) *mxGetPr(prhs[0]);
int nbrOutp = (int) *mxGetPr(prhs[1]);
if ( nbrInp < 0 ) {
MEX_ERROR("ttInitKernel: nbrInputs must be positive or zero");
return;
}
if ( nbrOutp < 0 ) {
MEX_ERROR("ttInitKernel: nbrOutputs must be positive or zero");
return;
}
char buf[20];
mxGetString(prhs[2], buf, 20);
int dispatch;
if (strcmp(buf, "prioFP") == 0) {
dispatch = FP;
} else if (strcmp(buf, "prioRM") == 0) {
dispatch = RM;
} else if (strcmp(buf, "prioDM") == 0){
dispatch = DM;
} else if (strcmp(buf, "prioEDF") == 0) {
dispatch = EDF;
} else {
printf("ttInitKernel: Unknown priority function '%s', using fixed priority scheduling!\n",buf);
dispatch = FP; // default
}
if (nrhs == 3) {
ttInitKernel(nbrInp, nbrOutp, dispatch);
} else {
double contextSwitchOH = *mxGetPr(prhs[3]);
ttInitKernel(nbrInp, nbrOutp, dispatch, contextSwitchOH);
}
}
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;
}
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);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
rtsys = getrtsys() ; // Get pointer to rtsys
if (rtsys==NULL) {
return;
}
// Check number and type of arguments.
if (nrhs < 5 || nrhs > 6) {
MEX_ERROR("ttCreatePeriodicTask: Wrong number of input arguments! \nUsage: ttCreatePeriodicTask(name, offset, period, priority, codefcn)\n ttCreatePeriodicTask(name, offset, period, priority, codefcn, data)");
return;
}
if (mxIsChar(prhs[0]) != 1) {
MEX_ERROR("ttCreatePeriodicTask: name must be a string");
return;
}
if (!mxIsDoubleScalar(prhs[1])) {
MEX_ERROR("ttCreatePeriodicTask: offset must be a double scalar");
return;
}
if (!mxIsDoubleScalar(prhs[2])) {
MEX_ERROR("ttCreatePeriodicTask: period must be a double scalar");
return;
}
if (!mxIsDoubleScalar(prhs[3])) {
MEX_ERROR("ttCreatePeriodicTask: priority must be a double scalar");
return;
}
if (mxIsChar(prhs[4]) != 1 || mxGetM(prhs[4]) != 1) {
MEX_ERROR("ttCreatePeriodicTask: codeFcn must be a non-empty string");
return;
}
char name[100];
mxGetString(prhs[0], name, 100);
double offset = *mxGetPr(prhs[1]);
if (offset < -EPS) {
offset = 0.0;
printf("ttCreatePeriodicTask: negative offset changed to zero\n");
}
double period = *mxGetPr(prhs[2]);
double priority = *mxGetPr(prhs[3]);
char codeFcn[100];
mxGetString(prhs[4], codeFcn, 100);
// Make sure that the code function exists in Matlab path
mxArray *lhs[1];
mxArray *rhs[1];
rhs[0] = mxDuplicateArray(prhs[4]);
mexCallMATLAB(1, lhs, 1, rhs, "exist");
int number = (int) *mxGetPr(lhs[0]);
if (number == 0) {
char buf[200];
sprintf(buf, "ttCreatePeriodicTask: codeFcn '%s' not in path! Task '%s' not created!", codeFcn, name);
MEX_ERROR(buf);
return;
}
if (ttCreatePeriodicTask(name, offset, period, priority, NULL)) {
// Add name of code function (m-file) and data variable
DataNode *n = (DataNode*) rtsys->taskList->getLast();
UserTask *usertask = (UserTask*) n->data;
usertask->codeFcnMATLAB = new char[strlen(codeFcn)+1];
strcpy(usertask->codeFcnMATLAB, codeFcn);
mxArray* data;
if (nrhs == 5) { // no data specified
data = mxCreateDoubleMatrix(0, 0, mxREAL);
} else {
data = mxDuplicateArray((mxArray*)prhs[5]);
}
mexMakeArrayPersistent(data);
usertask->dataMATLAB = data;
}
}
void init() {
// Read the input argument from the block dialogue
mxArray *initarg = ttGetInitArg();
if (!mxIsDoubleScalar(initarg)) {
TT_MEX_ERROR("The init argument must be a number!\n");
return;
}
int implementation = (int)mxGetPr(initarg)[0];
// Allocate KernelData memory and store pointer in kernel
KernelData *kd = new KernelData;
ttSetUserData(kd);
// Allocate memory for implementation != 2
TaskData *d = new TaskData;
kd->d = d; // Store pointer in KernelData
// Allocate memory for implementation 2
double *d2 = new double;
kd->d2 = d2; // Store pointer in KernelData
// Allocate memory for implementation 4
int *hdl_data = new int;
kd->hdl_data = hdl_data; // Store pointer in KernelData
// Initialize TrueTime kernel
ttInitKernel(prioFP);
// Task attributes
double starttime = 0.0;
double period = 0.006;
double deadline = period;
// Controller parameters and states
d->K = 0.96;
d->Ti = 0.12;
d->Td = 0.049;
d->beta = 0.5;
d->N = 10.0;
d->h = period;
d->u = 0.0;
d->t = 0.0; // only used for implementation 3
d->Iold = 0.0;
d->Dold = 0.0;
d->yold = 0.0;
d->rChan = 1;
d->yChan = 2;
d->uChan = 1;
switch (implementation) {
case 1:
// IMPLEMENTATION 1: using the built-in support for periodic tasks
ttCreatePeriodicTask("pid_task", starttime, period, pid_code1, d);
break;
case 2:
// IMPLEMENTATION 2: calling Simulink block within code function
ttCreatePeriodicTask("pid_task", starttime, period, pid_code2, d2);
break;
case 3:
// IMPLEMENTATION 3: sleepUntil and loop back
ttCreateTask("pid_task", deadline, pid_code3, d);
ttCreateJob("pid_task");
break;
case 4:
// IMPLEMENTATION 4: sampling in timer handler, triggers task job
*hdl_data = 2; // y_chan for reading samples
ttCreateHandler("timer_handler", 1, sampler_code, hdl_data);
ttCreatePeriodicTimer("timer", starttime, period, "timer_handler");
ttCreateMailbox("Samples", 10);
ttCreateTask("pid_task", deadline, pid_code4, d);
break;
}
}
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);
}
static void mdlInitializeSizes(SimStruct *S)
{
const mxArray *arg;
ssSetNumSFcnParams(S, 4); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Parameter mismatch will be reported by Simulink */
}
if (!ssSetNumInputPorts(S, 5)) return;
if (!ssSetNumOutputPorts(S, 1)) return;
SMsys *smsys = new SMsys;
smsys->trigger = 0.0;
arg = ssGetSFcnParam(S, 0);
if (mxIsDoubleScalar(arg)) {
smsys->networkNbr = (int)*mxGetPr(arg);
}
arg = ssGetSFcnParam(S, 1);
if (mxIsDoubleScalar(arg)) {
smsys->sender = (int)*mxGetPr(arg);
}
arg = ssGetSFcnParam(S, 2);
if (mxIsDoubleScalar(arg)) {
smsys->indim = (int)*mxGetPr(arg);
}
arg = ssGetSFcnParam(S,3);
if(mxIsDoubleScalar(arg)) {
smsys->dynamicSegment = (int)*mxGetPr(arg)-1; // set smsys->dynamicSegment = 0 (static) or 1 (dynamic)
}
/* Input ports */
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortWidth(S, 0, 1); // receiver
ssSetInputPortWidth(S, 1, smsys->indim); // data
ssSetInputPortWidth(S, 2, 1); // length
ssSetInputPortWidth(S, 3, 1); // prio
ssSetInputPortWidth(S, 4, 1); // msgID
/* Output Ports */
ssSetOutputPortWidth(S, 0, 1); // snd trigger
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetUserData(S, smsys);
ssSetOptions(S, SS_OPTION_CALL_TERMINATE_ON_EXIT);
}
