ADDED mttroot/mtt/lib/rep/sfun_rep/Makefile Index: mttroot/mtt/lib/rep/sfun_rep/Makefile ================================================================== --- /dev/null +++ mttroot/mtt/lib/rep/sfun_rep/Makefile @@ -0,0 +1,13 @@ +#! /usr/bin/make -f + +$(SYS)_sfun.mexglx: $(SYS)_sfun.cc $(SYS)_def.h $(SYS)_state.mexglx $(SYS)_ode.mexglx $(SYS)_odeo.mexglx + cp -a $(SYS)_ode.mexglx .. + cp -a $(SYS)_odeo.mexglx .. + cp -a $(SYS)_state.mexglx .. + mex $(SYS)_sfun.cc + +$(SYS)_sfun.cc:: ${MTT_REP}/sfun_rep/sfun.cc.tmpl + cat $^ | sed 's//$(SYS)/g' > $@ + +%:: + mtt -q $(OPTS) `echo $* | sed 's/\(.*\)_\(.*\)\.\(.*\)/\1 \2 \3/'` ADDED mttroot/mtt/lib/rep/sfun_rep/sfun.cc.tmpl Index: mttroot/mtt/lib/rep/sfun_rep/sfun.cc.tmpl ================================================================== --- /dev/null +++ mttroot/mtt/lib/rep/sfun_rep/sfun.cc.tmpl @@ -0,0 +1,199 @@ +// -*-c++-*- +// _sfun.cc: +// Matlab S-function simulation of + +#define S_FUNCTION_NAME _sfun +#define S_FUNCTION_LEVEL 2 + +#include "simstruc.h" +#include +#include +#include "_def.h" + +mxArray * +get_parameters_from_simulink (SimStruct *S) +{ + static const mxArray *constpar = mxCreateDoubleMatrix (MTTNPAR, 1, mxREAL); + static mxArray * par = mxCreateDoubleMatrix (MTTNPAR, 1, mxREAL); + double *constptr; + double *ptr; + std::cerr << "Parameters:\t"; + for (int i = 0; i < MTTNPAR; i++) { + constpar = ssGetSFcnParam (S, i); + constptr = mxGetPr (constpar); + ptr = mxGetPr ( par); + *ptr = *constptr; + std::cerr << *ptr << ' '; + } + std::cerr << std::endl; + return par; +} + +mxArray * +get_states_from_simulink (SimStruct *S) +{ + static mxArray *x = mxCreateDoubleMatrix (MTTNX, 1, mxREAL); + static double *states; + static double *ptr = mxGetPr (x); + std::cerr << "States:\t\t"; + states = ssGetContStates (S); + for (int i = 0; i < MTTNX; i++) { + ptr [i] = states [i]; + std::cerr << ptr [i] << ' '; + } + std::cerr << std::endl; + return x; +} + +mxArray * +get_inputs_from_simulink (SimStruct *S) +{ + static mxArray *u = mxCreateDoubleMatrix (MTTNU, 1, mxREAL); + static double *ptr = mxGetPr (u); + std::cerr << "Inputs:\t\t"; + for (int i = 0; i < MTTNU; i++) { + ptr [i] = *ssGetInputPortRealSignalPtrs (S, i)[0]; + std::cerr << ptr [i] << ' '; + } + std::cerr << std::endl; + return u; +} + +mxArray * +get_time_from_simulink (SimStruct *S) +{ + static mxArray *t = mxCreateDoubleMatrix (1, 1, mxREAL); + double *ptr = mxGetPr (t); + ptr [0] = ssGetT (S); + std::cerr << "Time:\t\t" << ptr [0] << std::endl; + return t; +} + +// S-function methods + +static void mdlInitializeSizes(SimStruct *S) +{ + ssSetNumSFcnParams(S, MTTNPAR); + if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { + return; + } + + ssSetNumContStates(S, MTTNX); + ssSetNumDiscStates(S, 0); + + if (!ssSetNumInputPorts(S, 1)) return; + ssSetInputPortWidth(S, 0, 1); + for (int i = 0; i < MTTNU; i++) { + ssSetInputPortDirectFeedThrough(S, i, 1); + } + + if (!ssSetNumOutputPorts(S, MTTNY)) return; + for (int i = 0; i < MTTNY; i++) { + ssSetOutputPortWidth(S, i, 1); + } + + 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); +} + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME); + ssSetOffsetTime(S, 0, 0.0); +} + +#define MDL_INITIALIZE_CONDITIONS +static void mdlInitializeConditions(SimStruct *S) +{ + static mxArray *states = mxCreateDoubleMatrix (MTTNX, 1, mxREAL); + static double *ptr; + + mxArray *plhs[1]; + mxArray *prhs[1]; + + plhs[0] = states; + + prhs[0] = get_parameters_from_simulink (S); + + mexCallMATLAB (1, plhs, 1, prhs, "_state"); + ptr = mxGetPr (plhs[0]); + + std::cerr << "Initial states;\t"; + for (int i = 0; i < MTTNX; i++) { + ssGetContStates (S)[i] = ptr [i]; + std::cerr << ptr [i] << ' '; + } + std::cerr << std::endl; +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + static mxArray *outputs = mxCreateDoubleMatrix (MTTNY, 1, mxREAL); + static double *ptr; + + mxArray *plhs[1]; + mxArray *prhs[4]; + + plhs[0] = outputs; + + prhs[0] = get_states_from_simulink (S); + prhs[1] = get_inputs_from_simulink (S); + prhs[2] = get_time_from_simulink (S); + prhs[3] = get_parameters_from_simulink (S); + + UNUSED_ARG(tid); // not used in single tasking mode + + mexCallMATLAB (1, plhs, 4, prhs, "_odeo"); + ptr = mxGetPr (plhs [0]); + + std::cerr << "Outputs:\t"; + for (int i = 0; i < MTTNY; i++) { + ssGetOutputPortRealSignal (S,i)[0] = ptr [i]; + std::cerr << ptr [i] << ' '; + } + std::cerr << std::endl; +} + +#define MDL_DERIVATIVES +static void mdlDerivatives(SimStruct *S) +{ + static mxArray *rates = mxCreateDoubleMatrix (MTTNX, 1, mxREAL); + static double *ptr; + + mxArray *plhs[1]; + mxArray *prhs[4]; + + plhs[0] = rates; + + prhs[0] = get_states_from_simulink (S); + prhs[1] = get_inputs_from_simulink (S); + prhs[2] = get_time_from_simulink (S); + prhs[3] = get_parameters_from_simulink (S); + + mexCallMATLAB (1, plhs, 4, prhs, "_ode"); + ptr = mxGetPr (plhs[0]); + + std::cerr << "Rates:\t\t"; + for (int i = 0; i < MTTNX; i++) { + ssGetdX (S)[i] = ptr [i]; + std::cerr << ptr[i] << ' '; + } + std::cerr << std::endl; +} + +static void mdlTerminate(SimStruct *S) +{ + UNUSED_ARG(S); +} + +#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