/* 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;
}
}
static void mdlDerivatives(SimStruct *S)
{
const real_T *x = ssGetContStates(S);
real_T *dx = ssGetdX(S);
dx[0] = x[1];
dx[1] = 9.8*sin(x[0]);
}
/* 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)
{
const real_T *u0 = (const real_T*) ssGetInputPortSignal(S,0);
real_T *dx = ssGetdX(S);
real_T *xC = ssGetContStates(S);
real_T *y0 = (real_T *) ssGetOutputPortRealSignal(S,0);
builderC_Derivatives_wrapper(u0, y0,dx, xC);
}
static void mdlDerivatives ( SimStruct * S ) { nmsgyp54ig * _rtB ; jxfihej1dy
* _rtXdot ; pklu3vjy1t * _rtDW ; _rtDW = ( ( pklu3vjy1t * ) ssGetRootDWork (
S ) ) ; _rtXdot = ( ( jxfihej1dy * ) ssGetdX ( S ) ) ; _rtB = ( ( nmsgyp54ig
* ) _ssGetModelBlockIO ( S ) ) ; _rtXdot -> ouzmraehom = 0.0 ; jreinmfd0u ( &
_rtB -> bemu4fk2sq4 , & _rtDW -> bemu4fk2sq4 , & _rtXdot -> bemu4fk2sq4 ) ;
h5kbws31cy ( & _rtB -> cqabclhqip4 , & _rtDW -> cqabclhqip4 , & _rtXdot ->
cqabclhqip4 ) ; jreinmfd0u ( & _rtB -> n1qdzssqu3 , & _rtDW -> n1qdzssqu3 , &
_rtXdot -> n1qdzssqu3 ) ; h5kbws31cy ( & _rtB -> nfugx5ih43 , & _rtDW ->
nfugx5ih43 , & _rtXdot -> nfugx5ih43 ) ; }
/* 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)
{
const real_T *dm = (const real_T*) ssGetInputPortSignal(S,0);
const real_T *dmf = (const real_T*) ssGetInputPortSignal(S,1);
const real_T *dV = (const real_T*) ssGetInputPortSignal(S,2);
const real_T *dmfb = (const real_T*) ssGetInputPortSignal(S,3);
const real_T *dQCyl = (const real_T*) ssGetInputPortSignal(S,4);
const real_T *dHCyl = (const real_T*) ssGetInputPortSignal(S,5);
const real_T *dWCyl = (const real_T*)ssGetInputPortSignal(S, 6);
real_T *dx = ssGetdX(S);
ThdynCVNV2Zone_Derivatives_wrapper(dm,dmf,dV,dmfb,dQCyl,dHCyl,dWCyl,dx);
}
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
double *m=mxGetPr(ssGetSFcnParam(S,0));
real_T mm=m[0];
dx[0]=x[3]/mm;
dx[1]=x[4]/mm;
dx[2]=x[5]/mm;
dx[3]=U(0);
dx[4]=U(1);
dx[5]=U(2);
}
static void derivatives_c1_newtons_cradle(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
int32_T c1_i1;
real_T (*c1_p_dot)[3];
real_T (*c1_v_dot)[3];
real_T (*c1_v)[3];
real_T (*c1_p)[3];
c1_v_dot = (real_T (*)[3])(ssGetdX(chartInstance->S) + 3);
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p_dot = (real_T (*)[3])(ssGetdX(chartInstance->S) + 0);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
for (c1_i1 = 0; c1_i1 < 3; c1_i1++) {
(*c1_p_dot)[c1_i1] = 0.0;
(*c1_v_dot)[c1_i1] = 0.0;
}
_sfTime_ = (real_T)ssGetT(chartInstance->S);
for (c1_i1 = 0; c1_i1 < 3; c1_i1++) {
(*c1_p_dot)[c1_i1] = (*c1_v)[c1_i1];
(*c1_v_dot)[c1_i1] = muDoubleScalarSin((*c1_p)[c1_i1]);
(*c1_v_dot)[c1_i1] *= -9.81;
}
}
/* 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 t, *xC, *xD, *u, *sys;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
sys = ssGetdX(S);
block0_Derivatives(sys,t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlDerivatives */
static void mdlDerivatives(SimStruct *S)
{
double wn = ( mxGetPr(ssGetSFcnParam(S,0)) )[0];
double E = ( mxGetPr(ssGetSFcnParam(S,1)) )[0];
double In = ( mxGetPr(ssGetSFcnParam(S,2)) )[0];
double Kva = ( mxGetPr(ssGetSFcnParam(S,3)) )[0];
double Kvb = ( mxGetPr(ssGetSFcnParam(S,4)) )[0];
double KvlkA = ( mxGetPr(ssGetSFcnParam(S,5)) )[0];
double KvlkB = ( mxGetPr(ssGetSFcnParam(S,6)) )[0];
double ps = ( mxGetPr(ssGetSFcnParam(S,7)) )[0];
double pt = ( mxGetPr(ssGetSFcnParam(S,8)) )[0];
double up_dz = ( mxGetPr(ssGetSFcnParam(S,9)) )[0];
double lw_dz = ( mxGetPr(ssGetSFcnParam(S,10)) )[0];
double beta = ( mxGetPr(ssGetSFcnParam(S,11)) )[0];
double Va0 = ( mxGetPr(ssGetSFcnParam(S,12)) )[0];
double Vb0 = ( mxGetPr(ssGetSFcnParam(S,13)) )[0];
double Aa = ( mxGetPr(ssGetSFcnParam(S,14)) )[0];
double Ab = ( mxGetPr(ssGetSFcnParam(S,15)) )[0];
double L = ( mxGetPr(ssGetSFcnParam(S,16)) )[0];
double leak = ( mxGetPr(ssGetSFcnParam(S,17)) )[0];
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
dx[0]= x[1];
dx[1]= -wn*wn*x[0] - 2*E*wn*x[1] + wn*wn*sat(U(0),-In,In);
if ((sat(U(0),-In,In) >= up_dz) && (sat(U(0),-In,In) <= In))
{
dx[2] = (beta/(Va0 + Aa*U(1))) * (((Kva *(x[0]/In) + KvlkA) * sgn(ps-x[2]) * sqrt(abso(ps-x[2])) - KvlkA * sgn(x[2]-pt) * sqrt(abso(x[2]-pt))) - Aa*U(2)- leak*(x[2]-x[3]));
dx[3] = (beta/(Vb0 + Ab*(L - U(1)))) * (Ab*U(2) - ((Kvb *(x[0]/In) + KvlkB) * sgn(x[3]-pt) * sqrt(abso(x[3]-pt)) - KvlkB * sgn(ps-x[3]) * sqrt(abso(ps-x[3]))) + leak*(x[2]-x[3]));
}
else if ((sat(U(0),-In,In) >= -In) && (sat(U(0),-In,In) <= lw_dz))
{
dx[2] = (beta/(Va0 + Aa*U(1))) * ((-(Kva *(abso(x[0])/In) + KvlkA) * sgn(x[2]-pt) * sqrt(abso(x[2]-pt)) + KvlkA * sgn(ps-x[2]) * sqrt(abso(ps-x[2]))) - Aa*U(2)- leak*(x[2]-x[3]));
dx[3] = (beta/(Vb0 + Ab*(L - U(1)))) * (Ab*U(2) - (-(Kvb *(abso(x[0])/In) + KvlkB) * sgn(ps-x[3]) * sqrt(abso(ps-x[3])) + KvlkB * sgn(x[3]-pt) * sqrt(abso(x[3]-pt))) + leak*(x[2]-x[3]));
}
else
{
dx[2] = 0;
dx[3] = 0;
}
}
/* 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: 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: 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 t, *xC, *xD, *uWQ, *uES, *sys;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
sys = ssGetdX(S);
for (i=0; i<(int)B(S,"CStates"); i++)
{
sys[i]=0.0;
}
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[(int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
if (ozoncc_Derivatives(sys,t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3)))
{
// ssSetErrorStatus(S,StimelaErrorMessage);
}
}
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
// double *param0 = mxGetPr(ssGetSFcnParam(S,0));
int i, j, k;
double u[N_INPUTS];
// pointers to 'input structure'
double *alf = u, *mu = alf+NS, *wt = mu+NI;
// double a[NS], b[NI], Ko_b[NS], K_b[NS];
double *P__ = x;
double *dP__ = dx;
double dp;
double *P_[NS], *dP_[NS]; // array of pointers for lower triangular P, dP
double A_[NS2], A_BKo_[NS2];
double B_[NS*NI], Ko_[NI*NS];
double Q_[NS2];
/* index A & P with 1:NS (NOT 0:NS-1!) */
#define A(i,j) A_[((i)-1) + NS*((j)-1)]
#define Q(i,j) Q_[((i)-1) + NS*((j)-1)]
#define P(i,j) (( (j<=i) ? P_[i-1][j-1] : P_[j-1][i-1] )) // low tri
#define dP(i,j) ( dP_[i-1][j-1] ) // *** i <= j *** REQUIRED!
#define B(i,j) B_[ ((i)-1) + ((j)-1)*(NS) ] // columnwise
#define Ko(i,j) Ko_[ ((i)-1)*NS + ((j)-1) ] // rowwise
// set up lower triangular structure for P and dP
j = 0;
for (i=0; i<NS; i++) {
P_[i] = P__ + j;
dP_[i] = dP__ + j;
j += i+1;
}
/* copy inputs to u[] to ensure contiguity ... and easy access*/
for (i=0; i<N_INPUTS; i++)
u[i] = *uPtrs[i];
/* prontoTK spec:
dynamics(x,u,wt,ders, dx,y, fxu_x_,fxu_u_, q,q_fxu_x_x_,q_fxu_x_u_,q_fxu_u_u_);
ders: dx(1),y(2), A(4),B(8), Q(16), S(32), R(64)
cost(x,u,wlt,ders, lxu, lxu_x_,lxu_u_, lxu_x_x_,lxu_x_u_,lxu_u_u_);
ders: lxu(1), a(2),b(4), Q(8), S(16), R(32)
*/
// get A of linearization about (alf(t),mu(t))
// get A(4)
dynamics(alf,mu,wt, 4, NULL,NULL, A_,NULL, NULL, NULL,NULL,NULL);
/* set up Q matrix --- start with zero */
for (i=0; i<NS2; i++){
Q_[i] = 0.0;
}
/* add diagonal cost fcn terms */
for (i=1; i<=NS; i++){
Q(i,i) = Qr[i-1];
}
optBK(B_, Ko_, x, u);
/* only *lower triangle* of dP */
for (i=1; i<=NS; i++) {
for (j=1; j<=i; j++) {
dp = Q(i,j);
for (k=1; k<=NS; k++) {
dp += A(k,i)*P(k,j) + P(i,k)*A(k,j); // A^T P + P A
}
for (k=1; k<=NI; k++) {
dp -= Ko(k,i)*Rr[k-1]*Ko(k,j); // -P B R^-1 B^T P = -Ko^T R Ko
}
dP(i,j) /* = dP(j,i) */ = dp;
}
}
#undef B
#undef K
#undef Ko
#undef dP
#undef P
#undef A
}
