/* Function: mdlDerivatives =================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *apr = mxGetPr(A_PARAM(S));
const real_T *bpr = mxGetPr(B_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T i, j;
real_T accum;
/* Matrix Multiply: dx = Ax + Bu */
for (i = 0; i < nStates; i++) {
accum = 0.0;
/* Ax */
for (j = 0; j < nStates; j++) {
accum += apr[i + nStates*j] * x[j];
}
/* Bu */
for (j = 0; j < nInputs; j++) {
accum += bpr[i + nStates*j] * U(j);
}
dx[i] = accum;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *cpr = mxGetPr(C_PARAM(S));
const real_T *dpr = mxGetPr(D_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T nOutputs = ssGetOutputPortWidth(S,0);
int_T i, j;
real_T accum;
UNUSED_ARG(tid); /* not used in single tasking mode */
/* Matrix Multiply: y = Cx + Du */
for (i = 0; i < nOutputs; i++) {
accum = 0.0;
/* Cx */
for (j = 0; j < nStates; j++) {
accum += cpr[i + nOutputs*j] * x[j];
}
/* Du */
for (j = 0; j < nInputs; j++) {
accum += dpr[i + nOutputs*j] * U(j);
}
y[i] = accum;
}
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl.doc for more details on the macros below */
ssSetNumSFcnParams(S, 0); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
ssSetNumContStates(S, 1); /* how many continuous states? */
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, 1);
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, 1);
ssSetNumSampleTimes(S, 1);
/*
* If your Fortran code uses REAL for the state, input, and/or output
* datatypes, use these DWorks as work areas to downcast continuous
* states from double to REAL before calling your code. You could
* also put the work vectors in hard-coded local (stack) variables.
*
* For fixed step code, keep a copy of the variables to be output
* in a DWork vector so the mdlOutputs() function can provide output
* data when needed. You can use as many DWork vectors as you like
* for both input and output (or hard-code local variables).
*/
if(!ssSetNumDWork( S, 3)) return;
ssSetDWorkWidth( S, 0, ssGetOutputPortWidth(S,0));
ssSetDWorkDataType( S, 0, SS_SINGLE); /* use SS_DOUBLE if needed */
ssSetDWorkWidth( S, 1, ssGetInputPortWidth(S,0));
ssSetDWorkDataType( S, 1, SS_SINGLE);
ssSetDWorkWidth( S, 2, ssGetNumContStates(S));
ssSetDWorkDataType( S, 2, SS_SINGLE);
ssSetNumNonsampledZCs(S, 0);
/* Specify the sim state compliance to be same as a built-in block */
/* see sfun_simstate.c for example of other possible settings */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 0);
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* If the initial condition parameter (X0) is not an empty matrix,
* then use it to set up the initial conditions, otherwise,
* set the initial conditions to all 0.0
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *x0 = ssGetContStates(S);
int_T i, nStates;
nStates = ssGetNumContStates(S);
if (mxGetM(X0_PARAM(S)) != 0) {
const real_T *pr = mxGetPr(X0_PARAM(S));
for (i = 0; i < nStates; i++) {
*x0++ = *pr++;
}
} else {
for (i = 0; i < nStates; i++) {
*x0++ = 0.0;
}
}
}
/* Function: mdlDerivatives =================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *apr = mxGetPr(MAGICNUM_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T i, j;
real_T accum;
/* Matrix Multiply: dx = Ax + Bu */
real_T T_0 = x[0];
real_T A = U(0);
real_T x1 = U(1); //muscle length
real_T x2 = U(2); //muscle change of length (vel)
// real_T dT = dx[0];
real_T Kse = apr[0];
real_T Kpe = apr[1];
real_T b = apr[2];
dx[0] = Kse / b * (Kpe * (x1 - 1.0) + b*x2 - (1 + Kpe/Kse)*T_0 + A);
// for (i = 0; i < nStates; i++) {
// accum = 0.0;
//
// /* Ax */
// for (j = 0; j < nStates; j++) {
// accum += apr[i + nStates*j] * x[j];
// }
//
// /* Bu */
// for (j = 0; j < nInputs; j++) {
// accum += bpr[i + nStates*j] * U(j);
// }
//
// dx[i] = accum;
// }
}
/* 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
}
