returnValue Controller::feedbackStep( double currentTime,
const Vector& _y,
const VariablesGrid& _yRef
)
{
realClock.reset( );
if ( controlLaw == 0 )
return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
/* Do nothing if controller is disabled */
if ( isEnabled == BT_FALSE )
return SUCCESSFUL_RETURN;
// start real runtime measurement
realClock.start( );
Vector xEst, pEst;
/* 1) Call Estimator */
if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
/* 2) Evaluate reference trajectory */
VariablesGrid yRef( _yRef );
getCurrentReference( currentTime,yRef );
#ifdef SIM_DEBUG
yRef.print( "yRef feedback" );
#endif
controlLawClock.reset();
controlLawClock.start();
/* 3) Perform feedback step of control law */
if ( controlLaw->feedbackStep( currentTime,xEst,pEst,yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
controlLawClock.stop();
realClock.stop( );
#ifdef SIM_DEBUG
Vector uTmp;
getU( uTmp );
uTmp.print("u(0) after feedbackStep");
#endif
return SUCCESSFUL_RETURN;
}
returnValue Controller::feedbackStep( double currentTime,
const Vector& _y
)
{
if ( controlLaw == 0 )
return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
Vector xEst, pEst;
/* 1) Call Estimator */
if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
/* 2) Perform feedback step of control law */
if ( controlLaw->feedbackStep( currentTime,xEst,pEst ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
return SUCCESSFUL_RETURN;
}
returnValue Controller::step( double currentTime,
const Vector& _y,
const VariablesGrid& _yRef
)
{
/* Consistency check. */
if ( getStatus( ) != BS_READY )
return ACADOERROR( RET_BLOCK_NOT_READY );
if ( controlLaw == 0 )
return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
// start real runtime measurement
realClock.reset( );
realClock.start( );
RealClock clock;
//printf( "RTI? %d!!\n", controlLaw->isInRealTimeMode( ) );
if ( controlLaw->isInRealTimeMode( ) == BT_TRUE )
{
clock.reset();
clock.start();
if ( feedbackStep( currentTime,_y ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
double nextTime = currentTime;
if ( controlLaw != 0 )
nextTime += controlLaw->getSamplingTime( );
if ( preparationStep( nextTime,_yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
clock.stop();
logCollection.setLast( LOG_TIME_CONTROL_LAW,clock.getTime() );
}
else
{
/* 1) Call Estimator */
clock.reset();
clock.start();
Vector xEst, pEst;
if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
clock.stop();
logCollection.setLast( LOG_TIME_ESTIMATOR,clock.getTime() );
/* 2) Evaluate reference trajectory */
clock.reset();
clock.start();
VariablesGrid yRef( _yRef );
getCurrentReference( currentTime,yRef );
if ( controlLaw->step( currentTime,xEst,pEst,yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
clock.stop();
logCollection.setLast( LOG_TIME_CONTROL_LAW,clock.getTime() );
}
// stop real runtime measurement
realClock.stop( );
logCollection.setLast( LOG_TIME_CONTROLLER,realClock.getTime() );
return SUCCESSFUL_RETURN;
}