static void mdlStart(SimStruct *S) { SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); // init ROS if not yet done. initROS(S); void** vecPWork = ssGetPWork(S); int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2)); *ssGetIWork(S) = nRobots; ssGetIWork(S)[1] = *((mxChar*)mxGetData(ssGetSFcnParam(S, 4))); ros::NodeHandle nodeHandle(ros::this_node::getName()); // get Topic Strings size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1; char* prefix_topic = (char*)mxMalloc(prefix_buflen); char* postfix_topic = (char*)mxMalloc(postfix_buflen); mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen); mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen); //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); std::stringstream sstream; mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2)); for (unsigned int i = 0; i < nRobots; ++i) { sstream.str(std::string()); // build topicstring sstream << prefix_topic; sstream << (uint)robotIDs[i]; sstream << postfix_topic; GenericSubscriber<sensor_msgs::JointState>* sub = new GenericSubscriber<sensor_msgs::JointState>(nodeHandle, sstream.str(), 10); vecPWork[i] = sub; } // free char array mxFree(prefix_topic); mxFree(postfix_topic); // Create nRobot Spinners ros::AsyncSpinner* spinner = new ros::AsyncSpinner(nRobots); // nRobots Spinner spinner->start(); vecPWork[nRobots] = spinner; }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. */ static void mdlOutputs(SimStruct *S, int_T tid) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); // get Pointers // accessing inputs const real_T *position = (const real_T*) ssGetInputPortSignal(S,0); // const real_T *orientation = (const real_T*) ssGetInputPortSignal(S,1); geometry_msgs::PointStamped msg; for (unsigned int i = 0; i < nRobots; ++i) { GenericPublisher<geometry_msgs::PointStamped>* pub = (GenericPublisher<geometry_msgs::PointStamped>*)vecPWork[i]; // define send Time. msg.header.stamp = ros::Time::now(); msg.point.x = position[i*3 + 0]; msg.point.y = position[i*3 + 1]; msg.point.z = position[i*3 + 2]; pub->publish(msg); } }
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 }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. */ static void mdlOutputs(SimStruct *S, int_T tid) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); // Preparing Outputs real_T *rpy = (real_T*)ssGetOutputPortSignal(S,0); real_T *thrust = (real_T*)ssGetOutputPortSignal(S,1); real_T *estim_mass = (real_T*)ssGetOutputPortSignal(S,2); real_T *time = (real_T*)ssGetOutputPortSignal(S,3); telekyb_msgs::TKCommandsConstPtr lastMsg; for (unsigned int i = 0; i < nRobots; ++i) { GenericSubscriber<telekyb_msgs::TKCommands>* sub = (GenericSubscriber<telekyb_msgs::TKCommands>*)vecPWork[i]; lastMsg = sub->getLastMsg(); if (lastMsg){ // copy only if a message was actually received rpy[i*3 + 0] = lastMsg->roll; rpy[i*3 + 1] = lastMsg->pitch; rpy[i*3 + 2] = lastMsg->yaw; thrust[i + 0] = lastMsg->thrust; estim_mass[i + 0] = lastMsg->mass; time[i + 0] = lastMsg->header.stamp.toSec(); } } }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. */ static void mdlOutputs(SimStruct *S, int_T tid) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); // Preparing Outputs real_T *position = (real_T*)ssGetOutputPortSignal(S,0); real_T *quaternion = (real_T*)ssGetOutputPortSignal(S,1); real_T *time = (real_T*)ssGetOutputPortSignal(S,2); for (unsigned int i = 0; i < nRobots; ++i) { GenericSubscriber<geometry_msgs::PoseStamped>* sub = (GenericSubscriber<geometry_msgs::PoseStamped>*)vecPWork[i]; geometry_msgs::PoseStampedConstPtr lastMsg = sub->getLastMsg(); if (lastMsg){ // copy only if a message was actually received position[i*3 + 0] = lastMsg->pose.position.x; position[i*3 + 1] = lastMsg->pose.position.y; position[i*3 + 2] = lastMsg->pose.position.z; // w, x, y, z quaternion[i*4 + 0] = lastMsg->pose.orientation.w; quaternion[i*4 + 1] = lastMsg->pose.orientation.x; quaternion[i*4 + 2] = lastMsg->pose.orientation.y; quaternion[i*4 + 3] = lastMsg->pose.orientation.z; time[i + 0] = lastMsg->header.stamp.toSec(); } } }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. */ static void mdlOutputs(SimStruct *S, int_T tid) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); // get Pointers // accessing inputs const real_T *linVel = (const real_T*) ssGetInputPortSignal(S,0); const real_T *angVel = (const real_T*) ssGetInputPortSignal(S,1); geometry_msgs::WrenchStamped msg; for (unsigned int i = 0; i < nRobots; ++i) { GenericPublisher<geometry_msgs::WrenchStamped>* pub = (GenericPublisher<geometry_msgs::WrenchStamped>*)vecPWork[i]; // define send Time. msg.header.stamp = ros::Time::now(); msg.wrench.force.x = linVel[i*3 + 0]; msg.wrench.force.y = linVel[i*3 + 1]; msg.wrench.force.z = linVel[i*3 + 2]; // w, x, y, z msg.wrench.torque.x = angVel[i*3 + 0]; msg.wrench.torque.y = angVel[i*3 + 1]; msg.wrench.torque.z = angVel[i*3 + 2]; pub->publish(msg); } }
static void mdlOutputs(SimStruct *S, int_T tid) { /* Input and output vectors */ const real_T *u = (const real_T*) ssGetInputPortSignal(S,0); real_T *y = (real_T *)ssGetOutputPortRealSignal(S,0); /* Block name buffer length and string read status */ int_T buflen; int_T status; /* Block mask parameter struct */ ValveStruct valveStruct; valveStruct.VlvfullyOpen = *mxGetPr(VlvfullyOpen_p(S)); valveStruct.VlvdeadZone = *mxGetPr(VlvdeadZone_p(S)); valveStruct.Valve_Ae = *mxGetPr(Valve_Ae_p(S)); /* Vector & array data */ valveStruct.X_V_PRVec = mxGetPr(X_V_PRVec_p(S)); valveStruct.T_V_WcVec = mxGetPr(T_V_WcVec_p(S)); /* Dimensions of parameter arrays */ valveStruct.A = mxGetNumberOfElements(X_V_PRVec_p(S)); valveStruct.IWork = ssGetIWork(S); /* Get name of block from dialog parameter (string) */ buflen = mxGetN(BN_p(S))*sizeof(mxChar)+1; valveStruct.BlkNm = malloc(buflen); status = mxGetString(BN_p(S), valveStruct.BlkNm, buflen); /* Perform core block calculations */ Valve_TMATS_body(y, u, &valveStruct); free(valveStruct.BlkNm); }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. */ static void mdlOutputs(SimStruct *S, int_T tid) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); // Preparing Outputs real_T *force = (real_T*)ssGetOutputPortSignal(S,0); real_T *time = (real_T*)ssGetOutputPortSignal(S,1); telekyb_msgs::TKMotorCommandsConstPtr lastMsg; for (unsigned int i = 0; i < nRobots; ++i) { GenericSubscriber<telekyb_msgs::TKMotorCommands>* sub = (GenericSubscriber<telekyb_msgs::TKMotorCommands>*)vecPWork[i]; lastMsg = sub->getLastMsg(); if (lastMsg){ // copy only if a message was actually received if (lastMsg->force.size() == 4){ for (unsigned int j=0; j<4; j++){ force[i*4 + j] = lastMsg->force[j]; } } else { SFUNPRINTF("Wrong message size (%d).\n", (int)lastMsg->force.size()); } time[i + 0] = lastMsg->header.stamp.toSec(); } } }
/* Function: mdlOutputs ======================================================= * Abstract: * In this function, you compute the outputs of your S-function * block. */ static void mdlOutputs(SimStruct *S, int_T tid) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); const size_t nDofs = ssGetIWork(S)[1]; // Preparing Outputs real_T *jointPos = (real_T*)ssGetOutputPortSignal(S,0); real_T *jointVel = (real_T*)ssGetOutputPortSignal(S,1); real_T *jointEff = (real_T*)ssGetOutputPortSignal(S,2); real_T *time = (real_T*)ssGetOutputPortSignal(S,3); for (unsigned int i = 0; i < nRobots; ++i) { GenericSubscriber<sensor_msgs::JointState>* sub = (GenericSubscriber<sensor_msgs::JointState>*)vecPWork[i]; sensor_msgs::JointStateConstPtr lastMsg = sub->getLastMsg(); if (lastMsg){ // copy only if a message was actually received memcpy(jointPos+i*nDofs, lastMsg->position.data(),std::min(nDofs,lastMsg->position.size())*sizeof(double)); memcpy(jointVel+i*nDofs, lastMsg->velocity.data(),std::min(nDofs,lastMsg->velocity.size())*sizeof(double)); memcpy(jointEff+i*nDofs, lastMsg->effort.data(),std::min(nDofs,lastMsg->effort.size())*sizeof(double)); for (size_t k = std::min(nDofs,lastMsg->position.size()); k<nDofs; ++k){ jointPos[i*nDofs+k] = NAN; } for (size_t k = std::min(nDofs,lastMsg->velocity.size()); k<nDofs; ++k){ jointVel[i*nDofs+k] = NAN; } for (size_t k = std::min(nDofs,lastMsg->effort.size()); k<nDofs; ++k){ jointEff[i*nDofs+k] = NAN; } time[i + 0] = lastMsg->header.stamp.toSec(); } } }
/* 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) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); for (unsigned int i = 0; i < nRobots; ++i) { GenericPublisher<geometry_msgs::PointStamped>* pub = (GenericPublisher<geometry_msgs::PointStamped>*)vecPWork[i]; // cleanup delete pub; } SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); }
/* 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) { // get Objects ros::NodeHandle nodeHandle(ros::this_node::getName()); void** vecPWork = ssGetPWork(S); ros::ServiceClient* start_client = (ros::ServiceClient*)vecPWork[0]; delete start_client; ros::ServiceClient* stop_client = (ros::ServiceClient*)vecPWork[1]; vrep_common::simRosStopSimulation stopSrv; stop_client->call(stopSrv); if (stopSrv.response.result == -1){ ssWarning(S,"Error stopping V-REP simulation."); } delete stop_client; ros::ServiceClient* synchronous_client = (ros::ServiceClient*)vecPWork[2]; vrep_common::simRosSynchronous syncSrv; syncSrv.request.enable = false; synchronous_client->call(syncSrv); if (syncSrv.response.result == -1){ ssWarning(S,"Error setting V-REP synchronous simulation mode."); } delete synchronous_client; ros::ServiceClient* trigger_client = (ros::ServiceClient*)vecPWork[3]; delete trigger_client; // delete all subscribers and spinners const int_T nWaitTopic = ssGetIWork(S)[0]; for (int_T i = 0; i<nWaitTopic; ++i){ GenericSubscriber<topic_tools::ShapeShifter>* sub = (GenericSubscriber<topic_tools::ShapeShifter>*) vecPWork[4+i]; delete sub; } if (nWaitTopic>0){ ros::AsyncSpinner* spinner = (ros::AsyncSpinner*) vecPWork[4+nWaitTopic]; delete spinner; } SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); }
/* 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) { // get Objects void** vecPWork = ssGetPWork(S); int_T nRobots = *ssGetIWork(S); for (unsigned int i = 0; i < nRobots; ++i) { GenericSubscriber<telekyb_msgs::TKMotorCommands>* sub = (GenericSubscriber<telekyb_msgs::TKMotorCommands>*)vecPWork[i]; // cleanup delete sub; } ros::AsyncSpinner* spinner = (ros::AsyncSpinner*)vecPWork[nRobots]; delete spinner; SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); }
/* Function: mdlUpdate ====================================================== * Abstract: * This function is called once for every major integration time step. * Discrete states are typically updated here, but this function is useful * for performing any tasks that should only take place once per * integration step. */ static void mdlUpdate(SimStruct *S, int_T tid) { void** vecPWork = ssGetPWork(S); ros::ServiceClient* trigger_client = (ros::ServiceClient*)vecPWork[3]; const real_T timeout = mxGetScalar(ssGetSFcnParam(S, 3)); const int_T nWaitTopic = ssGetIWork(S)[0]; // reset all semaphores for (int_T i = 0; i<nWaitTopic; ++i){ GenericSubscriber<topic_tools::ShapeShifter>* sub = (GenericSubscriber<topic_tools::ShapeShifter>*) vecPWork[4+i]; sub->resetSem(); } vrep_common::simRosSynchronousTrigger trigSrv; trigger_client->call(trigSrv); if (trigSrv.response.result == -1){ ssSetErrorStatus(S, "Error triggering V-REP simulation"); } // wait for all messages for (int_T i = 0; i<nWaitTopic; ++i){ GenericSubscriber<topic_tools::ShapeShifter>* sub = (GenericSubscriber<topic_tools::ShapeShifter>*) vecPWork[4+i]; if (sub->waitMsg(timeout) == -1){ const std::string tmp = std::string("Timeout expired for topic ") + sub->getTopicName(); ssWarning(S, tmp.c_str()); } else { } } const real_T extraWaitTime = mxGetScalar(ssGetSFcnParam(S, 4)); if (extraWaitTime>0){ // SFUNPRINTF("Waiting extra %f seconds\n", extraWaitTime); ros::Duration extra_wait(extraWaitTime); extra_wait.sleep(); } }
/* Function: mdlOutputs ======================================================= * Abstract: * * y1 = "sum" * "gain" where sum operation is defined by a list of * '+' and '-' characters. * y2 = "modified" average of the input signals (i.e. sum/nInputPorts). */ static void mdlOutputs(SimStruct *S, int_T tid) { InputRealPtrsType enablePtrs; int *enabled = ssGetIWork(S); int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT); int signalTid = ssGetInputPortSampleTimeIndex(S,SIGNAL_IPORT); real_T enableTs = ssGetInputPortSampleTime(S,ENABLE_IPORT); real_T enableTsOffset = ssGetInputPortOffsetTime(S,ENABLE_IPORT); if (enableTs == CONTINUOUS_SAMPLE_TIME && enableTsOffset == 0.0) { if (ssIsMajorTimeStep(S) && ssIsContinuousTask(S,tid)) { if (signalTid == enableTid || ssIsSpecialSampleHit(S, signalTid, enableTid, tid)) { enablePtrs = ssGetInputPortRealSignalPtrs(S,ENABLE_IPORT); *enabled = (*enablePtrs[0] > 0.0); } } } else { int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT); if (ssIsSampleHit(S, enableTid, tid)) { if (enableTid == signalTid || ssIsSpecialSampleHit(S, signalTid, enableTid, tid)) { enablePtrs = ssGetInputPortRealSignalPtrs(S,ENABLE_IPORT); *enabled = (*enablePtrs[0] > 0.0); } } } if (ssIsSampleHit(S, signalTid, tid) && (*enabled)) { InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,SIGNAL_IPORT); real_T signal = *uPtrs[0]; int i; for (i = 0; i < NOUTPUTS; i++) { int outTid = ssGetOutputPortSampleTimeIndex(S,i); if (outTid==signalTid || ssIsSpecialSampleHit(S, outTid, signalTid, tid)) { real_T *y = ssGetOutputPortRealSignal(S,i); *y = signal; } } } } /* end mdlOutputs */
/* 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 mdlStart(SimStruct *S) { SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); // init ROS if not yet done. initROS(S); void** vecPWork = ssGetPWork(S); // save nRobots in IWorkVector int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2)); *ssGetIWork(S) = nRobots; ros::NodeHandle nodeHandle(ros::this_node::getName()); // get Topic Strings size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1; char* prefix_topic = (char*)mxMalloc(prefix_buflen); char* postfix_topic = (char*)mxMalloc(postfix_buflen); mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen); mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen); //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); std::stringstream sstream; mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2)); for (unsigned int i = 0; i < nRobots; ++i) { sstream.str(std::string()); // build topicstring sstream << prefix_topic; sstream << (uint)robotIDs[i]; sstream << postfix_topic; GenericPublisher<geometry_msgs::PointStamped>* pub = new GenericPublisher<geometry_msgs::PointStamped>(nodeHandle, sstream.str(), 10); vecPWork[i] = pub; } // free char array mxFree(prefix_topic); mxFree(postfix_topic); }
static void mdlTerminate(SimStruct *S) { #ifndef MATLAB_MEX_FILE int index = (int)COMEDI_DEVICE - 1; void *dev = (void *)ssGetPWork(S)[0]; int subdev = ssGetIWork(S)[0]; char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"}; if (ssGetErrorStatus(S) == NULL) { ComediDev_InUse[index]--; ComediDev_AIInUse[index]--; if (!ComediDev_AIInUse[index]) { comedi_unlock(dev, subdev); } if (!ComediDev_InUse[index]) { comedi_close(dev); printf("\nCOMEDI %s closed.\n\n", devname[index]); ComediDev[index] = NULL; } } #endif }
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 }
static void mdlStart(SimStruct *S) { SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); // init ROS if not yet done. initROS(S); void** vecPWork = ssGetPWork(S); ros::NodeHandle nodeHandle(ros::this_node::getName()); // get base service name strings size_t buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; char* base_name = (char*)mxMalloc(buflen); mxGetString((ssGetSFcnParam(S, 1)), base_name, buflen); const std::string start_name = std::string(base_name) + "/simRosStartSimulation"; ros::ServiceClient* start_client = new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosStartSimulation>(start_name)); vecPWork[0] = start_client; if (!start_client->exists()){ ssSetErrorStatus(S,"Start service does not exist!"); } const std::string stop_name = std::string(base_name) + "/simRosStopSimulation"; ros::ServiceClient* stop_client = new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosStopSimulation>(stop_name)); vecPWork[1] = stop_client; if (!stop_client->exists()){ ssSetErrorStatus(S,"Stop service does not exist!"); } const std::string synchronous_name = std::string(base_name) + "/simRosSynchronous"; ros::ServiceClient* synchronous_client = new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosSynchronous>(synchronous_name)); vecPWork[2] = synchronous_client; if (!synchronous_client->exists()){ ssSetErrorStatus(S,"Synchronous service does not exist!"); } const std::string trigger_name = std::string(base_name) + "/simRosSynchronousTrigger"; ros::ServiceClient* trigger_client = new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosSynchronousTrigger>(trigger_name)); vecPWork[3] = trigger_client; if (!trigger_client->exists()){ ssSetErrorStatus(S,"Trigger service does not exist!"); } const std::string info_name = std::string(base_name) + "/simRosGetInfo"; ros::ServiceClient* info_client = new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosGetInfo>(info_name)); if (!info_client->exists()){ ssSetErrorStatus(S,"Info service does not exist!"); } const std::string parameter_name = std::string(base_name) + "/simRosSetFloatingParameter"; ros::ServiceClient* parameter_client = new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosSetFloatingParameter>(parameter_name)); if (!parameter_client->exists()){ ssSetErrorStatus(S,"Set parameter service does not exist!"); } mxFree(base_name); // Set V-Rep time step to this block time step const real_T tSim = mxGetScalar(ssGetSFcnParam(S, 0)); vrep_common::simRosSetFloatingParameter paramSrv; paramSrv.request.parameter = sim_floatparam_simulation_time_step; paramSrv.request.parameterValue = tSim; parameter_client->call(paramSrv); if (paramSrv.response.result == -1){ ssSetErrorStatus(S,"Error setting V-REP simulation time step."); } vrep_common::simRosGetInfo infoSrv; info_client->call(infoSrv); const real_T vrepTimeStep = ROUND(infoSrv.response.timeStep*1e6)*1e-6; if (fabs(vrepTimeStep-tSim) > 1e-5){ char str[100]; sprintf(str, "V-REP time step (%e) is different from block time step (%e).", vrepTimeStep, tSim); ssSetErrorStatus(S,str); } // Set synchronous mode vrep_common::simRosSynchronous syncSrv; syncSrv.request.enable = true; synchronous_client->call(syncSrv); if (syncSrv.response.result == -1){ ssSetErrorStatus(S,"Error setting V-REP synchronous simulation mode."); } // Start V-Rep simulation vrep_common::simRosStartSimulation startSrv; start_client->call(startSrv); if (startSrv.response.result == -1){ ssSetErrorStatus(S,"Error starting V-REP simulation."); } // Subscribe to all the topics to wait size_t wait_buflen = mxGetN((ssGetSFcnParam(S, 2)))*sizeof(mxChar)+1; char* wait_name = (char*)mxMalloc(wait_buflen); mxGetString((ssGetSFcnParam(S, 2)), wait_name, wait_buflen); int_T nWaitTopic = 0; char* topic = strtok(wait_name, TOPIC_SEPARATORS); while(topic != NULL){ GenericSubscriber<topic_tools::ShapeShifter>* sub = new GenericSubscriber<topic_tools::ShapeShifter>(nodeHandle, topic, 1); vecPWork[4+nWaitTopic] = sub; nWaitTopic++; topic = strtok (NULL, TOPIC_SEPARATORS); } if (nWaitTopic>0){ ros::AsyncSpinner* spinner = new ros::AsyncSpinner(nWaitTopic); spinner->start(); vecPWork[4+nWaitTopic] = spinner; ssGetIWork(S)[0] = nWaitTopic; } mxFree(wait_name); }
static void mdlOutputs(SimStruct *S, int_T tid) { /*--------Define Parameters-------*/ const real_T s_V_Ae_vlv = *mxGetPr(s_V_Ae_vlv_p(S)); const real_T s_V_Ae_byp = *mxGetPr(s_V_Ae_byp_p(S)); const real_T s_V_Ae_th = *mxGetPr(s_V_Ae_th_p(S)); /*-------- vector & array data -------*/ const real_T *X_V_FAR_vec = mxGetPr(X_V_FAR_vec_p(S)); const real_T *T_V_Rt_vec = mxGetPr(T_V_Rt_vec_p(S)); const real_T *Y_V_Tt_vec = mxGetPr(Y_V_Tt_vec_p(S)); const real_T *T_V_gamma_array = mxGetPr(T_V_gamma_array_p(S)); /*------get dimensions of parameter arrays-------*/ const int_T A1 = mxGetNumberOfElements(X_V_FAR_vec_p(S)); const int_T B1 = mxGetNumberOfElements(Y_V_Tt_vec_p(S)); /*---------Define Inputs--------*/ const real_T *u = (const real_T*) ssGetInputPortSignal(S,0); double WbyIn = u[0]; /* Bypass flow path flow rate [pps] */ double TtbyIn = u[1]; /* Bypass Temperature [degR] */ double PtbyIn = u[2]; /* Bypass disch. pressure [psia] */ double FARcbyIn = u[3]; /* Bypass combusted fuel to air ratio [frac] */ double VlvPosIn = u[4]; /* Valve Position [frac, 0-1] */ double WmfpIn = u[5]; /* Main flow path flow rate [pps] */ double TtmfpIn = u[6]; /* Main flow path Temprature [degR] */ double PtmfpIn = u[7]; /* Main flow path Pressure Input [psia] */ double FARcmfpIn= u[8]; /* Main flow path combusted fuel to air ratio [frac] */ real_T *y = (real_T *)ssGetOutputPortRealSignal(S,0); /* Output Array */ /*--------Define Constants-------*/ double Ath, Rb, gamb, Cpb, Pe, Me, Tcr_o_Te, Ae_o_Acr, Ath_o_Acr; double Mth0, Mth1, Tcr_o_Tth0, Tcr_o_Tth1, Ath_o_Acr0, Ath_o_Acr1, err0, err1, err; double Mth, Tcr_o_Tth_it, Ath_o_Acr_it, Tcr_o_T0, Tth_o_T0, Tth, Pth, rhoth, Vth, Wth, Wbyp_noz, Whpc; double MthOut, Test, WthOut; int interpErr = 0; int count; /* ------- get strings -------------- */ char * BlkNm; int_T buflen; int_T status; /* Get name of block from dialog parameter (string) */ buflen = mxGetN(BN_p(S))*sizeof(mxChar)+1; BlkNm = mxMalloc(buflen); status = mxGetString(BN_p(S), BlkNm, buflen); /* Input validation */ if ((WbyIn <= 0 || WmfpIn <= 0) && ssGetIWork(S)[Er1]==0){ printf("Flow rates must be nonzero !!"); ssSetIWorkValue(S,Er1,1); } if (VlvPosIn > 0){ if (VlvPosIn < 0.001) VlvPosIn = 0.001; Ath = VlvPosIn*s_V_Ae_th; /* throat area [in^2] */ /* define gas constants for booster discharge air */ Rb = interp1Ac(X_V_FAR_vec,T_V_Rt_vec,FARcmfpIn,A1,&interpErr); if (interpErr == 1 && ssGetIWork(S)[Er2]==0){ printf("Warning in %s, Error calculating Rb. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,Er2,1); } gamb = interp2Ac(X_V_FAR_vec,Y_V_Tt_vec,T_V_gamma_array,FARcmfpIn,TtmfpIn,A1,B1,&interpErr); if (interpErr == 1 && ssGetIWork(S)[Er3]==0){ printf("Warning in %s, Error calculating gamb. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,Er3,1); } Cpb = Rb*gamb*divby(gamb-1); /* determine static pressure at the exit plane (entering fan); */ /* assume bypass flow >> bleed flow */ Pe = calc_Pstatic(PtbyIn,TtbyIn,WbyIn,s_V_Ae_byp,X_V_FAR_vec,T_V_Rt_vec,Y_V_Tt_vec,T_V_gamma_array,FARcbyIn,A1,B1); /* compute exit to critical area ratio */ Me = sqrtT(2*divby(gamb-1)*(powT(Pe*divby(PtmfpIn), (1-gamb)*divby(gamb))-1)); Tcr_o_Te = (2*divby(gamb+1))*(1 + 0.5*(gamb-1)*Me*Me); Ae_o_Acr = powT(Tcr_o_Te, (gamb+1)*divby(2*(gamb-1)))*divby(Me); /* obtain throat to critical area ratio */ Ath_o_Acr = Ae_o_Acr*Ath*divby(s_V_Ae_vlv); /* determine throat Mach no. iteratively; initialize guesses, errors */ Mth0 = 0.1; /* subsonic guess values */ Mth1 = 0.2; Tcr_o_Tth0 = (2*divby(gamb+1))*(1 + 0.5*(gamb-1)*Mth0*Mth0); Tcr_o_Tth1 = (2*divby(gamb+1))*(1 + 0.5*(gamb-1)*Mth1*Mth1); Ath_o_Acr0 = powT(Tcr_o_Tth0, (gamb+1)*divby(2*(gamb-1)))*divby(Mth0); Ath_o_Acr1 = powT(Tcr_o_Tth1, (gamb+1)*divby(2*(gamb-1)))*divby(Mth1); err0 = Ath_o_Acr - Ath_o_Acr0; err1 = Ath_o_Acr - Ath_o_Acr1; err = 100; count = 0; while (fabs(err) > 0.002 && (err0 - err1) != 0 && count < 100){ /* compute new Mach no. guess */ Mth = Mth0 - err0*(Mth0 - Mth1)*divby(err0 - err1); if (Mth > 1.0) Mth = 1.0; /* compute error to drive solution towards specified area ratio */ Tcr_o_Tth_it = (2*divby(gamb+1))*(1 + 0.5*(gamb-1)*Mth*Mth); Ath_o_Acr_it = powT(Tcr_o_Tth_it, (gamb+1)*divby(2*(gamb-1)))*divby(Mth); err = Ath_o_Acr - Ath_o_Acr_it; /* propagate errors & guesses */ Mth1 = Mth0; err1 = err0; Mth0 = Mth; err0 = err; count++; } /* compute throat static pressure, temperature and Mach no.; */ /* modify if choked */ Tcr_o_T0 = 2*divby(gamb+1); Tth_o_T0 = 1*divby(1 + 0.5*(gamb-1)*Mth*Mth); if (Tth_o_T0 < Tcr_o_T0) Tth_o_T0 = Tcr_o_T0; Tth = TtmfpIn*Tth_o_T0; Pth = PtmfpIn*powT(Tth_o_T0, gamb*divby(gamb-1)); /* recompute the actual flow rate, assume no pressure loss */ rhoth = Pth*144*divby(Rb*JOULES_CONST*Tth); /* [lb/ft^3] */ Vth = sqrtT(2*Cpb*C_GRAVITY*JOULES_CONST*(TtmfpIn - Tth)); /* [ft/s] */ Wth = rhoth*Ath/144*Vth; /* [lb/s] */ Mth = Vth*divby(sqrtT(gamb*Rb*C_GRAVITY*JOULES_CONST*Tth)); } else { Wth = 0; Mth = 0; } WthOut = Wth; Test = Vth; /*------Assign output values------------*/ y[0] = WthOut; /* Valve throat flow [pps] */ y[1] = MthOut; /* Mach no. at throat */ y[2] = Test; /* Output Test Point */ }
static void mdlOutputs(SimStruct *S, int_T tid) { /*--------parameters defined in S-function block--------*/ const real_T AFARc = *mxGetPr(AFARc_p(S)); /*-------- vector & array data -------*/ const real_T *X_A_AltVec = mxGetPr(X_A_AltVec_p(S)); const real_T *T_A_TsVec = mxGetPr(T_A_TsVec_p(S)); const real_T *T_A_PsVec = mxGetPr(T_A_PsVec_p(S)); const real_T *X_A_FARVec = mxGetPr(X_A_FARVec_p(S)); const real_T *T_A_RtArray = mxGetPr(T_A_RtArray_p(S)); const real_T *Y_A_TVec = mxGetPr(Y_A_TVec_p(S)); const real_T *T_A_gammaArray = mxGetPr(T_A_gammaArray_p(S)); /*------get dimensions of parameter arrays-------*/ const int_T A = mxGetNumberOfElements(X_A_AltVec_p(S)); const int_T B = mxGetNumberOfElements(X_A_FARVec_p(S)); const int_T C = mxGetNumberOfElements(Y_A_TVec_p(S)); /*---------Define Inputs--------*/ const real_T *u = (const real_T*) ssGetInputPortSignal(S,0); double AltIn = u[0]; /* Altitude(ft) */ double dTempIn = u[1]; /* delta Temperature [degF] */ double MNIn = u[2]; /* Mach Number (frac) */ real_T *y = (real_T *)ssGetOutputPortRealSignal(S,0); /* Output Array */ /*--------Define Constants-------*/ double PsOut, TsOut, TtOut, PtOut, VengOut, TsStDayOut, Vsound; double Ttg, Ptg, Vg, Vsg, MNg, Sout, htg, gammasg, Rs, Rt; double hs, htOut; double er, er_old, erthr, Ptg_new, Ptg_old, FAR, FAROut; int iter, maxiter; int interpErr = 0; /* ------- get strings -------------- */ char * BlkNm; int_T buflen; int_T status; /* Get name of block from dialog parameter (string) */ buflen = mxGetN(BN_p(S))*sizeof(mxChar)+1; BlkNm = mxMalloc(buflen); status = mxGetString(BN_p(S), BlkNm, buflen); FAR = AFARc; Rt = interp1Ac(X_A_FARVec,T_A_RtArray,FAR,B,&interpErr); if (interpErr == 1 && ssGetIWork(S)[0]==0){ printf("Warning in %s, Error calculating Rt. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,0,1); } Rs = Rt; /* Static Temperature */ TsStDayOut = interp1Ac(X_A_AltVec,T_A_TsVec,AltIn,A,&interpErr); if (interpErr == 1 && ssGetIWork(S)[1]==0){ printf("Warning in %s, Error calculating TsStDayOut. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,0,1); } TsOut = TsStDayOut + dTempIn; /* Static Pressure*/ PsOut = interp1Ac(X_A_AltVec,T_A_PsVec,AltIn,A,&interpErr); if (interpErr == 1 && ssGetIWork(S)[2]==0){ printf("Warning in %s, Error calculating PsOut. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,1,1); } /* Calc output entropy */ Sout = pt2sc(PsOut, TsOut, FAR); /* Determine Static enthalpy */ hs = t2hc(TsOut,FAR); /* Pt guess */ /*------ Total Temperature ---------*/ Ttg = TsOut * (1+MNIn*MNIn*(C_GAMMA-1)/2); /*------ Total Pressure ---------*/ Ptg = PsOut/(pow((TsOut/Ttg),(C_GAMMA/(C_GAMMA-1)))); /* calculate total temperature */ Ttg = sp2tc(Sout,Ptg,FAR); /* calculate total enthalpy */ htg = t2hc(Ttg,FAR); /* calculate velocity */ Vg = sqrt(2 * (htg - hs)*C_GRAVITY*JOULES_CONST); gammasg = interp2Ac(X_A_FARVec,Y_A_TVec,T_A_gammaArray,FAR,TsOut,B,C,&interpErr); if (interpErr == 1 && ssGetIWork(S)[3]==0){ printf("Warning in %s, Error calculating iteration gammasg. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,3,1); } Vsg = sqrt(gammasg*Rs*TsOut*C_GRAVITY*JOULES_CONST); MNg = Vg/Vsg; er = MNIn - MNg; Ptg_new = Ptg + 0.05; maxiter = 15; iter = 0; erthr = 0.001; while (fabs(er) > erthr && iter < maxiter) { er_old = er; Ptg_old = Ptg; if(fabs(Ptg - Ptg_new) < 0.03) Ptg = Ptg + 0.05; else Ptg = Ptg_new; /* calculate Total emperature */ Ttg = sp2tc(Sout,Ptg,FAR); /* calculate total enthalpy */ htg = t2hc(Ttg,FAR); /* calculate velocity */ Vg = sqrt(2 * (htg - hs)*C_GRAVITY*JOULES_CONST); Vsg = sqrt(gammasg*Rs*TsOut*C_GRAVITY*JOULES_CONST); MNg = Vg/Vsg; er = MNIn - MNg; if (fabs(er) > erthr) { /* determine next guess pressure by secant algorithm */ Ptg_new = Ptg - er *(Ptg - Ptg_old)/(er - er_old); } iter = iter + 1; } if (iter == maxiter && ssGetIWork(S)[3]==0 ){ printf("Warning in %s, Error calculating Pt at input MN. There may be error in output pressure\n", BlkNm); ssSetIWorkValue(S,4,1); } htOut = htg; TtOut = Ttg; PtOut = Ptg; /*---- Engine Velocity ---------*/ Vsound = Vsg; VengOut = Vsound * MNIn; FAROut = FAR; /*------Assign output values------------*/ y[0] = htOut; /* Total enthalpy */ y[1] = TtOut; /* Total Temperature [degR] */ y[2] = PtOut; /* Total Pressure [psia] */ y[3] = FAROut; /* Fuel to Air Ratio */ y[4] = PsOut; /* Static Pressure [psia] */ y[5] = TsOut; /* Static Temperature [degR] */ y[6] = VengOut; /* Engine Velocity [ft/sec] */ }
static void mdlOutputs(SimStruct *S, int_T tid) { /*--------parameters defined in S-function block--------*/ const real_T AthroatIn = *mxGetPr(AthroatIn_p(S)); /* input throat area (sq-in) */ const real_T MNIn = *mxGetPr(MNIn_p(S)); /* input throat area (sq-in) */ const int_T SolveType = *mxGetPr(SolveType_p(S)); /* 0-solve based on Ath, 1-solve based on MNIn*/ /*-------- vector & array data -------*/ const real_T *X_FARVec = mxGetPr(X_FARVec_p(S)); const real_T *T_RtArray = mxGetPr(T_RtArray_p(S)); const real_T *Y_TtVec = mxGetPr(Y_TtVec_p(S)); const real_T *T_gammaArray = mxGetPr(T_gammaArray_p(S)); /*------get dimensions of parameter arrays-------*/ const int_T A = mxGetNumberOfElements(X_FARVec_p(S)); const int_T B = mxGetNumberOfElements(Y_TtVec_p(S)); /*---------Define Inputs--------*/ const real_T *u = (const real_T*) ssGetInputPortSignal(S,0); double WIn = u[0]; /* Input Flow [pps] */ double htIn = u[1]; /* enthaply [BTU/lbm] */ double TtIn = u[2]; /* Temperature Input [degR] */ double PtIn = u[3]; /* Pressure Input [psia] */ double FARcIn = u[4]; /* Combusted Fuel to Air Ratio [frac] */ real_T *y = (real_T *)ssGetOutputPortRealSignal(S,0); /* Output Array */ /*--------Define Constants-------*/ double PsOut, TsOut, rhosOut, MNOut, AthOut; double Sin, htin; double Rt, Rs; double TsMNg, PsMNg, MNg; double Tsg, Psg, Psg_new, Psg_old, Acalc, erA, erA_old; double gammatg, gammasg, hsg, rhosg, Vg; double erMN_old, erMN, PsMNg_old, PsMNg_new; double erthr; int maxiter, iter; int interpErr = 0; /* ------- get strings -------------- */ char * BlkNm; int_T buflen; int_T status; /* Get name of block from dialog parameter (string) */ buflen = mxGetN(BN_p(S))*sizeof(mxChar)+1; BlkNm = mxMalloc(buflen); status = mxGetString(BN_p(S), BlkNm, buflen); /* Calc entropy */ Sin = pt2sc(PtIn, TtIn, FARcIn); /*-- Compute Input enthalpy --------*/ htin = t2hc(TtIn,FARcIn); /* Where gas constant is R = f(FAR), but NOT P & T */ Rt = interp1Ac(X_FARVec,T_RtArray,FARcIn,A,&interpErr); if (interpErr == 1 && ssGetIWork(S)[0]==0){ printf("Warning in %s, Error calculating Rt. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,0,1); } Rs = Rt; /* Solve for Ts and Ps when MN is known*/ if (SolveType == 1) { /*---- set MN = MNIn and calc SS Ps for iteration IC --------*/ MNg = MNIn; gammatg = interp2Ac(X_FARVec,Y_TtVec,T_gammaArray,FARcIn,TtIn,A,B,&interpErr); if (interpErr == 1 && ssGetIWork(S)[1]==0){ printf("Warning in %s, Error calculating gammatg. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,1,1); } TsMNg = TtIn /(1+MNg*MNg*(gammatg-1)/2); PsMNg = PtIn*pow((TsMNg/TtIn),(gammatg/(gammatg-1))); PcalcStat(PtIn, PsMNg, TtIn, htin, FARcIn, Rt, &Sin, &TsMNg, &hsg, &rhosg, &Vg); gammasg = interp2Ac(X_FARVec,Y_TtVec,T_gammaArray,FARcIn,TsMNg,A,B,&interpErr); if (interpErr == 1 && ssGetIWork(S)[2]==0){ printf("Warning in %s, Error calculating gammasg. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,2,1); } MNg = Vg/sqrt(gammasg*Rs*TsMNg*C_GRAVITY*JOULES_CONST); if (Vg > 0.0001){ Acalc = WIn/(Vg * rhosg/C_SINtoSFT);} else { Acalc = 999; /* if velocity is close to zero assume a very large Ath */} erMN = MNIn - MNg; PsMNg_new = PsMNg + 0.05; maxiter = 15; iter = 0; erthr = 0.0001; /* if Ps is not close enough to Ps at MN = MNIn, iterate to find Ps at MN = MNIn */ while (fabs(erMN) > erthr && iter < maxiter) { erMN_old = erMN; PsMNg_old = PsMNg; if(fabs(PsMNg - PsMNg_new) < 0.003) PsMNg = PsMNg + 0.005; else PsMNg = PsMNg_new; PcalcStat(PtIn, PsMNg, TtIn, htin, FARcIn, Rt, &Sin, &TsMNg, &hsg, &rhosg, &Vg); gammasg = interp2Ac(X_FARVec,Y_TtVec,T_gammaArray,FARcIn,TsMNg,A,B,&interpErr); if (interpErr == 1 && ssGetIWork(S)[3]==0){ printf("Warning in %s, Error calculating iteration gammasg. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,3,1); } MNg = Vg/sqrt(gammasg*Rs*TsMNg*C_GRAVITY*JOULES_CONST); /* calculated Area */ if (Vg > 0.0001){ Acalc = WIn/(Vg * rhosg/C_SINtoSFT);} else { Acalc = 999; /* if velocity is close to zero assume a very large Ath */} erMN = MNIn - MNg; if (fabs(erMN) > erthr) { /* determine next guess pressure by secant algorithm */ PsMNg_new = PsMNg - erMN *(PsMNg - PsMNg_old)/(erMN - erMN_old); } iter = iter + 1; } if (iter == maxiter && ssGetIWork(S)[4]==0 ){ printf("Warning in %s, Error calculating Ps at MN = MNIn. There may be error in block outputs\n", BlkNm); ssSetIWorkValue(S,4,1); } TsOut = TsMNg; PsOut = PsMNg; rhosOut = rhosg; MNOut = MNIn; AthOut = Acalc; } /* Solve for Ts and Ps when Ath is known*/ else if (SolveType == 0) { /* guess Psout and calculate an initial Area error */ MNg = 0.4; gammatg = 1.4; Tsg = TtIn /(1+MNg*MNg*(gammatg-1)/2); Psg = PtIn*pow((Tsg/TtIn),(gammatg/(gammatg-1))); PcalcStat(PtIn, Psg, TtIn, htin, FARcIn, Rt, &Sin, &Tsg, &hsg, &rhosg, &Vg); Acalc = WIn/(Vg * rhosg/C_SINtoSFT); MNg = Vg/sqrt(gammasg*Rs*Tsg*C_GRAVITY*JOULES_CONST); /* determine guess error for static pressure iteration */ erA = (AthroatIn - Acalc)/AthroatIn; /* determine iteration constants */ iter = 0; maxiter = 1000; Psg_new = Psg + 0.05; erthr = 0.0001; while ( fabs(erA) > erthr && iter < maxiter){ erA_old = erA; Psg_old = Psg; if(fabs(Psg - Psg_new) < 0.0003) { Psg = Psg + 0.0005; } else { Psg = Psg_new; } /* calculate flow velocity and rhos */ PcalcStat(PtIn, Psg, TtIn, htin, FARcIn, Rt, &Sin, &Tsg, &hsg, &rhosg, &Vg); gammasg = interp2Ac(X_FARVec,Y_TtVec,T_gammaArray,FARcIn,Tsg,A,B,&interpErr); if (interpErr == 1 && ssGetIWork(S)[3]==0){ printf("Warning in %s, Error calculating iteration gammasg. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,3,1); } MNg = Vg/sqrt(gammasg*Rs*Tsg*C_GRAVITY*JOULES_CONST); if (Vg > 0.0001) { /* calculated Area */ Acalc = WIn/(Vg * rhosg/C_SINtoSFT); /*determine error */ erA = (AthroatIn - Acalc)/AthroatIn; } else { erA = 0; Psg = PtIn; Tsg = TtIn; Acalc = 999; } if (fabs(erA) > erthr) { /* determine next guess pressure by secant algorithm */ Psg_new = Psg - erA *(Psg - Psg_old)/(erA - erA_old); /* limit algorthim change */ if (Psg_new > 1.001*Psg) { Psg_new = 1.002 * Psg; } else if (Psg_new < 0.999 * Psg) { Psg_new = 0.998 * Psg; } } iter = iter + 1; } TsOut = Tsg; PsOut = Psg; rhosOut = rhosg; MNOut = MNg; AthOut = Acalc; } else { if (ssGetIWork(S)[5]==0 ){ printf("Warning in %s, SolveType_M is not valid. There may be error in block outputs\n", BlkNm); ssSetIWorkValue(S,5,1); } TsOut = TtIn; PsOut = PtIn; rhosOut = 1; MNOut = 0; AthOut = 100; } /*------Assign output values------------*/ y[0] = TsOut; /* static Temperature [degR] */ y[1] = PsOut; /* static Pressure [psia] */ y[2] = rhosOut; /* static rho [lbm/ft3]*/ y[3] = MNOut; /* mach number [frac]*/ y[4] = AthOut; /* throat area [in^2] */ }
static void mdlOutputs(SimStruct *S, int_T tid) { /*--------Define Parameters-------*/ const real_T s_T_Nc = *mxGetPr(s_T_Nc_p(S)); const real_T s_T_PR = *mxGetPr(s_T_PR_p(S)); const real_T s_T_Wc = *mxGetPr(s_T_Wc_p(S)); const real_T s_T_Eff = *mxGetPr(s_T_Eff_p(S)); const real_T NcDes = *mxGetPr(NcDes_p(S)); const real_T PRmapDes = *mxGetPr(PRmapDes_p(S)); const real_T EffDes = *mxGetPr(EffDes_p(S)); const real_T NDes = *mxGetPr(NDes_p(S)); const real_T IDes = *mxGetPr(IDesign_p(S)); const real_T s_T_hi = *mxGetPr(s_T_hi_p(S)); const real_T gamma_T = *mxGetPr(gamma_T_p(S)); const real_T Rt_T = *mxGetPr(Rt_T_p(S)); const int_T BldPosLeng = *mxGetPr(BldPosLeng_p(S)); const int_T CoolFlwEn = *mxGetPr(CoolFlwEn_p(S)); /* vector & array data */ const real_T *Y_T_NcpsiVec = mxGetPr(Y_T_NcpsiVec_p(S)); const real_T *X_T_PRpsiVec = mxGetPr(X_T_PRpsiVec_p(S)); const real_T *Y_T_NcwowVec = mxGetPr(Y_T_NcwowVec_p(S)); const real_T *X_T_PRwowVec = mxGetPr(X_T_PRwowVec_p(S)); const real_T *T_T_Map_WoWArray = mxGetPr(T_T_Map_WoWArray_p(S)); const real_T *T_T_Map_psiArray = mxGetPr(T_T_Map_psiArray_p(S)); const real_T *T_BldPos = mxGetPr(T_BldPos_p(S)); /*------get dimensions of parameter arrays-------*/ const int_T A = mxGetNumberOfElements(Y_T_NcpsiVec_p(S)); const int_T B = mxGetNumberOfElements(X_T_PRpsiVec_p(S)); const int_T C = mxGetNumberOfElements(Y_T_NcwowVec_p(S)); const int_T D = mxGetNumberOfElements(X_T_PRwowVec_p(S)); /*---------Define Inputs--------*/ const real_T *u = (const real_T*) ssGetInputPortSignal(S,0); double WIn = u[0]; /* Input Flow [pps] */ double htIn = u[1]; /* input enthalpy [BTU/lbm]*/ double TtIn = u[2]; /* Temperature Input [degR] */ double PtIn = u[3]; /* Pressure Input [psia] */ double FARcIn = u[4]; /* Compusted Fuel to Air Ratio [frac] */ double Nmech = u[5]; /* Mechancial Shaft Speed [rpm]*/ double psiMapIn = u[6]; /* PSI map [NA] */ /*---------Define Inputs for input port 2--------*/ /* N 5x1 vectors consisting of W, ht, Tt, Pt and FAR, where N is the number of cooling flows */ const real_T *CoolFlow = ssGetInputPortRealSignal(S, 1); int cfWidth = ssGetCurrentInputPortDimensions(S, 1, 0); real_T *y = (real_T *)ssGetOutputPortRealSignal(S,0); /* Output Array */ /*--------Define Constants-------*/ double WOut, htOut, TtOut, PtOut, FARcOut, TorqueOut, NErrorOut; double WcCalcin, WcMap, theta,delta, Pwrout, PRin, htin; double TtIdealout, Test, htIdealout, Sout, NcMap, Nc, EffMap, Eff; double dHcools1, dHcoolout, Wfcools1, Wfcoolout, Ws1in,hts1in, Tts1in, Pts1in, FARs1in; double Ss1in, dHout, Wcoolout, Wcools1, PRmapRead; double C_Eff, C_PR, C_Nc, C_Wc, TtOutIdeal; double WMap, psiMapI, delHtIdealMap, erT, erT_old, Ptoutg, Ptoutg_old; double TtOutIdealg, WpqAcrit, WoWMap, Ptoutg_new; int interpErr = 0; double Wcool[100]; double htcool[100]; double Ttcool[100]; double Ptcool[100]; double FARcool[100]; int Vtest, i; /* ------- get strings -------------- */ char * BlkNm; int_T buflen; int_T status; /* Get name of block from dialog parameter (string) */ buflen = mxGetN(BN_p(S))*sizeof(mxChar)+1; BlkNm = mxMalloc(buflen); status = mxGetString(BN_p(S), BlkNm, buflen); /* Verify input bleed vector is a multiple of 5 */ Vtest = cfWidth/5; if(5*Vtest != cfWidth && CoolFlwEn > 0.5 && ssGetIWork(S)[0]==0){ printf("Error in %s, one or more of the cooling flow input vector eleements is missing(Vector form; 5x1: W,ht,Tt,Pt,FAR)\n",BlkNm); ssSetIWorkValue(S,0,1); } else if(BldPosLeng != cfWidth/5 && CoolFlwEn > 0.5 && ssGetIWork(S)[1]==0){ printf("Errorin %s, number of cooling flow inputs does not match the length of the Cooling flow postion vector in the mask\n",BlkNm); ssSetIWorkValue(S,1,1); } /* unpack CoolFlow vector */ for (i = 0; i < cfWidth/5; i++) { if (CoolFlwEn < 0.5){ Wcool[i] = 0; htcool[i] = 0; Ttcool[i] = 0; Ptcool[i] = 0; FARcool[i] = 0; } else { Wcool[i] = CoolFlow[5*i]; Ttcool[i] = CoolFlow[5*i+2]; Ptcool[i] = CoolFlow[5*i+3]; FARcool[i] = CoolFlow[5*i+4]; htcool[i] = t2hc(Ttcool[i],FARcool[i]); } } /* Initialize cooling flow sum constants */ dHcools1 = 0; /* enthalpy * mass cooling flow rate at stage 1 of turbine */ dHcoolout = 0; /* enthalpy * mass cooling flow rate at exit of turbine */ Wcools1 = 0; /* total cooling flow at stage 1 of turbine*/ Wcoolout = 0; /* total cooling flow at output of turbine */ Wfcools1 = 0; /* combusted fuel flow in cooling at stage 1 of turbine */ Wfcoolout = 0; /* combusted fuel flow in cooling at exit of turbine */ /* calc cooling flow constants for stage 1 and output of the turbine */ for (i = 0; i < cfWidth/5; i++) { if ((T_BldPos[i] > 1 || T_BldPos[i] < 0) && CoolFlwEn > 0.5 && ssGetIWork(S)[2]==0){ printf(" Error in %s, cooling flow postion element %i needs to be defined as a 0 or 1\n",BlkNm,i+1); ssSetIWorkValue(S,2,1); } /* calc mass flow for cooling flows */ Wcools1 = Wcools1 + Wcool[i]*(1-T_BldPos[i]); Wcoolout = Wcoolout + Wcool[i]; /* calc fuel mass flow for cooling flows*/ Wfcools1 = Wfcools1 + FARcool[i]*Wcool[i]*(1-T_BldPos[i])/(1+FARcool[i]); Wfcoolout = Wfcoolout + FARcool[i]*Wcool[i]/(1+FARcool[i]); } /*-- Compute Total Flow --------*/ Ws1in = WIn + Wcools1; /* mass flow at station 1 */ WOut = WIn + Wcoolout; /* mass flow at turbine exit */ /*-- Compute Fuel to Air Ratios ---*/ FARs1in = (FARcIn* WIn/(1+FARcIn) + Wfcools1)/(WIn/(1+FARcIn) + Wcools1- Wfcools1); FARcOut = (FARcIn* WIn/(1+FARcIn)+ Wfcoolout)/(WIn/(1+FARcIn) + Wcoolout- Wfcoolout); /* calc input enthalpy of cooling flow for stage 1 */ for (i = 0; i < cfWidth/5; i++) { /* Compute cooling flow dH at stage 1 */ dHcools1 = dHcools1 + htcool[i]*Wcool[i]*(1-T_BldPos[i]); /* Compute cooling flow dH for the exit of the turbine assuming input htcool = htcoolout for turbine rear bleeds */ dHcoolout = dHcoolout + htcool[i]*Wcool[i]*T_BldPos[i]; } /*-- Compute avg enthalpy at stage 1 --------*/ htin = t2hc(TtIn,FARcIn); hts1in = (htin* WIn + dHcools1)/Ws1in; /*-- Compute stage 1 total temp--------*/ Tts1in = h2tc(hts1in,FARs1in); /*-- Compute Stage 1 entropy, assuming PtIn = Pts1in --------*/ Ss1in = pt2sc(PtIn,Tts1in,FARs1in); /*---- calculate misc. fluid condition related variables --------*/ delta = PtIn / C_PSTD; theta = TtIn / C_TSTD; /*------ Calculate corrected speed ---------*/ Nc = Nmech/sqrt(theta); if(IDes < 0.5) C_Nc = Nc / NcDes; else C_Nc = s_T_Nc; NcMap = Nc / C_Nc; /* ---- Calculate output entropy ----*/ Sout = Ss1in; /*-- Compute Turbine Efficiency (from Turbine map) --------*/ psiMapI = interp2Ac(X_T_PRpsiVec,Y_T_NcpsiVec,T_T_Map_psiArray,psiMapIn,NcMap,B,A,&interpErr); if (interpErr == 1 && ssGetIWork(S)[3]==0){ printf("Warning in %s, Error calculating psiMapI. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,3,1); } EffMap = psiMapIn/psiMapI; if(IDes < 0.5) C_Eff = EffDes / EffMap; else C_Eff = s_T_Eff; Eff = EffMap * C_Eff; /* ---- Ideal enthalpy ----*/ delHtIdealMap = psiMapI * (Nmech / 60)*(Nmech / 60); htIdealout = hts1in - delHtIdealMap * s_T_hi; /* ensure enthalpy is >= 0 */ if(htIdealout < 0) { htIdealout = 0; } /* Determine Ideal exit temp */ TtOutIdeal = h2tc(htIdealout,FARs1in); /* Determine starting point for iteration to find PR */ Ptoutg = PtIn*pow((TtOutIdeal/TtIn),(gamma_T/(gamma_T-1))); TtOutIdealg = sp2tc(Sout,Ptoutg,FARs1in); erT = 100*abs_D(TtOutIdealg - TtOutIdeal)/TtOutIdeal; Ptoutg_new = Ptoutg; /* iterate to find Ptout when TtOutIdeal guess = TtOutIdeal */ while (abs_D(erT) > 0.05) { erT_old = erT; Ptoutg_old = Ptoutg; if(abs_D(Ptoutg - Ptoutg_new) < 0.02) Ptoutg = Ptoutg + 0.05; else Ptoutg = Ptoutg_new; Ptoutg = Ptoutg + 0.05; TtOutIdealg = sp2tc(Sout,Ptoutg,FARs1in); erT = 100*(TtOutIdealg - TtOutIdeal)/TtOutIdeal; if (abs_D(erT) > 0.05) { /* determine next guess pressure by secant algorithm */ Ptoutg_new = Ptoutg - erT *(Ptoutg - Ptoutg_old)/(erT - erT_old); } } PRin = PtIn/Ptoutg; /*------ Compute pressure output --------*/ if(IDes < 0.5) C_PR = (PRin - 1)/(PRmapDes -1); else C_PR = s_T_PR; PRmapRead = (PRin -1)/C_PR + 1; PtOut = PtIn/PRin; /*-- Compute Total Flow input (from Turbine map) --------*/ WoWMap = interp2Ac(X_T_PRwowVec,Y_T_NcwowVec,T_T_Map_WoWArray,PRmapRead,NcMap,D,C,&interpErr); if (interpErr == 1 && ssGetIWork(S)[4]==0){ printf("Warning in %s, Error calculating WoWMap. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,4,1); } WpqAcrit = sqrt((gamma_T*C_GRAVITY)/(Rt_T*JOULES_CONST))/pow((1+(gamma_T-1)/2),((gamma_T+1)/(2*(gamma_T-1)))); WMap = WoWMap * WpqAcrit * (PtIn/sqrt(Tts1in)); WcMap = WMap * sqrt(theta)/delta; if(IDes < 0.5) C_Wc = Ws1in*sqrt(theta)/delta / WcMap; else C_Wc = s_T_Wc; WcCalcin = WcMap * C_Wc; /*-Compute power output only takes into account cooling flow that enters at front of engine (stage 1)-*/ Pwrout = ((hts1in - htIdealout)*Eff)*Ws1in * C_BTU_PER_SECtoHP; /* ---- enthalpy output ----*/ htOut = ((((htIdealout - hts1in)*Eff) + hts1in)*Ws1in + dHcoolout)/WOut; /*------ Compute Temperature output (empirical) ---------*/ TtOut = h2tc(htOut,FARcOut); /*----- Compute output Torque to shaft ----*/ TorqueOut = C_HP_PER_RPMtoFT_LBF * Pwrout/Nmech; /* ----- Compute Normalized Flow Error ----- */ if (IDes < 0.5 && NDes == 0) NErrorOut = 100; else if (IDes < 0.5) NErrorOut = (Nmech - NDes)/NDes; else if (Ws1in == 0) { NErrorOut = 100; } else { NErrorOut = (Ws1in*sqrt(theta)/delta-WcCalcin)/(Ws1in*sqrt(theta)/delta) ; } Test = Wcool[0]; /*------Assign output values------------ */ y[0] = WOut; /* Outlet Total Flow [pps] */ y[1] = htOut; /* Outlet Enthalpy [BTU/lbm]*/ y[2] = TtOut; /* Outlet Temperature [degR] */ y[3] = PtOut; /* Outlet Pressure [psia] */ y[4] = FARcOut; /* Outlet Fuel to Air Ratio [NA] */ y[5] = TorqueOut; /* Torque Output [lbf*ft] */ y[6] = NErrorOut; /* Normalized turbine Error [frac]*/ y[7] = C_Nc; /* Corrected Shaft Speed Scalar */ y[8] = C_Wc; /* Corrected Flow Scalar */ y[9] = C_PR; /* Pressure Ratio Scalar */ y[10] = C_Eff; /* Efficiency Scalar */ y[11] = Test; }
static void mdlOutputs(SimStruct *S, int_T tid) { /*--------Define Parameters-------*/ const real_T NcDes = *mxGetPr(NcDes_p(S)); const real_T PRDes = *mxGetPr(PRDes_p(S)); const real_T EffDes = *mxGetPr(EffDes_p(S)); const real_T RlineDes = *mxGetPr(RlineDes_p(S)); const real_T IDes = *mxGetPr(IDesign_p(S)); const real_T CustBldEn = *mxGetPr(CustBldEn_p(S)); const real_T FBldEn = *mxGetPr(FBldEn_p(S)); const real_T CustBldNm = *mxGetPr(CustBldNm_p(S)); const real_T FracBldNm = *mxGetPr(FracBldNm_p(S)); /* vector & array data */ const real_T *Y_C_Map_NcVec = mxGetPr(Y_C_Map_NcVec_p(S)); const real_T *X_C_RlineVec = mxGetPr(X_C_RlineVec_p(S)); const real_T *Z_C_AlphaVec = mxGetPr(Z_C_AlphaVec_p(S)); const real_T *T_C_Map_WcArray = mxGetPr(T_C_Map_WcArray_p(S)); const real_T *T_C_Map_PRArray = mxGetPr(T_C_Map_PRArray_p(S)); const real_T *T_C_Map_EffArray = mxGetPr(T_C_Map_EffArray_p(S)); const real_T *FracCusBldht = mxGetPr(FracCusBldht_p(S)); const real_T *FracCusBldPt = mxGetPr(FracCusBldPt_p(S)); const real_T *FracBldht = mxGetPr(FracBldht_p(S)); const real_T *FracBldPt = mxGetPr(FracBldPt_p(S)); const real_T *X_C_Map_WcSurgeVec = mxGetPr(X_C_Map_WcSurgeVec_p(S)); const real_T *T_C_Map_PRSurgeVec = mxGetPr(T_C_Map_PRSurgeVec_p(S)); /*------get dimensions of parameter arrays-------*/ const int_T A = mxGetNumberOfElements(Y_C_Map_NcVec_p(S)); const int_T B = mxGetNumberOfElements(X_C_RlineVec_p(S)); const int_T C = mxGetNumberOfElements(Z_C_AlphaVec_p(S)); const int_T D = mxGetNumberOfElements(X_C_Map_WcSurgeVec_p(S)); const int_T WcMapCol = *mxGetPr(WcMapCol_p(S)); const int_T PRMapCol = *mxGetPr(PRMapCol_p(S)); const int_T EffMapCol = *mxGetPr(EffMapCol_p(S)); const int_T WcMapRw = *mxGetPr(WcMapRw_p(S)); const int_T PRMapRw = *mxGetPr(PRMapRw_p(S)); const int_T EffMapRw = *mxGetPr(EffMapRw_p(S)); const int_T WcMapLay = *mxGetPr(WcMapLay_p(S)); const int_T PRMapLay = *mxGetPr(PRMapLay_p(S)); const int_T EffMapLay = *mxGetPr(EffMapLay_p(S)); /*---------Define Inputs for input port 1--------*/ const real_T *u = (const real_T*) ssGetInputPortSignal(S,0); double WIn = u[0]; /* Input Flow [pps] */ double htIn = u[1]; /* Input Enthalpy [BTU/lbm] */ double TtIn = u[2]; /* Temperature Input [degR] */ double PtIn = u[3]; /* Pressure Input [psia] */ double FARcIn = u[4]; /* Combusted Fuel to Air Ratio [frac] */ double Nmech = u[5]; /* Mechancial Shaft Speed [rpm] */ double Rline = u[6]; /* Rline [NA] */ double Alpha = u[7]; /* Alpha [NA] */ double s_C_Nc = u[8]; /* Nc map scalar [NA] */ double s_C_Wc = u[9]; /* Wc map scalar [NA] */ double s_C_PR = u[10]; /* PR map scalar [NA] */ double s_C_Eff = u[11]; /* Eff map scalar [NA] */ /*---------Define Inputs for input port 2--------*/ const real_T *Wcust = ssGetInputPortRealSignal(S, 1); int uWidth1 = CustBldNm; /*---------Define Inputs for input port 3--------*/ const real_T *FracWbld = ssGetInputPortSignal(S,2); int uWidth2 = FracBldNm; real_T *y = (real_T *)ssGetOutputPortRealSignal(S,0); /* Output Array port 1 */ real_T *y1 = (real_T *)ssGetOutputPortRealSignal(S,1); /* Output Array port 2 */ real_T *y2 = (real_T *)ssGetOutputPortRealSignal(S,2); /* Output Array port 3 */ /*--------Define Constants-------*/ double WOut, htOut, TtOut, PtOut, FARcOut, TorqueOut, NErrorOut; double C_Nc, C_Wc, C_PR, C_Eff; double htin, Sin, Wcin, WcCalcin, WcMap, theta,delta, Pwrout, Wbleeds, Wsumbleed; double TtIdealout, htIdealout, Test, Sout, NcMap, Nc, PRMap, PR, EffMap, Eff; double Wb4bleed, Pwrb4bleed, PwrBld; double SPR, SPRMap, SMavail, SMMap; /* Define Arrays for bleed calcs */ int MaxNumberBleeds = 100; double WcustOut[500]; double PtcustOut[500]; double TtcustOut[500]; double FARcustOut[500]; double WbldOut[500]; double FARbldOut[500]; double PtbldOut[500]; double TtbldOut[500]; double htbldOut[500]; double htcustOut[500]; double SMWcVec[500]; double SMPRVec[500]; int interpErr = 0; int i; /* ------- get strings -------------- */ char * BlkNm; int_T buflen; int_T status; /* Get name of block from dialog parameter (string) */ buflen = mxGetN(BN_p(S))*sizeof(mxChar)+1; BlkNm = mxMalloc(buflen); status = mxGetString(BN_p(S), BlkNm, buflen); /*-- Compute output Fuel to Air Ratio ---*/ FARcOut = FARcIn; /*-- Compute Input enthalpy --------*/ htin = t2hc(TtIn,FARcIn); /*-- Compute Input entropy --------*/ Sin = pt2sc(PtIn,TtIn,FARcIn); /*---- calculate misc. fluid condition related variables and corrected Flow --*/ delta = PtIn / C_PSTD; theta = TtIn / C_TSTD; Wcin = WIn*sqrtT(theta)*divby(delta); /*------ Calculate corrected speed ---------*/ Nc = Nmech*divby(sqrtT(theta)); if (IDes < 0.5) C_Nc = Nc *divby(NcDes) ; else C_Nc = s_C_Nc; NcMap = Nc *divby(C_Nc); /*-- Compute Total Flow input (from Compressor map) --------*/ if(C > 1) WcMap = interp3Ac(X_C_RlineVec,Y_C_Map_NcVec,Z_C_AlphaVec,T_C_Map_WcArray,Rline,NcMap,Alpha,B,A,C,&interpErr); else WcMap = interp2Ac(X_C_RlineVec,Y_C_Map_NcVec,T_C_Map_WcArray,Rline,NcMap,B,A,&interpErr); if ((WcMapCol != B || WcMapRw != A || WcMapLay !=C) && ssGetIWork(S)[Er1]==0){ printf("Warning in %s, Error calculating WcMap. Table size does not match axis vector lengths.\n", BlkNm); ssSetIWorkValue(S,Er1,1); } else if (interpErr == 1 && ssGetIWork(S)[Er1]==0){ printf("Warning in %s, Error calculating WcMap. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,Er1,1); } if (IDes < 0.5) C_Wc = Wcin*divby(WcMap); else C_Wc = s_C_Wc; WcCalcin = WcMap * C_Wc; /*-- Compute Pressure Ratio (from Compressor map) --------*/ if(C > 1) PRMap = interp3Ac(X_C_RlineVec,Y_C_Map_NcVec,Z_C_AlphaVec,T_C_Map_PRArray,Rline,NcMap,Alpha,B,A,C,&interpErr); else PRMap = interp2Ac(X_C_RlineVec,Y_C_Map_NcVec,T_C_Map_PRArray,Rline,NcMap,B,A,&interpErr); if ((PRMapCol != B || PRMapRw != A || PRMapLay !=C) && ssGetIWork(S)[Er2]==0){ printf("Warning in %s, Error calculating PRMap. Table size does not match axis vector lengths.\n", BlkNm); ssSetIWorkValue(S,Er2,1); } else if (interpErr == 1 && ssGetIWork(S)[Er2]==0){ printf("Warning in %s, Error calculating PRMap. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,Er2,1); } if (IDes < 0.5) C_PR = (PRDes -1)*divby(PRMap-1); else C_PR = s_C_PR; PR = C_PR*(PRMap - 1) + 1 ; /*-- Compute Efficiency (from Compressor map) ---*/ if(C > 1) EffMap = interp3Ac(X_C_RlineVec,Y_C_Map_NcVec,Z_C_AlphaVec,T_C_Map_EffArray,Rline,NcMap,Alpha,B,A,C,&interpErr); else EffMap = interp2Ac(X_C_RlineVec,Y_C_Map_NcVec,T_C_Map_EffArray,Rline,NcMap,B,A,&interpErr); if ((EffMapCol != B || EffMapRw != A || EffMapLay !=C) && ssGetIWork(S)[Er3]==0){ printf("Warning in %s, Error calculating EffMap. Table size does not match axis vector lengths.\n", BlkNm); ssSetIWorkValue(S,Er3,1); } else if (interpErr == 1 && ssGetIWork(S)[Er3]==0){ printf("Warning in %s, Error calculating EffMap. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,Er3,1); } if (IDes < 0.5) C_Eff = EffDes*divby(EffMap); else C_Eff = s_C_Eff; Eff = EffMap * C_Eff; /*------ Compute pressure output --------*/ PtOut = PtIn*PR; /*------ enthalpy calculations ---------*/ /* ---- Ideal enthalpy ----*/ Sout = Sin; TtIdealout = sp2tc(Sout,PtOut,FARcIn); htIdealout = t2hc(TtIdealout,FARcIn); /* ---- Final enthalpy output ----*/ htOut = ((htIdealout - htin)*divby(Eff)) + htin; /*------ Compute Temperature output ---------*/ TtOut = h2tc(htOut,FARcIn); /* initalize Bleed sums components */ Wbleeds = 0; PwrBld = 0; /* compute customer Bleed components */ for (i = 0; i < uWidth1; i++) { /* if customer bleed = 0 or Cust bld is not enabled set outputs to zero */ if (Wcust[i] == 0 || CustBldEn < 0.5){ WcustOut[i] = 0; htcustOut[i] = 0; TtcustOut[i] = 0; PtcustOut[i] = 0; FARcustOut[i] = 0; } else { /*-- Compute sum of customer Bleed Flow output --------*/ Wbleeds = Wbleeds + Wcust[i]; /* add to total bleed value */ WcustOut[i] = Wcust[i]; FARcustOut[i] = FARcIn; htcustOut[i] = htin + FracCusBldht[i]*(htOut - htin); /* calculate customer bleed enthalpy */ PtcustOut[i] = PtIn + FracCusBldPt[i]*(PtOut -PtIn); /* calculate customer bleed Total Pressure */ TtcustOut[i] = h2tc(htcustOut[i],FARcustOut[i]); /* calculate customer bleed Total Temp */ PwrBld = PwrBld + WcustOut[i]*(htcustOut[i]-htOut)*C_BTU_PER_SECtoHP; /* calculate customer bleed power */ } if (i > 4*MaxNumberBleeds && ssGetIWork(S)[Er4]==0){ printf("Error in %s, Number of bleeds in compressor exceeds 100... Array overflow! Reading Bad Data\n", BlkNm); ssSetIWorkValue(S,Er4,1); } } /*----Disable Fractional bleed when requested----*/ for (i = 0; i < uWidth2; i++) { if (FracWbld[i] <= 0 || FBldEn < 0.5 ){ WbldOut[i] = 0; htbldOut[i] = 0; FARbldOut[i] = 0; TtbldOut[i] = 0; PtbldOut[i] = 0; } else { /*-- Compute sum of Fractional Bleed Flow output --------*/ Wbleeds = Wbleeds + FracWbld[i]*WIn; /* add to total bleed value */ WbldOut[i] = FracWbld[i]*WIn; FARbldOut[i] = FARcIn; PtbldOut[i] = PtIn + FracBldPt[i]*(PtOut -PtIn); /* calculate bleed Total Pressure */ htbldOut[i] = htin + FracBldht[i]*(htOut - htin); /* calculate bleed enthalpy */ TtbldOut[i] = h2tc(htbldOut[i],FARbldOut[i]); /* calculate bleed Total Temp */ PwrBld = PwrBld + WbldOut[i]*(htbldOut[i]-htOut)*C_BTU_PER_SECtoHP; /* calculate bleed power */ } if (i > 4*MaxNumberBleeds && ssGetIWork(S)[Er4]==0){ printf("Error in %s, Number of bleeds in compressor exceeds 100... Array overflow! Reading Bad Data\n", BlkNm); ssSetIWorkValue(S,Er4,1); } } /*-- Compute Flows --------*/ Wb4bleed = WIn; WOut = WIn - Wbleeds; /*------ Compute Powers ---------*/ Pwrb4bleed = Wb4bleed * (htin - htOut) * C_BTU_PER_SECtoHP; Pwrout = Pwrb4bleed - PwrBld; /*----- Compute output Torque to shaft ----*/ TorqueOut = C_HP_PER_RPMtoFT_LBF * Pwrout*divby(Nmech); /* ----- Compute Normalized Flow Error ----- */ if (IDes < 0.5 && Rline == 0) NErrorOut = 100; else if (IDes < 0.5) NErrorOut = (Rline - RlineDes)*divby(Rline); else if (WIn == 0) NErrorOut = 100; else NErrorOut = (Wcin - WcCalcin)*divby(Wcin); /* Compute Stall Margin */ if (C > 1){ /* Define 1-D surge margin vectors based on alpha */ for (i = 0; i < D/C; i++){ SMWcVec[i] = interp1Ac(Z_C_AlphaVec, X_C_Map_WcSurgeVec + C*i, Alpha,C, &interpErr); if (interpErr == 1 && ssGetIWork(S)[Er5]==0){ printf("Warning in %s, Error calculating 1D SMWcVec. Vector definitions may need to be adjusted.\n", BlkNm); ssSetIWorkValue(S,Er5,1); } SMPRVec[i] = interp1Ac(Z_C_AlphaVec, T_C_Map_PRSurgeVec + C*i, Alpha,C, &interpErr); if (interpErr == 1 && ssGetIWork(S)[Er5]==0){ printf("Warning in %s, Error calculating 1D SMPRVec. Vector definitions may need to be adjusted.\n", BlkNm); ssSetIWorkValue(S,Er5,1); } } SPRMap = interp1Ac(SMWcVec, SMPRVec,WcMap,A,&interpErr); if (interpErr == 1 && ssGetIWork(S)[Er5]==0){ printf("Warning in %s, Error calculating 2D SPR. Vector definitions may need to be adjusted.\n", BlkNm); ssSetIWorkValue(S,Er5,1); } } else SPRMap = interp1Ac(X_C_Map_WcSurgeVec,T_C_Map_PRSurgeVec,WcMap,D,&interpErr); if (interpErr == 1 && ssGetIWork(S)[Er5]==0){ printf("Warning in %s, Error calculating SPR. Vector definitions may need to be expanded.\n", BlkNm); ssSetIWorkValue(S,Er5,1); } SPR = C_PR*(SPRMap - 1) + 1; SMavail = (SPR - PR)*divby(PR) * 100; SMMap = (SPRMap - PRMap)*divby(PRMap) * 100; /* Test variable */ Test = SPRMap; /*------Assign output values port1------------*/ y[0] = WOut; /* Outlet Total Flow [pps] */ y[1] = htOut; /* Output Enthalpy [BTU/lbm] */ y[2] = TtOut; /* Outlet Temperature [degR] */ y[3] = PtOut; /* Outlet Pressure [psia] */ y[4] = FARcOut; /* Exit Combusted Fuel Flow [frac] */ y[5] = TorqueOut; /* Outlet Torque [lbf*ft] */ y[6] = NErrorOut; /* Normalized compressor Error [frac]*/ y[7] = SMavail; /* Available Stall Margin [%] */ y[8] = C_Nc; /* Corrected shaft speed scalar */ y[9] = C_Wc; /* Corrected flow scalar */ y[10] = C_PR; /* Pressure Ratio scalar */ y[11] = C_Eff; /* Efficiency scalar */ y[12] = Wcin; /* Corrected input flow [pps] */ y[13] = Nc; /* Corrected speed [rpm]*/ y[14] = PR; /* Pressure ratio */ y[15] = NcMap; /* Map corrected speed */ y[16] = WcMap; /* Map corrected flow */ y[17] = PRMap; /* Map pressure ratio */ y[18] = EffMap; /* Map efficiency */ y[19] = SPR; /* Surge pressure ratio */ y[20] = Wbleeds; /* Bleed flow [pps]*/ y[21] = Pwrb4bleed; /* Power if there was no bleed [hp]*/ y[22] = PwrBld; /* Power loss due to bleed [hp] */ y[23] = Pwrout; /* Output power [hp]*/ y[24] = SMMap; /* Stall margin calculated from map values [%]*/ y[25] = SPRMap; /* Map stall pressure ratio*/ y[26] = Test; /* test signal */ /*------Assign output values port2------------*/ /* Customer or flow based bleed*/ for (i = 0; i < uWidth1; i++) { *y1++ = WcustOut[i]; *y1++ = htcustOut[i]; *y1++ = TtcustOut[i]; *y1++ = PtcustOut[i]; *y1++ = FARcustOut[i]; } /*------Assign output values port3------------*/ /* fractional bleed, typically used for turbine cooling flow */ for (i = 0; i < uWidth2; i++) { *y2++ = WbldOut[i]; *y2++ = htbldOut[i]; *y2++ = TtbldOut[i]; *y2++ = PtbldOut[i]; *y2++ = FARbldOut[i]; } }
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 }