ttkernel.cpp

一个很棒的网络控制系统仿真软件

#ifdef KERNEL_MATLAB  
  arg0 = (mxArray *)ssGetSFcnParam(S, 0);
  mxGetString(arg0, initfun, 100);
#endif
 
  rtsys = new RTsys;
  rtsys->initialize = true;

  /* Assign function pointers */
  rtsys->contextSwitchCode = contextSwitchCode;
  rtsys->periodicTaskHandlerCode = periodicTaskHandlerCode;

  rtsys->timeQCmpFcn = timeQCmpFcn;
  rtsys->readyQCmpFcn = readyQCmpFcn;
  rtsys->handlerQCmpFcn = handlerQCmpFcn;
  rtsys->timerQCmpFcn = timerQCmpFcn;
  rtsys->jobQCmpFcn = jobQCmpFcn;

  rtsys->default_release = default_release;
  rtsys->default_start = default_start;
  rtsys->default_suspend = default_suspend;
  rtsys->default_resume = default_resume;
  rtsys->default_finish = default_finish;

  rtsys->prioFP = prioFP;
  rtsys->prioRM = prioRM;
  rtsys->prioEDF = prioEDF;
  rtsys->prioDM = prioDM;

#ifdef KERNEL_MATLAB
  mexSetTrapFlag(1); // return control to the MEX file after an error
  /* Write rtsys pointer to UserData */
  mexCallMATLAB(1, lhs, 0, NULL, "gcbh");
  valuePtr = mxGetPr(lhs[0]);
  sprintf(rtsysp, "%p", rtsys);
  rhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
  *mxGetPr(rhs[0]) = *valuePtr;
  rhs[1] = mxCreateString("UserData");
  rhs[2] = mxCreateString(rtsysp);
  mexCallMATLAB(0, NULL, 3, rhs, "set_param");

  /* Evaluating user-defined init function (MATLAB) */
  if (mexCallMATLAB(0, NULL, 0, NULL, initfun) != 0) {
    mexErrMsgTxt("Call to init function failed!");
    ssSetStopRequested(S, 1);
  } else {
    /* Everything went fine */
    rtsys->initialize = false;
  }
  
#else
  /* Evaluating user-defined init function (C++) */
  init();
  rtsys->initialize = false;
#endif
  
  if (!ssSetNumInputPorts(S, 3)) return;
  ssSetInputPortDirectFeedThrough(S, 0, 0);
  ssSetInputPortDirectFeedThrough(S, 1, 0);
  ssSetInputPortDirectFeedThrough(S, 2, 0);
  if (!ssSetNumOutputPorts(S, 4)) return;

  /* Input Ports */

  if (rtsys->nbrOfInputs > 0) 
    ssSetInputPortWidth(S, 0, rtsys->nbrOfInputs);
  else
    ssSetInputPortWidth(S, 0, 1);
  
  if (rtsys->nbrOfTriggers > 0) 
    ssSetInputPortWidth(S, 1, rtsys->nbrOfTriggers);
  else
    ssSetInputPortWidth(S, 1, 1);

  if (rtsys->nbrOfNetworks > 0) 
    ssSetInputPortWidth(S, 2, rtsys->nbrOfNetworks); /* Network receive */
  else
    ssSetInputPortWidth(S, 2, 1);

  /* Output Ports */

  if (rtsys->nbrOfOutputs > 0) 
    ssSetOutputPortWidth(S, 0, rtsys->nbrOfOutputs);
  else
    ssSetOutputPortWidth(S, 0, 1);

  if (rtsys->nbrOfNetworks > 0) 
    ssSetOutputPortWidth(S, 1, (rtsys->nbrOfNetworks)); /* Network send */
  else
    ssSetOutputPortWidth(S, 1, 1);

  if (rtsys->nbrOfSchedTasks+rtsys->nbrOfSchedHandlers > 0) 
    ssSetOutputPortWidth(S, 2, rtsys->nbrOfSchedTasks+rtsys->nbrOfSchedHandlers);
  else
    ssSetOutputPortWidth(S, 2, 1);

  if (rtsys->nbrOfSchedMonitors > 0) 
    ssSetOutputPortWidth(S, 3, rtsys->nbrOfSchedMonitors*rtsys->nbrOfTasks);
  else
    ssSetOutputPortWidth(S, 3, 1);

  ssSetNumContStates(S, 0);
  ssSetNumDiscStates(S, 0);
  
  ssSetNumSampleTimes(S, 1);
    
  ssSetNumRWork(S, 0);
  ssSetNumIWork(S, 0);
  ssSetNumPWork(S, 0); 
  ssSetNumModes(S, 0);
  ssSetNumNonsampledZCs(S, 1);

  ssSetUserData(S, rtsys);
  
  ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_CALL_TERMINATE_ON_EXIT); 
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
  ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
  ssSetOffsetTime(S, 0, FIXED_IN_MINOR_STEP_OFFSET);
}


#define MDL_START
static void mdlStart(SimStruct *S)
{
  rtsys = (RTsys*) ssGetUserData(S);

  if (rtsys->initialize) {
    /* Failure during initialization */
    return;
  } 

  /* DATA ALLOCATION */

  if (rtsys->nbrOfInputs > 0) {
    rtsys->inputs = new double[rtsys->nbrOfInputs];
  }

  if (rtsys->nbrOfOutputs > 0) {
    rtsys->outputs = new double[rtsys->nbrOfOutputs];
  }
   
  if (rtsys->nbrOfTriggers > 0) {
    rtsys->interruptinputs = new double[rtsys->nbrOfTriggers];
    rtsys->oldinterruptinputs = new double[rtsys->nbrOfTriggers];
  }
  
  if (rtsys->nbrOfTasks > 0) {
    rtsys->taskSched = new double[rtsys->nbrOfTasks];
  }
  
  if (rtsys->nbrOfHandlers > 0) {
    rtsys->handlerSched = new double[rtsys->nbrOfHandlers];
  }

  if (rtsys->nbrOfMonitors > 0) {
    rtsys->monitorGraph = new double[rtsys->nbrOfTasks];
  }
  if (rtsys->nbrOfNetworks > 0) {
    rtsys->nwSnd = new double[rtsys->nbrOfNetworks];
    rtsys->networkinputs = new double[rtsys->nbrOfNetworks];
    rtsys->oldnetworkinputs = new double[rtsys->nbrOfNetworks];
  }
}


#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
  int i;
  rtsys = (RTsys*) ssGetUserData(S);

  if (rtsys->initialize) {
    /* Failure during initialization */
    return;
  }

  for (i=0; i<rtsys->nbrOfOutputs; i++)
    rtsys->outputs[i] = 0.0;
  
  for (i=0; i<rtsys->nbrOfInputs; i++) 
    rtsys->inputs[i] = *ssGetInputPortRealSignalPtrs(S,0)[i];
  
  for (i=0; i<rtsys->nbrOfTriggers; i++) {
    rtsys->interruptinputs[i] = 0.0;
    rtsys->oldinterruptinputs[i] = 0.0;
  }
  
  /* NETWORK */
  for (i=0; i<rtsys->nbrOfNetworks; i++) {
    rtsys->nwSnd[i] = 0.0;
    rtsys->networkinputs[i] = 0.0;
    rtsys->oldnetworkinputs[i] = 0.0;
  }
  if (rtsys->nbrOfNetworks > 0) {
    ttInitNetwork2(); /* do the rest of the network initialization */
  }
}


static void mdlOutputs(SimStruct *S, int_T tid)
{
  rtsys = (RTsys*) ssGetUserData(S);

  if (rtsys->initialize) {
    /* 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);
  int i, j, k, detected;
  double dTime;  
  TaskNode *tn;
  HandlerNode *hn;
  MonitorNode *mn;
  Monitor *mon;
  TriggerNode *trn;
  Trigger *trig;
  NetworkNode *nn;
  Network *network;
  
  /* Storing the time */

  rtsys->time = ssGetT(S);
  
  detected = 0;

  /* Check interrupts */
  
  i = 0;
  trn = (TriggerNode*) rtsys->triggerList->getFirst();
  while (trn != NULL) {
    if (fabs(rtsys->interruptinputs[i]-rtsys->oldinterruptinputs[i]) > 0.1) {
      trig = trn->getTrigger();
      if (rtsys->time - trig->prevHit > trig->latency) { 
	runHandler(trig->handler, NULL);    
	trig->prevHit = rtsys->time;
	detected = 1;
      } else { 
	printf("Call to interrupt handler %s ignored at time %f. Within interrupt latency!\n", trig->handler->name, rtsys->time);
      }
      rtsys->oldinterruptinputs[i] = rtsys->interruptinputs[i];
    }
    i++;
    trn = (TriggerNode*) trn->getNext();
  }

  /* Check network */

  if (rtsys->nbrOfNetworks > 0) {
    i = 0;
    nn = (NetworkNode*) rtsys->networkList->getFirst();
    while (nn != NULL) {
      if (fabs(rtsys->networkinputs[i] - rtsys->oldnetworkinputs[i]) > 0.1) {
	network = nn->getNetwork();
	runHandler(network->nwtrigger, NULL);
	detected = 1;
	rtsys->oldnetworkinputs[i] = rtsys->networkinputs[i];
      }
      i++;
      nn = (NetworkNode*) nn->getNext();
    }
  }
  
  /* Run kernel? */

  if ((rtsys->time >= rtsys->nextHit) || (detected > 0)) {
    dTime = runKernel();
    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 (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;
    }
  }

  
  /* 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;
  tn = (TaskNode*) rtsys->taskList->getFirst();
  while (tn != NULL) {
    rtsys->taskSched[i] = (double) (j+1);
    if (tn->getTask()->display) j++;
    tn = (TaskNode*) tn->getNext();
    i++;
  }
  
  tn = (TaskNode*) rtsys->readyQ->getFirst();
  while (tn != NULL) {
    rtsys->taskSched[tn->getTask()->taskID - 1] += 0.25;
    tn = (TaskNode*) tn->getNext();
  }
  
  if (rtsys->running != NULL)
    rtsys->taskSched[rtsys->running->taskID - 1] += 0.25;
    
  i = 0;
  j = 0;
  tn = (TaskNode*) rtsys->taskList->getFirst();
  while (tn != NULL) {
    if (tn->getTask()->display) {
      s[j] = rtsys->taskSched[i];
      j++;
    }
    tn = (TaskNode*) tn->getNext();
    i++;
  }
  

  /* Handler schedule */

  i = 0;
  j = 0;
  hn = (HandlerNode*) rtsys->handlerList->getFirst();
  while (hn != NULL) {
    rtsys->handlerSched[i] = (double) (j+rtsys->nbrOfSchedTasks+2);
    if (i==0 && rtsys->simContextSwitch) {
      // Context switch schedule, move graph down to task level
      rtsys->handlerSched[i] = rtsys->handlerSched[i] - 1;
    }
    if (hn->getHandler()->display) j++;
    hn = (HandlerNode*) hn->getNext();
    i++;
  }

  hn = (HandlerNode*) rtsys->handlerQ->getFirst();
  while (hn != NULL) {
    rtsys->handlerSched[hn->getHandler()->handlerID - 1] += 0.25;
    hn = (HandlerNode*) hn->getNext();
  }

  if (rtsys->activeHandler != NULL)
    rtsys->handlerSched[rtsys->activeHandler->handlerID - 1] += 0.25;

  i = 0;
  j = 0;
  hn = (HandlerNode*) rtsys->handlerList->getFirst();
  while (hn != NULL) {
    if (hn->getHandler()->display) {
      s[j+rtsys->nbrOfSchedTasks] = rtsys->handlerSched[i];
      j++;
    }
    hn = (HandlerNode*) hn->getNext();
    i++;
  }
  
  /* Monitor graph */
  
  k = 0;
  mn = (MonitorNode*) rtsys->monitorList->getFirst();
  while (mn != NULL) {
    mon = mn->getMonitor();  

    for (j=0; j<rtsys->nbrOfTasks; j++)
      rtsys->monitorGraph[j] = (double) (j+1+k*(1+rtsys->nbrOfTasks));

    tn = (TaskNode*) mon->waitingQ->getFirst();
    while (tn != NULL) {
      i = tn->getTask()->taskID;
      rtsys->monitorGraph[i-1] += 0.25;
      tn = (TaskNode*) tn->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++;
    }
    mn = (MonitorNode*) mn->getNext();
  }
} 


#define MDL_ZERO_CROSSINGS

static void mdlZeroCrossings(SimStruct *S)
{
  int i;
  rtsys = (RTsys*) ssGetUserData(S);
  
  if (rtsys->initialize) {
    /* Failure during initialization */
    return;
  }

  /* Copy analog inputs */
  for (i=0; i<rtsys->nbrOfInputs; i++) {
    rtsys->inputs[i] = *ssGetInputPortRealSignalPtrs(S,0)[i];
  }

  /* Copy interrupt inputs, check for events */
  for (i=0; i<rtsys->nbrOfTriggers; i++) {
    if (fabs(*ssGetInputPortRealSignalPtrs(S,1)[i]-rtsys->interruptinputs[i]) > 0.1) {
      rtsys->nextHit = ssGetT(S);
      //printf("mdlZeroCrossings: interrupt detected at %2.20g\n", ssGetT(S));
    }
    rtsys->interruptinputs[i] = *ssGetInputPortRealSignalPtrs(S,1)[i];
  }

  /* Copy network input, check for event */
  for (i=0; i<rtsys->nbrOfNetworks; i++) {
    if (fabs(*ssGetInputPortRealSignalPtrs(S,2)[i]-rtsys->networkinputs[i]) > 0.1) {
      rtsys->nextHit = ssGetT(S);
    }
    rtsys->networkinputs[i] = *ssGetInputPortRealSignalPtrs(S,2)[i];
  }

  ssGetNonsampledZCs(S)[0] = rtsys->nextHit - ssGetT(S);
}


static void mdlTerminate(SimStruct *S)
{   
  rtsys = (RTsys*) ssGetUserData(S);

  if (rtsys == NULL) {
    return;
  }
   
#ifdef KERNEL_MATLAB  
  if (!destroyed) {
    mxDestroyArray(segArray);
    destroyed = true;
  }
#else
  cleanup();
#endif

  delete rtsys;
}


#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif

#ifdef __cplusplus
} // end of extern "C" scope
#endif