mxArray* load_Model_optimization_info(boolean_T isRtwGen, boolean_T isModelRef,
boolean_T isExternal)
{
if (sOptimizationInfoFlags.isRtwGen != isRtwGen ||
sOptimizationInfoFlags.isModelRef != isModelRef ||
sOptimizationInfoFlags.isExternal != isExternal) {
unload_Model_optimization_info();
}
sOptimizationInfoFlags.isRtwGen = isRtwGen;
sOptimizationInfoFlags.isModelRef = isModelRef;
sOptimizationInfoFlags.isExternal = isExternal;
if (sRtwOptimizationInfoStruct==NULL) {
sRtwOptimizationInfoStruct = sf_load_rtw_optimization_info("Model", "Model");
mexMakeArrayPersistent(sRtwOptimizationInfoStruct);
}
return(sRtwOptimizationInfoStruct);
}
void ttSetDataAux(const char *nameOfTask, const mxArray *data) {
DataNode* dn = getNode(nameOfTask, rtsys->taskList);
if (dn == NULL) {
char buf[200];
sprintf(buf, "ttSetData: Non-existent task '%s'!", nameOfTask);
TT_MEX_ERROR(buf);
return;
}
UserTask* task = (UserTask*) dn->data;
if (task->dataMATLAB == NULL) {
char buf[200];
sprintf(buf, "ttSetData: Task '%s' doesn't have data!", nameOfTask);
TT_MEX_ERROR(buf);
return;
}
mxDestroyArray(task->dataMATLAB);
task->dataMATLAB = mxDuplicateArray(data);
mexMakeArrayPersistent(task->dataMATLAB);
}
static void mdlStart(SimStruct *S)
{
debugPrintf("'%s': mdlStart\n", S->path);
rtsys = (RTsys*) ssGetUserData(S);
// Display the TrueTime splash if global variable TTSPLASH not defined
mxArray* splvar = (mxArray*)mexGetVariablePtr("global", "TTSPLASH");
if (splvar == NULL) {
splvar = mxCreateDoubleMatrix(0, 0, mxREAL);
mexMakeArrayPersistent(splvar);
mexPutVariable("global", "TTSPLASH", splvar);
mexPrintf(
"--------------------------------------------------------------\n"
" Truetime 2.0 \n"
" Copyright (c) 2009 Lund University \n"
" Written by Anton Cervin, Dan Henriksson and Martin Ohlin, \n"
" Department of Automatic Control LTH, Lund University, Sweden \n"
"--------------------------------------------------------------\n"
);
}
if (rtsys->init_phase) {
/* Failure during initialization */
return;
}
/* DATA ALLOCATION */
if (rtsys->nbrOfTriggers > 0) {
rtsys->oldtriggerinputs = new double[rtsys->nbrOfTriggers];
}
if (rtsys->nbrOfNetworks > 0) {
rtsys->nwSnd = new double[rtsys->nbrOfNetworks];
rtsys->oldnwSnd = new double[rtsys->nbrOfNetworks];
rtsys->oldnetworkinputs = new double[rtsys->nbrOfNetworks];
}
}
void ttCallBlockSystem(int nOutp, double *outp, int nInp, double *inp, const char *blockName) {
mxArray *simsetRHS[8];
mxArray *simRHS[4];
mxArray *simLHS[3];
mxArray *options[1];
mxArray *states;
double *values;
double *state_values;
int i, j, n;
DataNode* dn;
Blockdata *bd;
// Make sure that the Simulink block exists in Matlab path
mxArray *lhs[1];
mxArray *rhs[1];
rhs[0] = mxCreateString(blockName);
mexCallMATLAB(1, lhs, 1, rhs, "exist");
int number = (int) *mxGetPr(lhs[0]);
if (number == 0) {
TT_MEX_ERROR("ttCallBlockSystem: Simulink block not in path!");
for (j=0; j < nOutp; j++) {
outp[j] = 0.0;
}
return;
}
Task* task = rtsys->running;
dn = (DataNode*) getNode(blockName, task->blockList);
if (dn==NULL) {
// Not found, create options mxArray (Matlab struct)
bd = new Blockdata(blockName);
simsetRHS[0] = mxCreateString("Solver");
simsetRHS[1] = mxCreateString("FixedStepDiscrete");
simsetRHS[2] = mxCreateString("FixedStep");
simsetRHS[3] = mxCreateDoubleScalar(1.0);
simsetRHS[4] = mxCreateString("MaxDataPoints");
simsetRHS[5] = mxCreateDoubleScalar(2.0);
simsetRHS[6] = mxCreateString("OutputVariables");
simsetRHS[7] = mxCreateString("xy");
mexCallMATLAB(1, options, 8, simsetRHS, "simset");
bd->options = options[0];
mexMakeArrayPersistent(bd->options);
for (i=0; i<8; i++) {
mxDestroyArray(simsetRHS[i]);
}
task->blockList->appendNode(new DataNode(bd, bd->blockName));
} else {
bd = (Blockdata*) dn->data;
}
// [t,x,outp] = sim(blockName,1,options,[0 inp])
simRHS[0] = mxCreateString(blockName);
simRHS[1] = mxCreateDoubleScalar(1.0);
simRHS[2] = bd->options;
simRHS[3] = mxCreateDoubleMatrix(1, nInp+1, mxREAL);
values = mxGetPr(simRHS[3]);
values[0] = 0.0;
for (j=0; j < nInp; j++) {
values[j+1] = inp[j];
}
i = mexCallMATLAB(3, simLHS, 4, simRHS, "sim");
mxDestroyArray(simRHS[0]);
mxDestroyArray(simRHS[1]);
mxDestroyArray(simRHS[3]);
if (i==0) { // Successful
// Update options mxArray with last row of returned state matrix
n = mxGetN(simLHS[1]); // Number of cols of state matrix
states = mxCreateDoubleMatrix(1, n, mxREAL);
values = mxGetPr(simLHS[1]);
state_values = mxGetPr(states);
// Transfer values
for (j=0; j < n; j++) {
state_values[j] = values[2*j+1]; // second row elements
}
mxArray* oldstates = mxGetField(bd->options, 0, "InitialState");
mxDestroyArray(oldstates);
mxSetField(bd->options, 0, "InitialState", states);
// Return the output of the simulation, first row of returned output matrix
values = mxGetPr(simLHS[2]);
for (j=0; j < nOutp; j++) {
outp[j] = values[2*j]; // first row elements
}
mxDestroyArray(simLHS[0]);
mxDestroyArray(simLHS[1]);
mxDestroyArray(simLHS[2]);
} else {
TT_MEX_ERROR("ttCallBlockSystem: Simulation failed!");
for (j=0; j < nOutp; j++) {
outp[j] = 0.0;
}
}
}
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;
}
void mexFunction(int nout, mxArray *output[],int nin, const mxArray *input[])
{
{
int s_m = mxGetM(input[0]);
int s_n = mxGetN(input[0]);
int i,j;
double *pr_int_d, *pr_int_n, *pr_int_dp, *pr_int_np, *pr_int_dn, *pr_int_nn;
double *pr_in = mxGetPr(input[0]);
double sigma = (*mxGetPr(input[1]));
double a_0,a_1,w_0,w_1,b_0,b_1,c_0,c_1;
double np[5],dp[5],nn[5],dn[5];
double accumulator, accumulator_norm;
double sum_incoming, sum_incoming_norm;
double sum_incoming_default;
int inner,offset;
double *pr_out, *pr_dx, *pr_dxx, *pr_dxy, *pr_dyy, *pr_dy;
output[0] = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
output[1] = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
output[2] = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
output[3] = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
output[4] = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
output[5] = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
pr_out = mxGetPr(output[0]);
pr_dx = mxGetPr(output[1]);
pr_dxx = mxGetPr(output[2]);
pr_dy = mxGetPr(output[3]);
pr_dyy = mxGetPr(output[4]);
pr_dxy = mxGetPr(output[5]);
mexAtExit(cleanup);
if (initialized==1)
{
int s_m_old = mxGetM(intermediate_norm_pos);
int s_n_old = mxGetN(intermediate_norm_pos);
if ((s_m_old!=s_m)||(s_n_old !=s_n ))
{
cleanup();
initialized =0;
}
}
if (!(initialized==1))
{
intermediate_norm_pos = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
intermediate_data_pos = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
intermediate_data_neg = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
intermediate_norm_neg = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
horizontal_smoothed = mxCreateDoubleMatrix(s_m,s_n,mxREAL);
mexMakeArrayPersistent(intermediate_data_pos);
mexMakeArrayPersistent(intermediate_norm_pos);
mexMakeArrayPersistent(intermediate_data_neg);
mexMakeArrayPersistent(intermediate_norm_neg);
mexMakeArrayPersistent(horizontal_smoothed);
initialized = 1;
}
// SETUP DERICHE FILTERS
a_0= 1.68;
a_1 = 3.735;
w_0 = .6318;
w_1 = 1.997;
b_0 = 1.783;
b_1 = 1.723;
c_0 = -.6803;
c_1 = -.2598;
np[0] = a_0 + c_0;
np[1] = exp(-b_1/sigma)*(c_1*sin(w_1/sigma) - (c_0 + 2*a_0)*cos(w_1/sigma)) + \
exp(-b_0/sigma)*(a_1*sin(w_0/sigma) - (2*c_0 + a_0)*cos(w_0/sigma));
np[2] = 2*exp(-b_0/sigma - b_1/sigma)*((a_0+c_0)*cos(w_1/sigma)*cos(w_0/sigma) -\
a_1*cos(w_1/sigma)*sin(w_0/sigma) - cos(w_0/sigma)*c_1*sin(w_1/sigma))\
+ c_0*exp(-2*b_0/sigma) + a_0*exp(-2*b_1/sigma);
np[3] = exp(-b_1/sigma - 2*b_0/sigma)*(c_1*sin(w_1/sigma) - c_0*cos(w_1/sigma)) +\
exp(-b_0/sigma - 2*b_1/sigma)*(a_1*sin(w_0/sigma) - a_0*cos(w_0/sigma));
np[4] = 0;
dp[4] = exp(-2*b_0/sigma - 2*b_1/sigma);
dp[3] = -2*cos(w_0/sigma)*exp(-b_0/sigma-2*b_1/sigma) - 2*cos(w_1/sigma)*exp(-b_1/sigma-2*b_0/sigma);
dp[2] = 4*cos(w_1/sigma)*cos(w_0/sigma)*exp(-b_0/sigma - b_1/sigma) + exp(-2*b_1/sigma) + exp(-2*b_0/sigma);
dp[1] = -2*exp(-b_1/sigma)*cos(w_1/sigma) - 2*exp(-b_0/sigma)*cos(w_0/sigma);
dp[0] = 1;
for (i=0; i<4;i++)
{
dn[i] = dp[i];
nn[i] = np[i] - dp[i]*np[0];
}
dn[4] = dp[4];
nn[4] = - dp[4]*np[0];
// APPLY HORIZONTAL FILTERS
//locator : j*size_m + i
pr_int_d = mxGetPr(intermediate_data_pos);
pr_int_n = mxGetPr(intermediate_norm_pos);
sum_incoming_default =np[0] + np[1] + np[2] + np[3] + np[4];
// scan each matrix line
for (i =0; i<s_m;i++)
{
// and apply the two iir filters of deriche
for (j=0;j<4;j++)
{
accumulator = 0;
accumulator_norm = 0;
sum_incoming = 0;
sum_incoming_norm = 0;
offset = j*s_m +i;
for (inner = 1; inner<=j; inner++)
{
accumulator = accumulator + dp[inner]*pr_int_d[offset - inner*s_m];
accumulator_norm = accumulator_norm + dp[inner]*pr_int_n[offset - inner*s_m];
}
for (inner = 0; inner<=j; inner++)
{
sum_incoming = sum_incoming + np[inner]*pr_in[offset - inner*s_m];
sum_incoming_norm = sum_incoming_norm + np[inner];
}
pr_int_d[j*s_m + i] = sum_incoming - accumulator;
pr_int_n[j*s_m + i] = sum_incoming_norm - accumulator_norm;
}
for (j=4; j<s_n; j++)
{
offset = j*s_m+i;
accumulator = -dp[1]*pr_int_d[offset - s_m] -dp[2]*pr_int_d[offset - 2*s_m] \
-dp[3]*pr_int_d[offset - 3*s_m] -dp[4]*pr_int_d[offset - 4*s_m];
accumulator_norm = -dp[1]*pr_int_n[offset - s_m] -dp[2]*pr_int_n[offset - 2*s_m] \
-dp[3]*pr_int_n[offset - 3*s_m] -dp[4]*pr_int_n[offset - 4*s_m];
sum_incoming = np[0]*pr_in[offset] + np[1]*pr_in[offset - s_m] + np[2]*pr_in[offset - 2*s_m] +\
np[3]*pr_in[offset -3*s_m] + np[4]*pr_in[offset - 4*s_m];
pr_int_d[offset] = sum_incoming + accumulator;
pr_int_n[offset] = sum_incoming_default + accumulator_norm;
}
}
pr_int_d = mxGetPr(intermediate_data_neg);
pr_int_n = mxGetPr(intermediate_norm_neg);
sum_incoming_default =nn[0] + nn[1] + nn[2] + nn[3] + nn[4];
// apply backward filter
for (i =0; i<s_m;i++)
{
for (j=0;j<4;j++)
{
accumulator = 0;
accumulator_norm = 0;
sum_incoming = 0;
sum_incoming_norm = 0;
offset = (s_n - j -1)*s_m +i;
for (inner = 1; inner<=j; inner++)
{
accumulator = accumulator + dn[inner]*pr_int_d[offset + inner*s_m];
accumulator_norm = accumulator_norm + dn[inner]*pr_int_n[offset + inner*s_m];
}
for (inner = 0; inner<=j; inner++)
{
sum_incoming = sum_incoming + nn[inner]*pr_in[offset + inner*s_m];
sum_incoming_norm = sum_incoming_norm + nn[inner];
}
pr_int_d[offset] = sum_incoming - accumulator;
pr_int_n[offset] = sum_incoming_norm - accumulator_norm;
}
for (j=4; j<s_n; j++)
{
offset = (s_n - j-1)*s_m +i;
accumulator = -dn[1]*pr_int_d[offset + s_m] -dn[2]*pr_int_d[offset + 2*s_m] \
-dn[3]*pr_int_d[offset + 3*s_m] -dn[4]*pr_int_d[offset + 4*s_m];
accumulator_norm = -dn[1]*pr_int_n[offset + s_m] -dn[2]*pr_int_n[offset + 2*s_m] \
-dn[3]*pr_int_n[offset + 3*s_m] -dn[4]*pr_int_n[offset + 4*s_m];
sum_incoming = nn[1]*pr_in[offset + s_m] + nn[2]*pr_in[offset + 2*s_m] +\
nn[3]*pr_in[offset + 3*s_m] + nn[4]*pr_in[offset + 4*s_m];
pr_int_d[offset] = sum_incoming + accumulator;
pr_int_n[offset] = sum_incoming_default + accumulator_norm;
}
}
pr_int_dp = mxGetPr(intermediate_data_pos);
pr_int_np = mxGetPr(intermediate_norm_pos);
pr_int_dn = mxGetPr(intermediate_data_neg);
pr_int_nn = mxGetPr(intermediate_norm_neg);
pr_in = mxGetPr(horizontal_smoothed);
for (i =0;i<s_m;i++)
for (j=0;j<s_n;j++)
{
offset = j*s_m + i;
pr_in[offset] = (pr_int_dp[offset] + pr_int_dn[offset])/(pr_int_np[offset] + pr_int_nn[offset]);
}
// -------------------------------------------------------------------------------
// NOW APPLY VERTICALLY
// -------------------------------------------------------------------------------
pr_int_d = mxGetPr(intermediate_data_pos);
pr_int_n = mxGetPr(intermediate_norm_pos);
sum_incoming_default =np[0] + np[1] + np[2] + np[3] + np[4];
// scan each matrix line
for (j =0; j<s_n;j++)
{
// and apply the two iir filters of deriche
for (i=0;i<4;i++)
{
accumulator = 0;
accumulator_norm = 0;
sum_incoming = 0;
sum_incoming_norm = 0;
offset = j*s_m +i;
for (inner = 1; inner<=i; inner++)
{
accumulator = accumulator + dp[inner]*pr_int_d[offset - inner];
accumulator_norm = accumulator_norm + dp[inner]*pr_int_n[offset - inner];
}
for (inner = 0; inner<=i; inner++)
{
sum_incoming = sum_incoming + np[inner]*pr_in[offset - inner];
sum_incoming_norm = sum_incoming_norm + np[inner];
}
pr_int_d[offset] = sum_incoming - accumulator;
pr_int_n[offset] = sum_incoming_norm - accumulator_norm;
}
for (i=4; i<s_m; i++)
{
offset = j*s_m+i;
accumulator = -dp[1]*pr_int_d[offset - 1] -dp[2]*pr_int_d[offset - 2] \
-dp[3]*pr_int_d[offset - 3] -dp[4]*pr_int_d[offset - 4];
accumulator_norm = -dp[1]*pr_int_n[offset -1] -dp[2]*pr_int_n[offset - 2] \
-dp[3]*pr_int_n[offset - 3] -dp[4]*pr_int_n[offset - 4];
sum_incoming = np[0]*pr_in[offset] + np[1]*pr_in[offset - 1] + np[2]*pr_in[offset - 2] +\
np[3]*pr_in[offset -3] + np[4]*pr_in[offset - 4];
pr_int_d[offset] = sum_incoming + accumulator;
pr_int_n[offset] = sum_incoming_default + accumulator_norm;
}
}
pr_int_d = mxGetPr(intermediate_data_neg);
pr_int_n = mxGetPr(intermediate_norm_neg);
sum_incoming_default =nn[0] + nn[1] + nn[2] + nn[3] + nn[4];
// apply backward filter
for (j =0; j<s_n;j++)
{
for (i=0;i<4;i++)
{
accumulator = 0;
accumulator_norm = 0;
sum_incoming = 0;
sum_incoming_norm = 0;
offset = (j)*s_m +(s_m - i -1);
for (inner = 1; inner<=i; inner++)
{
accumulator = accumulator + dn[inner]*pr_int_d[offset + inner];
accumulator_norm = accumulator_norm + dn[inner]*pr_int_n[offset + inner];
}
for (inner = 0; inner<=i; inner++)
{
sum_incoming = sum_incoming + nn[inner]*pr_in[offset + inner];
sum_incoming_norm = sum_incoming_norm + nn[inner];
}
pr_int_d[offset] = sum_incoming - accumulator;
pr_int_n[offset] = sum_incoming_norm - accumulator_norm;
}
for (i=4; i<s_m; i++)
{
offset = (j)*s_m +(s_m - i -1);
accumulator = -dn[1]*pr_int_d[offset + 1] -dn[2]*pr_int_d[offset + 2] \
-dn[3]*pr_int_d[offset + 3] -dn[4]*pr_int_d[offset + 4];
accumulator_norm = -dn[1]*pr_int_n[offset + 1] - dn[2]*pr_int_n[offset + 2] \
-dn[3]*pr_int_n[offset + 3] - dn[4]*pr_int_n[offset + 4];
sum_incoming = nn[1]*pr_in[offset + 1] + nn[2]*pr_in[offset + 2] +\
nn[3]*pr_in[offset + 3] + nn[4]*pr_in[offset + 4];
pr_int_d[offset] = sum_incoming + accumulator;
pr_int_n[offset] = sum_incoming_default + accumulator_norm;
}
}
pr_int_dp = mxGetPr(intermediate_data_pos);
pr_int_np = mxGetPr(intermediate_norm_pos);
pr_int_dn = mxGetPr(intermediate_data_neg);
pr_int_nn = mxGetPr(intermediate_norm_neg);
for (i =0;i<s_m;i++)
for (j=0;j<s_n;j++)
{
offset = j*s_m + i;
pr_out[offset] = (pr_int_dp[offset] + pr_int_dn[offset])/(pr_int_np[offset] + pr_int_nn[offset]); // + pr_int_dn[offset]);
}
for (i=0;i<s_m;i++)
{
for (j=1;j<s_n-1;j++)
{
offset = j*s_m +i;
pr_dx[offset] = (pr_out[offset+s_m] - pr_out[offset-s_m])/2;
}
pr_dx[i] = 0;
pr_dx[(s_n-1)*s_m +i] = 0;
}
for (i=0;i<s_m;i++)
{
for (j=1;j<s_n-1;j++)
{
offset = j*s_m +i;
pr_dxx[offset] = pr_out[offset+s_m] + pr_out[offset-s_m] - 2*pr_out[offset];
}
pr_dxx[i] = 0;
pr_dxx[(s_n-1)*s_m +i] = 0;
}
for (j=0;j<s_n;j++)
{
for (i=1;i<s_m-1;i++)
{
offset = j*s_m +i;
pr_dy[offset] = (pr_out[offset+1] - pr_out[offset-1])/2;
}
pr_dy[j*s_m] = 0;
pr_dy[j*s_m + (s_m-1)] = 0;
}
for (j=0;j<s_n;j++)
{
for (i=1;i<s_m-1;i++)
{
offset = j*s_m +i;
pr_dyy[offset] = pr_out[offset+1] + pr_out[offset-1] -2*pr_out[offset];
}
pr_dyy[j*s_m] = 0;
pr_dyy[j*s_m + (s_m-1)] = 0;
}
// remember to change the boundary
for (j=0;j<s_n;j++)
{
for (i=1;i<s_m-1;i++)
{
offset = j*s_m +i;
pr_dxy[offset] = (pr_dx[offset+1] - pr_dx[offset-1])/2;
}
pr_dxy[j*s_m] = 0;
pr_dxy[j*s_m + (s_m-1)] = 0;
}
}
}
void mcaMonitorEventHandler(struct event_handler_args arg)
{ union db_access_val *pBuf;
struct mca_channel* PCHNL = (struct mca_channel*)arg.usr;
double *myDblPr;
chtype RequestType;
int i,Cnt;
mxArray* mymxArray;
pBuf = (union db_access_val *)arg.dbr;
//mexPrintf("MCA Monitor Event Handler Called\n");
PCHNL->MonitorEventCount++;
Cnt = ca_element_count(arg.chid);
RequestType = dbf_type_to_DBR(ca_field_type(arg.chid)); // Closest to the native
if(RequestType==DBR_STRING)
{ if(Cnt==1)
{ mxDestroyArray(PCHNL->CACHE); // clear mxArray that pointed to by PCHNL->CACHE
PCHNL->CACHE = mxCreateString((char*)((pBuf)->strval)); // Create new array with current string value
mexMakeArrayPersistent(PCHNL->CACHE);
}
else
{ for(i=0;i<Cnt;i++)
{ mymxArray = mxGetCell(PCHNL->CACHE,i);
mxDestroyArray(mymxArray);
mymxArray = mxCreateString( (char*) (&(pBuf->strval)+i) );
mexMakeArrayPersistent(mymxArray);
mxSetCell(PCHNL->CACHE,i,mymxArray);
}
}
}
else
{ myDblPr = mxGetPr(PCHNL->CACHE);
switch(RequestType)
{ case DBR_INT: // As defined in db_access.h DBR_INT = DBR_SHORT = 1
for (i = 0; i<Cnt;i++)
myDblPr[i]= (double)(*(&((pBuf)->intval)+i));
break;
case DBR_FLOAT:
for (i = 0; i<Cnt;i++)
myDblPr[i]= (double)(*(&((pBuf)->fltval)+i));
break;
case DBR_ENUM:
for (i = 0; i<Cnt;i++)
myDblPr[i]= (double)(*(&((pBuf)->enmval)+i));
break;
case DBR_CHAR:
for (i = 0; i<Cnt;i++)
myDblPr[i]= (double)(*(&((pBuf)->charval)+i));
break;
case DBR_LONG:
for (i = 0; i<Cnt;i++)
myDblPr[i]= (double)(*(&((pBuf)->longval)+i));
break;
case DBR_DOUBLE:
for (i = 0; i<Cnt;i++)
myDblPr[i]= (double)(*(&((pBuf)->doubleval)+i));
break;
}
}
if(PCHNL->MonitorCBString)
{
mexEvalString(PCHNL->MonitorCBString);
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
//mexPrintf("mdlOutputs at %g\n", ssGetT(S));
SMsys *smsys = (SMsys*) ssGetUserData(S);
RTnetwork *nwsys = smsys->nwsys;
// Read receiver input port
int receiver = (int)*ssGetInputPortRealSignalPtrs(S,0)[0];
if (receiver < 0 || receiver > smsys->nwsys->nbrOfNodes) {
mexPrintf("Receiver number %d out of bounds\n", smsys->sender);
ssSetErrorStatus(S, "ttsend: receiver number out of bounds");
return;
}
// Read data input port and create mxArray
mxArray *data = mxCreateDoubleMatrix(smsys->indim, 1, mxREAL);
for (int i=0; i<smsys->indim; i++) {
mxGetPr(data)[i] = *ssGetInputPortRealSignalPtrs(S,1)[i];
}
mexMakeArrayPersistent(data);
// Read length input port
int length = (int)*ssGetInputPortRealSignalPtrs(S,2)[0];
if (length < 0) length = 0;
// Read prio input port
double prio = (int)*ssGetInputPortRealSignalPtrs(S,3)[0];
// Read msgID input port
int msgID = (int)*ssGetInputPortRealSignalPtrs(S,4)[0];
// Create a network packet
NWmsg *nwmsg = new NWmsg();
nwmsg->sender = smsys->sender-1;
nwmsg->receiver = receiver-1;
nwmsg->data = NULL;
nwmsg->dataMATLAB = data;
nwmsg->length = length;
nwmsg->prio = prio;
nwmsg->timestamp = ssGetT(S);
nwmsg->signalPower = 0.0; // default, changed by wireless networks
nwmsg->msgID = msgID;
// do the dirty work: poke around inside nwsys of the network block
// set time when finished preprocessing
nwmsg->waituntil = ssGetT(S) + nwsys->nwnodes[nwmsg->sender]->predelay;
nwmsg->collided = 0; // This message has not collided (802.11)
// enqueue message in preprocQ
if (smsys->dynamicSegment == 0) {//node sending in the static segment
nwsys->nwnodes[nwmsg->sender]->statpreprocQ->appendNode(nwmsg);
} else {
nwsys->nwnodes[nwmsg->sender]->preprocQ->appendNode(nwmsg);
}
if (smsys->trigger == 0.0) {
smsys->trigger = 1.0; // trigger snd output
} else {
smsys->trigger = 0.0; // trigger snd output
}
ssGetOutputPortRealSignal(S,0)[0] = smsys->trigger;
}
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;
}
}
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;
}
static void mdlTerminate(SimStruct *S)
{
debugPrintf("'%s': mdlTerminate\n", S->path);
rtsys = (RTsys*) ssGetUserData(S);
if (rtsys == NULL) {
return;
}
// Write task execution logs to the MATLAB workspace
if (rtsys->taskList != NULL) {
DataNode *dn = (DataNode*) rtsys->taskList->getFirst();
while (dn != NULL) {
UserTask *task = (UserTask*) dn->data;
for (int j=0; j<NBRLOGS; j++) {
Log *log = task->logs[j];
if (log) {
mxArray *ptr = mxCreateDoubleMatrix(log->entries, 1, mxREAL);
for (int n=0; n<log->entries; n++) {
mxGetPr(ptr)[n] = log->vals[n];
}
mexMakeArrayPersistent(ptr);
mexPutVariable("base",log->variable,ptr);
}
}
dn = (DataNode*) dn->getNext();
}
}
// Write user-defined logs to the MATLAB workspace
if (rtsys->logList != NULL) {
DataNode *dn = (DataNode*) rtsys->logList->getFirst();
while (dn != NULL) {
Log *log = (Log*) dn->data;
mxArray *ptr = mxCreateDoubleMatrix(log->entries, 1, mxREAL);
for (int n=0; n<log->entries; n++) {
mxGetPr(ptr)[n] = log->vals[n];
}
mexMakeArrayPersistent(ptr);
mexPutVariable("base",log->variable,ptr);
dn = (DataNode*) dn->getNext();
}
}
#ifdef KERNEL_MATLAB
mxDestroyArray(rtsys->segArray);
#else
cleanup();
#endif
// Delete rtsys and all data structures within
delete rtsys;
#ifdef KERNEL_MATLAB
mxArray* rhs[2];
rhs[0] = mxCreateString("global");
rhs[1] = mxCreateString("_rtsys");
mexCallMATLAB(0, NULL, 2, rhs, "clear");
#endif
}
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[])
{
int i,j, k, status,buflen,Cnt, Hndl, L,M,N, NumHandles, commandswitch;
int *HndlArray;
mxArray *mymxArray;
double *myDblPr;
chtype RequestType;
char PVName[PV_NAME_LENGTH_MAX+1];
// char MCAMessageString[MCA_MESSAGE_STRING_LENGTH_MAX+1];
dbr_string_t StrBuffer;
const char *MCAInfoFields[]={"PVName","ElementCount","NativeType","State","MCAMessage","Host"};
char *NativeTypeStrings[] = {"STRING","INT","FLOAT","ENUM","CHAR","LONG","DOUBLE"};
if(!CA_INITIALIZED) // Initialize CA if not initialized (first call)
{ mexPrintf("Initializing MATLAB Channel Access ... \n");
status = ca_task_initialize();
if(status!=ECA_NORMAL)
mexErrMsgTxt("Unable to initialise Challel Access\n");
CA_INITIALIZED = true;
// Register a function to be called when a this mex-file is cleared from memory
// with 'clear' or when exitting MATLAB
mexAtExit(mca_cleanup);
// Lock the mex-file so that it can not be cleared without explicitly
// mexUnclock
mexLock();
//start periodic polling:
/* PollTimerHandle = SetTimer(NULL,NULL,MCA_POLL_PERIOD,background_poll);
if(PollTimerHandle)
mexPrintf("Periodic CA polling started! System Timer ID: %u\n",PollTimerHandle);
else
mexWarnMsgTxt("Failed to start periodic CA polling\n");
*/
}
commandswitch = (int)mxGetScalar(prhs[0]);
switch(commandswitch)
{ case 0:
mexUnlock();
break;
case 1: // MCAOPEN - add channel(s) by PV names, all arguments following prhs[0]
// must be strings - names of PV's
for(i=1;i<nrhs;i++)
{ mxGetString(prhs[i],PVName,PV_NAME_LENGTH_MAX+1);
status = ca_search(PVName,&(CHNLS[HandlesUsed].CHID));
if(status == ECA_NORMAL) // if not - go on to the next PV name
{ status = ca_pend_io(MCA_SEARCH_TIMEOUT);
if (status == ECA_NORMAL)
{ // Allocate persistent memory for the DataBuffer on this channel
// to hold all elements of the DBR_XXX type
// nearest to the native type
// RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[HandlesUsed].CHID));
// Cnt=ca_element_count(CHNLS[HandlesUsed].CHID);
CHNLS[HandlesUsed].NumElements = ca_element_count(CHNLS[HandlesUsed].CHID);
CHNLS[HandlesUsed].NativeType2DBR = dbf_type_to_DBR(ca_field_type(CHNLS[HandlesUsed].CHID));
CHNLS[HandlesUsed].MonitorEventCount = 0;
switch(CHNLS[HandlesUsed].NativeType2DBR)
{ case DBR_STRING:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_string_t));
break;
case DBR_INT: // As defined in db_access.h DBR_INT = DBR_SHORT = 1
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_short_t));
break;
case DBR_FLOAT:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_float_t));
break;
case DBR_ENUM:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_enum_t));
break;
case DBR_CHAR:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_char_t));
break;
case DBR_LONG:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_short_t));
break;
case DBR_DOUBLE:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_double_t));
break;
}
mexMakeMemoryPersistent(CHNLS[HandlesUsed].DataBuffer);
if(CHNLS[HandlesUsed].NativeType2DBR==DBR_STRING) // CACHE
{ if(CHNLS[HandlesUsed].NumElements==1) // Create MATLAB string - originally empty
CHNLS[HandlesUsed].CACHE = mxCreateString("");
else // Create MATLAB cell array of strings
{ CHNLS[HandlesUsed].CACHE = mxCreateCellMatrix(1,CHNLS[HandlesUsed].NumElements);
for(k=0;k<CHNLS[HandlesUsed].NumElements;k++)
{ mymxArray = mxCreateString("");
mexMakeArrayPersistent(mymxArray);
mxSetCell(CHNLS[HandlesUsed].CACHE, k, mymxArray);
}
}
}
else // Make CACHE a numeric mxArray
{ CHNLS[HandlesUsed].CACHE = mxCreateDoubleMatrix(1,CHNLS[HandlesUsed].NumElements,mxREAL);
}
mexMakeArrayPersistent(CHNLS[HandlesUsed].CACHE);
plhs[i-1]=mxCreateScalarDouble(++HandlesUsed);
}
else
plhs[i-1]=mxCreateScalarDouble(0);
}
else
plhs[i-1]=mxCreateScalarDouble(0);
} break;
case 2:// MCAOPEN - add channel(s) by PV names. The arguments following prhs[0]
// argument must be a cell array of strings - PV names
L = mxGetM(prhs[1])*mxGetN(prhs[1]);
plhs[0] = mxCreateDoubleMatrix(1,L,mxREAL);
myDblPr = mxGetPr(plhs[0]);
for(i=0;i<L;i++)
{ mymxArray = mxGetCell(prhs[1],i);
mxGetString(mymxArray,PVName,PV_NAME_LENGTH_MAX+1);
status = ca_search(PVName,&(CHNLS[HandlesUsed].CHID));
if(status == ECA_NORMAL) // if not - go on to the next PV name
{ status = ca_pend_io(MCA_IO_TIMEOUT);
if (status == ECA_NORMAL)
{ // Allcate persistent memory for the DataBuffer on this channel
//RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[HandlesUsed].CHID));
CHNLS[HandlesUsed].NativeType2DBR = dbf_type_to_DBR(ca_field_type(CHNLS[HandlesUsed].CHID));
CHNLS[HandlesUsed].NumElements = ca_element_count(CHNLS[HandlesUsed].CHID);
CHNLS[HandlesUsed].MonitorEventCount = 0;
//Cnt=ca_element_count(CHNLS[HandlesUsed].CHID);
switch(CHNLS[HandlesUsed].NativeType2DBR)
{ case DBR_STRING:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_string_t));
break;
case DBR_INT: // As defined in db_access.h DBR_INT = DBR_SHORT = 1
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_short_t));
break;
case DBR_FLOAT:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_float_t));
break;
case DBR_ENUM:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_enum_t));
break;
case DBR_CHAR:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_char_t));
break;
case DBR_LONG:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_short_t));
break;
case DBR_DOUBLE:
CHNLS[HandlesUsed].DataBuffer = mxCalloc(CHNLS[HandlesUsed].NumElements,sizeof(dbr_double_t));
break;
}
mexMakeMemoryPersistent(CHNLS[HandlesUsed].DataBuffer);
if(CHNLS[HandlesUsed].NativeType2DBR == DBR_STRING) // CACHE
{ CHNLS[HandlesUsed].CACHE = mxCreateCellMatrix(1,CHNLS[HandlesUsed].NumElements);
for(k=0;k<CHNLS[HandlesUsed].NumElements;k++)
{ mymxArray = mxCreateString(StrBuffer);
mexMakeArrayPersistent(mymxArray);
mxSetCell(CHNLS[HandlesUsed].CACHE, k, mymxArray);
}
}
else
{ CHNLS[HandlesUsed].CACHE = mxCreateDoubleMatrix(1,CHNLS[HandlesUsed].NumElements,mxREAL);
}
mexMakeArrayPersistent(CHNLS[HandlesUsed].CACHE);
myDblPr[i] = ++HandlesUsed;
}
else
myDblPr[i] = 0;
}
else
myDblPr[i] = 0;
} break;
case 3: // MCAOPEN Return names of connected channels as cell array of strings
plhs[0] = mxCreateCellArray(1, &HandlesUsed);
for(i=0;i<HandlesUsed;i++)
{ if(CHNLS[i].CHID!=NULL)
{ mymxArray = mxCreateString(ca_name(CHNLS[i].CHID));
mxSetCell(plhs[0], i, mymxArray);
}
else
{ mymxArray = mxCreateString("");
//mexPrintf("Handle: %d PV: %s\n",i+1, "Cleared Channel");
mxSetCell(plhs[0], i, mymxArray);
}
} break;
case 5: // MCACLOSE permanently clear channel
Hndl = (int)mxGetScalar(prhs[1]);
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Handle out of range");
// If a monitor is installed, set the EVID pointer to NULL
// ca_clear_event dos not do it by itself
if(CHNLS[Hndl-1].EVID)
CHNLS[Hndl-1].EVID = NULL;
// If there is Callback String - destroy it
if(CHNLS[Hndl-1].MonitorCBString)
{ mxFree(CHNLS[Hndl-1].MonitorCBString);
CHNLS[Hndl-1].MonitorCBString =NULL;
}
if(ca_state(CHNLS[Hndl-1].CHID)==3)
mexWarnMsgTxt("Channel previously cleared");
else
if(ca_clear_channel(CHNLS[Hndl-1].CHID)!=ECA_NORMAL)
mexErrMsgTxt("ca_clear_channel failed");
break;
case 10: // MCAINFO return channels info as MATLAB structure array
if(HandlesUsed>0)
{ plhs[0] = mxCreateStructMatrix(1,HandlesUsed,6,MCAInfoFields);
for(i=0;i<HandlesUsed;i++)
{ mxSetFieldByNumber(plhs[0],i,0,mxCreateString(ca_name(CHNLS[i].CHID)));
mxSetFieldByNumber(plhs[0],i,1,mxCreateScalarDouble(ca_element_count(CHNLS[i].CHID)));
mxSetFieldByNumber(plhs[0],i,5,mxCreateString(ca_host_name(CHNLS[i].CHID)));
switch(ca_state(CHNLS[i].CHID))
{ case 1: // Disconnected due to Server or Network - may reconnect
mxSetFieldByNumber(plhs[0],i,2,mxCreateString("unknown"));
mxSetFieldByNumber(plhs[0],i,3,mxCreateString("disconnected"));
mxSetFieldByNumber(plhs[0],i,4,mxCreateString("Disconnected due to server or network problem"));
break;
case 2: // Normal connection
mxSetFieldByNumber(plhs[0],i,2,mxCreateString(NativeTypeStrings[ca_field_type(CHNLS[i].CHID)]));
mxSetFieldByNumber(plhs[0],i,3,mxCreateString("connected"));
mxSetFieldByNumber(plhs[0],i,4,mxCreateString("Normal connection"));
break;
case 3: // Disconnected by user
mxSetFieldByNumber(plhs[0],i,2,mxCreateString("unknown"));
mxSetFieldByNumber(plhs[0],i,3,mxCreateString("disconnected"));
mxSetFieldByNumber(plhs[0],i,4,mxCreateString("Permanently disconnected (cleared) by the user"));
break;
}
}
}
else
{ mexWarnMsgTxt("No connected PV's found");
plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
}
break;
case 11: // MCAINFO return info for 1 channel by handle number
Hndl = (int)mxGetScalar(prhs[1]);
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Handle out of range");
plhs[0] = mxCreateStructMatrix(1,1,6,MCAInfoFields);
mxSetFieldByNumber(plhs[0],0,0,mxCreateString(ca_name(CHNLS[Hndl-1].CHID)));
mxSetFieldByNumber(plhs[0],0,1,mxCreateScalarDouble(ca_element_count(CHNLS[Hndl-1].CHID)));
mxSetFieldByNumber(plhs[0],0,5,mxCreateString(ca_host_name(CHNLS[Hndl-1].CHID)));
switch(ca_state(CHNLS[Hndl-1].CHID))
{ case 1: // Disconnected due to Server or Network - may reconnect
mxSetFieldByNumber(plhs[0],0,2,mxCreateString("unknown"));
mxSetFieldByNumber(plhs[0],0,3,mxCreateString("disconnected"));
mxSetFieldByNumber(plhs[0],0,4,mxCreateString("Disconnected due to server or network problem"));
break;
case 2: // Normal connection
mxSetFieldByNumber(plhs[0],0,2,mxCreateString(NativeTypeStrings[ca_field_type(CHNLS[Hndl-1].CHID)]));
mxSetFieldByNumber(plhs[0],0,3,mxCreateString("connected"));
mxSetFieldByNumber(plhs[0],0,4,mxCreateString("Normal connection"));
break;
case 3: // Disconnected by user
mxSetFieldByNumber(plhs[0],0,2,mxCreateString("unknown"));
mxSetFieldByNumber(plhs[0],0,3,mxCreateString("disconnected"));
mxSetFieldByNumber(plhs[0],0,4,mxCreateString("Permanently disconnected (cleared) by the user"));
break;
};
break;
case 12: // MCASTATE return an array of status (1 - OK, 0 - disconnected or cleared)
if(HandlesUsed>0)
{ plhs[0] = mxCreateDoubleMatrix(1,HandlesUsed,mxREAL);
myDblPr = mxGetPr(plhs[0]);
for(i=0;i<HandlesUsed;i++)
myDblPr[i] = (double)(ca_state(CHNLS[i].CHID)==2);
}
else
{ mexWarnMsgTxt("No connected PV's found");
plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
}
break;
case 30: // poll
ca_poll();
break;
case 50: // MCAGET Get PV values by their MCA handles
for(i=0;i<nrhs-1;i++) // First loop: place all ca_get requests in the buffer
{ Hndl = (int)mxGetScalar(prhs[1+i]); //start from[1]: [0] argument is the commnads switch
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Invalid Handle");
RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[Hndl-1].CHID));
Cnt = ca_element_count(CHNLS[Hndl-1].CHID);
status = ca_array_get(RequestType,Cnt,CHNLS[Hndl-1].CHID,CHNLS[Hndl-1].DataBuffer);
if(status!=ECA_NORMAL)
mexPrintf("Error in call to ca_array_get\n");
}
status = ca_pend_io(MCA_GET_TIMEOUT);
if(status!=ECA_NORMAL)
mexErrMsgTxt("... ca_pend_io call timed out \n");
for(i=0;i<nrhs-1;i++) // Another loop to copy data from temp structures to MATLAB
{ Hndl = (int)mxGetScalar(prhs[1+i]);
RequestType = RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[Hndl-1].CHID));
Cnt = ca_element_count(CHNLS[Hndl-1].CHID);
if(RequestType==DBR_STRING)
{ if(Cnt==1)
plhs[i] = mxCreateString((char*)(*((dbr_string_t*)(CHNLS[Hndl-1].DataBuffer))));
else
{ plhs[i] = mxCreateCellMatrix(1,Cnt);
for(j=0;j<Cnt;j++)
mxSetCell(plhs[i], j, mxCreateString((char*)(*((dbr_string_t*)(CHNLS[Hndl-1].DataBuffer)+j))));
}
}
else
{ plhs[i] = mxCreateDoubleMatrix(1,Cnt,mxREAL);
myDblPr = mxGetPr(plhs[i]);
switch(dbf_type_to_DBR(ca_field_type(CHNLS[Hndl-1].CHID)))
{ case DBR_INT: // As defined in db_access.h DBR_INT = DBR_SHORT = 1
for(j=0;j<Cnt;j++)
myDblPr[j]= (double)(*((dbr_short_t*)(CHNLS[Hndl-1].DataBuffer)+j));
break;
case DBR_FLOAT:
for(j=0;j<Cnt;j++)
myDblPr[j]= (double)(*((dbr_float_t*)(CHNLS[Hndl-1].DataBuffer)+j));
break;
case DBR_ENUM:
for(j=0;j<Cnt;j++)
myDblPr[j]= (double)(*((dbr_enum_t*)(CHNLS[Hndl-1].DataBuffer)+j));
break;
case DBR_CHAR:
for(j=0;j<Cnt;j++)
myDblPr[j]= (double)(*((dbr_char_t*)(CHNLS[Hndl-1].DataBuffer)+j));
break;
case DBR_LONG:
for(j=0;j<Cnt;j++)
myDblPr[j]= (double)(*((dbr_long_t*)(CHNLS[Hndl-1].DataBuffer)+j));
break;
case DBR_DOUBLE:
for(j=0;j<Cnt;j++)
myDblPr[j]= (double)(*((dbr_double_t*)(CHNLS[Hndl-1].DataBuffer)+j));
break;
}
}
} break;
case 51: // MCAGET Get scalar PV of the same type
// second argument is an array of handles
// returns an array of values
myDblPr = mxGetPr(prhs[1]);
M = mxGetM(prhs[1]);
N = mxGetN(prhs[1]);
NumHandles = M*N;
plhs[0] = mxCreateDoubleMatrix(M,N,mxREAL);
for(i=0;i<NumHandles;i++) // First loop: place all ca_get requests in the buffer
{ Hndl = (int)myDblPr[i];
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Invalid Handle");
RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[Hndl-1].CHID));
status = ca_array_get(DBR_DOUBLE,1,CHNLS[Hndl-1].CHID,mxGetPr(plhs[0])+i);
if(status!=ECA_NORMAL)
mexPrintf("Error in call to ca_array_get\n");
}
status = ca_pend_io(MCA_GET_TIMEOUT);
if(status!=ECA_NORMAL)
mexErrMsgTxt("... ca_pend_io call timed out \n");
break;
case 70: // MCAPUT
NumHandles = (nrhs-1)/2;
for(i=0;i<NumHandles;i++)
{ j = 2+i*2;
Hndl = (int)mxGetScalar(prhs[1+i*2]);
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Handle out of range - no values written");
// Set the status to 0 - mcaput_callback will write 1, if successful
CHNLS[Hndl-1].LastPutStatus = 0;
RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[Hndl-1].CHID));
Cnt = ca_element_count(CHNLS[Hndl-1].CHID);
// If a value to write is passed as a string - the number of elements to write
// is 1 , NOT the length of the string returned by mxGetNumberOfElements
if(mxIsChar(prhs[j]))
L=1;
else
L = min(mxGetNumberOfElements(prhs[j]),Cnt);
// Copy double or string data from MATLAB prhs[] to DataBuffer
// on each channel
if(RequestType==DBR_STRING)
{ // STRING type is is passed as a cell array of strings
// A a 1-row MATLAB character array (1 string) may also be passed as a value
if(mxIsChar(prhs[j]))
{ mxGetString(prhs[j], StrBuffer, sizeof(dbr_string_t));
strcpy((char*)(*((dbr_string_t*)(CHNLS[Hndl-1].DataBuffer))),StrBuffer);
}
else if(mxIsCell(prhs[j]))
{ for(k=0;k<L;k++)
{ mxGetString(mxGetCell(prhs[j],k), StrBuffer, sizeof(dbr_string_t));
strcpy((char*)(*((dbr_string_t*)(CHNLS[Hndl-1].DataBuffer)+k)),StrBuffer);
}
}
}
else
{ myDblPr = mxGetPr(prhs[j]);
switch(RequestType)
{
case DBR_INT: // As defined in db_access.h DBR_INT = DBR_SHORT = 1
for(k=0;k<L;k++)
*((dbr_short_t*)(CHNLS[Hndl-1].DataBuffer)+k) = (dbr_short_t)(myDblPr[k]);
break;
case DBR_FLOAT:
for(k=0;k<L;k++)
*((dbr_float_t*)(CHNLS[Hndl-1].DataBuffer)+k) = (dbr_float_t)(myDblPr[k]);
break;
case DBR_ENUM:
for(k=0;k<L;k++)
*((dbr_enum_t*)(CHNLS[Hndl-1].DataBuffer)+k) = (dbr_enum_t)(myDblPr[k]);
break;
case DBR_CHAR:
for(k=0;k<L;k++)
*((dbr_char_t*)(CHNLS[Hndl-1].DataBuffer)+k) = (dbr_char_t)(myDblPr[k]);
break;
case DBR_LONG:
for(k=0;k<L;k++)
*((dbr_long_t*)(CHNLS[Hndl-1].DataBuffer)+k) = (dbr_long_t)(myDblPr[k]);
break;
case DBR_DOUBLE:
for(k=0;k<L;k++)
*((dbr_double_t*)(CHNLS[Hndl-1].DataBuffer)+k) = (dbr_double_t)(myDblPr[k]);
break;
}
}
// place request in the que
status = ca_array_put_callback(RequestType,L,CHNLS[Hndl-1].CHID,CHNLS[Hndl-1].DataBuffer,
mcaput_callback,&(CHNLS[Hndl-1].LastPutStatus));
if(status!=ECA_NORMAL)
mexPrintf("ca_array_put_callback failed\n");
}
status = ca_pend_event(MCA_PUT_TIMEOUT);
plhs[0]=mxCreateDoubleMatrix(1,NumHandles,mxREAL);
myDblPr = mxGetPr(plhs[0]);
for(i=0;i<NumHandles;i++)
{ Hndl = (int)mxGetScalar(prhs[1+i*2]);
myDblPr[i] = (double)CHNLS[Hndl-1].LastPutStatus;
}
break;
case 80: // MCAPUT - fast unconfirmed put for scalar numeric PV's
myDblPr = mxGetPr(prhs[1]);
M = mxGetM(prhs[1]);
N = mxGetN(prhs[1]);
NumHandles = M*N;
plhs[0] = mxCreateDoubleMatrix(M,N,mxREAL);
myDblPr = mxGetPr(plhs[0]);
for(i=0;i<NumHandles;i++)
{ myDblPr = mxGetPr(plhs[0]);
Hndl = (int)(*(mxGetPr(prhs[1])+i));
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Handle out of range - no values written");
status = ca_array_put(DBR_DOUBLE,1,CHNLS[Hndl-1].CHID,mxGetPr(prhs[2])+i);
if(status!=ECA_NORMAL)
{ myDblPr[i] = 0;
//mexPrintf("ca_array_put_callback failed\n");
}
else
{ myDblPr[i] = 1;
}
}
status = ca_pend_io(MCA_PUT_TIMEOUT);
break;
case 100: // MCAMON install Monitor or replace MonitorCBString
Hndl = (int)mxGetScalar(prhs[1]);
// Check if the handle is within range
if(Hndl<1 || Hndl>HandlesUsed)
{ plhs[0]=mxCreateScalarDouble(0);
mexErrMsgTxt("Invalid Handle");
}
if(CHNLS[Hndl-1].EVID) // if VID is not NULL - another monitor is already installed - replace MonitorCBString
{ if(CHNLS[Hndl-1].MonitorCBString) // Free memory for occupied by the old MonitorCBString
{ mxFree(CHNLS[Hndl-1].MonitorCBString);
CHNLS[Hndl-1].MonitorCBString = NULL;
}
if(nrhs>2) // Check if the new string is specified
{ if(mxIsChar(prhs[2]))
{ buflen = mxGetM(prhs[2])*mxGetN(prhs[2])+1;
CHNLS[Hndl-1].MonitorCBString = (char *)mxMalloc(buflen);
mexMakeMemoryPersistent(CHNLS[Hndl-1].MonitorCBString);
mxGetString(prhs[2],CHNLS[Hndl-1].MonitorCBString,buflen);
}
else
mexErrMsgTxt("Third argument must be a string\n");
}
plhs[0]=mxCreateScalarDouble(1);
}
else // No monitor is presently installed;
{ RequestType = dbf_type_to_DBR(ca_field_type(CHNLS[Hndl-1].CHID)); // Closest to the native
if(nrhs>2)
{ if(mxIsChar(prhs[2]))
{ buflen = mxGetM(prhs[2])*mxGetN(prhs[2])+1;
CHNLS[Hndl-1].MonitorCBString = (char *)mxMalloc(buflen);
mexMakeMemoryPersistent(CHNLS[Hndl-1].MonitorCBString);
mxGetString(prhs[2],CHNLS[Hndl-1].MonitorCBString,buflen);
}
else
mexErrMsgTxt("Third argument must be a string\n");
}
else
CHNLS[Hndl-1].MonitorCBString = NULL; // Set MonitorCBString to NULL so that mcaMonitorEventHandler only copies data to CACHE
// Count argument set to 0 - native count
status = ca_add_array_event(RequestType,0,CHNLS[Hndl-1].CHID, mcaMonitorEventHandler, &CHNLS[Hndl-1], 0.0, 0.0, 0.0, &(CHNLS[Hndl-1].EVID));
if(status!=ECA_NORMAL)
{ mexPrintf("ca_add_array_event failed\n");
plhs[0]=mxCreateScalarDouble(0);
}
else
{ ca_poll();
plhs[0]=mxCreateScalarDouble(1);
}
}
break;
case 200: // Clear Monitor MCACLEARMON
Hndl = (int)mxGetScalar(prhs[1]);
if(Hndl<1 || Hndl>HandlesUsed)
mexErrMsgTxt("Invalid Handle");
if(!CHNLS[Hndl-1].EVID)
mexErrMsgTxt("No monitor installed - can not clear");
status = ca_clear_event(CHNLS[Hndl-1].EVID);
if(status!=ECA_NORMAL)
mexPrintf("ca_clear_event failed\n");
// Set the EVID pointer to NULL (ca_clear_event dos not do it by itself)
// to use as a FLAG that no monitors are installed
CHNLS[Hndl-1].EVID = NULL;
// Reset
CHNLS[Hndl-1].MonitorEventCount = 0;
// If there is Callback String - destroy it
if(CHNLS[Hndl-1].MonitorCBString)
{ mxFree(CHNLS[Hndl-1].MonitorCBString);
CHNLS[Hndl-1].MonitorCBString =NULL;
}
break;
case 300: // MCACACHE Get Cached values of a monitored PV
for(i=0;i<nrhs-1;i++)
{ Hndl = (int)mxGetScalar(prhs[1+i]);
// if(Hndl<1 || Hndl>HandlesUsed || !CHNLS[Hndl-1].CACHE)
if(Hndl<1 || Hndl>HandlesUsed)
plhs[i] = mxCreateDoubleMatrix(0,0,mxREAL);
else
{ plhs[i] = mxDuplicateArray(CHNLS[Hndl-1].CACHE);
CHNLS[Hndl-1].MonitorEventCount = 0;
}
}
break;
case 500: // MCAMON Info on installed monitors
L = 0;
HndlArray = (int*)mxCalloc(HandlesUsed,sizeof(int));
for(i=0;i<HandlesUsed;i++) // Count installed monitors
{ if(CHNLS[i].EVID)
HndlArray[L++]=i+1;
}
if(L>0)
{ plhs[0] = mxCreateDoubleMatrix(1,L,mxREAL);
myDblPr = mxGetPr(plhs[0]);
plhs[1] = mxCreateCellMatrix(1,L);
for(i=0;i<L;i++)
{ myDblPr[i] = (double)HndlArray[i];
mxSetCell(plhs[1],i,mxCreateString(CHNLS[HndlArray[i]-1].MonitorCBString));
}
}
else
{ plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
plhs[1] = mxCreateCellMatrix(0,0);
}
break;
case 510: // MCAMONEVENTS Event count fot monitors
plhs[0] = mxCreateDoubleMatrix(1,HandlesUsed,mxREAL);
myDblPr = mxGetPr(plhs[0]);
for(i=0;i<HandlesUsed;i++)
myDblPr[i]=(double)(CHNLS[i].MonitorEventCount);
break;
case 1000: // print timeout settings
plhs[0] = mxCreateDoubleMatrix(3,1,mxREAL);
mexPrintf("MCA timeout settings\n:");
mexPrintf("mcaopen\t%f [s]\n", MCA_SEARCH_TIMEOUT );
mexPrintf("mcaget\t%f [s]\n", MCA_GET_TIMEOUT );
mexPrintf("mcaput\t%f [s]\n", MCA_PUT_TIMEOUT );
myDblPr = mxGetPr(plhs[0]);
myDblPr[0] = MCA_SEARCH_TIMEOUT;
myDblPr[1] = MCA_GET_TIMEOUT;
myDblPr[2] = MCA_PUT_TIMEOUT;
break;
case 1001: // set MCA_SEARCH_TIMEOUT
// return delay value
MCA_SEARCH_TIMEOUT = mxGetScalar(prhs[1]);
plhs[0] = mxCreateScalarDouble(MCA_SEARCH_TIMEOUT);
break;
case 1002: // set MCA_GET_TIMEOUT
// return delay value
MCA_GET_TIMEOUT = mxGetScalar(prhs[1]);
plhs[0] = mxCreateScalarDouble(MCA_GET_TIMEOUT);
break;
case 1003: // set MCA_PUT_TIMEOUT
// return delay value
MCA_PUT_TIMEOUT = mxGetScalar(prhs[1]);
plhs[0] = mxCreateScalarDouble(MCA_PUT_TIMEOUT);
break;
}
}
// Calling convention:
// comp_filterbank(f,g,a);
void mexFunction( int UNUSED(nlhs), mxArray *plhs[],
int UNUSED(nrhs), const mxArray *prhs[] )
{
static int atExitRegistered = 0;
if(!atExitRegistered)
{
atExitRegistered = 1;
mexAtExit(filterbankAtExit);
}
const mxArray* mxf = prhs[0];
const mxArray* mxg = prhs[1];
const mxArray* mxa = prhs[2];
// input data length
const mwSize L = mxGetM(mxf);
const mwSize W = mxGetN(mxf);
// filter number
const mwSize M = mxGetNumberOfElements(mxg);
// a col count
mwSize acols = mxGetN(mxa);
// pointer to a
double *a = (double*) mxGetData(mxa);
if (acols > 1)
{
int isOnes = 1;
for (mwIndex m = 0; m < M; m++)
{
isOnes = isOnes && a[M + m] == 1;
}
if (isOnes)
{
acols = 1;
}
}
// Cell output
plhs[0] = mxCreateCellMatrix(M, 1);
// Stuff for sorting the filters
mwSize tdCount = 0;
mwSize fftCount = 0;
mwSize fftblCount = 0;
mwIndex tdArgsIdx[M];
mwIndex fftArgsIdx[M];
mwIndex fftblArgsIdx[M];
// WALK the filters to determine what has to be done
for (mwIndex m = 0; m < M; m++)
{
mxArray * gEl = mxGetCell(mxg, m);
if (mxGetField(gEl, 0, "h") != NULL)
{
tdArgsIdx[tdCount++] = m;
continue;
}
if (mxGetField(gEl, 0, "H") != NULL)
{
if (acols == 1 && L == mxGetNumberOfElements(mxGetField(gEl, 0, "H")))
{
fftArgsIdx[fftCount++] = m;
continue;
}
else
{
fftblArgsIdx[fftblCount++] = m;
continue;
}
}
}
if (tdCount > 0)
{
/*
Here, we have to reformat the inputs and pick up results to comply with:
c=comp_filterbank_td(f,g,a,offset,ext);
BEWARE OF THE AUTOMATIC DEALLOCATION!! by the Matlab engine.
Arrays can be very easily freed twice causing segfaults.
This happends particulary when using mxCreateCell* which stores
pointers to other mxArray structs. Setting all such pointers to
NULL after they are used seems to solve it.
*/
mxArray* plhs_td[1];
mxArray* prhs_td[5];
prhs_td[0] = (mxArray*) mxf;
prhs_td[1] = mxCreateCellMatrix(tdCount, 1);
prhs_td[2] = mxCreateDoubleMatrix(tdCount, 1, mxREAL);
double* aPtr = mxGetData(prhs_td[2]);
prhs_td[3] = mxCreateDoubleMatrix(tdCount, 1, mxREAL);
double* offsetPtr = mxGetData(prhs_td[3]);
prhs_td[4] = mxCreateString("per");
for (mwIndex m = 0; m < tdCount; m++)
{
mxArray * gEl = mxGetCell(mxg, tdArgsIdx[m]);
mxSetCell(prhs_td[1], m, mxGetField(gEl, 0, "h"));
// This has overhead
//mxSetCell((mxArray*)prhs_td[1],m,mxDuplicateArray(mxGetField(gEl,0,"h")));
aPtr[m] = a[tdArgsIdx[m]];
offsetPtr[m] = mxGetScalar(mxGetField(gEl, 0, "offset"));
}
// Finally call it!
// comp_filterbank_td(1,plhs_td,5, prhs_td);
// This has overhead:
mexCallMATLAB(1, plhs_td, 5, prhs_td, "comp_filterbank_td");
// Copy pointers to a proper index in the output + unset all duplicate cell elements
for (mwIndex m = 0; m < tdCount; m++)
{
mxSetCell(plhs[0], tdArgsIdx[m], mxGetCell(plhs_td[0], m));
mxSetCell(plhs_td[0], m, NULL);
mxSetCell(prhs_td[1], m, NULL);
}
mxDestroyArray(plhs_td[0]);
mxDestroyArray(prhs_td[1]);
mxDestroyArray(prhs_td[2]);
mxDestroyArray(prhs_td[3]);
mxDestroyArray(prhs_td[4]);
}
if (fftCount > 0 || fftblCount > 0)
{
// Need to do FFT of mxf
mwIndex ndim = 2;
const mwSize dims[] = {L, W};
if (mxF == NULL || mxGetM(mxF) != L || mxGetN(mxF) != W || mxGetClassID(mxF) != mxGetClassID(mxf))
{
if (mxF != NULL)
{
mxDestroyArray(mxF);
mxF = NULL;
// printf("Should be called just once\n");
}
if (mxIsDouble(mxf))
{
mxF = mxCreateNumericArray(ndim, dims, mxDOUBLE_CLASS, mxCOMPLEX);
fftw_iodim fftw_dims[1];
fftw_iodim howmanydims[1];
fftw_dims[0].n = L;
fftw_dims[0].is = 1;
fftw_dims[0].os = 1;
howmanydims[0].n = W;
howmanydims[0].is = L;
howmanydims[0].os = L;
if (p_double == NULL)
p_double = (fftw_plan*) malloc(sizeof(fftw_plan));
else
fftw_destroy_plan(*p_double);
// FFTW_MEASURE sometimes hangs here
*p_double = fftw_plan_guru_split_dft(
1, fftw_dims,
1, howmanydims,
mxGetData(mxF), mxGetImagData(mxF), mxGetData(mxF), mxGetImagData(mxF),
FFTW_ESTIMATE);
}
else if (mxIsSingle(mxf))
{
mxF = mxCreateNumericArray(ndim, dims, mxSINGLE_CLASS, mxCOMPLEX);
// mexPrintf("M= %i, N= %i\n",mxGetM(mxF),mxGetN(mxF));
fftwf_iodim fftw_dims[1];
fftwf_iodim howmanydims[1];
fftw_dims[0].n = L;
fftw_dims[0].is = 1;
fftw_dims[0].os = 1;
howmanydims[0].n = W;
howmanydims[0].is = L;
howmanydims[0].os = L;
if (p_float == NULL)
p_float = (fftwf_plan*) malloc(sizeof(fftwf_plan));
else
fftwf_destroy_plan(*p_float);
*p_float = fftwf_plan_guru_split_dft(
1, fftw_dims,
1, howmanydims,
mxGetData(mxF), mxGetImagData(mxF),
mxGetData(mxF), mxGetImagData(mxF),
FFTW_ESTIMATE);
}
}
if (mxIsDouble(mxf))
{
memcpy(mxGetPr(mxF), mxGetPr(mxf), L * W * sizeof(double));
memset(mxGetPi(mxF), 0, L * W * sizeof(double));
if (mxIsComplex(mxf))
memcpy(mxGetPi(mxF), mxGetPi(mxf), L * W * sizeof(double));
fftw_execute(*p_double);
}
else if (mxIsSingle(mxf))
{
memcpy(mxGetPr(mxF), mxGetPr(mxf), L * W * sizeof(float));
memset(mxGetPi(mxF), 0, L * W * sizeof(float));
if (mxIsComplex(mxf))
memcpy(mxGetPi(mxF), mxGetPi(mxf), L * W * sizeof(float));
fftwf_execute(*p_float);
}
}
if (fftCount > 0)
{
mxArray* plhs_fft[1];
mxArray* prhs_fft[3];
prhs_fft[0] = mxF;
prhs_fft[1] = mxCreateCellMatrix(fftCount, 1);
prhs_fft[2] = mxCreateDoubleMatrix(fftCount, 1, mxREAL);
double* aPtr = mxGetData(prhs_fft[2]);
for (mwIndex m = 0; m < fftCount; m++)
{
mxArray * gEl = mxGetCell(mxg, fftArgsIdx[m]);
mxSetCell(prhs_fft[1], m, mxGetField(gEl, 0, "H"));
// This has overhead
//mxSetCell((mxArray*)prhs_td[1],m,mxDuplicateArray(mxGetField(gEl,0,"h")));
aPtr[m] = a[fftArgsIdx[m]];
}
//comp_filterbank_fft(1,plhs_fft,3, prhs_fft);
mexCallMATLAB(1, plhs_fft, 3, prhs_fft, "comp_filterbank_fft");
for (mwIndex m = 0; m < fftCount; m++)
{
mxSetCell(plhs[0], fftArgsIdx[m], mxGetCell(plhs_fft[0], m));
mxSetCell(plhs_fft[0], m, NULL);
mxSetCell(prhs_fft[1], m, NULL);
}
mxDestroyArray(plhs_fft[0]);
mxDestroyArray(prhs_fft[1]);
mxDestroyArray(prhs_fft[2]);
}
if (fftblCount > 0)
{
mxArray* plhs_fftbl[1];
mxArray* prhs_fftbl[5];
prhs_fftbl[0] = mxF;
prhs_fftbl[1] = mxCreateCellMatrix(fftblCount, 1);
prhs_fftbl[2] = mxCreateDoubleMatrix(fftblCount, 1, mxREAL);
prhs_fftbl[3] = mxCreateDoubleMatrix(fftblCount, 2, mxREAL);
prhs_fftbl[4] = mxCreateDoubleMatrix(fftblCount, 1, mxREAL);
double* foffPtr = mxGetData(prhs_fftbl[2]);
double* aPtr = mxGetData(prhs_fftbl[3]);
double* realonlyPtr = mxGetData(prhs_fftbl[4]);
// Set all realonly flags to zero
memset(realonlyPtr, 0, fftblCount * sizeof * realonlyPtr);
for (mwIndex m = 0; m < fftblCount; m++)
{
mxArray * gEl = mxGetCell(mxg, fftblArgsIdx[m]);
mxSetCell(prhs_fftbl[1], m, mxGetField(gEl, 0, "H"));
foffPtr[m] = mxGetScalar(mxGetField(gEl, 0, "foff"));
aPtr[m] = a[fftblArgsIdx[m]];
if (acols > 1)
aPtr[m + fftblCount] = a[fftblArgsIdx[m] + M];
else
aPtr[m + fftblCount] = 1;
// Only if realonly is specified
mxArray* mxrealonly;
if ((mxrealonly = mxGetField(gEl, 0, "realonly")))
realonlyPtr[m] = mxGetScalar(mxrealonly);
}
// comp_filterbank_fftbl(1,plhs_fftbl,5, prhs_fftbl);
mexCallMATLAB(1, plhs_fftbl, 5, prhs_fftbl, "comp_filterbank_fftbl");
for (mwIndex m = 0; m < fftblCount; m++)
{
mxSetCell(plhs[0], fftblArgsIdx[m], mxGetCell(plhs_fftbl[0], m));
mxSetCell(plhs_fftbl[0], m, NULL);
mxSetCell(prhs_fftbl[1], m, NULL);
}
mxDestroyArray(plhs_fftbl[0]);
mxDestroyArray(prhs_fftbl[1]);
mxDestroyArray(prhs_fftbl[2]);
mxDestroyArray(prhs_fftbl[3]);
mxDestroyArray(prhs_fftbl[4]);
}
if (mxF != NULL)
mexMakeArrayPersistent(mxF);
if (L * W > MAXARRAYLEN && mxF != NULL)
{
//printf("Damn. Should not get here\n");
mxDestroyArray(mxF);
mxF = NULL;
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
// Get the command string
char cmd[64];
if (nrhs < 1 || mxGetString(prhs[0], cmd, sizeof(cmd)))
mexErrMsgTxt("First input should be a command string less than 64 characters long.");
// New
if (!strcmp("new", cmd)) {
// Check parameters
if (nlhs != 1)
mexErrMsgTxt("New: One output expected.");
// Return a handle to a new C++ instance
//plhs[0] = convertPtr2Mat<dummy>(new dummy);
plhs[0] = convertPtr2Mat<CK4Wv2OpenCVModule>(new CK4Wv2OpenCVModule());
return;
}
// Check there is a second input, which should be the class instance handle
if (nrhs < 2)
mexErrMsgTxt("Second input should be a class instance handle.");
// Delete
if (!strcmp("delete", cmd)) {
// Destroy the C++ object
destroyObject<CK4Wv2OpenCVModule>(prhs[1]);
// Warn if other commands were ignored
if (nlhs != 0 || nrhs != 2)
mexWarnMsgTxt("Delete: Unexpected arguments ignored.");
return;
}
// Get the class instance pointer from the second input
CK4Wv2OpenCVModule *dummy_instance = convertMat2Ptr<CK4Wv2OpenCVModule>(prhs[1]);
// Call the various class methods
// Train
if (!strcmp("InitializeKinectDevice", cmd)) {
// Check parameters
if (nlhs < 0 || nrhs < 2)
mexErrMsgTxt("InitializeKinectDevice: Unexpected arguments.");
// Call the method
dummy_instance->InitializeKinectDevice();
//plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL);;
//double * data = (double *)mxGetData(plhs[0]);
//data[0] = *dummy_instance->dynamicData;
return;
}
// Test
if (!strcmp("UpdateData", cmd)) {
// Check parameters
if (nlhs < 0 || nrhs < 2)
mexErrMsgTxt("UpdateData: Unexpected arguments.");
// Call the method
//dummy_instance->UpdateData();
return;
}
if (!strcmp("GrabDepth", cmd)) {
// Check parameters
if (nlhs < 0 || nrhs < 2)
mexErrMsgTxt("GrabDepth: Unexpected arguments.");
// Call the method
if (persistent_array_ptr==NULL) {
mexPrintf("MEX-file initializing, creating array\n");
persistent_array_ptr = mxCreateNumericMatrix(dummy_instance->DEPTH_FRAME_WIDTH, dummy_instance->DEPTH_FRAME_HEIGHT, mxUINT16_CLASS, mxREAL);
mexMakeArrayPersistent(persistent_array_ptr);
mexAtExit(exitFcn);
}
plhs[0] = persistent_array_ptr;
UINT16 *dynamicData = (UINT16*)mxGetPr(plhs[0]);
int nop = dummy_instance->DEPTH_FRAME_HEIGHT* dummy_instance->DEPTH_FRAME_WIDTH;
dummy_instance->UpdateData(dynamicData);
return;
}
// Got here, so command not recognized
mexErrMsgTxt("Command not recognized.");
}
