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

   /* 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 (!IS_PARAM_INT8(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 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.");  }}

/* 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){    real_T t, *xC, *xD, *u;    int i;    SS=S;    t  = ssGetT(S);    xC = ssGetContStates(S);    xD = ssGetDiscStates(S);    u  = ssGetInputPortRealSignal(S,0);    mengpH_Update(t,xC,xD,u,                  ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));}

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 mdlInitializeSampleTimes(SimStruct *S){    real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0));    ssSetSampleTime(S, 0, Tsim);                      //DISCRETE_SAMPLE_TIME);    ssSetOffsetTime(S, 0, 0.0);}

static void mdlUpdate(SimStruct *S, int_T tid) {    yarp::os::BufferedPort<yarp::os::Bottle> *yPortIn = (yarp::os::BufferedPort<yarp::os::Bottle> *) ssGetPWork(S)[1];  yarp::os::Bottle *bottleIn = yPortIn->read(false); // shouldwait = false  if(bottleIn != NULL) {#ifdef DEBUG        mexPrintf("Receiving: #%s#/n", bottleIn->toString());    std::string strNull = std::string("is NULL: ") + std::string((bottleIn == NULL ? "yes": "no")) + std::string("/n");    mexPrintf(strNull.c_str());#endif    char_T buf03[LENGTH];    mxGetString(ssGetSFcnParam(S, 2), buf03, LENGTH);    std::string strVarName(buf03);        for (int bb=0;bb<(bottleIn->size()-1);bb++){      yarp::os::Value item = bottleIn->get(bb);      std::string strKey = item.asList()->get(0).asString();      double fValue = item.asList()->get(1).asDouble();      if(!strKey.compare(strVarName)){	  real_T *y = ssGetOutputPortRealSignal(S, 0);	  y[0] = fValue;	}    }  }  } 

  /* 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){    Context* context;        context = (Context*)malloc(sizeof(Context));    if (context == NULL)        setErrorStatus(S, "mdlStart: memeory allocation error");    memset(context, 0, sizeof(context));        //save context in SS    ssGetPWork(S)[0] = context;            context->enableSpacemouse = (int)(*((double*)mxGetData(ssGetSFcnParam(S, 0))));    #  if defined(RT)        if (context->enableSpacemouse) {        context->tmcd2005.deviceBall.handle= tmcdDeviceBallOpen("/dev/tmcd2005/ball/ser1");        if ( !context->tmcd2005.deviceBall.handle )            setErrorStatus(S, "mdlStart: Could not open TMCD2005 @ /dev/tmcd2005/ser1");    }    else {        context->tmcd2005.device.handle= tmcdDeviceOpen("/dev/tmcd2005/noball/ser1");        if ( !context->tmcd2005.device.handle )            setErrorStatus(S, "mdlStart: Could not open TMCD2005 @ /dev/tmcd2005/ser1");    }#  endif            }

/* 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){    real_T t, *xC, *xD, *uWQ, *uES, *sysWQ, *sysEM;    int i;   	SS=S;    t  = ssGetT(S);    xC = ssGetContStates(S);    xD = ssGetDiscStates(S);    uWQ  = ssGetInputPortRealSignal(S,0);    sysWQ  = ssGetOutputPortRealSignal(S,0);        for (i=0;i<U(S,"Number")*B(S,"WaterIn")*B(S,"WaterOut");i++)  	{	    sysWQ[i] = uWQ[i];  	}  	// verwijzing naar exra inputs  	if (B(S,"Setpoints")>0)  	{  	  uES = &(uWQ[ (int) (U(S,"Number")*B(S,"WaterIn"))]);   	}  	else  	{  	  uES=NULL;  	}  	  	// verwijzing naar exra metingen  	if (B(S,"Measurements")>0)  	{  	  sysEM = &(sysWQ[(int) (U(S,"Number")*B(S,"WaterOut"))]);       for (i=0;i<B(S,"Measurements");i++)    	{	      sysEM[i] = 0.0;  	  }  	}  	else  	{  	  sysEM=NULL;  	}  		    wsgetq_Outputs(sysWQ,sysEM,t,xC,xD,uWQ,uES,                   ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));}

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

/* 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.c for more details on the macros below */	ssSetNumSFcnParams(S, 5);  /* Number of expected parameters */#if defined(MATLAB_MEX_FILE)	if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {		mdlCheckParameters(S);		if (ssGetErrorStatus(S) != NULL) {			return;		}	} else {		return; /* Parameter mismatch will be reported by Simulink. */	}#endif	ssSetNumContStates(S, 0);	ssSetNumDiscStates(S, 0);	if (!ssSetNumInputPorts(S, 0)) return;	if (!ssSetNumOutputPorts(S, 0)) return;	ssSetNumSampleTimes(S, 1);	ssSetNumRWork(S, 0);	ssSetNumIWork(S, 1); // nWaitTopic	size_t wait_buflen = mxGetN((ssGetSFcnParam(S, 2)))*sizeof(mxChar)+1;	char* wait_name = (char*)mxMalloc(wait_buflen);	mxGetString((ssGetSFcnParam(S, 2)), wait_name, wait_buflen);	char * topic = strtok(wait_name, TOPIC_SEPARATORS);	unsigned int nWaitTopic = 0;	while (topic != NULL){		nWaitTopic++;		topic = strtok(NULL, TOPIC_SEPARATORS);	}	mxFree(wait_name);	ssSetNumPWork(S, 4+nWaitTopic+1); //Start + Stop + Synchronous + Trigger services + wait_topic Subscriber(s) + AsyncSpinner	ssSetNumModes(S, 0);	ssSetNumNonsampledZCs(S, 0);	/* Specify the sim state compliance to be same as a built-in block */	ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);	ssSetOptions(S, 0);}

/* 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){    real_T t, *xC, *xD, *u, *sys;    int i;    SS=S;    t  = ssGetT(S);    xC = ssGetContStates(S);    xD = ssGetDiscStates(S);    u  = ssGetInputPortRealSignal(S,0);    sys  = ssGetOutputPortRealSignal(S,0);    mengpH_Outputs(sys,t,xC,xD,u,                   ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));}

  /* Function: mdlDerivatives =================================================   * Abstract:   *    In this function, you compute the S-function block's derivatives.   *    The derivatives are placed in the derivative vector, ssGetdX(S).   */  static void mdlDerivatives(SimStruct *S)  {    real_T t, *xC, *xD, *u, *sys;    int i;        t  = ssGetT(S);    xC = ssGetContStates(S);    xD = ssGetDiscStates(S);    u  = ssGetInputPortRealSignal(S,0);    sys  = ssGetdX(S);    block0_Derivatives(sys,t,xC,xD,u,      ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));      }

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}

/* 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){	void** vecPWork = ssGetPWork(S);	ros::ServiceClient* trigger_client = (ros::ServiceClient*)vecPWork[3];	const real_T timeout = mxGetScalar(ssGetSFcnParam(S, 3));	const int_T nWaitTopic = ssGetIWork(S)[0];	// reset all semaphores	for (int_T i = 0; i<nWaitTopic; ++i){		GenericSubscriber<topic_tools::ShapeShifter>* sub =				(GenericSubscriber<topic_tools::ShapeShifter>*) vecPWork[4+i];		sub->resetSem();	}	vrep_common::simRosSynchronousTrigger trigSrv;	trigger_client->call(trigSrv);	if (trigSrv.response.result == -1){		ssSetErrorStatus(S, "Error triggering V-REP simulation");	}	// wait for all messages	for (int_T i = 0; i<nWaitTopic; ++i){		GenericSubscriber<topic_tools::ShapeShifter>* sub =				(GenericSubscriber<topic_tools::ShapeShifter>*) vecPWork[4+i];		if (sub->waitMsg(timeout) == -1){			const std::string tmp = std::string("Timeout expired for topic ") + sub->getTopicName();			ssWarning(S, tmp.c_str());		} else {		}	}	const real_T extraWaitTime = mxGetScalar(ssGetSFcnParam(S, 4));	if (extraWaitTime>0){		//				  SFUNPRINTF("Waiting extra %f seconds/n", extraWaitTime);		ros::Duration extra_wait(extraWaitTime);		extra_wait.sleep();	}}

/* Function: mdlInitializeSizes =============================================== * Abstract: *    The sizes information is used by Simulink to determine the S-function *    block's characteristics (number of inputs, outputs, states, etc.). */int CheckPar(SimStruct *S){  // B check fields  // x0 check real + lengte  // U check Number + items  // P check Parameters?!  if (mxGetField(ssGetSFcnParam(S, 0),0,"WaterIn")==NULL)  {        ssSetErrorStatus(S,"Missing B.WaterIn in Block parameters. Adjust wsgetq_i.m!");        return 1;  }  if (mxGetField(ssGetSFcnParam(S, 0),0,"WaterOut")==NULL)  {        ssSetErrorStatus(S,"Missing B.WaterOut in Block parameters. Adjust wsgetq_i.m!");        return 1;  }  return 0;}

/* * Check to make sure that each parameter is 1-d and positive */static void mdlCheckParameters(SimStruct *S) {  const mxArray *pVal0 = ssGetSFcnParam(S,0);  //  if ( !IS_PARAM_DOUBLE(pVal0)) {  if ( !IS_PARAM_CARRAY(pVal0)) {    //    ssSetErrorStatus(S, "Parameter to S-function must be a double scalar");    ssSetErrorStatus(S, "Parameter to S-function must be a text");    return;  } }

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){       real_T* vecRWork = ssGetRWork(S);    void** vecPWork = ssGetPWork(S);    int_T* vecIWork = ssGetIWork(S);    ros::Time* firstExecTime = (ros::Time*)vecPWork[0];    ros::Time* lastExecTime = (ros::Time*)vecPWork[1];    int_T showEveryNumCycles = (int_T)mxGetScalar(ssGetSFcnParam(S, 2));    real_T slowDownFactor = (real_T)mxGetScalar(ssGetSFcnParam(S,3));        //simulationTime += Tsim;    vecRWork[1] += slowDownFactor * vecRWork[0];        ros::Time currentTime = ros::Time::now();    ros::Time timeElapsed((currentTime - *firstExecTime).toSec());    *lastExecTime = currentTime;    ros::Duration sleepTime = ros::Time(vecRWork[1]) - timeElapsed;    // output of sleepTime    real_T *output = (real_T*)ssGetOutputPortSignal(S,0);    output[0] = sleepTime.toSec();    if (sleepTime.toSec() > 0.0) {        sleepTime.sleep();    }    else if (sleepTime.toSec() < -1.0 * mxGetScalar(ssGetSFcnParam(S, 1))) { // show error only if the delay time is above a certain threshold (2nd parameter)	    if (showEveryNumCycles > 0) { // never show error when (3rd parameter) <= 0            	if (vecIWork[0] >= showEveryNumCycles) { // show only every (3rd parameter) cycles 		            SFUNPRINTF("Beware! rtTimer is negative! Time: %f/n", sleepTime.toSec());			vecIWork[0] = 1;	    	}            	else {	    	    ++vecIWork[0];            	}	    }    }}

static void mdlStart(SimStruct *S) {#ifdef CO_DEBUG    printf("co_udp_receiver :: in mdlStart/n");#endif    //allocate my_data_t block which stores all variable used in this block    struct my_data_t *my_data = new my_data_t;    memset(my_data,0,sizeof(my_data_t));    ssSetPWorkValue(S,0,(void*)my_data);    int co_port = (int)*mxGetPr(ssGetSFcnParam(S,0));    int nonblockingmode = (int)*mxGetPr(ssGetSFcnParam(S,1));#ifdef CO_DEBUG    printf(" co_udp_receiver :: CO Port number received is %d/n", co_port);#endif    if(co_port < 1 || co_port > 64000) { /* print error message and exit */        fprintf(stderr," co_udp_receiver :: Bad port number %d/n",co_port);    }    co_udp_server_init(co_port,nonblockingmode,my_data);}

/* 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.c for more details on the macros below */    ssSetNumSFcnParams(S, 4);  /* Number of expected parameters */#if defined(MATLAB_MEX_FILE)    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {        mdlCheckParameters(S);        if (ssGetErrorStatus(S) != NULL) {            return;        }    } else {        return; /* Parameter mismatch will be reported by Simulink. */    }#endif    int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2));    ssSetNumContStates(S, 0);    ssSetNumDiscStates(S, 0);    if (!ssSetNumInputPorts(S, 2)) return;    ssSetInputPortMatrixDimensions(S, 0, 3, nRobots); // position    ssSetInputPortMatrixDimensions(S, 1, 3, nRobots); // orientation	for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) {		/*direct input signal access*/    	ssSetInputPortRequiredContiguous(S, i, true); 				/*		 * 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, i, 1);	}    if (!ssSetNumOutputPorts(S, 0)) return;    ssSetNumSampleTimes(S, 1);    ssSetNumRWork(S, 0);    ssSetNumIWork(S, 1); // nRobots    ssSetNumPWork(S, nRobots); // nRobots x GenericPub    ssSetNumModes(S, 0);    ssSetNumNonsampledZCs(S, 0);    /* Specify the sim state compliance to be same as a built-in block */    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);    ssSetOptions(S, 0);}

static void mdlOutputs(SimStruct *S,int_T tid){  real_T *y = ssGetOutputPortRealSignal(S,0);  real_T *p = mxGetPr(ssGetSFcnParam(S,0));  int_T i;  InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);  real_T u[INPUTPORT0WIDTH];  for(i=0; i<INPUTPORT0WIDTH; i++) {    u[i]=*uPtrs[i];  }  calcOutputs(y,u,p);}

static void mdlStart(SimStruct *S) {	SFUNPRINTF("Creating Instance of %s./n", TOSTRING(S_FUNCTION_NAME));	// init ROS if not yet done.	initROS(S);	void** vecPWork = ssGetPWork(S);	ros::NodeHandle nodeHandle(ros::this_node::getName());	// get Topic Strings	size_t buflen = mxGetN((ssGetSFcnParam(S, 1))) * sizeof(mxChar) + 1;	char* topic = (char*) mxMalloc(buflen);	mxGetString((ssGetSFcnParam(S, 1)), topic, buflen);	//SFUNPRINTF("The string being passed as a Paramater is - %s/n ", topic);	GenericPublisher<shape_msgs::Mesh>* pub = new GenericPublisher<			shape_msgs::Mesh>(nodeHandle, topic, 10);	vecPWork[0] = pub;	mxFree(topic);}

/* Function: mdlSetDefaultPortDimensionInfo ==================================== *    This routine is called when Simulink is not able to find dimension *    candidates for ports with unknown dimensions. This function must set the *    dimensions of all ports with unknown dimensions. */static void mdlSetDefaultPortDimensionInfo(SimStruct *S){    char temp[3];    int i;    char* pp2=(char*)mxGetPr(ssGetSFcnParam(S,1));    int param_width2=mxGetNumberOfElements(ssGetSFcnParam(S,1));      char* pp3=(char*)mxGetPr(ssGetSFcnParam(S,2));    int param_width3=mxGetNumberOfElements(ssGetSFcnParam(S,2));          memset(temp, 0,3* sizeof(char));    for (i=0;i<param_width2;i++)        temp[i]=pp2[i*2];    if( atoi(temp)<=0)    {        if(!ssSetInputPortMatrixDimensions(S, 0, 1, 1)) return;    }    else    {        /*if(!ssSetInputPortMatrixDimensions(S, 0, 1, atoi(temp))) return;remove?*/    }            memset(temp, 0,3* sizeof(char));    for (i=0;i<param_width3;i++)		{        temp[i]=pp3[i*2];	printf("%c_",temp[i]);}    if( atoi(temp)>0)    {        if(!ssSetOutputPortMatrixDimensions(S, 0, 1, atoi(temp))) return;    }    else    {        if(!ssSetOutputPortMatrixDimensions(S, 0, 1, 1)) return;    }} /* end mdlSetDefaultPortDimensionInfo */

   /* 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','2'};     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_UINT32(pVal0)) {	    validParam = true;	    paramIndex = 0;	    goto EXIT_POINT;	  }	 }	 {	  const mxArray *pVal1 = ssGetSFcnParam(S,1);	  if (!IS_PARAM_UINT32(pVal1)) {	    validParam = true;	    paramIndex = 1;	    goto EXIT_POINT;	  }	 }           EXIT_POINT:      if (validParam) {	  parameterErrorMsg[PrmNumPos] = paramVector[paramIndex];	  ssSetErrorStatus(S,parameterErrorMsg);      }	return;    }

static void mdlOutputs(SimStruct *S,int_T tid) {	InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0);	InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1);	real_T prev = ssGetRWorkValue(S,0);	bool dataPort = PARAM(2)[0];	int_T i;#ifndef MATLAB_MEX_FILE	rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);	SEM *sem = (SEM *)ssGetPWorkValue(S,1);#endif	char_T *msg;	unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);	UNUSED_ARG(tid);	 /* not used in single tasking mode */	if (U0(0) > 0.5 && U0(0) > prev) {		msg = (char_T *)malloc(strlen);		mxGetString(ssGetSFcnParam(S,1), msg, strlen);#ifndef MATLAB_MEX_FILE		if (dataPort) {			for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {				asprintf(&msg, "%s %f", msg, U1(i));			}		}		if (rt_sem_wait_if(sem) != 0) {			memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE);			shm->msg.state = NEW_VALUE;			rt_sem_signal(sem);		}#else		switch ((int)PARAM(0)[0]) {			case 1: printf("DEBUG"); break;			case 2: printf("INFO"); break;			case 3: printf("WARN"); break;			case 4: printf("ERROR"); break;			case 5: printf("FATAL"); break;			default: printf("NONE"); break;		}		printf(": %s", msg);		if (dataPort) {			for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {				printf(" %f", U1(i));			}		}		printf("/n");#endif		free(msg);	}	ssSetRWorkValue(S,0,U0(0));}

static void mdlDerivatives(SimStruct *S){    real_T *dx = ssGetdX(S);    real_T *x = ssGetContStates(S);    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);    double *m=mxGetPr(ssGetSFcnParam(S,0));    real_T mm=m[0];    dx[0]=x[3]/mm;    dx[1]=x[4]/mm;    dx[2]=x[5]/mm;    dx[3]=U(0);    dx[4]=U(1);    dx[5]=U(2);}

  /* Function: mdlDerivatives =================================================   * Abstract:   *    In this function, you compute the S-function block's derivatives.   *    The derivatives are placed in the derivative vector, ssGetdX(S).   */  static void mdlDerivatives(SimStruct *S)  {    real_T t, *xC, *xD, *uWQ, *uES, *sys;	int i;        t  = ssGetT(S);    xC = ssGetContStates(S);    xD = ssGetDiscStates(S);    uWQ  = ssGetInputPortRealSignal(S,0);    sys  = ssGetdX(S);    for (i=0; i<(int)B(S,"CStates"); i++)    {        sys[i]=0.0;    }      	// verwijzing naar exra inputs  	if (B(S,"Setpoints")>0)  	{  	  uES = &(uWQ[(int) (U(S,"Number")*B(S,"WaterIn"))]);   	}  	else  	{  	  uES=NULL;  	}  	    	 if (ozoncc_Derivatives(sys,t,xC,xD,uWQ,uES,                          ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3)))   {//     ssSetErrorStatus(S,StimelaErrorMessage);   }     }

static void mdlStart(SimStruct *S) {#ifndef MATLAB_MEX_FILE	unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);	int_T rate = (int_T)PARAM(0)[0];	char_T *str = (char_T *)malloc(strlen);	mxGetString(ssGetSFcnParam(S,1), str, strlen);	if (rate > 0) {		rosConfig.rate = rate;	}	memcpy(rosConfig.ns, str, MAX_NAMES_SIZE);	rosConfig.pubStackSize = PARAM(2)[0];	rosConfig.subStackSize = PARAM(3)[0];	rosConfig.exposeParams = PARAM(4)[0] - 1;	free(str);#endif}

static void mdlStart(SimStruct *S){    SFUNPRINTF("Starting Instance of %s./n", TOSTRING(S_FUNCTION_NAME));    // init ROS if not yet done.    initROS(S);    void** vecPWork = ssGetPWork(S);    int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2));    *ssGetIWork(S) = nRobots;    ssGetIWork(S)[1] = *((mxChar*)mxGetData(ssGetSFcnParam(S, 4)));    ros::NodeHandle nodeHandle(ros::this_node::getName());    // get Topic Strings    size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;    size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1;    char* prefix_topic = (char*)mxMalloc(prefix_buflen);    char* postfix_topic = (char*)mxMalloc(postfix_buflen);    mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen);    mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen);    //SFUNPRINTF("The string being passed as a Paramater is - %s/n ", topic);    std::stringstream sstream;    mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2));    for (unsigned int i = 0; i < nRobots; ++i) {        sstream.str(std::string());        // build topicstring        sstream << prefix_topic;        sstream << (uint)robotIDs[i];        sstream << postfix_topic;        GenericSubscriber<sensor_msgs::JointState>* sub                = new GenericSubscriber<sensor_msgs::JointState>(nodeHandle, sstream.str(), 10);        vecPWork[i] = sub;    }    // free char array    mxFree(prefix_topic);    mxFree(postfix_topic);    // Create nRobot Spinners    ros::AsyncSpinner* spinner = new ros::AsyncSpinner(nRobots); // nRobots Spinner    spinner->start();    vecPWork[nRobots] = spinner;  }

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){    const real_T *p = (const real_T*) ssGetInputPortSignal(S,0);    const real_T *T = (const real_T*) ssGetInputPortSignal(S,1);    const real_T *F = (const real_T*) ssGetInputPortSignal(S,2);    const real_T  *fs  = (const real_T *)mxGetData(ssGetSFcnParam(S, 0));    real_T       *R = ssGetOutputPortSignal(S,0);    real_T       *h = ssGetOutputPortSignal(S,1);    real_T       *u = ssGetOutputPortSignal(S,2);    real_T       *s = ssGetOutputPortSignal(S,3);    real_T       *Cp = ssGetOutputPortSignal(S,4);    real_T       unused1,unused2,unused3,unused4,unused5,unused6,unused7,            unused8,unused9,unused10,unused11;    GetThdynCombGasZachV1(p[0], T[0], F[0], fs[0], R, h, s, u, &unused1,             &unused2, &unused3, &unused4, &unused5, &unused6, &unused7,            &unused8, &unused9, Cp, &unused10, &unused11);}

static void mdlStart(SimStruct *S){    char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0));    std::string className(classNameStr);    mxFree(classNameStr);    wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className);    ssSetPWorkValue(S, 0, block);    wbt::Error error;    if (!block || !block->initialize(S, &error)) {        yError() << "[mdlStart]" << error.message;        static char errorBuffer[1024];        sprintf(errorBuffer, "[mdlStart]%s", error.message.substr(0, 1023 - strlen("[mdlStart]")).c_str());        ssSetErrorStatus(S, errorBuffer);    }}