callblocksystem.cpp

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

#ifndef CALL_BLOCK_SYSTEM
#define CALL_BLOCK_SYSTEM

bool ttCallBlockSystem(int nOutp, double *outp, int nInp, double *inp, char *blockName) {
  
  mxArray *simsetRHS[8];
  mxArray *simRHS[4];
  mxArray *simLHS[3];
  mxArray *options[1];
  mxArray *states;
  double *values;
  double *state_values;
  int i, j, n;
  BlockdataNode *b;
  Blockdata *bd;
  
  // Make sure that the Simulink block exists in Matlab path
  mxArray *lhs[1];
  mxArray *rhs[1];
  rhs[0] = mxCreateString(blockName);
  mexCallMATLAB(1, lhs, 1, rhs, "exist");
  int number = (int) *mxGetPr(lhs[0]);
  if (number == 0) {
    MEX_ERROR("ttCallBlockSystem: Simulink block not in path!");
    for (j=0; j < nOutp; j++) {
      outp[j] = 0.0;
    }
    return false;
  }

  b = (BlockdataNode*) rtsys->executing->blockList->getFirst();
  while (b!=NULL) {
    if (strcmp(b->getBlockdata()->blockName, blockName) == 0)
      break;
    b = (BlockdataNode*) b->getNext();
  }
  
  if (b==NULL) {
    // Not found, create options mxArray (Matlab struct) 
    
    bd = new Blockdata;
    bd->blockName = new char[strlen(blockName)+1];
    strcpy(bd->blockName, blockName);
    
    simsetRHS[0] = mxCreateString("Solver");
    simsetRHS[1] = mxCreateString("FixedStepDiscrete");
    simsetRHS[2] = mxCreateString("FixedStep");
    simsetRHS[3] = mxCreateScalarDouble(1.0);
    simsetRHS[4] = mxCreateString("MaxDataPoints");
    simsetRHS[5] = mxCreateScalarDouble(2.0);
    simsetRHS[6] = mxCreateString("OutputVariables");
    simsetRHS[7] = mxCreateString("xy");
    
    mexCallMATLAB(1, options, 8, simsetRHS, "simset");
    
    bd->options = options[0];
    mexMakeArrayPersistent(bd->options);
    
    for (i=0; i<8; i++) {
      mxDestroyArray(simsetRHS[i]);
    }

    b = new BlockdataNode(bd);
    rtsys->executing->blockList->appendNode(b);
  }
  
  bd = b->getBlockdata();
  
  // [t,x,outp] = sim(blockName,1,options,[0 inp]) 
  
  simRHS[0] = mxCreateString(blockName);
  simRHS[1] = mxCreateScalarDouble(1.0);
  simRHS[2] = bd->options;
  simRHS[3] = mxCreateDoubleMatrix(1, nInp+1, mxREAL);
  values = mxGetPr(simRHS[3]);
  values[0] = 0.0;
  for (j=0; j < nInp; j++) {
    values[j+1] = inp[j];
  }

  i = mexCallMATLAB(3, simLHS, 4, simRHS, "sim");

  mxDestroyArray(simRHS[0]);
  mxDestroyArray(simRHS[1]);
  mxDestroyArray(simRHS[3]);

  if (i==0) { // Successful
    // Update options mxArray with last row of returned state matrix 
    n = mxGetN(simLHS[1]); // Number of cols of state matrix 
    
    states = mxCreateDoubleMatrix(1, n, mxREAL);

    values = mxGetPr(simLHS[1]);
    state_values = mxGetPr(states);
    
    // Transfer values 
    for (j=0; j < n; j++) {
      state_values[j] = values[2*j+1]; // second row elements
    }

    mxArray* oldstates = mxGetField(bd->options, 0, "InitialState");
    mxDestroyArray(oldstates);
    mxSetField(bd->options, 0, "InitialState", states);
    
    // Return the output of the simulation, first row of returned output matrix 
    values = mxGetPr(simLHS[2]);
    for (j=0; j < nOutp; j++) {
      outp[j] = values[2*j];          // first row elements
    }

    mxDestroyArray(simLHS[0]);
    mxDestroyArray(simLHS[1]);
    mxDestroyArray(simLHS[2]);

    return true;
    
  } else {
    MEX_ERROR("ttCallBlockSystem: Simulation failed!");
    for (j=0; j < nOutp; j++) {
      outp[j] = 0.0;
    }
    return false;
  }
}

#endif