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