/* Function: mdlInitializeConditions ========================================
* Abstract:
* Initialize states to zeros.
*/
static void mdlInitializeConditions(SimStruct *S)
{
int_T i;
InputRealPtrsType M, q;
/* checks the input matrices for NaN and Inf */
/* checking M */
M = ssGetInputPortRealSignalPtrs(S,0);
for (i=0; i<NSTATES(S)*NSTATES(S); i++) {
if ( !mxIsFinite(*M[i]) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input matrix M.");
return;
}
}
/* checking q */
q = ssGetInputPortRealSignalPtrs(S,1);
for (i=0; i<NSTATES(S); i++) {
if ( !mxIsFinite(*q[i]) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input vector q.");
return;
}
}
}
static void mdlZeroCrossings(SimStruct *S)
{
//printf("mdlZeroCrossings at %g\n", ssGetT(S));
if (!rtsys->mdlzerocalled) {
rtsys->mdlzerocalled = true;
}
int i;
rtsys = (RTsys*) ssGetUserData(S);
if (rtsys->init_phase) {
/* Failure during initialization */
return;
}
/* Copy analog inputs */
InputRealPtrsType inputs = ssGetInputPortRealSignalPtrs(S,0);
for (i=0; i<rtsys->nbrOfInputs; i++) {
rtsys->inputs[i] = *inputs[i];
}
/* Copy interrupt inputs, check for events */
inputs = ssGetInputPortRealSignalPtrs(S,1);
for (i=0; i<rtsys->nbrOfTriggers; i++) {
if (fabs(*inputs[i]-rtsys->interruptinputs[i]) > 0.1) {
if (ssGetT(S) < rtsys->nextHit) {
rtsys->nextHit = ssGetT(S);
}
//printf("mdlZeroCrossings: interrupt detected at %2.20g\n", ssGetT(S));
}
rtsys->interruptinputs[i] = *inputs[i];
}
/* Copy network input, check for event */
inputs = ssGetInputPortRealSignalPtrs(S,2);
for (i=0; i<rtsys->nbrOfNetworks; i++) {
if (fabs(*inputs[i]-rtsys->networkinputs[i]) > 0.1) {
if (ssGetT(S) < rtsys->nextHit) {
rtsys->nextHit = ssGetT(S);
}
}
rtsys->networkinputs[i] = *inputs[i];
}
/* Check the energy level */
rtsys->energyLevel = *ssGetInputPortRealSignalPtrs(S,3)[0];
ssGetNonsampledZCs(S)[0] = rtsys->nextHit - ssGetT(S);
}
static void mdlOutputs(SimStruct *S,int_T tid) {
InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1);
real_T prev = ssGetRWorkValue(S,0);
bool dataPort = PARAM(2)[0];
int_T i;
#ifndef MATLAB_MEX_FILE
rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);
SEM *sem = (SEM *)ssGetPWorkValue(S,1);
#endif
char_T *msg;
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
UNUSED_ARG(tid); /* not used in single tasking mode */
if (U0(0) > 0.5 && U0(0) > prev) {
msg = (char_T *)malloc(strlen);
mxGetString(ssGetSFcnParam(S,1), msg, strlen);
#ifndef MATLAB_MEX_FILE
if (dataPort) {
for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
asprintf(&msg, "%s %f", msg, U1(i));
}
}
if (rt_sem_wait_if(sem) != 0) {
memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE);
shm->msg.state = NEW_VALUE;
rt_sem_signal(sem);
}
#else
switch ((int)PARAM(0)[0]) {
case 1: printf("DEBUG"); break;
case 2: printf("INFO"); break;
case 3: printf("WARN"); break;
case 4: printf("ERROR"); break;
case 5: printf("FATAL"); break;
default: printf("NONE"); break;
}
printf(": %s", msg);
if (dataPort) {
for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
printf(" %f", U1(i));
}
}
printf("\n");
#endif
free(msg);
}
ssSetRWorkValue(S,0,U0(0));
}
/* Function: mdlOutputs =======================================================
* Abstract:
*
* y0 = u * 0.1
* y1 = u * 0.2
* y2 = u * 0.3
* y3 = u * 0.4
* y4 = u * 0.5
* y5 = u * 0.6
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
boolean_T u0IsComplex = ssGetInputPortComplexSignal(S, 0) == COMPLEX_YES;
int_T iWidth = ssGetInputPortWidth(S, 0);
int_T i, j;
real_T gain = 0.0;
for (j=0; j<6; j++) {
real_T *y = ssGetOutputPortRealSignal(S,j);
/* Output of jth Port is gain [(j+1)*(0.1)] */
if (j != 2) {
gain += 0.1;
} else {
/* special case due to normalize numerical precision */
gain = 0.3;
}
for (i=0; i<iWidth; i++) {
*y++ = uPtrs[i][0] * gain;
if (u0IsComplex) {
*y++ = uPtrs[i][1] * gain;
}
}
}
}
/* 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;
}
}
static void mdlInitializeConditions(SimStruct *S)
{
//printf("mdlInit\n");
int i;
rtsys = (RTsys*) ssGetUserData(S);
if (rtsys->init_phase) {
/* Failure during initialization */
return;
}
for (i=0; i<rtsys->nbrOfInputs; i++)
rtsys->inputs[i] = *ssGetInputPortRealSignalPtrs(S,0)[i];
for (i=0; i<rtsys->nbrOfTriggers; i++) {
rtsys->interruptinputs[i] = 0.0;
rtsys->oldinterruptinputs[i] = 0.0;
}
/* NETWORK */
for (i=0; i<rtsys->nbrOfNetworks; i++) {
rtsys->nwSnd[i] = 0.0;
rtsys->networkinputs[i] = 0.0;
rtsys->oldnetworkinputs[i] = 0.0;
}
if (rtsys->nbrOfNetworks > 0) {
ttInitNetwork2(); /* do the rest of the network initialization */
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
short i;
int_T portWidth;
InputRealPtrsType uPtrs;
//Read the inputs
for(i=0;i<numberOfInputs;i++)
{
portWidth = ssGetInputPortWidth(S,i);
uPtrs = ssGetInputPortRealSignalPtrs(S,i);
if (portWidth>0)
{
if (uPtrs[0]!=NULL)
{
privateSetInputValByIndex(i,*uPtrs[0]);
}
}
}
//Update the outputs
for(i=0;i<numberOfOutputs;i++)
{
double *y = (double *)ssGetOutputPortSignal(S,i);
y[0] = privateGetOutputValByIndex(i);
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* Issue ssCallSystemWithTid on 1st or 2nd output element of 1st output port
* and then update 2nd output port with the state.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *x = ssGetRealDiscStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,1);
/*
* ssCallSystemWithTid is used to execute a function-call subsystem. The
* 2nd argument is the element of the 1st output port index which
* connected to the function-call subsystem. Function-call subsystems
* can be driven by the first output port of s-function blocks.
*/
UNUSED_ARG(tid); /* not used in single tasking mode */
if (((int)*uPtrs[0]) % 2 == 1) {
if (!ssCallSystemWithTid(S,0,tid)) {
/* Error occurred which will be reported by Simulink */
return;
}
} else {
if (!ssCallSystemWithTid(S,1,tid)) {
/* Error occurred which will be reported by Simulink */
return;
}
}
y[0] = x[0];
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = 2*u
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i;
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T width = ssGetOutputPortWidth(S,0);
for (i=0; i<width; i++) {
/*
* This example does not implement complex signal handling.
* To find out see an example about how to handle complex signal in
* S-function, see sdotproduct.c for details.
*/
g_slsf_in = (int) *uPtrs[i];
*y = 2.0 *(*uPtrs[i]);
g_slsf_out = (int) *y;
y++;
}
if(!is_main_called){
main();
is_main_called = true;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
*
* y1 = "sum" * "gain" where sum operation is defined by a list of
* '+' and '-' characters.
* y2 = "modified" average of the input signals (i.e. sum/nInputPorts).
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType enablePtrs;
int *enabled = ssGetIWork(S);
int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT);
int signalTid = ssGetInputPortSampleTimeIndex(S,SIGNAL_IPORT);
real_T enableTs = ssGetInputPortSampleTime(S,ENABLE_IPORT);
real_T enableTsOffset = ssGetInputPortOffsetTime(S,ENABLE_IPORT);
if (enableTs == CONTINUOUS_SAMPLE_TIME && enableTsOffset == 0.0) {
if (ssIsMajorTimeStep(S) && ssIsContinuousTask(S,tid)) {
if (signalTid == enableTid ||
ssIsSpecialSampleHit(S, signalTid, enableTid, tid)) {
enablePtrs = ssGetInputPortRealSignalPtrs(S,ENABLE_IPORT);
*enabled = (*enablePtrs[0] > 0.0);
}
}
} else {
int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT);
if (ssIsSampleHit(S, enableTid, tid)) {
if (enableTid == signalTid ||
ssIsSpecialSampleHit(S, signalTid, enableTid, tid)) {
enablePtrs = ssGetInputPortRealSignalPtrs(S,ENABLE_IPORT);
*enabled = (*enablePtrs[0] > 0.0);
}
}
}
if (ssIsSampleHit(S, signalTid, tid) && (*enabled)) {
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,SIGNAL_IPORT);
real_T signal = *uPtrs[0];
int i;
for (i = 0; i < NOUTPUTS; i++) {
int outTid = ssGetOutputPortSampleTimeIndex(S,i);
if (outTid==signalTid ||
ssIsSpecialSampleHit(S, outTid, signalTid, tid)) {
real_T *y = ssGetOutputPortRealSignal(S,i);
*y = signal;
}
}
}
} /* end mdlOutputs */
static void mdlUpdate(SimStruct *S, int_T tid)
{
debug("( trace )");
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S, 0);
for (size_t i = 0; i < protocol->p_input_width; i++)
intermediate[i] = *uPtrs[i];
Protocol_send_data(protocol, intermediate);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetRealDiscStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid); /* not used in single tasking mode */
/* y=Cx+Du */
y[0]=C[0][0]*x[0]+C[0][1]*x[1]+D[0][0]*U(0)+D[0][1]*U(1);
y[1]=C[1][0]*x[0]+C[1][1]*x[1]+D[1][0]*U(0)+D[1][1]*U(1);
}
static void mdlOutputs(SimStruct *S,int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *p = mxGetPr(ssGetSFcnParam(S,0));
int_T i;
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T u[INPUTPORT0WIDTH];
for(i=0; i<INPUTPORT0WIDTH; i++) {
u[i]=*uPtrs[i];
}
calcOutputs(y,u,p);
}
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);
}
/* 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);
}
}
static void mdlZeroCrossings(SimStruct *S)
{
int i;
double now = ssGetT(S);
RTnetwork *nwsys = (RTnetwork*)ssGetUserData(S);
InputRealPtrsType input_0 = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType input_1 = ssGetInputPortRealSignalPtrs(S,1);
InputRealPtrsType input_2 = ssGetInputPortRealSignalPtrs(S,2);
/* Check for external events */
for (i=0; i < nwsys->nbrOfNodes; i++) {
if (fabs(*input_0[i] - nwsys->inputs[i]) > 0.1) {
nwsys->nextHit = now;
break;
}
}
/* Copy inputs */
for (i=0; i < nwsys->nbrOfNodes; i++) {
nwsys->inputs[i] = *input_0[i];
nwsys->nwnodes[i]->xCoordinate = *input_1[i];
nwsys->nwnodes[i]->yCoordinate = *input_2[i];
}
ssGetNonsampledZCs(S)[0] = nwsys->nextHit - now;
}
/* Function: mdlUpdate ======================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
real_T tempX[2] = {0.0, 0.0};
real_T *x = ssGetRealDiscStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid); /* not used in single tasking mode */
/* xdot=Ax+Bu */
tempX[0]=A[0][0]*x[0]+A[0][1]*x[1]+B[0][0]*U(0)+B[0][1]*U(1);
tempX[1]=A[1][0]*x[0]+A[1][1]*x[1]+B[1][0]*U(0)+B[1][1]*U(1);
x[0]=tempX[0];
x[1]=tempX[1];
}
/* Function: mdlOutputs */
static void mdlOutputs(SimStruct *S, int_T tid)
{
double Aa = ( mxGetPr(ssGetSFcnParam(S,14)) )[0];
double Ab = ( mxGetPr(ssGetSFcnParam(S,15)) )[0];
double fv = ( mxGetPr(ssGetSFcnParam(S,18)) )[0];
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid);
/* OUTPUT equation*/
y[0] = (Aa*x[2] - Ab*x[3]) - fv*U(2);
}
/* Function: mdlOutputs =======================================================
* Abstract:
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType u = ssGetInputPortRealSignalPtrs(S,0);
Data *y = ssGetOutputPortSignal(S,0);
if (*u[0] > 127 || mxIsInf(*u[0])) {
y->value = 127;
y->res = LO_RES;
} else if (*u[0] < 1.0 && *u[0] > -1.0) {
y->value = (int8_T) (127.0 * *u[0]);
y->res = HI_RES;
} else {
y->value = (int8_T) *u[0];
y->res = LO_RES;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = 2*u
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i;
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T width = ssGetOutputPortWidth(S,0);
for (i=0; i<width; i++) {
/*
* This example does not implement complex signal handling.
* To find out see an example about how to handle complex signal in
* S-function, see sdotproduct.c for details.
*/
*y++ = 3.0 *(*uPtrs[i]);
}
}
/* 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
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
float data;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWorkValue(S,0);
data = (float)*uPtrs[0];
if (data < -1.0) data = -1.0;
if (data > 1.0) data = 1.0;
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &data, sizeof(data));
#else
rt_mbx_send_if(mbx, &data, sizeof(data));
#endif
#endif
}
/* 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;
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
// double *params0 = mxGetPr(ssGetSFcnParam(S,0));
int i;
double u[N_INPUTS];
double B_[NS*NI], *Ko_ = y;
/* copy inputs to u[] to ensure contiguity ... and easy access*/
for (i=0; i<N_INPUTS; i++)
u[i] = *uPtrs[i];
/* output new optimal gain */
optBK(B_, Ko_, x, u);
}
/* 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)
{
// get PWM motor input commands
InputRealPtrsType servo_cmd = ssGetInputPortRealSignalPtrs(S,0);
#if defined(__linux)
// get the PWM device pointers from PWork vector
PWMDevice *servo1_p, *servo4_p;
servo1_p = (PWMDevice*)ssGetPWorkValue(S, 4);
servo4_p = (PWMDevice*)ssGetPWorkValue(S, 5);
// apply the command to the motors, casting it as uint
// have to test out servo/motor range to find possible input range
servo1_p->move((unsigned int)*servo_cmd[0]);
servo4_p->move((unsigned int)*servo_cmd[3]);
#endif
}
/* 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;
// }
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
int_T *dim = ssGetInputPortDimensions(S,0);
struct {
float u[dim[0]*dim[1]];
} data;
int i;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWork(S)[0];
for (i = 0; i < (dim[0]*dim[1]); i++) {
data.u[i] = (float)*uPtrs[i];
}
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &data, sizeof(data));
#else
rt_mbx_send_if(mbx, &data, sizeof(data));
#endif
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
unsigned int led_mask = 0;
int i;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWorkValue(S,0);
for (i = 0; i < NUM_LEDS; i++) {
if (*uPtrs[i] > 0.) {
led_mask += (1 << i);
} else {
led_mask += (0 << i);
}
}
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &led_mask, sizeof(led_mask));
#else
rt_mbx_send_if(mbx, &led_mask, sizeof(led_mask));
#endif
#endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
*
* y = q * floor(fabs(u/q) + 0.5) * (u >= 0 ? 1.0 : -1.0);
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T yWidth = ssGetOutputPortWidth(S,0);
const mxArray *quant = (const mxArray *) QUANTIZATION_PARAM(S);
int_T intervalSize = mxGetM (quant) * mxGetN (quant);
const real_T *q = mxGetPr(quant);
int_T i;
UNUSED_ARG(tid); /* not used in single tasking mode */
if (intervalSize == 1) {
for (i = 0; i < yWidth; i++) {
y[i] = q[0] * floor(fabs(U(i)/q[0]) + 0.5) *
(U(i) >= 0.0 ? 1.0 : -1.0);
}
} else {
for (i = 0; i < yWidth; i++) {
y[i] = q[i] * floor(fabs(U(i)/q[i]) + 0.5) *
(U(i) >= 0.0 ? 1.0 : -1.0);
}
}
}
