/* -*-c-*- Put emacs into c-mode
* <mtt_model_name>_sfun.c:
* Matlab S-function simulation of <mtt_model_name>
*/
#define S_FUNCTION_NAME <mtt_model_name>_sfun
#define S_FUNCTION_LEVEL 2
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "simstruc.h"
#include "sfun_debug.h"
#include "useful-functions.hh"
#include "<mtt_model_name>_def.h"
#include "<mtt_model_name>_sympar.h"
#include "<mtt_model_name>_cr.h"
static double *mttdx; /* pointer to rates */
static double *mttu; /* pointer to inputs */
static double *mttpar; /* pointer to parameters */
static double *mttx; /* pointer to states */
static double *mtty; /* pointer to outputs */
static double *mttyz; /* pointer to residuals */
static double mttt; /* time */
static unsigned int i; /* loop counter */
/* system equations */
static void
<mtt_model_name>_ae (void)
{
#include "<mtt_model_name>_ae.c"
PRINT_LEAVE;
}
static void
<mtt_model_name>_numpar (void)
{
#include "<mtt_model_name>_numpar.c"
PRINT_LEAVE;
}
static void
<mtt_model_name>_ode (void)
{
#include "<mtt_model_name>_ode.c"
PRINT_LEAVE;
}
static void
<mtt_model_name>_odeo (void)
{
#include "<mtt_model_name>_odeo.c"
PRINT_LEAVE;
}
static void
<mtt_model_name>_state (void)
{
#include "<mtt_model_name>_state.c"
PRINT_LEAVE;
}
/* utility procedures */
static double *
array_of_double (size_t n)
{
void *p = calloc (n, sizeof (double));
if (! p) {
fprintf (stderr, "*** Error: failed to allocate memory\n");
}
return (double *) p;
}
static void
initialise_arrays (void)
{
PRINT_ENTER;
mttdx = array_of_double (MTTNX);
mttpar = array_of_double (MTTNPAR);
mttu = array_of_double (MTTNU + MTTNYZ);
mttx = array_of_double (MTTNX);
mtty = array_of_double (MTTNY);
mttyz = array_of_double (MTTNYZ);
PRINT_LEAVE;
}
static void
update_states_from_simulink (SimStruct *S)
{
PRINT_ENTER;
for (i = 0; i < MTTNX; i++) {
mttx[i] = ssGetContStates (S)[i];
}
PRINT_LEAVE;
}
static void
update_inputs_from_simulink (SimStruct *S)
{
PRINT_ENTER;
for (i = 0; i < MTTNU; i++) {
mttu[i] = *ssGetInputPortRealSignalPtrs (S, 0)[i];
}
PRINT_LEAVE;
}
static void
update_inputs_from_solver (void)
{
mxArray *MTT_MATLAB_P;
mxArray *MTT_MATLAB_T;
mxArray *MTT_MATLAB_U;
mxArray *MTT_MATLAB_Ui;
mxArray *MTT_MATLAB_X;
double *p;
PRINT_ENTER;
/* starting value for solver - start with zero */
MTT_MATLAB_Ui = mxCreateDoubleMatrix (MTTNYZ, 1, mxREAL);
mxSetName (MTT_MATLAB_Ui, "MTT_Ui");
p = mxGetPr (MTT_MATLAB_Ui);
for (i = 0; i < MTTNYZ; i++) {
p[i] = 0.0;
}
mexPutArray (MTT_MATLAB_Ui, "base");
/* put states into matlab workspace */
MTT_MATLAB_X = mxCreateDoubleMatrix (MTTNX, 1, mxREAL);
mxSetName (MTT_MATLAB_X, "MTT_X");
p = mxGetPr (MTT_MATLAB_X);
for (i = 0; i < MTTNX; i++) {
p[i] = mttx[i];
}
mexPutArray (MTT_MATLAB_X, "base");
/* put known inputs into matlab workspace */
MTT_MATLAB_U = mxCreateDoubleMatrix (MTTNU, 1, mxREAL);
mxSetName (MTT_MATLAB_U, "MTT_U");
p = mxGetPr (MTT_MATLAB_U);
for (i = 0; i < MTTNU; i++) {
p[i] = mttu[i];
}
mexPutArray (MTT_MATLAB_U, "base");
/* put time into matlab workspace */
MTT_MATLAB_T = mxCreateDoubleMatrix (1, 1, mxREAL);
mxSetName (MTT_MATLAB_T, "MTT_T");
*mxGetPr (MTT_MATLAB_T) = mttt;
mexPutArray (MTT_MATLAB_T, "base");
/* put parameters into matlab workspace */
MTT_MATLAB_P = mxCreateDoubleMatrix (MTTNPAR, 1, mxREAL);
mxSetName (MTT_MATLAB_P, "MTT_P");
p = mxGetPr (MTT_MATLAB_P);
for (i = 0; i < MTTNPAR; i++) {
p[i] = mttpar[i];
}
mexPutArray (MTT_MATLAB_P, "base");
/* call solver */
mexEvalString ("MTT_Ui = lsqnonlin (@<mtt_model_name>_sfun_ae, MTT_Ui, [], [], optimset('display','off','diagnostics','off','TolX',1e-2,'TolFun',1e-2), MTT_X, MTT_U, MTT_T, MTT_P);");
/* retrieve result */
MTT_MATLAB_Ui = mexGetArray ("MTT_Ui", "base");
p = mxGetPr (MTT_MATLAB_Ui);
for (i = 0; i < MTTNYZ; i++) {
mttu[MTTNU + i] = p[i];
}
/* free memory */
mxDestroyArray (MTT_MATLAB_P);
mxDestroyArray (MTT_MATLAB_T);
mxDestroyArray (MTT_MATLAB_U);
mxDestroyArray (MTT_MATLAB_Ui);
mxDestroyArray (MTT_MATLAB_X);
PRINT_LEAVE;
}
static void
update_simtime_from_simulink (SimStruct *S)
{
PRINT_ENTER;
mttt = ssGetT (S);
PRINT_LEAVE;
}
/* S-function methods */
static void mdlInitializeSizes(SimStruct *S)
{
PRINT_ENTER;
ssSetNumSFcnParams(S, 0);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
PRINT_LEAVE;
return;
}
ssSetNumContStates(S, MTTNX);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, MTTNU);
ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 2)) return;
ssSetOutputPortWidth(S, 0, MTTNX);
ssSetOutputPortWidth(S, 1, MTTNY);
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
initialise_arrays ();
PRINT_LEAVE;
}
static void mdlInitializeSampleTimes(SimStruct *S)
{
PRINT_ENTER;
ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
ssSetOffsetTime(S, 0, 0.0);
PRINT_LEAVE;
}
#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
PRINT_ENTER;
<mtt_model_name>_numpar ();
<mtt_model_name>_state ();
for (i = 0; i < MTTNX; i++) {
ssGetContStates (S)[i] = mttx[i];
}
PRINT_LEAVE;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
PRINT_ENTER;
update_states_from_simulink (S);
update_inputs_from_simulink (S);
if (MTTNYZ > 0) {
update_inputs_from_solver ();
}
update_simtime_from_simulink (S);
UNUSED_ARG(tid); /* not used in single tasking mode */
<mtt_model_name>_odeo ();
for (i = 0; i < MTTNX; i++) {
ssGetOutputPortRealSignal (S, 0)[i] = mttx[i];
}
for (i = 0; i < MTTNY; i++) {
ssGetOutputPortRealSignal (S, 1)[i] = mtty[i];
}
PRINT_LEAVE;
}
#define MDL_DERIVATIVES
static void mdlDerivatives(SimStruct *S)
{
PRINT_ENTER;
update_states_from_simulink (S);
update_inputs_from_simulink (S);
if (MTTNYZ > 0) {
update_inputs_from_solver ();
}
update_simtime_from_simulink (S);
<mtt_model_name>_ode ();
for (i = 0; i < MTTNX; i++) {
ssGetdX (S)[i] = mttdx[i];
}
PRINT_LEAVE;
}
static void mdlTerminate(SimStruct *S)
{
PRINT_ENTER;
UNUSED_ARG(S);
free (mttdx);
free (mttpar);
free (mttu);
free (mttx);
free (mtty);
free (mttyz);
PRINT_LEAVE;
}
#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */
#include "simulink.c" /* MEX-file interface mechanism */
#else
#include "cg_sfun.h" /* Code generation registration function */
#endif