static void mdlSetInputPortDataType(SimStruct *S, int_T port, DTypeId dataType) { int portWidth; if ( port == 0 ) { portWidth = ssGetInputPortWidth(S,0); if( isAcceptableDataType( S, dataType,portWidth ) ) { /* * Accept proposed data type if it is an unsigned integer type * force all data ports to use this data type. */ ssSetInputPortDataType( S, 0, dataType ); } else { /* Reject proposed data type */ ssSetErrorStatus(S,"Invalid input signal width, data type or input port is not connected. See mask help for valid data types."); goto EXIT_POINT; } } else { /* * Should not end up here. Simulink will only call this function * for existing input ports whose data types are unknown. */ ssSetErrorStatus(S, "Error setting input port data type."); goto EXIT_POINT; } EXIT_POINT: return; }
static void mdlStart(SimStruct * S) { uchar_T ipAddress[16]; int_T ipPort = (int_T) mxGetPr(IP_PORT_ARG)[0]; int_T noBytes = (int_T) mxGetPr(NO_BYTES_ARG)[0]; int_T id = 0; uint8_T *buffer = (uint8_T *) calloc(noBytes, sizeof(uint8_T)); if (buffer == NULL) { sprintf(msg, "Error UDP Receive: Memory allocation failed"); ssSetErrorStatus(S, msg); return; } ssSetPWorkValue(S, 0, (void *) buffer); /* initialize buffer with zeros */ memset((void *) buffer, 0, noBytes * sizeof(uint8_T)); mxGetString(IP_ADDRESS_ARG, ipAddress, 16); id = udpOpenReceive(ipPort, ipAddress, noBytes); if(id < 0){ sprintf(msg, "Error: No UDP channel free or UDP error"); ssSetErrorStatus(S, msg); } ssSetIWorkValue(S, 0, id); ssSetIWorkValue(S, 1, noBytes); }
/* Function: mdlCheckParameters ============================================= * Abstract: * Validate our parameters to verify they are okay. */ static void mdlCheckParameters(SimStruct *S) { int paramIndex = 0; bool validParam = false; /* All parameters must match the S-function Builder Dialog */ { const mxArray *pVal0 = ssGetSFcnParam(S,0); if (!mxIsDouble(pVal0)) { ssSetErrorStatus(S,"Sample time parameter SampleTime must be of type double"); return; } } { const mxArray *pVal0 = ssGetSFcnParam(S,0); if (!IS_PARAM_DOUBLE(pVal0)) { validParam = true; paramIndex = 0; goto EXIT_POINT; } } EXIT_POINT: if (validParam) { char parameterErrorMsg[1024]; sprintf(parameterErrorMsg, "The data type and or complexity of parameter %d does not match the " "information specified in the S-function Builder dialog. " "For non-double parameters you will need to cast them using int8, int16, " "int32, uint8, uint16, uint32 or boolean.", paramIndex + 1); ssSetErrorStatus(S,parameterErrorMsg); } return; }
/* Function CheckInputSampleTimes ============================================== * Abstract: * We know both input sample times, now verify that the make sense * (see top of file). */ static void CheckInputSampleTimes(SimStruct *S) { real_T enableTs = ssGetInputPortSampleTime(S, ENABLE_IPORT); real_T signalTs = ssGetInputPortSampleTime(S, SIGNAL_IPORT); int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT); if (enableTs != CONTINUOUS_SAMPLE_TIME) { /* signal must be a multiple of the enable */ real_T k = floor(signalTs/enableTs+0.5); if (k < 1.0 || fabs(k - signalTs/enableTs) > mxGetEps()*128.0) { ssSetErrorStatus(S, "Sample time of source signal (port 2) " "must be a positive multiple of enable " "(port 1) signal"); return; } } if (enableTid == CONSTANT_TID) { ssSetErrorStatus(S, "Constant sample time is not allowed " "at enable port (port 1)"); return; } } /* end CheckInputSampleTimes */
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); }
static void mdlSetInputPortComplexSignal(SimStruct *S, int_T port, int_T iPortComplexSignal) { int_T oPortComplexSignal = ssGetOutputPortComplexSignal(S,0); /* Set the complex signal of the input ports */ ssSetInputPortComplexSignal(S, port, iPortComplexSignal); if(iPortComplexSignal == COMPLEX_YES){ /* Output port must be a complex signal */ if(oPortComplexSignal == COMPLEX_INHERITED){ ssSetOutputPortComplexSignal(S, 0, COMPLEX_YES); }else if(oPortComplexSignal == COMPLEX_NO){ ssSetErrorStatus(S, "Output port must be complex."); } }else if(oPortComplexSignal != COMPLEX_NO){ /* * The current input port is a real signal. If the other input port * is a real signal, the output port must be a real signal. */ int_T otherPort = (port == 0)? 1 : 0; int_T otherPortComplexSignal = ssGetInputPortComplexSignal(S, otherPort); if(otherPortComplexSignal == COMPLEX_NO){ /* Both input ports are real signals */ if(oPortComplexSignal == COMPLEX_INHERITED){ ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO); }else if(oPortComplexSignal == COMPLEX_YES){ ssSetErrorStatus(S, "Output port must be real."); } } } }
/* Function to initialize sizes */ static void mdlInitializeSizes(SimStruct *S) { /* checksum */ ssSetChecksumVal(S, 0, 4008323717U); ssSetChecksumVal(S, 1, 2543693537U); ssSetChecksumVal(S, 2, 4289145620U); ssSetChecksumVal(S, 3, 3389381616U); /* options */ ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE); /* Accelerator check memory map size match for DWork */ if (ssGetSizeofDWork(S) != sizeof(D_Work)) { ssSetErrorStatus(S,"Unexpected error: Internal DWork sizes do " "not match for accelerator mex file."); } /* Accelerator check memory map size match for BlockIO */ if (ssGetSizeofGlobalBlockIO(S) != sizeof(BlockIO)) { ssSetErrorStatus(S,"Unexpected error: Internal BlockIO sizes do " "not match for accelerator mex file."); } /* model parameters */ _ssSetDefaultParam(S, (real_T *) &rtDefaultParameters); }
static void mdlInitializeConditions(SimStruct *S) { /* Now we can be sure that the network blocks have been initialized and that network pointers have been written to workspace. Do the remaining initialization */ RMsys *rmsys = (RMsys*) ssGetUserData(S); // Get the network pointer from the MATLAB workspace char nwsysbuf[MAXCHARS]; sprintf(nwsysbuf, "_nwsys_%d", rmsys->networkNbr); mxArray *var = (mxArray*)mexGetVariablePtr("global", nwsysbuf); if (var == NULL) { mexPrintf("Network %d not found!\n", rmsys->networkNbr); ssSetErrorStatus(S, "ttreceive: cannot connect to network block"); return; } rmsys->nwsys = (RTnetwork *)(*((long long *)mxGetPr(var))); // Check the mask input arguments if (rmsys->receiver < 1 || rmsys->receiver > rmsys->nwsys->nbrOfNodes) { mexPrintf("Receiver number %d out of bounds\n", rmsys->receiver); ssSetErrorStatus(S, "ttreceive: receiver number out of bounds"); return; } }
nneyi35s1a * ) & _rtP -> nfugx5ih43 , & _rtZCSV -> nfugx5ih43 ) ; } static void mdlInitializeSizes ( SimStruct * S ) { ssSetChecksumVal ( S , 0 , 2573679304U ) ; ssSetChecksumVal ( S , 1 , 1732126455U ) ; ssSetChecksumVal ( S , 2 , 2644021823U ) ; ssSetChecksumVal ( S , 3 , 2167879752U ) ; { mxArray * slVerStructMat = NULL ; mxArray * slStrMat = mxCreateString ( "simulink" ) ; char slVerChar [ 10 ] ; int status = mexCallMATLAB ( 1 , & slVerStructMat , 1 , & slStrMat , "ver" ) ; if ( status == 0 ) { mxArray * slVerMat = mxGetField ( slVerStructMat , 0 , "Version" ) ; if ( slVerMat == NULL ) { status = 1 ; } else { status = mxGetString ( slVerMat , slVerChar , 10 ) ; } } mxDestroyArray ( slStrMat ) ; mxDestroyArray ( slVerStructMat ) ; if ( ( status == 1 ) || ( strcmp ( slVerChar , "8.7" ) != 0 ) ) { return ; } } ssSetOptions ( S , SS_OPTION_EXCEPTION_FREE_CODE ) ; if ( ssGetSizeofDWork ( S ) != sizeof ( pklu3vjy1t ) ) { ssSetErrorStatus ( S , "Unexpected error: Internal DWork sizes do " "not match for accelerator mex file." ) ; } if ( ssGetSizeofGlobalBlockIO ( S ) != sizeof ( nmsgyp54ig ) ) { ssSetErrorStatus ( S , "Unexpected error: Internal BlockIO sizes do " "not match for accelerator mex file." ) ; } { int ssSizeofParams ; ssGetSizeofParams ( S , & ssSizeofParams ) ; if ( ssSizeofParams != sizeof ( bbqvqz25ov ) ) { static char msg [ 256 ] ; sprintf ( msg , "Unexpected error: Internal Parameters sizes do " "not match for accelerator mex file." ) ; } } _ssSetModelRtp ( S , ( real_T * ) & aaesvjrkn2 ) ; _ssSetConstBlockIO ( S , & j0x3c3z5bk ) ; rt_InitInfAndNaN ( sizeof ( real_T ) ) ; ( ( bbqvqz25ov * ) ssGetModelRtp ( S ) ) -> nfugx5ih43 . P_6 = rtInf ; ( ( bbqvqz25ov * ) ssGetModelRtp ( S ) ) -> cqabclhqip4 . P_6 = rtInf ; } static void mdlInitializeSampleTimes (
/* Function: mdlInitializeConditions ======================================== * Abstract: * Initialize states to zeros. */ static void mdlInitializeConditions(SimStruct *S) { int_T i; InputRealPtrsType M, q; /* checks the input matrices for NaN and Inf */ /* checking M */ M = ssGetInputPortRealSignalPtrs(S,0); for (i=0; i<NSTATES(S)*NSTATES(S); i++) { if ( !mxIsFinite(*M[i]) ) { ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input matrix M."); return; } } /* checking q */ q = ssGetInputPortRealSignalPtrs(S,1); for (i=0; i<NSTATES(S); i++) { if ( !mxIsFinite(*q[i]) ) { ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input vector q."); return; } } }
void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) /* ========================================================================*/ { if (dimsInfo->numDims >= 2 && dimsInfo->dims[1] > 1) ssSetErrorStatus(S, "Wrong port dimensions") else if (BUFFER_SIZE / BYTES_PER_SAMPLE % dimsInfo->dims[0]) ssSetErrorStatus(S, "Frame size must be a power of two (<= 2^18)") else ssSetInputPortDimensionInfo(S, port, dimsInfo); }
/* Function: mdlCheckParameters ============================================= * Abstract: * Validate our parameters to verify they are okay. */ static void mdlCheckParameters(SimStruct *S) { unsigned int i; if ( !mxIsChar(paramHostName) || (mxGetNumberOfElements(paramHostName) < 1) || (mxGetNumberOfElements(paramHostName) > 100) ) { ssSetErrorStatus(S, "The Host Name parameter must be a nonempty string with max. 100 elements."); return; } if ( (!IS_PARAM_DOUBLE(paramImageSizeXYN)) || (mxGetNumberOfElements(paramImageSizeXYN) < 2) || (mxGetNumberOfElements(paramImageSizeXYN) > 3) ) { ssSetErrorStatus(S, "The image size must have two elements [X Y] or three elements [X Y frames]."); return; } else { for( i=0; i<mxGetNumberOfElements(paramImageSizeXYN); i++ ) { if( intval(mxGetPr(paramImageSizeXYN)[i]) <= 0 ) { ssSetErrorStatus(S, "The image dimensions have to be positive integers."); return; } } } if ( (!IS_PARAM_DOUBLE(paramPixelFormat)) || (mxGetNumberOfElements(paramPixelFormat) != 1) ) { ssSetErrorStatus(S, "Pixel format must be a scalar"); return; } else if (intval(mxGetPr(paramPixelFormat)[0]) != 1 && intval(mxGetPr(paramPixelFormat)[0]) != 5) { ssSetErrorStatus(S, "Pixel format must be Mono8(1) or Rgb24(5)"); return; } if ( (!IS_PARAM_DOUBLE(paramTimeout)) || (mxGetNumberOfElements(paramTimeout) != 1) ) { ssSetErrorStatus(S, "Timeout must be a scalar"); return; } else if (mxGetPr(paramTimeout)[0] < 0) { ssSetErrorStatus(S, "Timeout must be non-negative"); return; } if ( (!IS_PARAM_DOUBLE(paramSampleTime)) || (mxGetNumberOfElements(paramSampleTime) != 1) ) { ssSetErrorStatus(S, "Wrong sample time."); return; } if ( !((mxGetPr(paramSampleTime)[0] == -1) || (mxGetPr(paramSampleTime)[0] > 0)) ) { ssSetErrorStatus(S, "The sample time must be inherited (-1) or discrete (a positive number)."); return; } }
/* Function: propPortComplexity =========================================== */ void propPortComplexity(SimStruct *S) { CSignal_T cY = ssGetOutputPortComplexSignal( S, 0); CSignal_T cU = ssGetInputPortComplexSignal( S, 0); /* if input is complex, then output must be complex */ if ( cU == COMPLEX_YES ) { /* if output complexity is not known then set it */ if ( cY == COMPLEX_INHERITED ) { ssSetOutputPortComplexSignal(S, 0, COMPLEX_YES); } /* if the output is real, then an error has occurred */ else if ( cY == COMPLEX_NO ) { ssSetErrorStatus(S,"Output is REAL, but input is COMPLEX."); return; } } /* if input is real then output must be real */ else if( cU == COMPLEX_NO ) { /* if output complexity is not known then set it */ if ( cY == COMPLEX_INHERITED ) { ssSetOutputPortComplexSignal(S, 0, COMPLEX_NO); } /* if the output is complex, then an error has occurred */ else if ( cY == COMPLEX_YES ) { ssSetErrorStatus(S,"Output is COMPLEX, but input is REAL."); return; } } else /* Input is COMPLEX_INHERITED) */ { if ( cY == COMPLEX_NO ) { ssSetInputPortComplexSignal(S, 0, COMPLEX_NO ); } else if ( cY == COMPLEX_YES ) { ssSetInputPortComplexSignal(S, 0, COMPLEX_YES ); } } }
/* Function: mdlCheckParameters ============================================= * Abstract: * Validate our parameters to verify they are okay. */ static void mdlCheckParameters(SimStruct *S) { /* Check 1st parameter: A-Matrix */ { if ( mxGetN(A_PARAM(S)) != NSTATES || !IS_PARAM_DOUBLE(A_PARAM(S)) ) { ssSetErrorStatus(S,"1st parameter to S-function " "\"A-Matrix\" must be square and double"); return; } } /* Check 2nd parameter: B-Matrix */ { if (mxGetM(B_PARAM(S)) != NSTATES || !IS_PARAM_DOUBLE(B_PARAM(S)) ) { ssSetErrorStatus(S,"2nd parameter to S-function " "\"B-Matrix\" is not dimensioned " "correctly"); return; } } /* Check 3rd parameter: C-Matrix */ { if (mxGetN(C_PARAM(S)) != NSTATES || !IS_PARAM_DOUBLE(C_PARAM(S)) ) { ssSetErrorStatus(S,"3rd parameter to S-function " "\"C-Matrix\" is not dimensioned " "correctly"); return; } } /* Check 4th parameter: D-Matrix */ { if (mxGetM(D_PARAM(S)) != NOUTPUTS || mxGetN(D_PARAM(S)) != NINPUTS || !IS_PARAM_DOUBLE(D_PARAM(S)) ) { ssSetErrorStatus(S,"4th parameter to S-function " "\"D-Matrix\" is not dimensioned " "correctly"); return; } } /* Check 5th parameter: X0 */ { if ( ((mxGetM(X0_PARAM(S)) != 0) && (mxGetM(X0_PARAM(S)) != NSTATES)) || !OK_EMPTY_DOUBLE_PARAM(X0_PARAM(S)) ) { ssSetErrorStatus(S,"5th parameter to S-function " "\"X0-Matrix\" is not dimensioned " "correctly"); return; } } }
/* Function: mdlCheckParameters ============================================= * Abstract: * Validate our parameters to verify: * o The numerator must be of a lower order than the denominator. * o The sample time must be a real positive nonzero value. */ static void mdlCheckParameters(SimStruct *S) { // SFUNPRINTF("Calling mdlCheckParameters"); // Tsim if (mxIsEmpty( ssGetSFcnParam(S,0)) || mxIsSparse( ssGetSFcnParam(S,0)) || mxIsComplex( ssGetSFcnParam(S,0)) || mxIsLogical( ssGetSFcnParam(S,0)) || !mxIsNumeric( ssGetSFcnParam(S,0)) || !mxIsDouble( ssGetSFcnParam(S,0)) || mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { ssSetErrorStatus(S,"Simulation time must be a single double Value"); return; } // Vrep services base name if (!mxIsChar( ssGetSFcnParam(S,1)) ) { ssSetErrorStatus(S,"Vrep services base name must be a char array (string)"); return; } // wait topic name if (!mxIsChar( ssGetSFcnParam(S,2)) ) { ssSetErrorStatus(S,"Wait topic name must be a char array (string)"); return; } // wait timeout if (mxIsEmpty( ssGetSFcnParam(S,3)) || mxIsSparse( ssGetSFcnParam(S,3)) || mxIsComplex( ssGetSFcnParam(S,3)) || mxIsLogical( ssGetSFcnParam(S,3)) || !mxIsNumeric( ssGetSFcnParam(S,3)) || !mxIsDouble( ssGetSFcnParam(S,3)) || mxGetNumberOfElements(ssGetSFcnParam(S,3)) != 1) { ssSetErrorStatus(S,"Wait timeout must be a single double Value"); return; } // wait extra_wait if (mxIsEmpty( ssGetSFcnParam(S,4)) || mxIsSparse( ssGetSFcnParam(S,4)) || mxIsComplex( ssGetSFcnParam(S,4)) || mxIsLogical( ssGetSFcnParam(S,4)) || !mxIsNumeric( ssGetSFcnParam(S,4)) || !mxIsDouble( ssGetSFcnParam(S,4)) || mxGetNumberOfElements(ssGetSFcnParam(S,4)) != 1) { ssSetErrorStatus(S,"Extra wait time must be a single double Value"); return; } }
static void mdlStart(SimStruct *S) { // ######### YARP INITIALIZATION STUFF ################## Network::init(); fprintf(stderr,"YARP NETWORK INITIALIZED\n"); if (!Network::checkNetwork() || !Network::initialized()){ ssSetErrorStatus(S,"YARP server wasn't found active!! \n"); return; } else cout<<"YARP is running!!\n"<<endl; int_T buflen, status; char *String; buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_1)))*sizeof(mxChar)+1; String = static_cast<char*>(mxMalloc(buflen)); status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)),String,buflen); if (status) { ssSetErrorStatus(S,"Cannot retrieve string from parameter 1!! \n"); return; } //string port_name = String; char *port_name = String; //FROM port name buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_2)))*sizeof(mxChar)+1; String = static_cast<char*>(mxMalloc(buflen)); status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)),String,buflen); if (status) { ssSetErrorStatus(S,"Cannot retrieve string from parameter 2!! \n"); return; } char *toPort_name = String; // ######## CHECKING INPUT PARAMETERS ############ BufferedPort<Vector> *toPort; toPort = new BufferedPort<Vector>; toPort->open(toPort_name); ConstString toPortName = toPort->getName(); cout<<"[From] Port name will be: "<<port_name<<endl; cout<<"[To] Port name will be: "<<toPort->getName()<<endl; ssGetPWork(S)[0] = toPort; Network::connect(port_name,toPortName); }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. Generally outputs are placed in the output vector, ssGetY(S). */ static void mdlOutputs(SimStruct *S, int_T tid){ #if defined(RT) TmcdDeviceBallActual* actualBall = 0; TmcdDeviceActual* actual = 0; # endif Context* context = (Context*)(ssGetPWork(S)[0]); #if defined(RT) if (context->enableSpacemouse) { actualBall = tmcdDeviceBallGetActual(context->tmcd2005.deviceBall.handle); if ( !actualBall ) ssSetErrorStatus(S,"could not get state from TMCD2005"); // copy events to simulink outputs *((boolean_T*)ssGetOutputPortSignal(S, 0)) = actualBall->on; ((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actualBall->keys[0]; ((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actualBall->keys[1]; ((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actualBall->keys[2]; ((real_T*)ssGetOutputPortSignal(S, 2))[0] = actualBall->ballData[0]; ((real_T*)ssGetOutputPortSignal(S, 2))[1] = actualBall->ballData[1]; ((real_T*)ssGetOutputPortSignal(S, 2))[2] = actualBall->ballData[2]; ((real_T*)ssGetOutputPortSignal(S, 2))[3] = actualBall->ballData[3]; ((real_T*)ssGetOutputPortSignal(S, 2))[4] = actualBall->ballData[4]; ((real_T*)ssGetOutputPortSignal(S, 2))[5] = actualBall->ballData[5]; *((boolean_T*)ssGetOutputPortSignal(S, 3)) = actualBall->isBallActive; } else { actual = tmcdDeviceGetActual(context->tmcd2005.device.handle); if ( !actual ) ssSetErrorStatus(S,"could not get state from TMCD2005"); // copy events to simulink outputs *((boolean_T*)ssGetOutputPortSignal(S, 0)) = actual->on; ((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actual->keys[0]; ((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actual->keys[1]; ((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actual->keys[2]; ((boolean_T*)ssGetOutputPortSignal(S, 1))[3] = actual->keys[3]; } #endif }
/* Function: mdlCheckParameters ============================================= * Abstract: * Validate our parameters to verify: * o The numerator must be of a lower order than the denominator. * o The sample time must be a real positive nonzero value. */ static void mdlCheckParameters(SimStruct *S) { // SFUNPRINTF("Calling mdlCheckParameters"); // Tsim if (mxIsEmpty( ssGetSFcnParam(S,0)) || mxIsSparse( ssGetSFcnParam(S,0)) || mxIsComplex( ssGetSFcnParam(S,0)) || mxIsLogical( ssGetSFcnParam(S,0)) || !mxIsNumeric( ssGetSFcnParam(S,0)) || !mxIsDouble( ssGetSFcnParam(S,0)) || mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { ssSetErrorStatus(S,"Simulation time must be a single double Value"); return; } // Prefix Topic if (!mxIsChar( ssGetSFcnParam(S,1)) ) { ssSetErrorStatus(S,"Prefix value must be char array (string)"); return; } // Robot Array if (mxIsEmpty( ssGetSFcnParam(S,2)) || mxIsSparse( ssGetSFcnParam(S,2)) || mxIsComplex( ssGetSFcnParam(S,2)) || mxIsLogical( ssGetSFcnParam(S,2)) || !mxIsChar( ssGetSFcnParam(S,2)) ) { ssSetErrorStatus(S,"Robot Vector must be a char vector of robot ids"); return; } // Postfix Topic if (!mxIsChar( ssGetSFcnParam(S,3)) ) { ssSetErrorStatus(S,"Postfix value must be char array (string)"); return; } // N. of dofs if (mxIsEmpty( ssGetSFcnParam(S,4)) || mxIsSparse( ssGetSFcnParam(S,4)) || mxIsComplex( ssGetSFcnParam(S,4)) || mxIsLogical( ssGetSFcnParam(S,4)) || !mxIsChar( ssGetSFcnParam(S,4)) || mxGetNumberOfElements(ssGetSFcnParam(S,4)) != 1) { ssSetErrorStatus(S,"N. of dofs must be a char"); return; } }
static void mdlInitializeSizes(SimStruct *S) { int_T msgType; int_T initValLen; ssSetNumSFcnParams(S,5); if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return; /* Parameter mismatch will be reported by Simulink */ } ssSetNumContStates(S,0); ssSetNumDiscStates(S,0); if (!ssSetNumInputPorts(S,0)) return; msgType = (int_T)*mxGetPr(ssGetSFcnParam(S,0)); switch (msgType) { case SUBSCRIBER_TWIST_STAMPED: case PUBLISHER_POINT_STAMPED: if (!ssSetNumOutputPorts(S,2)) return; ssSetOutputPortWidth(S,1,2); break; default: if (!ssSetNumOutputPorts(S,1)) return; } switch (msgType) { case SUBSCRIBER_FLOAT64: case SUBSCRIBER_INT32: case SUBSCRIBER_BOOL: case SUBSCRIBER_TIME: ssSetOutputPortWidth(S,0,1); break; case SUBSCRIBER_FLOAT64ARRAY: ssSetOutputPortWidth(S,0,DYNAMICALLY_SIZED); break; case SUBSCRIBER_TWIST: case SUBSCRIBER_TWIST_STAMPED: ssSetOutputPortWidth(S,0,6); break; case PUBLISHER_POINT: case PUBLISHER_POINT_STAMPED: case PUBLISHER_POSE2D: ssSetOutputPortWidth(S,0,3); break; default: ssSetErrorStatus(S, "Unknown message type!"); return; } initValLen = mxGetNumberOfElements(ssGetSFcnParam(S,2)); if (initValLen > 1) { ssSetOutputPortWidth(S,0,initValLen); } ssSetNumSampleTimes(S,1); ssSetNumRWork(S,0); ssSetNumIWork(S,1); ssSetNumPWork(S,2); ssSetNumModes(S,0); ssSetNumNonsampledZCs(S,0); ssSetOptions(S,0); }
/* Function: mdlCheckParameters ============================================= * Abstract: * Validate our parameters to verify they are okay. */ static void mdlCheckParameters(SimStruct *S) { #define PrmNumPos 46 int paramIndex = 0; bool validParam = false; char paramVector[] ={'1'}; static char parameterErrorMsg[] ="The data type and/or complexity of parameter does not match the information " "specified in the S-function Builder dialog. For non-double parameters you will need to cast them using int8, int16," "int32, uint8, uint16, uint32 or boolean."; /* All parameters must match the S-function Builder Dialog */ { const mxArray *pVal0 = ssGetSFcnParam(S,0); if (!IS_PARAM_DOUBLE(pVal0)) { validParam = true; paramIndex = 0; goto EXIT_POINT; } } EXIT_POINT: if (validParam) { parameterErrorMsg[PrmNumPos] = paramVector[paramIndex]; ssSetErrorStatus(S,parameterErrorMsg); } return; }
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) { boolean_T isNotVector = ((dimsInfo->numDims == 2 ) && (dimsInfo->dims[0] > 1 && dimsInfo->dims[1] > 1)) ; if(isNotVector){ ssSetErrorStatus(S, "The block only accepts vector signals. " "It does not accept a [mxn] matrix signal " "where m > 1 and n > 1."); }else{ int otherPort = (port == 0) ? 1 : 0; if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return; /* * If other port width is unknown, set the other port width. * Note1: we cannot update other port dimension info. * Note2: For simplicity, this block cannot accept partial dimension, * however, it may partially set other port dimension info. */ if(ssGetInputPortWidth(S, otherPort) == DYNAMICALLY_SIZED && ssGetInputPortWidth(S, port) != DYNAMICALLY_SIZED){ DECL_AND_INIT_DIMSINFO(dimsInfo); dimsInfo.width = ssGetInputPortWidth (S, port); dimsInfo.numDims = ssGetInputPortNumDimensions(S, otherPort); dimsInfo.dims = ssGetInputPortDimensions (S, otherPort); if(!ssSetInputPortDimensionInfo(S, otherPort, &dimsInfo)) return; } } }
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) { /* This should never occur! */ ssSetErrorStatus(S, "Error setting output port width."); }
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) { # ifdef DEBUG printf(" co_udp_receiver :: Now in mdlSetOutputPortDimensionInfo.\n"); # endif /* * If the block has scalar parameter, the output dimensions are unknown. * Set the input and output port to have the same dimensions. */ if(!ssSetOutputPortDimensionInfo(S, port, dimsInfo)) return; /* The block only accepts 2-D signals. Check number of dimensions. */ if(dimsInfo->numDims != 1) { ssSetErrorStatus(S, "Invalid output port dimensions. The output signal " "must be a vector signal."); return; } else { /* Set the input port dimensions */ if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return; } }
/* Function: mdlStart ======================================================= * Abstract: * This function is called once at start of model execution. If you * have states that should be initialized once, this is the place * to do it. */ static void mdlStart(SimStruct *S) { int i; /* Bus Information */ slDataTypeAccess *dta = ssGetDataTypeAccess(S); const char *bpath = ssGetPath(S); DTypeId u_busId = ssGetDataTypeId(S, "u_bus"); DTypeId x_busId = ssGetDataTypeId(S, "x_bus"); int_T *busInfo = (int_T *)malloc((ACADO_NX+ACADO_NU)*2*sizeof(int_T)); if(busInfo==NULL) { ssSetErrorStatus(S, "Memory allocation failure"); return; } /* Calculate offsets of all primitive elements of the bus */ for( i = 0; i < ACADO_NX; i++ ) { busInfo[2*i] = dtaGetDataTypeElementOffset(dta, bpath,x_busId,i); busInfo[2*i+1] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double")); } for( i = 0; i < ACADO_NU; i++ ) { busInfo[2*(ACADO_NX+i)] = dtaGetDataTypeElementOffset(dta, bpath,u_busId,i); busInfo[2*(ACADO_NX+i)+1] = dtaGetDataTypeSize(dta, bpath, ssGetDataTypeId(S, "double")); } ssSetUserData(S, busInfo); }
static void mdlUpdate(SimStruct *S, int_T tid) { UNUSED(tid); TimePoint *wall_clock_start_time = static_cast<TimePoint *>(ssGetPWorkValue(S, 0)); if (!wall_clock_start_time) { // t0 not set yet, set it now. (note: used to set this in mdlStart, but // there can be a big pause between mdlStart and the full simulation start) wall_clock_start_time = new TimePoint(); *wall_clock_start_time = TimeClock::now(); ssSetPWorkValue(S, 0, wall_clock_start_time); } double sim_time = ssGetT(S); double realtime_factor = mxGetScalar(ssGetSFcnParam(S, 1)); TimePoint wall_time = TimeClock::now(); TimePoint desired_time = *wall_clock_start_time + TimeDuration(sim_time / realtime_factor); if (desired_time > wall_time) { // could probably just call sleep_until, but just in case std::this_thread::sleep_until(desired_time); } else if (wall_time > desired_time + std::chrono::duration<double>( 1.0 / realtime_factor)) { mexPrintf("at time %f, I'm behind by more than 1 (scaled) second\n", sim_time); ssSetErrorStatus(S, "Simulink is not keeping up with real time. Consider " "reducing demands on your ODE solver, or optimizing your " "code."); } }
static void mdlUpdate(SimStruct *S, int_T tid) { UNUSED(tid); struct timeval tv; gettimeofday(&tv, NULL); double *wall_t0 = (double*) ssGetDWork(S,0); if (*wall_t0 == TIME_NOT_YET_SET) { // t0 not set yet, set it now. (note: used to set this in mdlStart, but there can be a big pause between mdlStart and the full simulation start) *wall_t0 = timevalToDouble(tv); return; } double wall_t = timevalToDouble(tv), simtime = ssGetT(S), realtime_factor = mxGetScalar(ssGetSFcnParam(S, 1)); double t_diff = (wall_t - *wall_t0)*realtime_factor - simtime; // mexPrintf("wall time: %f, sim time: %f, (scaled) diff: %f\n", wall_t-*wall_t0, simtime, t_diff); if (t_diff<0.0) { // then simtime > scaled wall_time timespec tosleep; tv = doubleToTimeval(-t_diff); tosleep.tv_sec = tv.tv_sec; tosleep.tv_nsec = tv.tv_usec * 1000; // mexPrintf("sleeping... %d sec, %d nsec\n", tosleep.tv_sec, tosleep.tv_nsec); nanosleep(&tosleep, NULL); } else if (t_diff>1.0) { // then I'm behind by more than 1 second mexPrintf("at time %f, I'm behind by %f sec\n", simtime, t_diff); ssSetErrorStatus(S, "Simulink is not keeping up with real time. Consider reducing demands on your ODE solver, or optimizing your code."); } }
static void mdlInitializeSizes(SimStruct *S) { ssSetNumSFcnParams(S, 5); /* Number of expected parameters */ #ifndef TRES_SIMULINK_DISABLE_MASK_PROTECTION // Perform mask params validity check // TODO very basic error check (to be improved). const mxArray *mxMsdVarName = ssGetSFcnParam(S,MSG_DESCR_VARNAME); if ((mxGetM(mxMsdVarName) != 1) || (mxGetN(mxMsdVarName) == 0)) { ssSetErrorStatus(S, "The message-set description variable cannot be empty"); return; } const mxArray *mxNdVarName = ssGetSFcnParam(S,NTWK_DESCR_VARNAME); if ((mxGetM(mxNdVarName) != 1) || (mxGetN(mxNdVarName) == 0)) { ssSetErrorStatus(S, "The network description variable cannot be empty (you must specify at least the network topology!)"); return; } const mxArray *mxAddLibsPath = ssGetSFcnParam(S,OTHER_DEPS); if ((mxGetM(mxAddLibsPath) != 1) || (mxGetN(mxAddLibsPath) == 0)) { ssSetErrorStatus(S, "The Additional Libraries (see the Simulator tab) field cannot be empty"); return; } #endif ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); // Set the number of input ports to 0 if (!ssSetNumInputPorts(S, 0)) return; // Set the output port to have a dynamic dimension if (!ssSetNumOutputPorts(S, 1)) return; ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED); ssSetNumSampleTimes(S, 1); ssSetNumDWork(S, 1); // store the `New pending activations available' flag ssSetDWorkWidth(S, 0, 1); ssSetDWorkDataType(S, 0, SS_BOOLEAN); ssSetNumPWork(S, 1); // store the tres::Network ssSetNumRWork(S, 1); // store the time_resolution ssSetNumNonsampledZCs(S, 1); // next hit }
void TT_CALLBACK_ERROR(SimStruct *S, const char *error_msg) { mxArray *rhs[1]; rhs[0] = mxCreateString(error_msg); mexCallMATLAB(0, NULL, 1, rhs, "error"); mexPrintf("??? %s\n\nIn block ==> %s\nSimulation aborted!\n", error_msg, ssGetBlockName(S)); ssSetErrorStatus(S, ""); }
static void mdlInitializeSizes(SimStruct *S) { int i,nChannels; static char msg[256]; ssSetNumSFcnParams(S, NUMBER_OF_ARGS); /* Number of expected parameters */ if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { /* Return if number of expected != number of actual parameters */ sprintf(msg,"Wrong Number of input arguments passed.\n%d arguments are expected\n",NUMBER_OF_ARGS); ssSetErrorStatus(S,msg); return; } ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); ssSetNumInputPorts(S, 0);//there is no input //if (!ssSetNumInputPorts(S, 1)) return; nChannels=(uint_T)mxGetN(CHANNEL_ARG); //if (!ssSetNumOutputPorts(S, 1)) return; ssSetNumOutputPorts(S,nChannels);//set number of outputs ports for(i=0;i<nChannels;i++) { ssSetOutputPortWidth(S, i, 1);//set width of each port to 1 } ssSetInputPortWidth(S, 0, 1); ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/ /* * 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. */ ssSetInputPortDirectFeedThrough(S, 0, 1); ssSetNumSampleTimes(S, 1);//set number of sample time to 1,which means all input ports share same Ts ssSetNumRWork(S, NO_R_WORKS);//number of float in work vector ssSetNumIWork(S, NO_I_WORKS);//number of int in work vector ssSetNumPWork(S, 0);//number of pointer in work vector ssSetNumModes(S, 0); ssSetNumNonsampledZCs(S, 0);//no need for pass-zero check ssSetSFcnParamNotTunable(S,0); ssSetSFcnParamNotTunable(S,1); ssSetSFcnParamNotTunable(S,2); ssSetSFcnParamNotTunable(S,3); ssSetSFcnParamNotTunable(S,4); ssSetOptions(S,SS_OPTION_EXCEPTION_FREE_CODE|SS_OPTION_PLACE_ASAP); /* Specify the sim state compliance to be same as a built-in block */ ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); }
/* 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) { char host[] = "localhost"; int port = 9002; trigger = 0; /* See sfuntmpl_doc.c for more details on the macros below */ ssSetNumSFcnParams(S, 1); /* Number of expected parameters */ if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { /* Return if number of expected != number of actual parameters */ return; } ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); /* This functions receives no inputs, but it could use inputs instead of hard-coded parameters (e.g. port and host) */ if (!ssSetNumInputPorts(S, 0)) return; /*ssSetInputPortWidth(S, 0, 1);*/ /*ssSetInputPortRequiredContiguous(S, 0, true); /direct input signal access*/ /* * 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, 2)) return; ssSetOutputPortWidth(S, 0, PAYLOAD_LEN/2); ssSetOutputPortDataType(S, 0, SS_UINT16); ssSetOutputPortWidth(S, 1, 1); ssSetNumSampleTimes(S, 1); ssSetNumRWork(S, 0); ssSetNumIWork(S, 0); ssSetNumPWork(S, 0); ssSetNumModes(S, 0); ssSetNumNonsampledZCs(S, 0); ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE); /* Connect to the SerialForwarder server using functions linked in sfsource.o */ fd = open_sf_source(host, port); if (fd < 0){ ssSetErrorStatus(S,"SerialForwarder not running in the specified host and port"); return; } }