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
}
