/* 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)
{
real_T t, *xC, *xD, *uWQ, *uES;
SS=S;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[(int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
wsgetq_Update(t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlDerivatives =================================================
* Abstract:
* In this function, you compute the S-function block's derivatives.
* The derivatives are placed in the derivative vector, ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
real_T t, *xC, *xD, *uWQ, *uES, *sys;
int i;
SS=S;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
sys = ssGetdX(S);
for (i=0; i<(int)B(S,"CStates"); i++)
{
sys[i]=0.0;
}
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[(int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
wsgetq_Derivatives(sys,t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
int j;
ocp_nlp_dims *nlp_dims = (ocp_nlp_dims *) ssGetPWork(S)[0];
ocp_nlp_in *nlp_in = (ocp_nlp_in *) ssGetPWork(S)[1];
ocp_nlp_out *nlp_out = (ocp_nlp_out *) ssGetPWork(S)[2];
ocp_nlp_solver *nlp_solver = (ocp_nlp_solver *) ssGetPWork(S)[4];
ocp_nlp_constraints_bgh_model **constraints = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints;
ocp_nlp_constraints_bgh_dims **constraints_dims = (ocp_nlp_constraints_bgh_dims **) nlp_dims->constraints;
ocp_nlp_cost_nls_model **cost = (ocp_nlp_cost_nls_model **) nlp_in->cost;
const double *x0 = ssGetInputPortRealSignal(S, 0);
const double *reference = ssGetInputPortRealSignal(S, 1);
// bounds
double lb_0[] = {-10000, -10000, 50, 50, 1.14275, 1.53787};
double ub_0[] = {+10000, +10000, 50, 50, 1.14275, 1.53787};
for (j = 0; j < NUM_STATES; ++j) {
lb_0[NUM_CONTROLS+j] = x0[j];
ub_0[NUM_CONTROLS+j] = x0[j];
}
ocp_nlp_constraints_bounds_set(config, dims, nlp_in, 0, "lb", lb_0);
ocp_nlp_constraints_bounds_set(config, dims, nlp_in, 0, "ub", ub_0);
for (j = 0; j <= NUM_STAGES; ++j)
BLASFEO_DVECEL(&cost[j]->y_ref, 0) = *reference;
int status = ocp_nlp_solve(nlp_solver, nlp_in, nlp_out);
double *u0_opt = ssGetOutputPortRealSignal(S, 0);
double *x1 = ssGetOutputPortRealSignal(S, 1);
double *status_out = ssGetOutputPortRealSignal(S, 2);
double *comp_time = ssGetOutputPortRealSignal(S, 3);
blasfeo_unpack_dvec(NUM_CONTROLS, &nlp_out->ux[0], 0, u0_opt);
blasfeo_unpack_dvec(NUM_STATES, &nlp_out->ux[1], NUM_CONTROLS, x1);
*status_out = (double) status;
*comp_time = nlp_out->total_time;
}
/* 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)
{
const real_T *u = (const real_T*) ssGetInputPortRealSignal(S,0);
struct comStruc_OUT **dataOUT = (struct comStruc_OUT**) ssGetDWork(S,DVECSHMOUT);
dataOUT[0]->index_counter = (int32_t) u[0];
dataOUT[0]->sin_value = (float) u[1];
dataOUT[0]->cos_value = (float) u[2];
printf("value OUT 1 %d\n", dataOUT[0]->index_counter);
printf("value OUT 2 %f\n", dataOUT[0]->sin_value);
printf("value OUT 3 %f\n\n", dataOUT[0]->cos_value);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = f(X,U), where f(.) is the static mapping defined in eval_treepartition
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
ParStruc *Par = ssGetUserData(S);
const real_T *Reg;
UNUSED_ARG(tid); /* not used in single tasking mode */
/* Execute the nonlinearity mapping function*/
Reg = ssGetInputPortRealSignal(S,0); /* input signals are contiguous */
evaluate_pwlinear(y, Reg, 1, Par);
}
/* 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)
{
real_T t, *xC, *xD, *u;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
block0_Update(t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T t, *xC, *xD, *u, *sys;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
sys = ssGetOutputPortRealSignal(S,0);
block0_Outputs(sys,t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T t, *xC, *xD, *uWQ, *uES, *sysWQ, *sysEM;
int i;
SS=S;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
sysWQ = ssGetOutputPortRealSignal(S,0);
for (i=0;i<U(S,"Number")*B(S,"WaterIn")*B(S,"WaterOut");i++)
{
sysWQ[i] = uWQ[i];
}
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[ (int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
// verwijzing naar exra metingen
if (B(S,"Measurements")>0)
{
sysEM = &(sysWQ[(int) (U(S,"Number")*B(S,"WaterOut"))]);
for (i=0;i<B(S,"Measurements");i++)
{
sysEM[i] = 0.0;
}
}
else
{
sysEM=NULL;
}
wsgetq_Outputs(sysWQ,sysEM,t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlDerivatives =================================================
* Abstract:
* In this function, you compute the S-function block's derivatives.
* The derivatives are placed in the derivative vector, ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
real_T t, *xC, *xD, *u, *sys;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
sys = ssGetdX(S);
buffer_Derivatives(sys,t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
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 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;
}
/* Function: mdlJacobian ======================================================
* Abstract: populate the model's Jacobian data.
* See the on-line documentation for mxCreateSparse for
* information regarding the format of Ir, Jc, and Pr data.
*
* [ A | B ]
* J = [ --+-- ] (= D here)
* [ C | D ]
*
*/
static void mdlJacobian(SimStruct *S)
{
#if defined(MATLAB_MEX_FILE)
real_T *Pr = ssGetJacobianPr(S);
real_T *out;
real_T *temp;
uint_T nr = (uint_T) mxGetScalar(NUMREG(S));
ParStruc *Par = ssGetUserData(S);
const real_T *Reg = ssGetInputPortRealSignal(S,0); /* input signals are contiguous */
int_T k;
mxArray *plhs;
mxArray *prhs[3];
mxArray *ParStruct, *TreeParStruct;
mxArray *TypeStr;
const char **fnamesPar;
const char **fnamesTree;
/* plhs = mxCreateDoubleMatrix(1,nr,mxREAL); */
TypeStr = mxCreateString("treepartition");
fnamesPar = mxCalloc(8, sizeof(*fnamesPar));
fnamesTree = mxCalloc(5, sizeof(*fnamesTree));
/* memory error check */
if ( (fnamesPar==NULL) || (fnamesTree==NULL) || (TypeStr==NULL) || (prhs==NULL)){
ssSetErrorStatus(S, "Could not allocate memory for Jacobian computation.");
return;
}
/* Tree struct field names */
fnamesTree[0] = "TreeLevelPntr";
fnamesTree[1] = "AncestorDescendantPntr";
fnamesTree[2] = "LocalizingVectors";
fnamesTree[3] = "LocalCovMatrix";
fnamesTree[4] = "LocalParVector";
/* Parameter struct field names */
fnamesPar[0] = "NumberOfUnits";
fnamesPar[1] = "Threshold";
fnamesPar[2] = "RegressorMean";
fnamesPar[3] = "OutputOffset";
fnamesPar[4] = "LinearCoef";
fnamesPar[5] = "SampleLength";
fnamesPar[6] = "NoiseVariance";
fnamesPar[7] = "Tree";
TreeParStruct = mxCreateStructMatrix(1,1,5,fnamesTree);
ParStruct = mxCreateStructMatrix(1,1,8,fnamesPar);
/* do memory error check */
if ((TreeParStruct==NULL) || (ParStruct==NULL)){
ssSetErrorStatus(S, "Could not allocate memory for Jacobian computation.");
return;
}
/* set fields of Parameters.Tree struct */
mxSetFieldByNumber(TreeParStruct, 0, 0, mxDuplicateArray(TREE_TREELEVELPNTR(S)));
mxSetFieldByNumber(TreeParStruct, 0, 1, mxDuplicateArray(TREE_ANCESTORDESCENDANTPNTR(S)));
mxSetFieldByNumber(TreeParStruct, 0, 2, mxDuplicateArray(TREE_LOCALIZINGVECTORS(S)));
mxSetFieldByNumber(TreeParStruct, 0, 3, mxDuplicateArray(TREE_LOCALCOVMATRIX(S)));
mxSetFieldByNumber(TreeParStruct, 0, 4, mxDuplicateArray(TREE_LOCALPARVECTOR(S)));
/* set fields of Paramater struct */
mxSetFieldByNumber(ParStruct, 0, 0, mxDuplicateArray(NUMUNITS(S)));
mxSetFieldByNumber(ParStruct, 0, 1, mxDuplicateArray(OPT_THRESHOLD(S)));
mxSetFieldByNumber(ParStruct, 0, 2, mxDuplicateArray(PAR_REGRESSORMEAN(S)));
mxSetFieldByNumber(ParStruct, 0, 3, mxDuplicateArray(PAR_OUTPUTOFFSET(S)));
mxSetFieldByNumber(ParStruct, 0, 4, mxDuplicateArray(PAR_LINEARCOEF(S)));
mxSetFieldByNumber(ParStruct, 0, 5, mxDuplicateArray(PAR_SAMPLELENGTH(S)));
mxSetFieldByNumber(ParStruct, 0, 6, mxDuplicateArray(PAR_NOISEVARIANCE(S)));
mxSetFieldByNumber(ParStruct, 0, 7, TreeParStruct);
prhs[0] = mxCreateDoubleMatrix(1,nr,mxREAL);
temp = mxGetPr(prhs[0]);
for (k=0; k<nr; k++){
temp[k] = Reg[k];
}
/* mxSetPr(prhs[0],Reg); */
prhs[1] = ParStruct;
prhs[2] = TypeStr;
/*
* Call utEvalStateJacobian to compute the regressors
* M file: dydx = utEvalStateJacobian(x,par,type)
*/
mexCallMATLAB(1,&plhs,3,prhs,"utEvalStateJacobian");
out = mxGetPr(plhs);
for(k=0; k<nr; k++){
Pr[k] = out[k];
}
mxFree((void *)fnamesTree);
mxFree((void *)fnamesPar);
mxDestroyArray(plhs);
mxDestroyArray(prhs[0]);
mxDestroyArray(TypeStr);
mxDestroyArray(ParStruct);
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
PCONFIG_DATA config;
const real_T *in_xdot;
const real_T *in_x0;
InputRealPtrsType pin_params;
const boolean_T *in_reset;
real_T *out_x;
real_T *out_t;
real_T *xd;
real_T *xd_temp;
real_T *xd_temp2;
time_T initial_time;
time_T final_time;
quaternion phi;
quaternion q;
vector omegadot_temp;
const real_T *pomegadot;
int_T i;
/* Retrieve C object from the pointers vector */
config = ssGetPWork(S)[0];
xd = (real_T*) ssGetDWork(S,0);
xd_temp = (real_T*) ssGetDWork(S,1);
if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
out_x = ssGetOutputPortRealSignal(S, 1);
if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);
switch( intval(mxGetScalar(paramSpecificationsSource)) )
{
case 1:
initial_time = config->initial_time;
final_time = ssGetTaskTime(S,0);
break;
case 2:
initial_time = mxGetScalar(paramInitialTime);
final_time = mxGetScalar(paramFinalTime);
break;
case 3:
initial_time = *(pin_params[0]);
final_time = *(pin_params[1]);
break;
default:
ssSetErrorStatus(S,"Wrong integration algorithm selected");
return;
}
/* ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */
/* Reset the states */
if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
{
ssSetIWorkValue(S, 0, 0);
if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
{
/* Internal initial conditions */
for( i=0; i<config->nstates; i++ )
{
xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
}
}
else
{
/* External initial conditions */
memcpy(xd, in_x0, config->nstates*sizeof(real_T));
}
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = initial_time;
}
if( final_time > initial_time )
{
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
{
/* Euler algorithm */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
QuaternionProd(&xd[i], phi, &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd[i] += in_xdot[i] * (final_time-initial_time);
i++;
}
}
}
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
{
/* Runge-Kutta algorithm */
/* f1 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
QuaternionProd(&xd_temp[i], &xd_temp[i], q);
QuaternionProd(&xd_temp[i], q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] = in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f2 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f3 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(phi, phi, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = final_time;
/* f4 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd_temp[i] += in_xdot[i];
xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
i++;
}
}
}
}
else
{
if( !ssCallSystemWithTid(S,0,tid) ) return;
}
config->initial_time = final_time;
/* Update the outputs */
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = final_time;
}
static void mdlUpdate(SimStruct *S, int_T tid)
{
/*********************************
* Entradas :
* 0: Variador (16)
* 1: Valvula Solenoide (13)
* 2: Valvula Motorizada (12)
* 3: Calefactor 1 (20)
* 4: Calefactor 2 (22)
* 5: Calefactor 3 (21)
* 6: Agitador (23)
* 7: Valv. Solenoide 1 (24)
* 8: Valv. Solenoide 2 (25)
* 9: Luces Lab. (26)
*********************************/
O22SnapIoMemMap *Brain;
long nResult,tempDig;
float tempAna;
Brain = (O22SnapIoMemMap *) ssGetPWork(S)[0];
const real_T *u = ssGetInputPortRealSignal(S,0);
// Variador de Frecuencia
if( u[0]<5 )
tempAna = 4.0;
else
tempAna = 4.0 + (float)u[0]*16.0/100.0;
nResult=Brain->SetAnaPtValue(16,tempAna);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato del variador de frecuencia.");
return;
}
// Valvula Solenoide
tempAna = 4.0 + (float)u[1]*16.0/100.0;
nResult=Brain->SetAnaPtValue(13,tempAna);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato de la valvula solenoide.");
return;
}
// Valvula Motorizada
tempAna = 4.0 + (float)u[2]*16.0/100.0;
nResult=Brain->SetAnaPtValue(12,tempAna);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato de la valvula motorizada.");
return;
}
// Calefactor 1
if( (float)u[3] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(20,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato del calefactor 1.");
return;
}
// Calefactor 2
if( (float)u[4] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(22,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato del calefactor 2.");
return;
}
// Calefactor 3
if( (float)u[5] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(21,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato del calefactor 3.");
return;
}
// Agitador
if( (float)u[6] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(23,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato del agitador.");
return;
}
// Valv. Solenoide 1
if( (float)u[7] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(24,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato de la valvula solenoide 1.");
return;
}
// Valv. Solenoide 2
if( (float)u[8] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(25,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato de la valvula solenoide 2.");
return;
}
// Luces Lab.
if( (float)u[9] > 0.5 )
tempDig = 1;
else
tempDig = 0;
nResult=Brain->SetDigPtState(26,tempDig);
if ( nResult != SIOMM_OK )
{
ssSetErrorStatus(S,"Error al transmitir el dato de las luces del laboratorio.");
return;
}
}
/* Function: mdlJacobian ======================================================
* Abstract: populate the model's Jacobian data.
* See the on-line documentation for mxCreateSparse for
* information regarding the format of Ir, Jc, and Pr data.
*
* [ A | B ]
* J = [ --+-- ] (= D here)
* [ C | D ]
*
*/
static void mdlJacobian(SimStruct *S)
{
#if defined(MATLAB_MEX_FILE)
real_T *Pr = ssGetJacobianPr(S);
real_T *out;
real_T *temp;
ParStruc *Par = ssGetUserData(S);
const real_T *Reg = ssGetInputPortRealSignal(S,0); /* input signals are contiguous */
mxArray *plhs;
mxArray *prhs[3];
mxArray *ParStruct;
mxArray *TypeStr;
const char **fnamesPar;
/* plhs = mxCreateDoubleMatrix(1,nr,mxREAL); */
TypeStr = mxCreateString("pwlinear");
fnamesPar = mxCalloc(5, sizeof(*fnamesPar));
/* memory error check */
if ( (fnamesPar==NULL) || (TypeStr==NULL) || (prhs==NULL)){
ssSetErrorStatus(S, "Could not allocate memory for Jacobian computation.");
return;
}
/* Parameter struct field names */
fnamesPar[0] = "NumberOfUnits";
fnamesPar[1] = "LinearCoef";
fnamesPar[2] = "OutputCoef";
fnamesPar[3] = "OutputOffset";
fnamesPar[4] = "Translation";
ParStruct = mxCreateStructMatrix(1,1,5,fnamesPar);
/* do memory error check */
if (ParStruct==NULL) {
ssSetErrorStatus(S, "Could not allocate memory for Jacobian computation.");
return;
}
/* set fields of Paramater struct */
mxSetFieldByNumber(ParStruct, 0, 0, mxDuplicateArray(NUMUNITS(S)));
mxSetFieldByNumber(ParStruct, 0, 1, mxDuplicateArray(PAR_LINEARCOEF(S)));
mxSetFieldByNumber(ParStruct, 0, 2, mxDuplicateArray(PAR_OUTPUTCOEF(S)));
mxSetFieldByNumber(ParStruct, 0, 3, mxDuplicateArray(PAR_OUTPUTOFFSET(S)));
mxSetFieldByNumber(ParStruct, 0, 4, mxDuplicateArray(PAR_TRANSLATION(S)));
/* nr = dimension of regressor = 1 */
prhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
temp = mxGetPr(prhs[0]);
temp[0] = Reg[0];
prhs[1] = ParStruct;
prhs[2] = TypeStr;
/*
* Call utEvalStateJacobian to compute the regressors
* M file: dydx = utEvalStateJacobian(x,par,type)
*/
mexCallMATLAB(1,&plhs,3,prhs,"utEvalStateJacobian");
out = mxGetPr(plhs);
Pr[0] = out[0];
mxFree((void *)fnamesPar);
mxDestroyArray(plhs);
mxDestroyArray(prhs[0]);
mxDestroyArray(TypeStr);
mxDestroyArray(ParStruct);
#endif
}
