int call_fbGetPixmapInfo(int *pDatabuf)
{
str_fbGetPixmapInfo *function;
int width, height, bitsperpixel, stride;
void *bits = NULL;
int ret;
function = (str_fbGetPixmapInfo *)pDatabuf;
fbGetPixmapInfo(function->Pixmap, &width, &height, &bitsperpixel, &stride, &bits);
function->Width = width;
function->Height = height;
function->BitsPerPixel = bitsperpixel;
function->Stride = stride;
memset(function->Bits, 0, PARAM_SIZE(fbGetPixmapInfo));
if(bits != NULL)
memcpy(function->Bits, bits, PARAM_SIZE(fbGetPixmapInfo));
if(function->hd.retFlag == RT_REQUEST){
function->hd.retFlag = RT_RESPONSE;
ret = write(dev, function, MAX_BUF_SIZE);
}
return 0;
}
static void mdlStart(SimStruct *S) {
real_T *y = ssGetOutputPortRealSignal(S,0);
const real_T *initVal = PARAM(2);
int_T initValLen = PARAM_SIZE(2);
int i;
#ifndef MATLAB_MEX_FILE
int num;
rosShmData_t *shm;
SEM *sem;
rosBlockInitResult_t res;
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
char_T *str = (char_T *)malloc(strlen);
mxGetString(ssGetSFcnParam(S,1), str, strlen);
res = registerRosBlock(S, str, SUBSCRIBER, PARAM(0)[0]);
shm = res.shm;
sem = res.sem;
num = res.num;
rt_sem_wait(sem);
shm->length = ssGetOutputPortWidth(S,0);
for (i = 0; i < shm->length; ++i) {
if (initValLen > 1) {
shm->data[i] = initVal[i];
} else {
shm->data[i] = initVal[0];
}
}
rt_sem_signal(sem);
ssSetIWorkValue(S,0,num);
ssSetPWorkValue(S,0,(void *)shm);
ssSetPWorkValue(S,1,(void *)sem);
free(str);
#endif
for (i = 0; i < ssGetOutputPortWidth(S,0); ++i) {
if (initValLen > 1) {
y[i] = initVal[i];
} else {
y[i] = initVal[0];
}
}
}
static void mdlOutputs(SimStruct *S,int_T tid) {
InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1);
real_T prev = ssGetRWorkValue(S,0);
bool dataPort = PARAM(2)[0];
int_T i;
#ifndef MATLAB_MEX_FILE
rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);
SEM *sem = (SEM *)ssGetPWorkValue(S,1);
#endif
char_T *msg;
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
UNUSED_ARG(tid); /* not used in single tasking mode */
if (U0(0) > 0.5 && U0(0) > prev) {
msg = (char_T *)malloc(strlen);
mxGetString(ssGetSFcnParam(S,1), msg, strlen);
#ifndef MATLAB_MEX_FILE
if (dataPort) {
for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
asprintf(&msg, "%s %f", msg, U1(i));
}
}
if (rt_sem_wait_if(sem) != 0) {
memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE);
shm->msg.state = NEW_VALUE;
rt_sem_signal(sem);
}
#else
switch ((int)PARAM(0)[0]) {
case 1: printf("DEBUG"); break;
case 2: printf("INFO"); break;
case 3: printf("WARN"); break;
case 4: printf("ERROR"); break;
case 5: printf("FATAL"); break;
default: printf("NONE"); break;
}
printf(": %s", msg);
if (dataPort) {
for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
printf(" %f", U1(i));
}
}
printf("\n");
#endif
free(msg);
}
ssSetRWorkValue(S,0,U0(0));
}
static void mdlStart(SimStruct *S) {
#ifndef MATLAB_MEX_FILE
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
int_T rate = (int_T)PARAM(0)[0];
char_T *str = (char_T *)malloc(strlen);
mxGetString(ssGetSFcnParam(S,1), str, strlen);
if (rate > 0) {
rosConfig.rate = rate;
}
memcpy(rosConfig.ns, str, MAX_NAMES_SIZE);
rosConfig.pubStackSize = PARAM(2)[0];
rosConfig.subStackSize = PARAM(3)[0];
rosConfig.exposeParams = PARAM(4)[0] - 1;
free(str);
#endif
}
static void mdlStart(SimStruct *S) {
#ifndef MATLAB_MEX_FILE
int num;
rosShmData_t *shm;
SEM *sem;
rosBlockInitResult_t res;
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
res = registerRosBlock(S, "rosout", LOGGER, 0);
shm = res.shm;
sem = res.sem;
num = res.num;
shm->msg.level = PARAM(0)[0]-1;
ssSetIWorkValue(S,0,num);
ssSetPWorkValue(S,0,(void *)shm);
ssSetPWorkValue(S,1,(void *)sem);
#endif
ssSetRWorkValue(S,0,0.0);
}
static void mdlEnable(SimStruct *S) {
#ifndef MATLAB_MEX_FILE
const real_T *initVal = PARAM(2);
int_T initValLen = PARAM_SIZE(2);
bool reset = PARAM(4)[0];
int i;
rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);
SEM *sem = (SEM *)ssGetPWorkValue(S,1);
if (!reset) return;
if (rt_sem_wait_if(sem) != 0) {
for (i = 0; i < shm->length; ++i) {
if (initValLen > 1) {
shm->data[i] = initVal[i];
} else {
shm->data[i] = initVal[0];
}
}
rt_sem_signal(sem);
}
#endif
}
