static void mdlSetOutputPortWidth(SimStruct *S, int_T port, int_T outputPortWidth)
{
if( ((ssGetInputPortWidth(S, 0) != DYNAMICALLY_SIZED) && (ssGetInputPortWidth(S, 0) != outputPortWidth)) || \
((ssGetOutputPortWidth(S, 1) != DYNAMICALLY_SIZED) && (ssGetOutputPortWidth(S, 1) != outputPortWidth)) )
{
ssSetErrorStatus(S,"xdot and x must have the same size");
return;
}
if( intval(mxGetScalar(paramInitialConditionSource)) > 1 )
{
if( (ssGetInputPortWidth(S, 1) != DYNAMICALLY_SIZED) && (ssGetInputPortWidth(S, 1) != outputPortWidth) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
ssSetInputPortWidth(S, 1, outputPortWidth);
}
else
{
if( (mxGetNumberOfElements(paramInitialCondition) != 1) && (mxGetNumberOfElements(paramInitialCondition) != outputPortWidth) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
}
ssSetInputPortWidth(S, 0, outputPortWidth);
ssSetOutputPortWidth(S, 1, outputPortWidth);
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
uint8_T *y0 = (uint8_T *)ssGetOutputPortRealSignal(S,0);
const int_T y_width = ssGetOutputPortWidth(S,0);
int i;
char* buffer[1];
int connected=ssGetIWorkValue(S, 0);
int curCon = ssGetIWorkValue(S, 2);
char* last = (void*) ssGetPWorkValue(S, 1);
if(connected==1)
{
fcntl(curCon, F_SETFL, O_NONBLOCK);
if(recv(curCon,buffer, 1, MSG_PEEK)>0)
{
read(curCon,last,y_width);
/*printf("%.*s\n",y_width,last);*/
}
}
else
{
connected=tryConnect(S);
if (connected)
printf("Connected To Client\n");
}
/*fflush(stdout);*/
for(i=0;i<y_width;i++)
y0[i]=last[i];
ssSetIWorkValue(S, 0, connected);
/*ssSetPWorkValue(S, 0, (void*) newsockfd);*/
}
// Function: mdlOutputs =======================================================
// Abstract:
// In this function, you compute the outputs of your S-function
// block.
static void mdlOutputs(SimStruct *S, int_T tid)
{
BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
Vector *v = toPort->read(false); // Read from the port. Waits until data arrives.
if (v!=NULL)
{
for (int i = 0; i < SIZE_READING_PORT; i++)
{
real_T *pY = (real_T *)ssGetOutputPortSignal(S,i);
int_T widthPort = ssGetOutputPortWidth(S,i);
if (widthPort == 1)
{
for(int_T j=0; j<widthPort; j++)
if (i < (v->length())){
pY[j] = v->data()[i];
//cout<<v->data()[i]<<" ";
}
else
pY[j] = 0;
}
else
cout << "ERROR: something wrong with port dimensions \n";
}
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = 2*u
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i;
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T width = ssGetOutputPortWidth(S,0);
for (i=0; i<width; i++) {
/*
* This example does not implement complex signal handling.
* To find out see an example about how to handle complex signal in
* S-function, see sdotproduct.c for details.
*/
g_slsf_in = (int) *uPtrs[i];
*y = 2.0 *(*uPtrs[i]);
g_slsf_out = (int) *y;
y++;
}
if(!is_main_called){
main();
is_main_called = true;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *cpr = mxGetPr(C_PARAM(S));
const real_T *dpr = mxGetPr(D_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T nOutputs = ssGetOutputPortWidth(S,0);
int_T i, j;
real_T accum;
UNUSED_ARG(tid); /* not used in single tasking mode */
/* Matrix Multiply: y = Cx + Du */
for (i = 0; i < nOutputs; i++) {
accum = 0.0;
/* Cx */
for (j = 0; j < nStates; j++) {
accum += cpr[i + nOutputs*j] * x[j];
}
/* Du */
for (j = 0; j < nInputs; j++) {
accum += dpr[i + nOutputs*j] * U(j);
}
y[i] = accum;
}
}
/* Function: mdlInitializeConditions ======================================
* Abstract:
* Initialize outputs to zero.
*/
static void mdlInitializeConditions(SimStruct *S)
{
// Initialize the standard output port to 0.0
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T width = ssGetOutputPortWidth(S,0);
for (int i = 0; i < width; i++)
y[i] = 0.0;
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl.doc for more details on the macros below */
ssSetNumSFcnParams(S, 0); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
ssSetNumContStates(S, 1); /* how many continuous states? */
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, 1);
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, 1);
ssSetNumSampleTimes(S, 1);
/*
* If your Fortran code uses REAL for the state, input, and/or output
* datatypes, use these DWorks as work areas to downcast continuous
* states from double to REAL before calling your code. You could
* also put the work vectors in hard-coded local (stack) variables.
*
* For fixed step code, keep a copy of the variables to be output
* in a DWork vector so the mdlOutputs() function can provide output
* data when needed. You can use as many DWork vectors as you like
* for both input and output (or hard-code local variables).
*/
if(!ssSetNumDWork( S, 3)) return;
ssSetDWorkWidth( S, 0, ssGetOutputPortWidth(S,0));
ssSetDWorkDataType( S, 0, SS_SINGLE); /* use SS_DOUBLE if needed */
ssSetDWorkWidth( S, 1, ssGetInputPortWidth(S,0));
ssSetDWorkDataType( S, 1, SS_SINGLE);
ssSetDWorkWidth( S, 2, ssGetNumContStates(S));
ssSetDWorkDataType( S, 2, SS_SINGLE);
ssSetNumNonsampledZCs(S, 0);
/* Specify the sim state compliance to be same as a built-in block */
/* see sfun_simstate.c for example of other possible settings */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 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];
}
}
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const int8_T *u0 = (const int8_T*) ssGetInputPortSignal(S,0);
int8_T *y0 = (int8_T *)ssGetOutputPortRealSignal(S,0);
const int_T y_width = ssGetOutputPortWidth(S,0);
const int_T u_width = ssGetInputPortWidth(S,0);
jpeg2bmp_Outputs_wrapper(u0, y0, y_width, u_width);
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const real_T *u0 = (const real_T*) ssGetInputPortSignal(S,0);
uint32_T *y0 = (uint32_T *)ssGetOutputPortRealSignal(S,0);
const int_T y_width = ssGetOutputPortWidth(S,0);
const int_T u_width = ssGetInputPortWidth(S,0);
fptofix_Outputs_wrapper(u0, y0, y_width, u_width, S);
}
static void mdlSetWorkWidths(SimStruct *S)
{
/* Set the width of DWork(s) used for marshalling the IOs */
if (isDWorkPresent) {
/* Update dwork 0 */
ssSetDWorkWidth(S, 0, ssGetOutputPortWidth(S, 1));
}
}
/* Helper function to make function disables from non-inlined S-functions. */
int_T rt_DisableSys(SimStruct *S, int_T element, int_T tid)
{
(*(S)->callSys.fcns[3*ssGetOutputPortWidth(S,0)+element])
((S)->callSys.args1[element],
(S)->callSys.args2[element], tid);
if (ssGetErrorStatus(S) != (NULL)) {
return 0;
} else {
return 1;
}
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const uint32_T *u0 = (const uint32_T*) ssGetInputPortSignal(S,0);
real_T *y0 = (real_T *)ssGetOutputPortRealSignal(S,0);
const int_T p_width0 = mxGetNumberOfElements(PARAM_DEF0(S));
const real_T *scale = mxGetData(PARAM_DEF0(S));
const int_T y_width = ssGetOutputPortWidth(S,0);
const int_T u_width = ssGetInputPortWidth(S,0);
fixtofpwithscale_Outputs_wrapper(u0, y0, scale, p_width0, y_width, u_width, S);
}
static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
int_T size_sig;
if( (intval(mxGetScalar(paramInitialConditionSource)) == 1) && (mxGetNumberOfElements(paramInitialCondition) > 1) )
{
size_sig = mxGetNumberOfElements(paramInitialCondition);
if( ssGetInputPortWidth(S, 0) == DYNAMICALLY_SIZED )
{
ssSetInputPortWidth(S, 0, size_sig);
}
if( ssGetOutputPortWidth(S, 1) == DYNAMICALLY_SIZED )
{
ssSetOutputPortWidth(S, 1, size_sig);
}
if( ssGetInputPortWidth(S, 0) != ssGetOutputPortWidth(S, 1) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
}
}
void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
DECL_AND_INIT_DIMSINFO(dimsInfo);
/* Either 2nd input or 2nd output should be already known */
if (ssGetOutputPortWidth(S, 0) != DYNAMICALLY_SIZED) {
/* It is the output that is known, get the dimensions first */
dimsInfo.width = ssGetOutputPortWidth(S, 0);
dimsInfo.numDims = ssGetOutputPortNumDimensions(S, 0);
dimsInfo.dims = ssGetOutputPortDimensions(S, 0);
/*set second input, first output if unknown */
if (ssGetInputPortWidth(S, 0) == DYNAMICALLY_SIZED) {
if(!ssSetInputPortDimensionInfo(S, 0, &dimsInfo)) return;
}
return;
}
else {
if (ssGetInputPortWidth(S, 0) != DYNAMICALLY_SIZED) {
/* It is the input that is known, get the dimensions */
dimsInfo.width = ssGetInputPortWidth(S, 0);
dimsInfo.numDims = ssGetInputPortNumDimensions(S, 0);
dimsInfo.dims = ssGetInputPortDimensions(S, 0);
/*set first and second output if unknown */
if (ssGetOutputPortWidth(S, 0) == DYNAMICALLY_SIZED) {
if(!ssSetOutputPortDimensionInfo(S, 0, &dimsInfo)) return;
}
return;
}
else
{
/* Default everything to a scalar signal */
ssSetOutputPortVectorDimension(S, 0, 1);
ssSetInputPortVectorDimension(S, 0, 1);
return;
}
}
} /* end mdlSetDefaultPortDimensionInfo */
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T num_channels = ssGetOutputPortWidth(S,0);
int_T first_chan = ssGetIWorkValue(S,BASE_ADDR_I_IND);
int i;
int val[8];
#ifndef MATLAB_MEX_FILE
read_adc(first_chan, num_channels, val);
for(i=0; i<num_channels; i++){
*y++=(real_T) i2v(val[i]);
}
#endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = 2*u
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i;
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T width = ssGetOutputPortWidth(S,0);
for (i=0; i<width; i++) {
/*
* This example does not implement complex signal handling.
* To find out see an example about how to handle complex signal in
* S-function, see sdotproduct.c for details.
*/
*y++ = 3.0 *(*uPtrs[i]);
}
}
/* Function: mdlUpdate ======================================================
* Abstract:
* This function is called once for every major integration time step.
* Discrete states are typically updated here, but this function is useful
* for performing any tasks that should only take place once per
* integration step.
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
#ifndef VARIABLE_STEP
/*
* For Fixed Step Code Only
* ------------------------
* If your Fortran code runs at a fixed time step that advances
* each time you call it, it is best to call it here instead of
* in mdlOutputs(). The states in the Fortran code need not be
* continuous if you call your code from here.
*/
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
float *sampleArgs = (float *) ssGetDWork(S,1);
double *y = ssGetOutputPortRealSignal(S,0);
float *sampleOutput = (float *) ssGetDWork(S,0);
int k;
/*
* If the datatype in the Fortran code is REAL
* then you have to downcast the I/O and states from
* double to float as copies before sending them
* to your code (or change the Fortran code).
*/
for (k=0; k < ssGetDWorkWidth(S,1); k++) {
sampleArgs[k] = (float) (*uPtrs[k]);
}
/* ==== Call the Fortran routine (args are pass-by-reference) */
/* nameofsub_(sampleArgs, sampleOutput ); */
/*
* If needed, convert the float outputs to the
* double (y) output array
*/
for (k=0; k < ssGetOutputPortWidth(S,0); k++) {
y[k] = (double) sampleOutput[k];
}
#endif
}
static void mdlStart(SimStruct *S)
{
const real_T *pp1 = mxGetData(PARAM_DEF0(S));
const real_T *pp2 = mxGetData(PARAM_DEF1(S));
int P2=*pp2;
if(P2!=0)
ssSetOutputPortWidth(S, 0,P2);
int connected=0,accCon,portno;
struct sockaddr_in serv_addr;
char* P1=(char*)mxGetPr(ssGetSFcnParam(S,0));
char* last;
int num=mxGetNumberOfElements(ssGetSFcnParam(S,0));
int count;
char *port;
char porttemp[20];
/*Determine Port Number*/
portno =*pp1;
const int_T y_width = ssGetOutputPortWidth(S,0);
int yes=1;int i;
accCon = socket(AF_INET, SOCK_STREAM, 0);
if (accCon < 0)
error("ERROR opening socket");
bzero((char *) &serv_addr, sizeof(serv_addr));
setsockopt(accCon,SOL_SOCKET,SO_REUSEADDR,&yes,sizeof(yes));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(portno);
if (bind(accCon, (struct sockaddr *) &serv_addr,
sizeof(serv_addr)) < 0)
error("ERROR on binding");
fcntl(accCon, F_SETFL, O_NONBLOCK);
last = malloc( y_width);
for(i=0;i<y_width;i++)
last[i]=128;
ssSetIWorkValue(S, 0, connected);
ssSetIWorkValue(S, 1, accCon);
/*ssSetPWorkValue(S, 2, (void*) &cli_addr);*/
ssSetPWorkValue(S, 1, (void*) last);
}
static void mdlSetOutputPortDataType(SimStruct *S, int port, DTypeId dType)
{
int portWidth;
// set up the data-port data type
if (P_SHOW_DATA) {
// data port is port 0
if (port == 0) {
portWidth = ssGetOutputPortWidth(S,port);
if (isAcceptableOutputDataType(S,dType,portWidth)){
ssSetOutputPortDataType(S,port,dType);
}else{
ssSetErrorStatus(S,"Incompatible DataType or Size specified");
}
}
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i;
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T num_channels = ssGetOutputPortWidth(S,0);
uint_T base_dio=ssGetIWorkValue(S,BASE_ADDR_I_IND);
int_T port=ssGetIWorkValue(S,OUTPORT_I_IND);
int_T input;
#ifndef MATLAB_MEX_FILE
input=inb(base_dio+port);
for(i=0; i<num_channels; i++){
*y++=(input & (1 << i)) >> i;
}
#endif
}
static void mdlUpdate(SimStruct *S, int_T tid)
{
// Check the `New pending activations available' flag
boolean_T *pendingActivsAvail = (boolean_T*) ssGetDWork(S,0);
if (pendingActivsAvail[0])
{
mexPrintf("\n%s\n\t%s\n\t\tat time %.6f\n", ssGetPath(S), __FUNCTION__, ssGetT(S));
// Reset the values on the out ports
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T width = ssGetOutputPortWidth(S,0);
for (int i = 0; i < width; i++)
y[i] = 0.0;
// Reset the `New pending activations available' flag
pendingActivsAvail[0] = false;
}
}
/* Function: mdlSetInputPortDimensionInfo ====================================
*/
static void mdlSetInputPortDimensionInfo(SimStruct *S,
int_T portIndex,
const DimsInfo_T *dimsInfo)
{
int outPortWidth = 0;
if(!ssSetInputPortDimensionInfo(S, portIndex, dimsInfo)) return;
outPortWidth = ssGetOutputPortWidth( S, 0 );
if ( outPortWidth == DYNAMICALLY_SIZED )
{
if(!ssSetOutputPortDimensionInfo(S, 0, dimsInfo)) return;
}
else if ( outPortWidth != dimsInfo->width )
{
ssSetErrorStatus(S,"Input port width not compatible with output port width.");
}
} /* mdlSetInputPortDimensionInfo */
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T num_channels = ssGetOutputPortWidth(S,0);
uint_T base_adc=ssGetIWorkValue(S,BASE_ADDR_I_IND);
uint_T base_status=ssGetIWorkValue(S,BASE_ADDR_I_IND+1);
int_T i,res;
#ifndef MATLAB_MEX_FILE
for(i=0; i<num_channels; i++){
outw(0x00,base_adc);
while((inw(base_status) & 0x1000)==0x0);
res=inw(base_adc);
*y++=20.0*res/4096-10.0;
}
#endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int len, i;
uint16_T *y = ssGetOutputPortSignal(S,0);
real_T *t = ssGetOutputPortSignal(S,1);
/* Read packet from SerialForwarder server using functions linked in sfsource.o */
const unsigned char *packet = read_sf_packet(fd, &len);
int_T width = ssGetOutputPortWidth(S,0);
if (len == (width*2 + HEADER_LEN)){
for (i=HEADER_LEN; i < (width*2); i=i+2) {
*y++ = packet[i] << 8 | packet[i+1];
}
trigger = (trigger == 1 ? 0 : 1);
*t = trigger;
}
}
static void mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
unsigned int base_adc, base_status, base_pci;
unsigned int addr_mat[5];
int_T num_channels = ssGetOutputPortWidth(S,0);
get_pci_addr(PCI_VENDOR_ID_CBOARDS,0x0F,addr_mat);
base_pci = addr_mat[0];
base_status = addr_mat[1];
base_adc = addr_mat[2];
outw(((num_channels-1)<<4) | 0x0400,base_status+0x02);
outw(0x0000,base_adc+0x02);
cal_ad(base_pci,base_status);
ssSetIWorkValue(S,BASE_ADDR_I_IND,(int_T) base_adc);
ssSetIWorkValue(S,BASE_ADDR_I_IND+1,(int_T) base_status);
#endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
*
* y = q * floor(fabs(u/q) + 0.5) * (u >= 0 ? 1.0 : -1.0);
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T yWidth = ssGetOutputPortWidth(S,0);
const mxArray *quant = (const mxArray *) QUANTIZATION_PARAM(S);
int_T intervalSize = mxGetM (quant) * mxGetN (quant);
const real_T *q = mxGetPr(quant);
int_T i;
UNUSED_ARG(tid); /* not used in single tasking mode */
if (intervalSize == 1) {
for (i = 0; i < yWidth; i++) {
y[i] = q[0] * floor(fabs(U(i)/q[0]) + 0.5) *
(U(i) >= 0.0 ? 1.0 : -1.0);
}
} else {
for (i = 0; i < yWidth; i++) {
y[i] = q[i] * floor(fabs(U(i)/q[i]) + 0.5) *
(U(i) >= 0.0 ? 1.0 : -1.0);
}
}
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int param_width1=mxGetNumberOfElements(ssGetSFcnParam(S,0));
const real_T *pu0 = (const real_T*) ssGetInputPortSignal(S,0);
real_T *py0 = (real_T *)ssGetOutputPortRealSignal(S,0);
const int_T py_width = ssGetOutputPortWidth(S,0);
const int_T pu_width = ssGetInputPortWidth(S,0);
int j,i,charPos=0,inPos=0,temp,tVal;
char b[1];char tempBuf[5];
memset(msg, 0,100* sizeof(char));
/*Begin Write Block------------------------------*/
msg[charPos++]='[';
/*printf ("pow\n");
for(i=0;i<%<pu_width>;i++)
printf("ins= %d\n",((int)*%<pu0>+i));*/
for(i=0;i<20;i++)
{
if((prefix[i]=='O' || prefix[i]=='P'|| prefix[i]=='M'|| prefix[i]=='T'|| prefix[i]=='S'))
{
temp=(int)(*(pu0+inPos));
if(lasts[i]!=temp)
{
/*printf("pref=%c\n",prefix[i]);*/
msg[charPos++]='W';
if(prefix[i]=='O')
msg[charPos++]='D';
else
msg[charPos++]=prefix[i];
msg[charPos++]=i+48;
memcpy(msg+charPos,&temp, sizeof(int));/*integer-based value*/
charPos+=sizeof(int);
/*msg[charPos++]=(int)(*(%<pu0>+inPos));*/
lasts[i]=(int)(*(pu0+inPos));
}
inPos++;
}
}
if(py_width>=1)/*If need to read*/
{
msg[charPos++]='R';/*Append a read all command at the end*/
msg[charPos++]='+';
msg[charPos++]='0';
msg[charPos++]='0';
msg[charPos++]='0';
msg[charPos++]='0';
msg[charPos++]='0';
}
msg[charPos++]=']';
if(charPos>2)
{
printf("out=");
for(i=0;i<charPos;i++)
printf("%c",*(msg+i));
printf("\n");
j=write(fd,msg,charPos);
if (j < 0)
fputs("write() of bytes failed!\n", stderr);
}
/*End Write Block------------------------------*/
/*Begin Read Block------------------------------*/
if(py_width>=1)/*If need to read*/
{
memset(msg, 0,100* sizeof(char));
i=0;
do {
j=read(fd, b, 1); /* read a char at a time*/
if( j==-1)
{
fputs("read of bytes failed!\n", stderr);
goto OUT2;
}
if( j==0 ) {
printf("Reconfiguring USB\n");
j=write(fd,"!!!!!!X",7);
j=write(fd,conf,param_width1);
j=write(fd,"[R+00000]",9);
i=0;
goto OUT2;
}
msg[i] = b[0]; i++;
} while( b[0] != ']');
msg[i] = 0; /* null terminate the string*/
printf("msg=%s\n",msg+2);
j=0;inPos=0;
while(msg[j++]!='['){if (j>3)goto OUT2;}
while(j<i-3)
{
temp=0;
charPos=msg[j++]-48;
memcpy(&tVal, msg+j, 4);
j+=4;
if(charPos<=19 && inPos<py_width)
*(py0+inPos)=(double)tVal;
inPos++;
}
}
OUT2:;;
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. The default datatype for signals in Simulink is double,
* but you can use other intrinsic C datatypes or even custom
* datatypes if you wish. See Simulink document "Writing S-functions"
* for details on datatype topics.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
#ifdef VARIABLE_STEP
/*
* For Variable Step Code WITH CONTINUOUS STATES
* ---------------------------------------------
* For Fortran code that implements continuous states and uses
* the mdlDerivatives interface, call your Fortran code's output
* routines from here. If it alters the states, you have to
* reset the solver. Remember, in Simulink the continuous states
* must be of type double, so be prepared to copy them to float
* if your Fortran code uses REAL as the datatype for the states.
*
* ... or, NO STATES
* -----------------
* If your code has no states and you want it to execute in
* a continuous model, keep the uPtrs, sampleArgs, y, and
* sampleOutput variables and delete x, xf, and nx. Adjust
* the function call accordingly.
*/
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
float *sampleArgs = (float *) ssGetDWork(S,1);
double *y = ssGetOutputPortRealSignal(S,0);
float *sampleOutput = (float *) ssGetDWork(S,0);
double *x = ssGetContStates(S);
float *xf = (float *) ssGetDWork(S,2);
int nx = ssGetNumContStates(S);
int k;
/*
* If the datatype in the Fortran code is REAL
* then you have to downcast the I/O and states from
* double to float as copies before sending them
* to your code (or change the Fortran code).
*/
for (k=0; k < ssGetDWorkWidth(S,1); k++) {
sampleArgs[k] = (float) (*uPtrs[k]);
}
/*
* It is recommended to use a DWork vector to
* allocate the space for the float copy of
* the states (if needed).
*/
for (k=0; k < nx; k++) {
xf[k] = (float) x[k];
}
/* ==== Call the Fortran routine (args are pass-by-reference) */
/* nameofsub_(sampleArgs, xf, &nx, sampleOutput ); */
/*
* If needed, convert the float outputs to the
* double (y) output array
*/
for (k=0; k < ssGetOutputPortWidth(S,0); k++) {
y[k] = (double) sampleOutput[k];
}
#else
/*
* For Fixed Step Code
* -------------------
* If the Fortran code implements discrete states (implicitly or
* registered with Simulink, it doesn't matter), call the code
* from mdlUpdates() and save the output values in a DWork vector.
* The variable step solver may call mdlOutputs() several
* times in between calls to mdlUpdate, and you must extract the
* values from the DWork vector and copy them to the block output
* variables.
*
* Be sure that the ssSetDWorkDataType(S,0) declaration in
* mdlInitializeSizes() uses SS_DOUBLE for the datatype when
* this code is active.
*/
double *copyOfOutputs = (double *) ssGetDWork(S, 0);
double *y = ssGetOutputPortRealSignal(S,0);
int k;
for (k=0; k < ssGetOutputSignalWidth(S,0); k++ ) {
y[k] = copyOfOutputs[k];
}
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
/* Retrieve C object from the pointers vector */
tCamera* Camera = (tCamera*) ssGetPWork(S)[0];
uint8_T *py0;
int_T y_len;
int frames_idx_in;
int frames_idx_out;
int frames_idx_out2;
int frames_avail;
int frames_avail2;
int frames_ignore;
int frames_reject;
tPvFrame* Frame;
unsigned long idx_x;
unsigned long idx_y;
unsigned long color_plane_size;
unsigned char* buf;
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
DWORD rc;
# else
struct timespec timeout;
int rc;
# endif
if( ssGetSimMode(S) != SS_SIMMODE_NORMAL )
{
return;
}
py0 = (uint8_T *)ssGetOutputPortSignal(S,0);
y_len = ssGetOutputPortWidth(S, 0);
/* Suspend thread and wait for N frames */
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
if( Camera->timeout_msec )
{
rc = WaitForSingleObject( Camera->event, Camera->timeout_msec );
if( rc != WAIT_OBJECT_0 )
{
ssSetErrorStatus(S,"Timeout occured.");
return;
}
}
EnterCriticalSection( &(Camera->csec) );
frames_idx_in = Camera->frames_idx_in;
frames_idx_out = Camera->frames_idx_out;
LeaveCriticalSection( &(Camera->csec) );
# else
if( Camera->timeout_sec || Camera->timeout_nsec )
{
clock_gettime(CLOCK_REALTIME, &timeout);
timeout.tv_sec += Camera->timeout_sec;
timeout.tv_nsec += Camera->timeout_nsec;
if( timeout.tv_nsec >= 1000000000l )
{
timeout.tv_nsec -= 1000000000l;
timeout.tv_sec++;
}
rc = 0;
pthread_mutex_lock(&(Camera->mutex));
while (!Camera->Avail && rc==0)
{
rc = pthread_cond_timedwait(&(Camera->cond), &(Camera->mutex), &timeout);
}
if( rc == ETIMEDOUT )
{
ssSetErrorStatus(S,"Timeout occured.");
return;
}
Camera->Avail = false;
frames_idx_in = Camera->frames_idx_in;
frames_idx_out = Camera->frames_idx_out;
pthread_mutex_unlock(&(Camera->mutex));
}
else
{
pthread_mutex_lock(&(Camera->mutex));
frames_idx_in = Camera->frames_idx_in;
frames_idx_out = Camera->frames_idx_out;
pthread_mutex_unlock(&(Camera->mutex));
}
# endif
frames_avail = frames_idx_in - frames_idx_out;
if( frames_avail < 0 )
{
frames_avail += Camera->frames_count + 1;
}
frames_ignore = ( frames_avail > (int)Camera->dim_N ? frames_avail-Camera->dim_N : 0 );
frames_reject = 0;
frames_avail2 = frames_avail;
frames_idx_out2 = Camera->frames_idx_out;
while( frames_avail2 )
{
Frame = Camera->frames_queue[frames_idx_out2];
if( Frame->Status != ePvErrSuccess )
{
frames_reject++;
}
frames_avail2--;
}
frames_avail2 = frames_avail - frames_reject - frames_ignore;
*((uint32_T *)ssGetOutputPortSignal(S,1)) = frames_avail2;
/* Software trigger */
if( intval(mxGetPr(paramSoftwareTrigger)[0]) )
{
ss_PvCommandRun(Camera->Handle, "FrameStartTriggerSoftware");
}
frames_avail2 = frames_avail;
while( frames_avail2 )
{
Frame = Camera->frames_queue[Camera->frames_idx_out];
if( Frame->Status == ePvErrSuccess )
{
if( frames_ignore )
{
frames_ignore--;
}
else
{
if( Camera->dim_pixel == 1)
{
for( idx_x = 0; idx_x < Camera->dim_x; idx_x++ )
{
buf = (unsigned char*)Frame->ImageBuffer + idx_x;
for( idx_y = 0; idx_y < Camera->dim_y; idx_y++ )
{
*py0++ = *buf;
buf += Camera->dim_x;
}
}
}
else if( Camera->dim_pixel == 3)
{
color_plane_size = Camera->dim_x * Camera->dim_y;
for( idx_x = 0; idx_x < Camera->dim_x; idx_x++ )
{
buf = (unsigned char*)Frame->ImageBuffer + idx_x*3;
for( idx_y = 0; idx_y < Camera->dim_y; idx_y++ )
{
*py0 = *buf;
*(py0+color_plane_size) = *(buf+1);
*(py0++ +2*color_plane_size) = *(buf+2);
buf += Camera->dim_x*3;
}
}
}
}
}
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
EnterCriticalSection( &(Camera->csec) );
# else
pthread_mutex_lock( &(Camera->mutex) );
# endif
if( ++Camera->frames_idx_out >= Camera->frames_count+1 )
{
Camera->frames_idx_out = 0;
}
# if defined(_WIN32) || defined(_WIN64) || defined(__LCC__)
LeaveCriticalSection( &(Camera->csec) );
# else
pthread_mutex_unlock( &(Camera->mutex) );
# endif
ss_PvCaptureQueueFrame(Camera->Handle, Frame, callback_fn);
frames_avail2--;
}
}
