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