/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
//Initialize the states before Init method is called
real_T *x2 = ssGetRealDiscStates(S);
x2[0] = u_fps;
x2[1] = v_fps;
x2[2] = w_fps;
x2[3] = p_radsec;
x2[4] = q_radsec;
x2[5] = r_radsec;
x2[6] = h_sl_ft;
x2[7] = long_gc_deg;
x2[8] = lat_gc_deg;
x2[9] = phi_rad;
x2[10] = theta_rad;
x2[11] = psi_rad;
//x[12] = alpha_rad;
//x[13] = beta_rad;
/* 8/25/10 Code added to support FlightViz visualization */
mexEvalString("do_flightvue");
char v_buf[128];
mwSize v_buflen;
v_buflen = mxGetNumberOfElements(verbosity) + 1;
mxGetString(verbosity, v_buf, v_buflen);//v_buf contains the verbosity char string
int is_debug = strcmp(v_buf,"debug");
if(is_debug == 0){
mexPrintf("\nMDL Start.\n");
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* Issue ssCallSystemWithTid on 1st or 2nd output element of 1st output port
* and then update 2nd output port with the state.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *x = ssGetRealDiscStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,1);
/*
* ssCallSystemWithTid is used to execute a function-call subsystem. The
* 2nd argument is the element of the 1st output port index which
* connected to the function-call subsystem. Function-call subsystems
* can be driven by the first output port of s-function blocks.
*/
UNUSED_ARG(tid); /* not used in single tasking mode */
if (((int)*uPtrs[0]) % 2 == 1) {
if (!ssCallSystemWithTid(S,0,tid)) {
/* Error occurred which will be reported by Simulink */
return;
}
} else {
if (!ssCallSystemWithTid(S,1,tid)) {
/* Error occurred which will be reported by Simulink */
return;
}
}
y[0] = x[0];
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* Initialize the states
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *xD = ssGetRealDiscStates(S);
xD[0] = 0;
}
static void mdlUpdate(SimStruct *S, int_T tid)
{
real_T *x = ssGetRealDiscStates(S);
UNUSED_ARG(tid); /* not used in single tasking mode */
x[0]++;
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* Initialize both discrete states to one.
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *x0 = ssGetRealDiscStates(S);
int_T lp;
for (lp=0;lp<2;lp++) {
*x0++=1.0;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetRealDiscStates(S);
// InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid); /* not used in single tasking mode */
y[0]=x[0];
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetRealDiscStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid); /* not used in single tasking mode */
/* y=Cx+Du */
y[0]=C[0][0]*x[0]+C[0][1]*x[1]+D[0][0]*U(0)+D[0][1]*U(1);
y[1]=C[1][0]*x[0]+C[1][1]*x[1]+D[1][0]*U(0)+D[1][1]*U(1);
}
/* 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)
{
/* send update inputs to JSBSimInterface, run one cycle,
retrieve state vector, and update sfunction discrete state vector
*/
//mexPrintf("Before JII pointer object creation.\n");
JSBSimInterface *JII = (JSBSimInterface *) ssGetPWork(S)[0]; // retrieve C++ object pointers vector
//mexPrintf("After JII pointer creation.\n");
real_T *x2 = ssGetRealDiscStates(S);
//real_T *dx = ssGetdX(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
double *derivatives = (double *) ssGetDWork(S,2);
double *inputs = (double *) ssGetDWork(S,0);
double *states = (double *) ssGetDWork(S,1);
double *controls = (double *) ssGetDWork(S,3);
double *propulsion = (double *) ssGetDWork(S,4);
double *outputs = (double *) ssGetDWork(S,5);
double *aero = (double *) ssGetDWork(S,6);
int k;
for (k=0; k < ssGetDWorkWidth(S,0); k++) {
inputs[k] = (*uPtrs[k]);
}
/*
for (k=0; k < ssGetDWorkWidth(S,1); k++) {
states[k] = x2[k];
}
*/
JII->UpdateStates(inputs, derivatives, states, controls, propulsion, outputs, aero); // call to JSBSimInterface to get updated states from JSBSim
//mexPrintf("After JII->UpdateStates.\n");
for (k=0; k < ssGetDWorkWidth(S,1); k++) {
x2[k] = states[k];
}
/*
for (k=0; k < ssGetDWorkWidth(S,2); k++) {
dx[k] = derivatives[k];
}*/
char v_buf[128];
mwSize v_buflen;
v_buflen = mxGetNumberOfElements(verbosity) + 1;
mxGetString(verbosity, v_buf, v_buflen);//v_buf contains the verbosity char string
int is_debug = strcmp(v_buf,"debug");
if(is_debug == 0){
mexPrintf("\nMDL Update.\n");
//UNUSED_ARG(tid);
}
}
/* Function: mdlUpdate ======================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
real_T tempX[2] = {0.0, 0.0};
real_T *x = ssGetRealDiscStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid); /* not used in single tasking mode */
/* xdot=Ax+Bu */
tempX[0]=A[0][0]*x[0]+A[0][1]*x[1]+B[0][0]*U(0)+B[0][1]*U(1);
tempX[1]=A[1][0]*x[0]+A[1][1]*x[1]+B[1][0]*U(0)+B[1][1]*U(1);
x[0]=tempX[0];
x[1]=tempX[1];
}
/* Function: mdlUpdate ======================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
// real_T tempX[2] = {0.0, 0.0};
real_T *x = ssGetRealDiscStates(S);
// InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
BOOL out_qpf;
LARGE_INTEGER tmp_qpf;
LONGLONG ticsTarget;
LONGLONG tmpTicsPeriode;
UNUSED_ARG(tid); /* not used in single tasking mode */
/*
x[0]=x[0]+U(0);
x[1]=x[1]+U(1);
*/
if (primeraIteracio)
{
primeraIteracio=0;
out_qpf = QueryPerformanceCounter( &tmp_qpf);
ticsAnteriors=tmp_qpf.QuadPart;
}
tmpTicsPeriode=ticsPerSegon/((LONGLONG)(1.0/sampleTime));
ticsTarget=ticsAnteriors+tmpTicsPeriode;
out_qpf = QueryPerformanceCounter( &tmp_qpf);
//if (tmp_qpf.QuadPart>ticsTarget)
// x[0]=101;
//else
x[0]=100-((ticsTarget-tmp_qpf.QuadPart)*100)/tmpTicsPeriode;
do
{
out_qpf = QueryPerformanceCounter( &tmp_qpf);
} while(tmp_qpf.QuadPart<ticsTarget);
ticsAnteriors=ticsTarget;
//printf("Contador: %08X",(ticsTarget>>32)&0x0FFFFFFFF);
//printf("%08X\n",ticsTarget&0x0FFFFFFFF);
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* In this function, you should initialize the continuous and discrete
* states for your S-function block. The initial states are placed
* in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
* You can also perform any other initialization activities that your
* S-function may require. Note, this routine will be called at the
* start of simulation and if it is present in an enabled subsystem
* configured to reset states, it will be call when the enabled subsystem
* restarts execution to reset the states.
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *xC = ssGetContStates(S);
real_T *xD = ssGetRealDiscStates(S);
int i;
i = mxGetNumberOfElements(Mx0(S));
if (i!= (int) (B(S,"CStates")+B(S,"DStates")))
{
ssSetErrorStatus(S,"Number of states and number of IC do not match" );
}
for (i=0; i<(int)B(S,"CStates"); i++)
{
xC[i]=x0(S)[i];
}
for (i=0; i<(int)B(S,"DStates"); i++)
{
xD[i]=x0(S)[(int) (B(S,"CStates"))+i];
}
}
/* 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 *dx = ssGetdX(S);
//real_T *x = ssGetContStates(S);
real_T *x2 = ssGetRealDiscStates(S);
real_T *y1 = ssGetOutputPortRealSignal(S, 0);//states
real_T *y2 = ssGetOutputPortRealSignal(S, 1);//flight controls
real_T *y3 = ssGetOutputPortRealSignal(S, 2);//propulsion
real_T *y4 = ssGetOutputPortRealSignal(S, 3);//calculated outputs
real_T *y5 = ssGetOutputPortRealSignal(S, 4);//state derivatives
real_T *y6 = ssGetOutputPortRealSignal(S, 5);//aerodynamic outputs
double *w2 = (double *) ssGetDWork(S,2);//get the derivatives work vector
double *w3 = (double *) ssGetDWork(S,3);//get the controls work vector
double *w4 = (double *) ssGetDWork(S,4);//get the propulsion work vector
double *w5 = (double *) ssGetDWork(S,5);//get the calculated outputs work vector
double *w6 = (double *) ssGetDWork(S,6);//get the aero outputs work vector
int i;
/*
for (i = 0; i < ssGetNumContStates(S); i++)
{
y1[i] = x[i]; // outputs are the states
}
*/
for (i = 0; i < ssGetNumDiscStates(S); i++)
{
y1[i] = x2[i]; /* outputs are the states */
}
for (i = 0; i < ssGetDWorkWidth(S,3); i++)
{
y2[i] = w3[i]; // outputs are the flight control outputs
}
for (i = 0; i < ssGetDWorkWidth(S,4); i++)
{
y3[i] = w4[i]; // outputs are the propulsion outputs
}
for (i = 0; i < ssGetDWorkWidth(S,5); i++)
{
y4[i] = w5[i]; // outputs are the calculated outputs
}
for (i = 0; i < ssGetDWorkWidth(S,2); i++)
{
y5[i] = w2[i]; // outputs are the state derivatives
}
for (i = 0; i < ssGetDWorkWidth(S,6); i++)
{
y6[i] = w6[i]; // outputs are the aero outputs
}
/* *************************************************************************************
Code to support FlightViz visualization
If enabled, a call to flightviz.m will be made every time MDL_OUTPUTS is executed. The call will need to be made using the mexCallMATLAB
method as flightviz.m is an m-language function.
***************************************************************************************
*/
if(fv_sim){
loop_fv++;
if(loop_fv == 12){
//double v_euler[3];
//double v_vel[3];
double phi, theta, psi, vel_x, vel_y, vel_z, mach, acc, alt_m, el, rud, ail, flp;
/*
v_euler[0] = x2[9];
v_euler[1] = x2[10];
v_euler[2] = x2[11];
v_vel[0] = y1[0];
v_vel[1] = y1[1];
v_vel[2] = y1[2];
alt_m = y1[6] * 3.28;
//rhs[2] = x2[1];
//fill_array(rhs[2], w3[1]);
mach = w5[9];
acc = w5[0];
el = y2[2] * -1;
ail = y2[1];
rud = y2[4];
flp = y2[5];
*/
phi = x2[9];
theta = x2[10];
psi = x2[11];
vel_x = x2[0];
vel_y = x2[1];
vel_z = x2[2];
alt_m = x2[6];
//rhs[2] = x2[1];
//fill_array(rhs[2], w3[1]);
mach = w5[9];
acc = w5[0];
el = y2[2] * -1;
ail = y2[1];
rud = y2[4];
flp = y2[5];
//const char *field_names[] = {"euler", "vel", "alt", "mach", "acc", "elevator_fv", "aileron_fv", "rudder_fv", "flap_fv"};
//struct flightviz_states fvs[] = {{v_euler[3], v_vel[3], alt_m, mach, acc, el, ail, rud, flp}};
const char *field_names[] = {"phi", "theta", "psi", "vel_x", "vel_y", "vel_z", "alt", "mach", "acc", "elevator_fv", "aileron_fv", "rudder_fv", "flap_fv"};
struct flightviz_states fvs[] = {phi, theta, psi, vel_x, vel_y, vel_z, alt_m, mach, acc, el, ail, rud, flp};
mwSize dims[2] = {1,1};
mxArray *rhs[1];
rhs[0] = mxCreateStructArray(2,dims,13, field_names);
//int euler_field, vel_field, alt_field, mach_field, acc_field, el_field, rud_field, ail_field, flp_field;
int phi_field, theta_field, psi_field, vel_x_field, vel_y_field, vel_z_field, alt_field, mach_field, acc_field, el_field, ail_field, rud_field, flp_field;
//euler_field = mxGetFieldNumber(rhs[0],"euler");
//vel_field = mxGetFieldNumber(rhs[0],"vel");
phi_field = mxGetFieldNumber(rhs[0],"phi");
theta_field = mxGetFieldNumber(rhs[0],"theta");
psi_field = mxGetFieldNumber(rhs[0],"psi");
vel_x_field = mxGetFieldNumber(rhs[0],"vel_x");
vel_y_field = mxGetFieldNumber(rhs[0],"vel_y");
vel_z_field = mxGetFieldNumber(rhs[0],"vel_z");
alt_field = mxGetFieldNumber(rhs[0],"alt");
mach_field = mxGetFieldNumber(rhs[0],"mach");
acc_field = mxGetFieldNumber(rhs[0],"acc");
el_field = mxGetFieldNumber(rhs[0],"elevator_fv");
ail_field = mxGetFieldNumber(rhs[0],"aileron_fv");
rud_field = mxGetFieldNumber(rhs[0],"rudder_fv");
flp_field = mxGetFieldNumber(rhs[0],"flap_fv");
//mxArray *euler_field_value, *vel_field_value, *alt_field_value, *mach_field_value, *acc_field_value, *el_field_value, *rud_field_value, *ail_field_value, *flp_field_value;
mxArray *phi_field_value, *theta_field_value, *psi_field_value, *vel_x_field_value, *vel_y_field_value, *vel_z_field_value;
mxArray *alt_field_value, *mach_field_value, *acc_field_value, *el_field_value, *ail_field_value, *rud_field_value, *flp_field_value;
/* Use mxSetFieldByNumber instead of mxSetField for efficiency
mxSetField(plhs[0],i,"name",mxCreateString(friends[i].name); */
//mxSetFieldByNumber(prhs[0],i,name_field,mxCreateString(ic[i].name));
//euler_field_value = mxCreateDoubleMatrix(1,3,mxREAL);
//vel_field_value = mxCreateDoubleMatrix(1,3,mxREAL);
phi_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
theta_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
psi_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
vel_x_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
vel_y_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
vel_z_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
alt_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
mach_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
acc_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
el_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
ail_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
rud_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
flp_field_value = mxCreateDoubleMatrix(1,1,mxREAL);
//*mxGetPr(euler_field_value) = fvs[0].euler[3];
//mxSetField(rhs[0],0,"euler",euler_field_value);
//*mxGetPr(vel_field_value) = fvs[0].vel[3];
//mxSetFieldByNumber(rhs[0],0,vel_field,vel_field_value);
*mxGetPr(phi_field_value) = fvs[0].phi;
mxSetFieldByNumber(rhs[0],0,phi_field,phi_field_value);
*mxGetPr(theta_field_value) = fvs[0].theta;
mxSetFieldByNumber(rhs[0],0,theta_field,theta_field_value);
*mxGetPr(psi_field_value) = fvs[0].psi;
mxSetFieldByNumber(rhs[0],0,psi_field,psi_field_value);
*mxGetPr(vel_x_field_value) = fvs[0].vel_x;
mxSetFieldByNumber(rhs[0],0,vel_x_field,vel_x_field_value);
*mxGetPr(vel_y_field_value) = fvs[0].vel_y;
mxSetFieldByNumber(rhs[0],0,vel_y_field,vel_y_field_value);
*mxGetPr(vel_z_field_value) = fvs[0].vel_z;
mxSetFieldByNumber(rhs[0],0,vel_z_field,vel_z_field_value);
*mxGetPr(alt_field_value) = fvs[0].alt;
mxSetFieldByNumber(rhs[0],0,alt_field,alt_field_value);
*mxGetPr(mach_field_value) = fvs[0].mach;
mxSetFieldByNumber(rhs[0],0,mach_field,mach_field_value);
*mxGetPr(acc_field_value) = fvs[0].acc;
mxSetFieldByNumber(rhs[0],0,acc_field,acc_field_value);
*mxGetPr(el_field_value) = fvs[0].elevator_fv;
mxSetFieldByNumber(rhs[0],0,el_field,el_field_value);
*mxGetPr(ail_field_value) = fvs[0].aileron_fv;
mxSetFieldByNumber(rhs[0],0,ail_field,ail_field_value);
*mxGetPr(rud_field_value) = fvs[0].rudder_fv;
mxSetFieldByNumber(rhs[0],0,rud_field,rud_field_value);
*mxGetPr(flp_field_value) = fvs[0].flap_fv;
mxSetFieldByNumber(rhs[0],0,flp_field,flp_field_value);
/* Use mxSetFieldByNumber instead of mxSetField for efficiency
mxSetField(plhs[0],i,"name",mxCreateString(friends[i].name); */
mexCallMATLAB(0,NULL,1,&rhs[0],"flightviz");
mxDestroyArray(rhs[0]);
loop_fv = 0;
}
}
char v_buf[128];
mwSize v_buflen;
v_buflen = mxGetNumberOfElements(verbosity) + 1;
mxGetString(verbosity, v_buf, v_buflen);//v_buf contains the verbosity char string
int is_debug = strcmp(v_buf,"debug");
if(is_debug == 0){
mexPrintf("\nMDL Outputs.\n");
}
}