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::preparationStep( double nextTime,
const VariablesGrid& _yRef
)
{
if ( controlLaw == 0 )
return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
/* 3) Evaluate reference trajectory */
VariablesGrid yRef( _yRef );
getCurrentReference( nextTime,yRef );
/* 4) Perform preparation step of control law */
if ( controlLaw->preparationStep( nextTime,yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
return SUCCESSFUL_RETURN;
}
returnValue Controller::preparationStep( double nextTime,
const VariablesGrid& _yRef
)
{
if ( controlLaw == 0 )
return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
/* Do nothing if controller is disabled */
if ( isEnabled == BT_FALSE )
return SUCCESSFUL_RETURN;
realClock.start();
controlLawClock.start();
/* 1) Evaluate reference trajectory */
VariablesGrid yRef( _yRef );
getCurrentReference( nextTime,yRef );
#ifdef SIM_DEBUG
yRef.print( "yRef preparation" );
#endif
/* 2) Perform preparation step of control law */
if ( controlLaw->preparationStep( nextTime,yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
controlLawClock.stop();
logCollection.setLast( LOG_TIME_CONTROL_LAW,controlLawClock.getTime() );
// stop real runtime measurement
realClock.stop();
logCollection.setLast( LOG_TIME_CONTROLLER,realClock.getTime() );
#ifdef SIM_DEBUG
Vector uTmp;
getU( uTmp );
uTmp.print("u(0) after preparationStep");
#endif
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;
}
returnValue Controller::init( double startTime,
const Vector& _x0,
const Vector& _p,
const VariablesGrid& _yRef
)
{
if ( controlLaw == 0 )
return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
/* 1) Initialize all sub-blocks. */
/* a) Estimator */
Vector xEst( _x0 );
Vector pEst( _p );
Vector xaEst, uEst, wEst;
if ( estimator != 0 )
{
if ( estimator->init( startTime,_x0,_p ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
if ( estimator->getOutputs( xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
}
/* b) Reference trajectory */
if ( ( referenceTrajectory != 0 ) && ( _yRef.isEmpty( ) == BT_TRUE ) )
if ( referenceTrajectory->init( startTime,xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
VariablesGrid yRef( _yRef );
getCurrentReference( startTime,yRef );
/* c) Control law */
if ( controlLaw->init( startTime,_x0,_p,yRef ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
/* 2) Consistency checks. */
if ( estimator != 0 )
{
if ( estimator->getNX( ) != controlLaw->getNX( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
if ( estimator->getNXA( ) != controlLaw->getNXA( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
if ( estimator->getNU( ) != controlLaw->getNU( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
if ( estimator->getNP( ) != controlLaw->getNP( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
if ( estimator->getNW( ) != controlLaw->getNW( ) )
return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
}
realClock.reset( );
setStatus( BS_READY );
return SUCCESSFUL_RETURN;
}
