/* 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:
* 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)
{
#ifndef VARIABLE_STEP
/*
* For Fixed Step Code Only
* ------------------------
* If your Fortran code runs at a fixed time step that advances
* each time you call it, it is best to call it here instead of
* in mdlOutputs(). The states in the Fortran code need not be
* continuous if you call your code from here.
*/
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
float *sampleArgs = (float *) ssGetDWork(S,1);
double *y = ssGetOutputPortRealSignal(S,0);
float *sampleOutput = (float *) ssGetDWork(S,0);
int k;
/*
* If the datatype in the Fortran code is REAL
* then you have to downcast the I/O and states from
* double to float as copies before sending them
* to your code (or change the Fortran code).
*/
for (k=0; k < ssGetDWorkWidth(S,1); k++) {
sampleArgs[k] = (float) (*uPtrs[k]);
}
/* ==== Call the Fortran routine (args are pass-by-reference) */
/* nameofsub_(sampleArgs, sampleOutput ); */
/*
* If needed, convert the float outputs to the
* double (y) output array
*/
for (k=0; k < ssGetOutputPortWidth(S,0); k++) {
y[k] = (double) sampleOutput[k];
}
#endif
}
/* 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 *dx = ssGetdX(S);
double *w2 = (double *) ssGetDWork(S,2);
for (int i = 0; i < ssGetDWorkWidth(S,2); i++)
{
dx[i] = w2[i]; //
}
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 Derivatives.\n");
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. The default datatype for signals in Simulink is double,
* but you can use other intrinsic C datatypes or even custom
* datatypes if you wish. See Simulink document "Writing S-functions"
* for details on datatype topics.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
#ifdef VARIABLE_STEP
/*
* For Variable Step Code WITH CONTINUOUS STATES
* ---------------------------------------------
* For Fortran code that implements continuous states and uses
* the mdlDerivatives interface, call your Fortran code's output
* routines from here. If it alters the states, you have to
* reset the solver. Remember, in Simulink the continuous states
* must be of type double, so be prepared to copy them to float
* if your Fortran code uses REAL as the datatype for the states.
*
* ... or, NO STATES
* -----------------
* If your code has no states and you want it to execute in
* a continuous model, keep the uPtrs, sampleArgs, y, and
* sampleOutput variables and delete x, xf, and nx. Adjust
* the function call accordingly.
*/
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
float *sampleArgs = (float *) ssGetDWork(S,1);
double *y = ssGetOutputPortRealSignal(S,0);
float *sampleOutput = (float *) ssGetDWork(S,0);
double *x = ssGetContStates(S);
float *xf = (float *) ssGetDWork(S,2);
int nx = ssGetNumContStates(S);
int k;
/*
* If the datatype in the Fortran code is REAL
* then you have to downcast the I/O and states from
* double to float as copies before sending them
* to your code (or change the Fortran code).
*/
for (k=0; k < ssGetDWorkWidth(S,1); k++) {
sampleArgs[k] = (float) (*uPtrs[k]);
}
/*
* It is recommended to use a DWork vector to
* allocate the space for the float copy of
* the states (if needed).
*/
for (k=0; k < nx; k++) {
xf[k] = (float) x[k];
}
/* ==== Call the Fortran routine (args are pass-by-reference) */
/* nameofsub_(sampleArgs, xf, &nx, sampleOutput ); */
/*
* If needed, convert the float outputs to the
* double (y) output array
*/
for (k=0; k < ssGetOutputPortWidth(S,0); k++) {
y[k] = (double) sampleOutput[k];
}
#else
/*
* For Fixed Step Code
* -------------------
* If the Fortran code implements discrete states (implicitly or
* registered with Simulink, it doesn't matter), call the code
* from mdlUpdates() and save the output values in a DWork vector.
* The variable step solver may call mdlOutputs() several
* times in between calls to mdlUpdate, and you must extract the
* values from the DWork vector and copy them to the block output
* variables.
*
* Be sure that the ssSetDWorkDataType(S,0) declaration in
* mdlInitializeSizes() uses SS_DOUBLE for the datatype when
* this code is active.
*/
double *copyOfOutputs = (double *) ssGetDWork(S, 0);
double *y = ssGetOutputPortRealSignal(S,0);
int k;
for (k=0; k < ssGetOutputSignalWidth(S,0); k++ ) {
y[k] = copyOfOutputs[k];
}
#endif
}
/* 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");
}
}
/* Function: mdlSetOutputPortWidth ==========================================
* Abstract:
* This method is called with the candidate width for a dynamically
* sized port. If the proposed width is acceptable, the method should
* go ahead and set the actual port width using ssSetOutputPortWidth. If
* the size is unacceptable an error should generated via
* ssSetErrorStatus. Note that any other dynamically sized input or
* output ports whose widths are implicitly defined by virtue of knowing
* the width of the given port can also have their widths set via calls
* to ssSetInputPortWidth or ssSetOutputPortWidth.
*/
static void mdlSetOutputPortWidth(SimStruct *S, int portIndex, int width)
{
ssSetOutputPortWidth(S, 2, ssGetDWorkWidth(S,4));
} /* end mdlSetOutputPortWidth */
