static void mdlOutputs ( SimStruct * S , int_T tid ) { real_T B_0_15_0 ;
real_T currentTime ; BlockIO_main * _rtB ; Parameters_main * _rtP ;
D_Work_main * _rtDW ; _rtDW = ( ( D_Work_main * ) ssGetRootDWork ( S ) ) ;
_rtP = ( ( Parameters_main * ) ssGetDefaultParam ( S ) ) ; _rtB = ( (
BlockIO_main * ) _ssGetBlockIO ( S ) ) ; ssCallAccelRunBlock ( S , 0 , 0 ,
SS_CALL_MDL_OUTPUTS ) ; if ( ssIsSampleHit ( S , 1 , 0 ) ) {
ssCallAccelRunBlock ( S , 0 , 1 , SS_CALL_MDL_OUTPUTS ) ; } _rtB -> B_0_2_0 =
_rtB -> B_0_0_0 [ 2 ] * _rtB -> B_0_0_0 [ 1 ] ; if ( ssIsSampleHit ( S , 1 ,
0 ) ) { ssCallAccelRunBlock ( S , 0 , 3 , SS_CALL_MDL_OUTPUTS ) ; } ( (
BlockIO_main * ) _ssGetBlockIO ( S ) ) -> B_0_4_0 = ssGetT ( S ) ; if (
ssIsSampleHit ( S , 1 , 0 ) ) { ssCallAccelRunBlock ( S , 0 , 5 ,
SS_CALL_MDL_OUTPUTS ) ; _rtB -> B_0_6_0 = _rtP -> P_0 ; } _rtB -> B_0_7_0 =
_rtB -> B_0_6_0 - _rtB -> B_0_0_0 [ 1 ] ; if ( ssIsSampleHit ( S , 1 , 0 ) )
{ ssCallAccelRunBlock ( S , 0 , 8 , SS_CALL_MDL_OUTPUTS ) ;
ssCallAccelRunBlock ( S , 0 , 9 , SS_CALL_MDL_OUTPUTS ) ; ssCallAccelRunBlock
( S , 0 , 10 , SS_CALL_MDL_OUTPUTS ) ; } currentTime = ssGetTaskTime ( S , 0
) ; if ( currentTime < _rtP -> P_1 ) { _rtB -> B_0_11_0 = _rtP -> P_2 ; }
else { _rtB -> B_0_11_0 = _rtP -> P_3 ; } ssCallAccelRunBlock ( S , 0 , 12 ,
SS_CALL_MDL_OUTPUTS ) ; if ( ssIsSampleHit ( S , 1 , 0 ) ) {
ssCallAccelRunBlock ( S , 0 , 13 , SS_CALL_MDL_OUTPUTS ) ; _rtB -> B_0_14_0 =
_rtDW -> Delay_DSTATE ; } B_0_15_0 = _rtP -> P_7 * ( ( ContinuousStates_main
* ) ssGetContStates ( S ) ) -> TransferFcn_CSTATE ; ssCallAccelRunBlock ( S ,
0 , 16 , SS_CALL_MDL_OUTPUTS ) ; _rtB -> B_0_17_0 = _rtB -> B_0_16_0 -
B_0_15_0 ; _rtB -> B_0_18_0 [ 0 ] = _rtB -> B_0_12_0 ; _rtB -> B_0_18_0 [ 1 ]
= B_0_15_0 ; if ( ssIsSampleHit ( S , 1 , 0 ) ) { ssCallAccelRunBlock ( S , 0
, 19 , SS_CALL_MDL_OUTPUTS ) ; ssCallAccelRunBlock ( S , 0 , 20 ,
SS_CALL_MDL_OUTPUTS ) ; } UNUSED_PARAMETER ( tid ) ; }
/* Outputs for root system: '<Root>' */
static void mdlOutputs(SimStruct *S, int_T tid)
{
((BlockIO *) _ssGetBlockIO(S))->B_0_1_0 = ((real_T)((D_Work *) ssGetRootDWork
(S))->clockTickCounter < ((Parameters *) ssGetDefaultParam(S))->P_6) &&
(((D_Work *) ssGetRootDWork(S))->clockTickCounter >= 0) ? ((Parameters *)
ssGetDefaultParam(S))->P_4 : 0.0;
if ((real_T)((D_Work *) ssGetRootDWork(S))->clockTickCounter >= ((Parameters *)
ssGetDefaultParam(S))->P_5 - 1.0) {
((D_Work *) ssGetRootDWork(S))->clockTickCounter = 0;
} else {
((D_Work *) ssGetRootDWork(S))->clockTickCounter = ((D_Work *)
ssGetRootDWork(S))->clockTickCounter + 1;
}
((BlockIO *) _ssGetBlockIO(S))->B_0_2_0 = (muDoubleScalarSin(((Parameters *)
ssGetDefaultParam(S))->P_2 * ssGetTaskTime(S,0) + ((Parameters *)
ssGetDefaultParam(S))->P_3) * ((Parameters *) ssGetDefaultParam(S))->P_0 +
((Parameters *) ssGetDefaultParam(S))->P_1) * ((BlockIO *) _ssGetBlockIO(S)
)->B_0_1_0;
/* Scope: '<Root>/Scope' */
/* Call into Simulink for Scope */
ssCallAccelRunBlock(S, 0, 3, SS_CALL_MDL_OUTPUTS);
/* Scope: '<Root>/Scope2' */
/* Call into Simulink for Scope */
ssCallAccelRunBlock(S, 0, 4, SS_CALL_MDL_OUTPUTS);
/* tid is required for a uniform function interface.
* Argument tid is not used in the function. */
UNUSED_PARAMETER(tid);
}
hldvdzyyjm * localDW ) { localDW -> agffdh025d = false ; } void cqabclhqip (
SimStruct * const S , boolean_T etkm24oeau , real_T didof4byc4 , fzgld4ioxx *
localB , hldvdzyyjm * localDW , nneyi35s1a * localP , jppzrcodeg * localX ,
mgleimd2gn * localXdis ) { real_T hdawcmvty5 ; if ( ssIsSampleHit ( S , 1 , 0
) && ssIsMajorTimeStep ( S ) ) { if ( etkm24oeau ) { if ( ! localDW ->
agffdh025d ) { if ( ssGetTaskTime ( S , 1 ) != ssGetTStart ( S ) ) {
ssSetSolverNeedsReset ( S ) ; } localXdis -> g4hwiaob0z = 0 ; localDW ->
agffdh025d = true ; } } else { if ( localDW -> agffdh025d ) {
ssSetSolverNeedsReset ( S ) ; localXdis -> g4hwiaob0z = 1 ; gjooyfv3bl (
localDW ) ; } } } if ( localDW -> agffdh025d ) { hdawcmvty5 = ssGetT ( S ) ;
localB -> piz2wxjgln = localX -> g4hwiaob0z ; { real_T * * uBuffer = ( real_T
* * ) & localDW -> mapbuxhz55 . TUbufferPtrs [ 0 ] ; real_T * * tBuffer = (
real_T * * ) & localDW -> mapbuxhz55 . TUbufferPtrs [ 1 ] ; real_T simTime =
ssGetT ( S ) ; real_T tMinusDelay = simTime - localP -> P_2 ; localB ->
c1a15sku2w = Deadbeat_CUK_acc_rt_TDelayInterpolate ( tMinusDelay , 0.0 , *
tBuffer , * uBuffer , localDW -> phyjdg23m0 . CircularBufSize , & localDW ->
phyjdg23m0 . Last , localDW -> phyjdg23m0 . Tail , localDW -> phyjdg23m0 .
Head , localP -> P_3 , 0 , ( boolean_T ) ( ssIsMinorTimeStep ( S ) && (
ssGetTimeOfLastOutput ( S ) == ssGetT ( S ) ) ) ) ; } if ( ssIsSampleHit ( S
, 1 , 0 ) ) { localB -> cs4dg4krvc = localP -> P_4 ; localB -> lbagd2xbpx =
localDW -> maqntnbpoj ; } if ( hdawcmvty5 >= localB -> cs4dg4krvc ) { localB
-> a51b5puwm1 = localB -> piz2wxjgln - localB -> c1a15sku2w ; localB ->
ekk3k2wrwu = localP -> P_0 * localB -> a51b5puwm1 ; localB -> f3hneacniz =
localB -> ekk3k2wrwu ; } else { localB -> f3hneacniz = localB -> lbagd2xbpx ;
} localB -> ndc0pt2kls = didof4byc4 * didof4byc4 ; if ( ssIsMajorTimeStep ( S
) ) { localDW -> psx5wvplz4 = localB -> f3hneacniz >= localP -> P_6 ? 1 :
localB -> f3hneacniz > localP -> P_7 ? 0 : - 1 ; srUpdateBC ( localDW ->
igwl1d5vuc ) ; } } } void hdynn0hvxj ( SimStruct * const S , fzgld4ioxx *
localDW ) { localDW -> nej2n2fr4t = false ; } void bemu4fk2sq ( SimStruct *
const S , boolean_T fuz4opuchm , real_T nkqzvcukuv , bkp1n1bm33 * localB ,
mdhkgmfpbe * localDW , mvw1gbh0c1 * localP , pfdaqg4dlv * localXdis ) { if (
ssIsSampleHit ( S , 1 , 0 ) && ssIsMajorTimeStep ( S ) ) { if ( fuz4opuchm )
{ if ( ! localDW -> nej2n2fr4t ) { if ( ssGetTaskTime ( S , 1 ) !=
ssGetTStart ( S ) ) { ssSetSolverNeedsReset ( S ) ; } ( void ) memset ( & (
localXdis -> fq23fwubtb ) , 0 , 2 * sizeof ( boolean_T ) ) ; localDW ->
nej2n2fr4t = true ; } } else { if ( localDW -> nej2n2fr4t ) {
ssSetSolverNeedsReset ( S ) ; ( void ) memset ( & ( localXdis -> fq23fwubtb )
, 1 , 2 * sizeof ( boolean_T ) ) ; mctqnytrkq ( localDW ) ; } } } if (
localDW -> nej2n2fr4t ) { localB -> obmjfhqv1x = ( muDoubleScalarSin ( localP
-> P_4 * ssGetTaskTime ( S , 0 ) + localP -> P_5 ) * localP -> P_2 + localP
-> P_3 ) * nkqzvcukuv ; localB -> n024o4o5og = ( muDoubleScalarSin ( localP
-> P_8 * ssGetTaskTime ( S , 0 ) + localP -> P_9 ) * localP -> P_6 + localP
-> P_7 ) * nkqzvcukuv ; if ( ssIsMajorTimeStep ( S ) ) { srUpdateBC ( localDW
-> mdukihclqs ) ; } } } void jreinmfd0u ( bkp1n1bm33 * localB , mdhkgmfpbe *
void ) { } static void mdlOutputs ( SimStruct * S , int_T tid ) { real_T
jzxguy1xyo ; ZCEventType zcEvent ; boolean_T is1galx1wr ; real_T iden3vpls2 ;
real_T kewj0jjgdw ; real_T c3hti1p3uf ; int8_T rtPrevAction ; int8_T rtAction
; nmsgyp54ig * _rtB ; bbqvqz25ov * _rtP ; n2g2llwf21 * _rtX ; aa2ep02qn5 *
_rtZCE ; pklu3vjy1t * _rtDW ; _rtDW = ( ( pklu3vjy1t * ) ssGetRootDWork ( S )
) ; _rtZCE = ( ( aa2ep02qn5 * ) _ssGetPrevZCSigState ( S ) ) ; _rtX = ( (
n2g2llwf21 * ) ssGetContStates ( S ) ) ; _rtP = ( ( bbqvqz25ov * )
ssGetModelRtp ( S ) ) ; _rtB = ( ( nmsgyp54ig * ) _ssGetModelBlockIO ( S ) )
; if ( ssIsSampleHit ( S , 4 , 0 ) ) { _rtB -> jbbkv4upvi = _rtDW ->
idaicj53o4 ; } if ( ssIsSampleHit ( S , 1 , 0 ) ) { is1galx1wr = ( _rtB ->
jbbkv4upvi != c22ngqieus ( S ) -> pactsh03am ) ; } jzxguy1xyo = ssGetT ( S )
; if ( ssIsSampleHit ( S , 1 , 0 ) ) { _rtB -> oojsaunz11 = _rtP -> P_5 ;
_rtB -> fmra2cutsa = _rtP -> P_6 ; } _rtB -> fs1ofsmvy2 = muDoubleScalarRem (
jzxguy1xyo + _rtB -> oojsaunz11 , _rtB -> fmra2cutsa ) * _rtP -> P_7 ; _rtB
-> kdd430tca4 = ( ( 2.0 * _rtB -> fs1ofsmvy2 - 1.0 ) + 1.0 ) * 0.5 ; if (
ssIsSampleHit ( S , 1 , 0 ) ) { if ( ssIsMajorTimeStep ( S ) ) { _rtDW ->
fiaf5yjt4n = ( _rtB -> jbbkv4upvi >= _rtB -> kdd430tca4 ) ; } _rtB ->
bonoh3t2uf = ( is1galx1wr && _rtDW -> fiaf5yjt4n ) ; } _rtB -> puvw0myvhu [ 0
] = _rtB -> bonoh3t2uf ; _rtB -> puvw0myvhu [ 1 ] = 0.0 ; _rtB -> puvw0myvhu
[ 2 ] = 0.0 ; _rtDW -> lhiprm3wjz [ 0 ] = ! ( _rtB -> puvw0myvhu [ 0 ] ==
_rtDW -> lhiprm3wjz [ 1 ] ) ; _rtDW -> lhiprm3wjz [ 1 ] = _rtB -> puvw0myvhu
[ 0 ] ; _rtB -> puvw0myvhu [ 3 ] = _rtDW -> lhiprm3wjz [ 0 ] ; if (
ssIsSampleHit ( S , 2 , 0 ) ) { ssCallAccelRunBlock ( S , 9 , 15 ,
SS_CALL_MDL_OUTPUTS ) ; ssCallAccelRunBlock ( S , 9 , 16 ,
SS_CALL_MDL_OUTPUTS ) ; } if ( ssIsSampleHit ( S , 4 , 0 ) ) { _rtB ->
ni0skqgprw = _rtB -> mgrpxotnda [ 2 ] ; } if ( ssIsSampleHit ( S , 2 , 0 ) )
{ ssCallAccelRunBlock ( S , 9 , 20 , SS_CALL_MDL_OUTPUTS ) ; } if (
ssIsSampleHit ( S , 1 , 0 ) ) { zcEvent = rt_ZCFcn ( ANY_ZERO_CROSSING , &
_rtZCE -> mpe2yble25 , ( _rtB -> fs1ofsmvy2 - _rtP -> P_11 ) ) ; if ( _rtDW
-> oosqu0tl1h == 0 ) { if ( zcEvent != NO_ZCEVENT ) { _rtB -> dgvfzxjzqp = !
( _rtB -> dgvfzxjzqp != 0.0 ) ; _rtDW -> oosqu0tl1h = 1 ; } else if ( _rtB ->
dgvfzxjzqp == 1.0 ) { if ( _rtB -> fs1ofsmvy2 != _rtP -> P_11 ) { _rtB ->
dgvfzxjzqp = 0.0 ; } } else { if ( _rtB -> fs1ofsmvy2 == _rtP -> P_11 ) {
_rtB -> dgvfzxjzqp = 1.0 ; } } } else { if ( _rtB -> fs1ofsmvy2 != _rtP ->
P_11 ) { _rtB -> dgvfzxjzqp = 0.0 ; } _rtDW -> oosqu0tl1h = 0 ; } } if (
ssIsSampleHit ( S , 2 , 0 ) ) { ssCallAccelRunBlock ( S , 9 , 23 ,
SS_CALL_MDL_OUTPUTS ) ; ssCallAccelRunBlock ( S , 9 , 33 ,
SS_CALL_MDL_OUTPUTS ) ; iden3vpls2 = _rtP -> P_13 * _rtB -> mgrpxotnda [ 8 ]
; } if ( ssIsSampleHit ( S , 4 , 0 ) ) { _rtB -> gp5orjzg5t = _rtB ->
mgrpxotnda [ 2 ] ; } if ( ssIsSampleHit ( S , 2 , 0 ) ) { kewj0jjgdw = (
iden3vpls2 - _rtB -> gp5orjzg5t ) * _rtB -> nzunlww04y ; } if ( ssIsSampleHit
( S , 4 , 0 ) ) { _rtB -> jw2hkpsksy = _rtB -> gp5orjzg5t * _rtB ->
dvca5kgoqr ; _rtB -> elobzetbdp = _rtB -> mgrpxotnda [ 8 ] ; } if (
ssIsSampleHit ( S , 2 , 0 ) ) { c3hti1p3uf = ( kewj0jjgdw + _rtB ->
jw2hkpsksy ) - _rtB -> elobzetbdp ; if ( ssIsSampleHit ( S , 4 , 0 ) ) { _rtB
-> bc5lqmbcwx = _rtB -> mgrpxotnda [ 9 ] ; } } if ( ssIsSampleHit ( S , 4 , 0
) ) { _rtB -> bpiny53cki = _rtB -> elobzetbdp + _rtB -> bc5lqmbcwx ; } if (
ssIsSampleHit ( S , 2 , 0 ) ) { iden3vpls2 = c3hti1p3uf / _rtB -> bpiny53cki
+ _rtB -> idha0n310x ; if ( iden3vpls2 > _rtP -> P_16 ) { _rtB -> ccrq35nhp3
= _rtP -> P_16 ; } else if ( iden3vpls2 < _rtP -> P_17 ) { _rtB -> ccrq35nhp3
= _rtP -> P_17 ; } else { _rtB -> ccrq35nhp3 = iden3vpls2 ; } } if (
ssIsSampleHit ( S , 4 , 0 ) ) { _rtB -> pvzuuxenvc = _rtB -> ccrq35nhp3 ; }
if ( ssIsSampleHit ( S , 2 , 0 ) ) { _rtB -> ii04l1a0ic = ( _rtB ->
mgrpxotnda [ 9 ] + _rtB -> mgrpxotnda [ 8 ] ) - _rtB -> mgrpxotnda [ 6 ] ;
ssCallAccelRunBlock ( S , 9 , 51 , SS_CALL_MDL_OUTPUTS ) ; rtPrevAction =
_rtDW -> dcumqbbyun ; if ( ssIsMajorTimeStep ( S ) ) { if ( _rtB ->
mgrpxotnda [ 9 ] < 90.199999999999989 ) { rtAction = 0 ; } else { rtAction =
1 ; } _rtDW -> dcumqbbyun = rtAction ; } else { rtAction = _rtDW ->
dcumqbbyun ; } if ( rtPrevAction != rtAction ) { switch ( rtPrevAction ) {
case 0 : ssSetSolverNeedsReset ( S ) ; break ; case 1 : ssSetSolverNeedsReset
( S ) ; break ; case 2 : ssSetSolverNeedsReset ( S ) ; break ; } } switch (
rtAction ) { case 0 : if ( rtAction != rtPrevAction ) { if ( ssGetTaskTime (
S , 2 ) != ssGetTStart ( S ) ) { ssSetSolverNeedsReset ( S ) ; } } _rtB ->
kx5rgsvmx3 = _rtP -> P_0 * _rtB -> mgrpxotnda [ 8 ] ; if ( ssIsMajorTimeStep
( S ) ) { srUpdateBC ( _rtDW -> o5k0zwtbn4 ) ; } break ; case 1 : if (
rtAction != rtPrevAction ) { if ( ssGetTaskTime ( S , 2 ) != ssGetTStart ( S
) ) { ssSetSolverNeedsReset ( S ) ; } } _rtB -> kx5rgsvmx3 = _rtP -> P_1 ; if
( ssIsMajorTimeStep ( S ) ) { srUpdateBC ( _rtDW -> ihoovge3z4 ) ; } break ;
case 2 : if ( rtAction != rtPrevAction ) { if ( ssGetTaskTime ( S , 2 ) !=
ssGetTStart ( S ) ) { ssSetSolverNeedsReset ( S ) ; } } _rtB -> pvzuuxenvc =
_rtP -> P_2 ; if ( ssIsMajorTimeStep ( S ) ) { srUpdateBC ( _rtDW ->
gunoszsdaf ) ; } break ; } } bemu4fk2sq ( S , _rtB -> kwjevd5eqt , 0.0 , &
_rtB -> bemu4fk2sq4 , & _rtDW -> bemu4fk2sq4 , ( mvw1gbh0c1 * ) & _rtP ->
bemu4fk2sq4 , & ( ( npjij2cecc * ) ssGetContStateDisabled ( S ) ) ->
bemu4fk2sq4 ) ; cqabclhqip ( S , _rtB -> donhz1jhkq , 0.0 , & _rtB ->
cqabclhqip4 , & _rtDW -> cqabclhqip4 , ( nneyi35s1a * ) & _rtP -> cqabclhqip4
, & _rtX -> cqabclhqip4 , & ( ( npjij2cecc * ) ssGetContStateDisabled ( S ) )
-> cqabclhqip4 ) ; bemu4fk2sq ( S , _rtB -> jwapqatga5 , 0.0 , & _rtB ->
n1qdzssqu3 , & _rtDW -> n1qdzssqu3 , ( mvw1gbh0c1 * ) & _rtP -> n1qdzssqu3 ,
& ( ( npjij2cecc * ) ssGetContStateDisabled ( S ) ) -> n1qdzssqu3 ) ;
cqabclhqip ( S , _rtB -> p5ierutvgv , 0.0 , & _rtB -> nfugx5ih43 , & _rtDW ->
nfugx5ih43 , ( nneyi35s1a * ) & _rtP -> nfugx5ih43 , & _rtX -> nfugx5ih43 , &
( ( npjij2cecc * ) ssGetContStateDisabled ( S ) ) -> nfugx5ih43 ) ;
UNUSED_PARAMETER ( tid ) ; } static void mdlOutputsTID5 ( SimStruct * S ,
/* Function: rsimOneStepMT =====================================================
*
* Perform one step of the model.
* Errors are set in the SimStruct's ErrorStatus, NULL means no errors.
*/
static void rsimOneStepMT(SimStruct *S)
{
int_T i;
const int_T FIRST_TID = gbl_raccel_tid01eq ? 1 : 0;
int_T* sampleHit = ssGetSampleHitPtr(S);
ssSetSimTimeStep(S, MAJOR_TIME_STEP);
/* Clear the flag that blocks set in their major time step methods (output,
update, etc.) to flag that they have changed their state. If this flag is
true then we need to run model outputs again at the first minor step
(i.e., at the same time as the last major step).*/
ssClearBlkStateChange(S);
#ifdef DEBUG_TIMING
rsimDisplayTimingData(S,
sizeof(struct SimStruct_tag),
sizeof(struct _ssMdlInfo));
#endif
if (gblExtModeEnabled) {
rtExtModeOneStep(ssGetRTWExtModeInfo(S),
ssGetNumSampleTimes(S),
(boolean_T*)&ssGetStopRequested(S));
if (ssGetStopRequested(S)) return;
}
/* Setup the task times and sample hit flags for the discrete rates */
rsimUpdateDiscreteTaskTimesAndSampleHits(S);
if (ssGetErrorStatus(S) != NULL) return;
ssSetLogOutput(S,TRUE);
/* Do output, log, update for the base rate */
MdlOutputs(FIRST_TID);
if (gblExtModeEnabled) {
rtExtModeUploadCheckTrigger(ssGetNumSampleTimes(S));
rtExtModeUpload(FIRST_TID, ssGetTaskTime(S, FIRST_TID));
}
if (ssGetLogOutput(S)) {
(void)rt_UpdateTXYLogVars(ssGetRTWLogInfo(S), ssGetTPtr(S));
if (ssGetErrorStatus(S) != NULL) return;
}
MdlUpdate(FIRST_TID);
if (ssGetErrorStatus(S) != NULL) return;
/* Do not log outputs during minor time steps */
ssSetLogOutput(S, FALSE);
ssSetTimeOfLastOutput(S,ssGetT(S));
/* Call the solver push time forward based on the continuous dynamics */
if (ssGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME &&
!ssGetStopRequested(S) ) {
rsimUpdateSolverStopTimeForFixedStepMultiTaskingSim(S);
if (!ssGetStopRequested(S)) rsimAdvanceSolver(S);
}
/* Do output and update for the remaining rates */
ssSetLogOutput(S, TRUE);
for (i = FIRST_TID+1; i < gbl_raccel_NumST; i++) {
if ( !sampleHit[i] ) continue;
MdlOutputs(i);
rtExtModeUpload(i, ssGetTaskTime(S,i));
MdlUpdate(i);
}
if (ssGetErrorStatus(S) != NULL) return;
ssSetLogOutput(S, FALSE);
if (gblExtModeEnabled) {
rtExtModeCheckEndTrigger();
}
/* Update the timing engine and determine the solver stop time */
rsimUpdateTimingEngineAndSolverStopTime(S);
if (ssGetErrorStatus(S) != NULL) return;
} /* rsimOneStepMT */
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;
}
