void init() {
// Initialize TrueTime kernel
ttInitKernel(1, 0, prioFP); // nbrOfInputs, nbrOfOutputs, fixed priority
// Create task data (local memory)
data = new PD_Data;
data->K = 1.5;
data->Td = 0.035;
data->N = 100000.0;
data->h = 0.010;
data->ad = data->Td/(data->N*data->h+data->Td);
data->bd = data->N*data->K*data->Td/(data->N*data->h+data->Td);
data->yold = 0.0;
data->Dold = 0.0;
data->u = 0.0;
// Controller task
double deadline = 0.010;
double prio = 2.0;
ttCreateTask("pid_task", deadline, prio, ctrl_code, data);
ttCreateJob("pid_task");
// Disturbance task (uncomment to add disturbance task)
// double offset = 0.0002;
// double period = 0.007;
// prio = 1.0;
// ttCreatePeriodicTask("dummy", offset, period, prio, dummy_code);
// Initialize network
ttCreateInterruptHandler("msgRcv", prio, msgRcvhandler);
ttInitNetwork(3, "msgRcv"); // node #3 in the network
ttCreateSemaphore("sem", 0);
}
void ttCreateTask(char *name, double deadline, double priority, double (*codeFcn)(int, void*), void* data) {
DataNode *n;
if (ttCreateTask(name, deadline, priority, codeFcn)) {
n = (DataNode*) rtsys->taskList->getLast();
((Task*) n->data)->data = data;
}
}
bool ttCreatePeriodicTask(char *name, double offset, double period, double priority, double (*codeFcn)(int, void*)) {
DataNode* dn;
InterruptHandler* hdl;
Timer *t;
if (ttCreateTask(name, period, priority, codeFcn)) {
dn = (DataNode*) rtsys->taskList->getLast(); // last created task
// Create interrupt handler invoked periodically to create task jobs
// see codefunctions.cpp for the code function
hdl = new InterruptHandler("perHandler");
hdl->codeFcn = rtsys->periodicTaskHandlerCode;
hdl->handlerID = rtsys->nbrOfHandlers + 1;
hdl->priority = -1000.0;
hdl->display = false;
hdl->data = (UserTask*) dn->data;
rtsys->handlerList->appendNode(new DataNode(hdl, NULL));
rtsys->nbrOfHandlers++;
// Creating periodic timer that triggers the handler
t = new Timer("perTimer");
t->time = offset; // First timer expiry at task offset
t->period = period;
t->isPeriodic = true;
hdl->timer = t;
hdl->moveToList(rtsys->timeQ);
hdl->type = TIMER;
((UserTask*) dn->data)->periodichandler = hdl;
return true;
} else {
return false;
}
}
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;
}
}
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 < 4 || nrhs > 5) {
MEX_ERROR("ttCreateTask: Wrong number of input arguments! \nUsage: ttCreateTask(name, deadline, priority, codefcn)\n ttCreateTask(name, deadline, priority, codefcn, data)");
return;
}
if (mxIsChar(prhs[0]) != 1) {
MEX_ERROR("ttCreateTask: name must be a string");
return;
}
if (!mxIsDoubleScalar(prhs[1])) {
MEX_ERROR("ttCreateTask: deadline must be a double scalar");
return;
}
if (!mxIsDoubleScalar(prhs[2])) {
MEX_ERROR("ttCreateTask: priority must be a double scalar");
return;
}
if (mxIsChar(prhs[3]) != 1 || mxGetM(prhs[3]) != 1) {
MEX_ERROR("ttCreateTask: codeFcn must be a non-empty string");
return;
}
char name[100];
mxGetString(prhs[0], name, 100);
double deadline = *mxGetPr(prhs[1]);
if (deadline < -EPS) {
deadline = 0.0;
printf("ttCreateTask: negative deadline changed to zero\n");
}
double priority = *mxGetPr(prhs[2]);
char codeFcn[100];
mxGetString(prhs[3], codeFcn, 100);
// Make sure that the code function exists in Matlab path
// and that the code function is syntactically correct.
mxArray *lhs[1];
mxArray *rhs[1];
rhs[0] = mxDuplicateArray(prhs[3]);
mexCallMATLAB(1, lhs, 1, rhs, "exist");
int number = (int) *mxGetPr(lhs[0]);
if (number == 0) {
char buf[200];
sprintf(buf, "ttCreateTask: codeFcn '%s' not in path! Task '%s' not created!", codeFcn, name);
MEX_ERROR(buf);
return;
}
if (ttCreateTask(name, deadline, 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 == 4) { // no data specified
data = mxCreateDoubleMatrix(0, 0, mxREAL);
} else {
data = mxDuplicateArray((mxArray*)prhs[4]);
}
mexMakeArrayPersistent(data);
usertask->dataMATLAB = data;
}
}
