static void mdlStart(SimStruct *S) { // Notify SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); // init ROS if not yet done. initROS(S); ros::NodeHandle nodeHandle(ros::this_node::getName()); void** vecPWork = ssGetPWork(S); ros::Time::init(); ros::Time* firstExecTime = new ros::Time(ros::Time::now()); vecPWork[0] = firstExecTime; ros::Time* lastExecTime = new ros::Time(*firstExecTime); vecPWork[1] = lastExecTime; real_T* vecRWork = ssGetRWork(S); vecRWork[0] = mxGetScalar(ssGetSFcnParam(S, 0)); // Tsim vecRWork[1] = 0.0; // simulationTime int_T* vecIWork = ssGetIWork(S); vecIWork[0] = 1; // initialize step counter }
static void mdlStart(SimStruct *S) { char *bufSimEng; // SIMULATION_ENGINE int bufSimEngLen; // Build the list of parameters that configure the tres::Network std::vector<std::string> ns_params = readMaskAndBuildConfVector(S); // Get the type of the adapter, i.e., the concrete implementation of tres::Network bufSimEngLen = mxGetN( ssGetSFcnParam(S,SIMULATION_ENGINE) )+1; bufSimEng = new char[bufSimEngLen]; mxGetString(ssGetSFcnParam(S,SIMULATION_ENGINE), bufSimEng, bufSimEngLen); std::string engine(bufSimEng); delete bufSimEng; // Instantiate the concrete representation of tres::Network std::unique_ptr<tres::Network> ns = Factory<tres::Network>::instance() .create(engine, ns_params); // Save the C++ object to the pointers vector tres::Network *_ns = ns.release(); ssGetPWork(S)[0] = _ns; // Set the `New pending activations available' flag to false boolean_T *pendingActivsAvail = (boolean_T*) ssGetDWork(S,0); pendingActivsAvail[0] = false; // Get the time resolution (actually, its floating point representation) std::vector<std::string>::iterator it = ns_params.end()-1; double time_resolution = atof((*it).c_str()); // Save the time resolution to the real vector workspace ssGetRWork(S)[0] = time_resolution; }
static void mdlZeroCrossings(SimStruct *S) { // Get the C++ object back from the pointers vector tres::Network *ns = static_cast<tres::Network *>(ssGetPWork(S)[0]); // Get the time resolution back from the real vector workspace double time_resolution = ssGetRWork(S)[0]; ssGetNonsampledZCs(S)[0] = ns->getTimeOfNextEvent()/(time_resolution) - ssGetT(S); }
static void mdlOutputs(SimStruct *S, int_T tid) { double *y = ssGetOutputPortRealSignal(S,0); #ifndef MATLAB_MEX_FILE unsigned int channel = (unsigned int)COMEDI_CHANNEL; unsigned int range = (unsigned int)COMEDI_RANGE; unsigned int aref = (unsigned int)COMEDI_REFERENCE - 1; void *dev = (void *)ssGetPWork(S)[0]; int subdev = ssGetIWork(S)[0]; double range_min = ssGetRWork(S)[0]; double range_max = ssGetRWork(S)[1]; lsampl_t data, maxdata = comedi_get_maxdata(dev, subdev, COMEDI_CHANNEL); double x; comedi_data_read(dev, subdev, channel, range, aref, &data); x = data; x /= maxdata; x *= (range_max - range_min); x += range_min; *y = x; #endif }
/* Function: mdlTerminate ===================================================== * Abstract: * In this function, you should perform any actions that are necessary * at the termination of a simulation. For example, if memory was * allocated in mdlStart, this is the place to free it. */ static void mdlTerminate(SimStruct *S) { real_T* vecRWork = ssGetRWork(S); void** vecPWork = ssGetPWork(S); ros::Time* firstExecTime = (ros::Time*)vecPWork[0]; ros::Time* lastExecTime = (ros::Time*)vecPWork[1]; SFUNPRINTF("Simulation Time: %f\n", vecRWork[1]); SFUNPRINTF("Real Time: %f\n", (*lastExecTime-*firstExecTime).toSec()); delete lastExecTime; SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); }
/* 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 mdlOutputs(SimStruct *S, int_T tid) { real_T mdl_time = ssGetT(S); // Get the C++ object back from the pointers vector tres::Network *ns = static_cast<tres::Network *>(ssGetPWork(S)[0]); // Get the time resolution back from the real vector workspace double time_resolution = ssGetRWork(S)[0]; // Save the time of next block hit long int next_hit_tick = ns->getNextWakeUpTime(); // If the current time is greater or equal than the next block hit if (ssGetT(S) - next_hit_tick/(time_resolution) >= 0.0) { mexPrintf("\n%s\n\t%s\n\t\tat *time* %.6f\n", ssGetPath(S), __FUNCTION__, mdl_time); do { // Get the first incoming event from the network simulator tres::NetworkEvent *e = ns->getNextEvent(); // Classify the event if (e->isGeneratedByAppLevelTraffic()) { // If the event is due to network traffic from/to the application layer, // get the instance of network message that has generated the event tres::SimMessage *m = e->getGeneratorMessage(); // debug mexPrintf("\t\t\tmessage '%s' transferred (3rdparty sim. time is %.6f)\n", m->getUID().c_str(), e->getTime()/(time_resolution)); // Add this message to the to-be-triggered message queue ns->addMessageToTriggerQueue(m); } // Process the next event in the underlying network simulator's event queue ns->processNextEvent(); } while ( ns->getTimeOfNextEvent() == next_hit_tick ); // Read which message have to be triggered std::vector<int> ports = ns->getPortsToTrigger(); // For each Message to trigger, set a value of 1.0 // onto the corresponding output port real_T *y = ssGetOutputPortRealSignal(S,0); for (std::vector<int>::iterator port = ports.begin(); port != ports.end(); ++port) { y[*port] = 1.0; } // Eventually set the `New pending activations available' flag to true if (!ports.empty()) { boolean_T *pendingActivsAvail = (boolean_T*) ssGetDWork(S,0); pendingActivsAvail[0] = true; } } }
static void mdlStart(SimStruct *S) { #ifndef MATLAB_MEX_FILE void *dev; int subdev; int index = (int)COMEDI_DEVICE - 1; unsigned int channel = (unsigned int)COMEDI_CHANNEL; unsigned int range = (unsigned int)COMEDI_RANGE; int n_channels; char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"}; char board[50]; static char_T errMsg[256]; comedi_krange krange; double range_min, range_max; if (!ComediDev[index]) { dev = comedi_open(devname[index]); if (!dev) { sprintf(errMsg, "Comedi open failed\n"); ssSetErrorStatus(S, errMsg); printf("%s", errMsg); return; } rt_comedi_get_board_name(dev, board); printf("COMEDI %s (%s) opened.\n\n", devname[index], board); ComediDev[index] = dev; } else { dev = ComediDev[index]; } if ((subdev = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0)) < 0) { sprintf(errMsg, "Comedi find_subdevice failed (No analog input)\n"); ssSetErrorStatus(S, errMsg); printf("%s", errMsg); comedi_close(dev); return; } if (!ComediDev_AIInUse[index] && comedi_lock(dev, subdev) < 0) { sprintf(errMsg, "Comedi lock failed for subdevice %d\n", subdev); ssSetErrorStatus(S, errMsg); printf("%s", errMsg); comedi_close(dev); return; } if ((n_channels = comedi_get_n_channels(dev, subdev)) < 0) { sprintf(errMsg, "Comedi get_n_channels failed for subdevice %d\n", subdev); ssSetErrorStatus(S, errMsg); printf("%s", errMsg); comedi_unlock(dev, subdev); comedi_close(dev); return; } if ((comedi_get_krange(dev, subdev, channel, range, &krange)) < 0) { sprintf(errMsg, "Comedi get range failed for subdevice %d\n", subdev); ssSetErrorStatus(S, errMsg); printf("%s", errMsg); comedi_unlock(dev, subdev); comedi_close(dev); return; } ComediDev_InUse[index]++; ComediDev_AIInUse[index]++; range_min = (double)(krange.min)*1.e-6; range_max = (double)(krange.max)*1.e-6; printf("AI Channel %d - Range : %1.2f [V] - %1.2f [V]\n", channel, range_min, range_max); ssGetPWork(S)[0] = (void *)dev; ssGetIWork(S)[0] = subdev; ssGetRWork(S)[0] = range_min; ssGetRWork(S)[1] = range_max; #endif }