static void endme(int dummy)
{
SEM *sem;
#ifndef DEBUG
ssSetStopRequested(rtS, 1);
/*printf("stop requested in pid %d\n", getpid());*/
#endif
if ((sem = sysAuxClkSem(rtai_main_buddy))) {
rt_sem_signal(sem);
}
}
/* Function: rt_MsgServerWork ==================================================
* Abstract:
* If not connected, establish communication of the message line and the
* data upload line. If connected, send/receive messages and parameters
* on the message line.
*/
PUBLIC boolean_T rt_MsgServerWork(SimStruct *S)
{
MsgHeader msgHdr;
boolean_T hdrAvail;
boolean_T error = EXT_NO_ERROR;
boolean_T disconnectOnError = FALSE;
/*
* If not connected, attempt to make connection to host.
*/
if (!connected) {
error = ExtOpenConnection(extUD,&connected);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
}
/*
* If ExtOpenConnection is not blocking and there are no pending
* requests to open a connection, we'll still be unconnected.
*/
if (!connected) goto EXIT_POINT; /* nothing do do */
/*
* Process messages.
*/
/* Wait for a message. */
error = GetMsgHdr(&msgHdr, &hdrAvail);
if (error != EXT_NO_ERROR) {
printf("\nError occured getting message header.\n");
disconnectOnError = TRUE;
goto EXIT_POINT;
}
if (!hdrAvail) goto EXIT_POINT; /* nothing to do */
/*
* This is the first message. Should contain the string:
* 'ext-mode'. Its contents are not important to us.
* It is used as a flag to start the handshaking process.
*/
if (!commInitialized) {
msgHdr.type = EXT_CONNECT;
}
/*
* At this point we know that we have a message: process it.
*/
switch(msgHdr.type) {
case EXT_GET_TIME:
{
time_T t = ssGetT(S);
/* Skip verbosity print out - we get too many of these */
/*PRINT_VERBOSE(("got EXT_GET_TIME message.\n"));*/
error = SendMsgToHost(
EXT_GET_TIME_RESPONSE,sizeof(time_T),(char_T *)&t);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_ARM_TRIGGER:
{
PRINT_VERBOSE(("got EXT_ARM_TRIGGER message.\n"));
UploadArmTrigger();
break;
}
case EXT_SELECT_SIGNALS:
{
const char *msg;
PRINT_VERBOSE(("got EXT_SELECT_SIGNALS message.\n"));
msg = GetMsg(msgHdr.size);
if (msg == NULL) {
error = EXT_ERROR;
goto EXIT_POINT;
}
error = UploadLogInfoInit(S, msg);
if (error != NO_ERR) {
printf(
"\nError in UploadLogInfoInit(). Most likely a memory\n"
"allocation error or an attempt to re-initialize the\n"
"signal selection during the data logging process\n"
"(i.e., multiple EXT_SELECT_SIGNAL messages were received\n"
"before the logging session terminated or an\n"
"EXT_CANCEL_LOGGING message was received)");
goto EXIT_POINT;
}
break;
}
case EXT_SELECT_TRIGGER:
{
const char *msg;
PRINT_VERBOSE(("got EXT_SELECT_TRIGGER message.\n"));
msg = GetMsg(msgHdr.size);
if (msg == NULL) {
error = EXT_ERROR;
goto EXIT_POINT;
}
error = UploadInitTrigger(S, msg);
if (error != EXT_NO_ERROR) {
printf("\nError in UploadInitTrigger\n");
goto EXIT_POINT;
}
break;
}
case EXT_CONNECT:
{
PRINT_VERBOSE(("got EXT_CONNECT message.\n"));
error = ProcessConnectMsg(S);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_SETPARAM:
{
PRINT_VERBOSE(("got EXT_SETPARAM message.\n"));
error = ProcessSetParamMsg(S, msgHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_GETPARAMS:
{
PRINT_VERBOSE(("got EXT_GETPARAMS message.\n"));
error = ProcessGetParamsMsg(S);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_DISCONNECT_REQUEST:
{
PRINT_VERBOSE(("got EXT_DISCONNECT_REQUEST message.\n"));
/*
* Note that from the target's point of view this is
* more a "notify" than a "request". The host needs to
* have this acknowledged before it can begin closing
* the connection.
*/
error = SendMsgToHost(EXT_DISCONNECT_REQUEST_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
DisconnectFromHost(S);
break;
}
case EXT_MODEL_START:
PRINT_VERBOSE(("got EXT_MODEL_START message.\n"));
#ifdef VXWORKS
{
extern SEM_ID startStopSem;
semGive(startStopSem);
}
#endif
startModel = TRUE;
error = SendMsgToHost(EXT_MODEL_START_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_MODEL_STOP:
PRINT_VERBOSE(("got EXT_MODEL_STOP message.\n"));
ssSetStopRequested(S, TRUE);
break;
case EXT_MODEL_PAUSE:
PRINT_VERBOSE(("got EXT_MODEL_PAUSE message.\n"));
modelStatus = TARGET_STATUS_PAUSED;
startModel = FALSE;
error = SendMsgToHost(EXT_MODEL_PAUSE_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_MODEL_STEP:
PRINT_VERBOSE(("got EXT_MODEL_STEP message.\n"));
if ((modelStatus == TARGET_STATUS_PAUSED) && !startModel) {
startModel = TRUE;
}
error = SendMsgToHost(EXT_MODEL_STEP_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_MODEL_CONTINUE:
PRINT_VERBOSE(("got EXT_MODEL_CONTINUE message.\n"));
if (modelStatus == TARGET_STATUS_PAUSED) {
modelStatus = TARGET_STATUS_RUNNING;
startModel = FALSE;
}
error = SendMsgToHost(EXT_MODEL_CONTINUE_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_CANCEL_LOGGING:
PRINT_VERBOSE(("got EXT_CANCEL_LOGGING message.\n"));
UploadCancelLogging();
break;
default:
fprintf(stderr,"received invalid message.\n");
break;
} /* end switch */
EXIT_POINT:
if (error != EXT_NO_ERROR) {
if (disconnectOnError) {
fprintf(stderr,
"Error occured in rt_MsgServerWork.\n"
"Disconnecting from host!\n");
DisconnectFromHost(S);
/*
* Patch by Gopal Santhanam 5/25/2002 (for VXWORKS)
* If there there was a problem and we have already disconnected
* from the host, there is no point in returning that error
* back to rt_MsgServer. That would cause the task servicing
* external messages to quit. Once disconnected, we could
* just as easily resume by waiting for a new connection.
*/
#ifdef VXWORKS
error = EXT_NO_ERROR;
#endif
}
}
return(error);
} /* end rt_MsgServerWork */
static void mdlOutputs(SimStruct *S, int_T tid)
{
//printf("mdlOutputs at %g\n", ssGetT(S));
rtsys = (RTsys*) ssGetUserData(S);
if (rtsys->init_phase) {
/* Failure during initialization */
return;
}
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *n = ssGetOutputPortRealSignal(S,1);
real_T *s = ssGetOutputPortRealSignal(S,2);
real_T *m = ssGetOutputPortRealSignal(S,3);
real_T *energyConsumption = ssGetOutputPortRealSignal(S,4);
int i, j, k, detected;
double dTime;
DataNode *dn;
Task* task;
UserTask* t;
InterruptHandler* hdl;
Monitor *mon;
if (!rtsys->started && ssGetT(S) == 0.0) {
rtsys->started = true;
return;
}
if (!rtsys->mdlzerocalled) {
printf("Zero crossing detection must be turned on in order to run TrueTime!\n");
ssSetErrorStatus(S, "Zero crossing detection must be turned on in order to run TrueTime!");
return;
}
/* Storing the time */
rtsys->time = ssGetT(S) * rtsys->clockDrift + rtsys->clockOffset;
detected = 0;
/* Check interrupts */
i = 0;
dn = (DataNode*) rtsys->triggerList->getFirst();
while (dn != NULL) {
if (fabs(rtsys->interruptinputs[i]-rtsys->oldinterruptinputs[i]) > 0.1) {
hdl = (InterruptHandler*) dn->data;
Trigger* trig = hdl->trigger;
if (rtsys->time - trig->prevHit > trig->latency) {
// Trigger interrupt handler
if (hdl->myList == rtsys->readyQ) {
// handler serving older interrupts
hdl->pending++;
} else {
hdl->moveToList(rtsys->readyQ);
detected = 1;
}
trig->prevHit = rtsys->time;
} else {
//printf("Call to interrupt handler %s ignored at time %f. Within interrupt latency!\n", hdl->name, rtsys->time);
}
rtsys->oldinterruptinputs[i] = rtsys->interruptinputs[i];
}
i++;
dn = (DataNode*) dn->getNext();
}
/* Check network */
dn = (DataNode*) rtsys->networkList->getFirst();
while (dn != NULL) {
hdl = (InterruptHandler*) dn->data;
Network* network = hdl->network;
i = network->networkID - 1;
//printf("mdlOutputs: checking network #%d inp: %d oldinp: %d\n",i,rtsys->networkinputs[i],rtsys->oldnetworkinputs[i]);
if (fabs(rtsys->networkinputs[i] - rtsys->oldnetworkinputs[i]) > 0.1) {
hdl->moveToList(rtsys->readyQ);
detected = 1;
rtsys->oldnetworkinputs[i] = rtsys->networkinputs[i];
}
dn = (DataNode*) dn->getNext();
}
/* Run kernel? */
double externTime = (rtsys->time- rtsys->clockOffset) / rtsys->clockDrift;
if ((externTime >= rtsys->nextHit) || (detected > 0)) {
dTime = runKernel(ssGetT(S));
if (rtsys->error) {
// Something went wrong executing a code function
mxArray *bn[1];
mexCallMATLAB(1, bn, 0, 0, "gcs"); // get current system
char buf[200];
mxGetString(bn[0], buf, 200);
for (unsigned int i=0; i<strlen(buf); i++) if (buf[i]=='\n') buf[i]=' ';
printf("In block ==> '%s'\nSimulation aborted!\n", buf);
ssSetStopRequested(S, 1);
} else {
rtsys->nextHit = (rtsys->time + dTime - rtsys->clockOffset) / rtsys->clockDrift;
}
}
/* Outputs */
for (i=0; i<rtsys->nbrOfOutputs; i++) {
y[i] = rtsys->outputs[i];
}
/* Network send */
for (i=0; i<rtsys->nbrOfNetworks; i++) {
n[i] = rtsys->nwSnd[i];
}
/* Task schedule */
i = 0;
j = 0;
dn = (DataNode*) rtsys->taskList->getFirst();
while (dn != NULL) {
t = (UserTask*) dn->data;
rtsys->taskSched[i] = (double) (j+1);
if (t->display) j++;
dn = (DataNode*) dn->getNext();
i++;
}
task = (Task*) rtsys->readyQ->getFirst();
while (task != NULL) {
if (task->isUserTask()) {
t = (UserTask*) task;
rtsys->taskSched[t->taskID - 1] += 0.25;
}
task = (Task*) task->getNext();
}
if ((rtsys->running != NULL) && (rtsys->running->isUserTask())) {
t = (UserTask*) rtsys->running;
rtsys->taskSched[t->taskID - 1] += 0.25;
}
i = 0;
j = 0;
dn = (DataNode*) rtsys->taskList->getFirst();
while (dn != NULL) {
t = (UserTask*) dn->data;
if (t->display) {
s[j] = rtsys->taskSched[i];
j++;
}
dn = (DataNode*) dn->getNext();
i++;
}
/* Handler schedule */
i = 0;
j = 0;
dn = (DataNode*) rtsys->handlerList->getFirst();
while (dn != NULL) {
rtsys->handlerSched[i] = (double) (j+rtsys->nbrOfSchedTasks+2);
if (i==0 && rtsys->contextSwitchTime > EPS) {
// Context switch schedule, move graph down to task level
rtsys->handlerSched[i] = rtsys->handlerSched[i] - 1;
}
hdl = (InterruptHandler*) dn->data;
if (hdl->display) j++;
dn = (DataNode*) dn->getNext();
i++;
}
task = (Task*) rtsys->readyQ->getFirst();
while (task != NULL) {
if (!(task->isUserTask())) {
hdl = (InterruptHandler*) task;
rtsys->handlerSched[hdl->handlerID - 1] += 0.25;
}
task = (Task*) task->getNext();
}
if ((rtsys->running != NULL) && (!(rtsys->running->isUserTask()))) {
hdl = (InterruptHandler*) rtsys->running;
rtsys->handlerSched[hdl->handlerID - 1] += 0.25;
}
i = 0;
j = 0;
dn = (DataNode*) rtsys->handlerList->getFirst();
while (dn != NULL) {
hdl = (InterruptHandler*) dn->data;
if (hdl->display) {
s[j+rtsys->nbrOfSchedTasks] = rtsys->handlerSched[i];
j++;
}
dn = (DataNode*) dn->getNext();
i++;
}
/* Monitor graph */
k = 0;
dn = (DataNode*) rtsys->monitorList->getFirst();
while (dn != NULL) {
mon = (Monitor*) dn->data;
for (j=0; j<rtsys->nbrOfTasks; j++)
rtsys->monitorGraph[j] = (double) (j+1+k*(1+rtsys->nbrOfTasks));
t = (UserTask*) mon->waitingQ->getFirst();
while (t != NULL) {
i = t->taskID;
rtsys->monitorGraph[i-1] += 0.25;
t = (UserTask*) t->getNext();
}
if (mon->heldBy != NULL) {
i = mon->heldBy->taskID;
rtsys->monitorGraph[i-1] += 0.5;
}
if (mon->display) {
for (j=0; j<rtsys->nbrOfTasks; j++)
m[j+k*rtsys->nbrOfTasks] = rtsys->monitorGraph[j];
k++;
}
dn = (DataNode*) dn->getNext();
}
/* Energy consumption */
energyConsumption[0] = rtsys->energyConsumption;
}
