自学教程：C++ ssSetErrorStatus函数代码示例

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

 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 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 ) ; } staticvoid 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: 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 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;        }}