Example #1
0
/* 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]; 
}
Example #2
0
/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block. Generally outputs are placed in the output vector, ssGetY(S).
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
   /* the output signal */
   uint8_T* data_out = (uint8_T*) ssGetOutputPortSignal(S,1);
   if ((getHandled()==0) && (getCurrent_State()==CCP_CONNECTED_STATE)) {
      /* message is unhandled, but we are connected so we must output
       * the Unknown Command response */
      data_out[0] = 0xFF;
      data_out[1] = 0x30;
      /* Command Counter */
      data_out[2] = getData(1);
      data_out[3] = 0;
      data_out[4] = 0;
      data_out[5] = 0;
      data_out[6] = 0;
      data_out[7] = 0;
   } 
   else if (getHandled()==1) {
      /* we have a valid response */
      data_out[0] = getData(0);
      data_out[1] = getData(1);
      data_out[2] = getData(2);
      data_out[3] = getData(3);
      data_out[4] = getData(4);
      data_out[5] = getData(5);
      data_out[6] = getData(6);
      data_out[7] = getData(7);
   }
   
   if ((getHandled()==1) || (getCurrent_State()==CCP_CONNECTED_STATE)) {
      /* We either handled the message and have a valid response, 
       * or we have an unknown command response */
      if (!ssCallSystemWithTid(S,0,tid)) {
         mexPrintf("Some error!\n");
         return;
      } 
   }
}
Example #3
0
static void mdlOutputs(SimStruct *S, int_T tid)
{
    PCONFIG_DATA config;
    const real_T         *in_xdot;
    const real_T         *in_x0;
    InputRealPtrsType    pin_params;
    const boolean_T      *in_reset;
    real_T               *out_x;
    real_T               *out_t;
    real_T               *xd;
    real_T               *xd_temp;
    real_T               *xd_temp2;
    time_T               initial_time;
    time_T               final_time;
    quaternion           phi;
    quaternion           q;
    vector               omegadot_temp;
    const real_T         *pomegadot;
    int_T                i;

    /* Retrieve C object from the pointers vector */
    config = ssGetPWork(S)[0];

    xd = (real_T*) ssGetDWork(S,0);
    xd_temp = (real_T*) ssGetDWork(S,1);
    if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
    in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
    if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
    if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
    if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
    out_x = ssGetOutputPortRealSignal(S, 1);
    if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);

    switch( intval(mxGetScalar(paramSpecificationsSource)) )
    {
        case 1:
            initial_time = config->initial_time;
            final_time   = ssGetTaskTime(S,0);
            break;
        case 2:
            initial_time = mxGetScalar(paramInitialTime);
            final_time   = mxGetScalar(paramFinalTime);
            break;
        case 3:
            initial_time = *(pin_params[0]);
            final_time   = *(pin_params[1]);
            break;
        default:
            ssSetErrorStatus(S,"Wrong integration algorithm selected");
            return;
    }
    
/*    ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */

    /* Reset the states */
    if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
    {
        ssSetIWorkValue(S, 0, 0);
        if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
        {
            /* Internal initial conditions */
            for( i=0; i<config->nstates; i++ )
            {
                xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
            }
        }
        else
        {
            /* External initial conditions */
            memcpy(xd, in_x0, config->nstates*sizeof(real_T));
        }
        memcpy(out_x, xd, config->nstates*sizeof(real_T));
        if( config->idxout_time ) out_t[0] = initial_time;
    }
    
    if( final_time > initial_time )
    {
        if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
        {
            /* Euler algorithm */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
                    QuaternionProd(&xd[i], phi, &xd[i]);
                    QuaternionNormalize(&xd[i]);
                    i += 4;
                }
                else
                {
                    xd[i] += in_xdot[i] * (final_time-initial_time);
                    i++;
                }
            }
        }
        
        if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
        {
            /* Runge-Kutta algorithm */
            /* f1 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
                    QuaternionProd(&xd_temp[i], &xd_temp[i], q);
                    QuaternionProd(&xd_temp[i], q, phi);
                    QuaternionProd(&xd[i], phi, &out_x[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] = in_xdot[i];
                    out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
                    i++;
                }
            }
            if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
            
            /* f2 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
                    QuaternionProd(q, q, phi);
                    QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
                    QuaternionProd(phi, q, phi);
                    QuaternionProd(&xd[i], phi, &out_x[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] += 2*in_xdot[i];
                    out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
                    i++;
                }
            }
            if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
            
            /* f3 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
                    QuaternionProd(q, q, phi);
                    QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
                    QuaternionProd(phi, q, phi);
                    QuaternionProd(phi, phi, phi);
                    QuaternionProd(&xd[i], phi, &out_x[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] += 2*in_xdot[i];
                    out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
                    i++;
                }
            }
            if( config->idxout_time ) out_t[0] = final_time;
            
            /* f4 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
                    QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
                    QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
                    QuaternionNormalize(&xd[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] += in_xdot[i];
                    xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
                    i++;
                }
            }
        }
    }
    else
    {
        if( !ssCallSystemWithTid(S,0,tid) ) return;
    }

    config->initial_time = final_time;

    /* Update the outputs */
    memcpy(out_x, xd, config->nstates*sizeof(real_T));
    if( config->idxout_time ) out_t[0] = final_time;
}