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--; } }