leakyIntegrator.hpp

Go to the documentation of this file.

/** \file leakyIntegrator.hpp
  * \author Jared R. Males (jaredmales@gmail.com)
  * \brief Declares and defines the leaky integrator controller class for AO control.
  * \ingroup mxAO_sim_files
  * 
  */
 
#ifndef __leakyIntegrator_hpp__
#define __leakyIntegrator_hpp__
 
namespace mx
{
namespace AO 
{
namespace sim 
{
   
template<typename _realT>
struct wfMeasurement
{
   typedef _realT realT;
   
   realT iterNo;
   
   typedef Eigen::Array< realT, Eigen::Dynamic, Eigen::Dynamic> commandT;
   
   commandT measurement;
};
 
///Implements the leaky integrator controller.
/**
  * \tparam _realT is the floating point type for all calculations.
  */  
template<typename _realT>
class leakyIntegrator
{
 
public:
   
   ///The real data type
   typedef _realT realT;
        
   ///The wavefront data type
   typedef wavefront<realT> wavefrontT;
   
   ///The command type
   typedef wfMeasurement<realT> commandT;
   
   ///The image type, used here as a general storage array
   typedef Eigen::Array< realT, Eigen::Dynamic, Eigen::Dynamic> imageT;
   
   ///Default c'tor.
   leakyIntegrator();
 
protected:
 
   int _nModes; ///< The number of modes being filtered.
 
   bool _openLoop; ///< If true, then commands are not integrated.
   
   int _closingDelay; ///< If > 0, then the gains are ramped linearly up to this value. Default = 0.
   
   int _lowOrders; ///< If > 0, then this sets the maximum mode number which is filtered. All remaining modes are set to 0.  Default = 0.
   
   
   imageT _gains;   ///< Column-vector of gains
   
   imageT _leaks; ///< Column-vector of leaks
 
   imageT _commands; ///< Column-vector past commands
 
   
   
 
public:
 
   int _lowOrderDelay;
   
   ///Allocate and initialize all state.
   /** 
     * \returns 0 on success, a negative integer otherwise.
     */ 
   int initialize(int nModes /**< [in] the number of modes to be filtered */ );
 
   ///Get the number of modes
   /** nModes is only set by calling initialize.
     *
     * \returns the current value of _nModes
     */ 
   int nModes();
   
   ///Set the _openLoop flag.
   /** If _openLoop is true, then commands are not filtered.
     *
     * \returns 0 on success, a negative integer on error. 
     */
   int openLoop(bool ol /**< The new value of _openLoop */ );
   
   ///Get the value of the _openLoop flag.
   /** 
     * \returns the current value of _openLoop.
     */
   bool openLoop();
   
   ///Set _closingDelay.
   /** If _closingDelay  > 0, then the gains are ramped linearly up to this value.
     *
     * \returns 0 on success, a negative integer on error. 
     */
   int closingDelay( int cd /**< The new value of _closingDelay */);
 
   ///Get the value of the _closingDelay.
   /** 
     * \returns the current value of _closingDelay.
     */   
   int closingDelay();
 
   ///Set _lowOrders.
   /** If _lowOrders > 0, then this sets the maximum mode number which is filtered. All remaining modes are set to 0. 
     *
     * \returns 0 on success, a negative integer on error. 
     */   
   int lowOrders( int lo /**< The new value of _lowOrders */);
 
   ///Get the value of the _lowOrders.
   /** 
     * \returns the current value of _lowOrders.
     */   
   int lowOrders();
   
   ///Get the gain for a single mode.
   /** \returns the gain value if mode exists.  
     */
   realT gain( int i /**< [in] the mode number*/);
   
   ///Set the gain for a single mode.
   /** 
     * \returns 0 on success, negative number on error.
     */
   int gain( int i,  ///< [in] the mode number 
             realT g ///< [in] the new gain value
           );
   
   ///Set the gain for all modes to a single value
   /** 
     * \returns 0 on success, negative number on error.
     */
   int gains(realT g /**< [in] the new gain value*/ );
 
   ///Set the gains for all modes, using a vector to specify each gain
   /** The vector must be exactly as long as _nModes. 
     * 
     * \returns 0 on success, negative number on error.
     */
   int gains(const std::vector<realT> & vgains /**< [in] vector of gains.*/);
   
   ///Set the gains for all modes, using a file to specify each gain
   /** The file format is a simple ASCII single column, with 1 gain per line.
     * Must be exactly as long as _nModes. 
     * 
     * \returns 0 on success, negative number on error.
     */
   int gains(const std::string & ogainf /**< [in] the name of the file, full path */);
   
   
   ///Set the leak for a single mode.
   /**
     * \returns 0 on success, negative number on error.
     */
   int leak( int i, ///< [in] the mode number
             realT l ///< [in] the new leak value for this mode
           );
   
   ///Set a leak for all modes.
   /**
     * \returns 0 on success, negative number on error.
     */
   int leaks( realT l /**< [in] the new leak value to set for all modes */ );
 
 
   ///Allocate the provided command structures
   /** Used by the calling system to allocate the commands being passed between components.
     *
     * \returns 0 on success, negative number on error.
     */ 
   int initMeasurements( commandT & filtAmps, ///< The structure to contain the filtered commands 
                         commandT & rawAmps   ///< The structure to contain the raw commands
                       );
 
      
   ///Apply the leaky integrator
   /**
     * \returns 0 on success, negative number on error.
     */ 
   int filterCommands( commandT & filtAmps, ///< [out] the filtered commands
                       commandT & rawAmps, ///< [in] the raw commands
                       int iterNo ///< [in] The current iteration number
                     );
 
   
 
};
 
 
template<typename realT>
leakyIntegrator<realT>::leakyIntegrator()
{
   _nModes = 0;
 
   _openLoop = false;
   
   _closingDelay = 0;
   
   _lowOrders = 0;
   _lowOrderDelay = 0;
 
}
 
 
 
template<typename realT>
int leakyIntegrator<realT>::initialize(int nModes)
{
   _nModes = nModes;
   
   _gains.resize(1,nModes);
   _leaks.resize(1, nModes);
   _commands.resize(1, nModes);
   
   _gains.setZero();
   
   _leaks.setZero();
   
   _commands.setZero();
   
   return 0;
   
}
 
template<typename realT>
int leakyIntegrator<realT>::nModes()
{
   return _nModes;
}
 
template<typename realT>
int leakyIntegrator<realT>::openLoop( bool ol )
{
   _openLoop = ol;
   return 0;
}
 
template<typename realT>
bool leakyIntegrator<realT>::openLoop()
{
   return _openLoop;
}
 
template<typename realT>
int leakyIntegrator<realT>::closingDelay( int cd )
{
   _closingDelay = cd;
   
   return 0;
}
 
template<typename realT>
int leakyIntegrator<realT>::closingDelay()
{
   return _closingDelay;
}
 
template<typename realT>
int leakyIntegrator<realT>::lowOrders( int lo )
{
   _lowOrders = lo;
   
   return 0;
}
 
template<typename realT>
int leakyIntegrator<realT>::lowOrders()
{
   return _lowOrders;
}
 
template<typename realT>
realT leakyIntegrator<realT>::gain(int i)
{
   if(i < 0 || i >= _nModes)
   {
      mxError("leakyIntegrator::gain", MXE_INVALIDARG, "mode index out of range");
      return 0; ///\retval 0 if the mode doesn't exist.
   }
   
   return _gains(i);
}
 
   
template<typename realT>
int leakyIntegrator<realT>::gain(int i, realT g)
{
   if(i < 0 || i >= _nModes)
   {
      mxError("leakyIntegrator::gain", MXE_INVALIDARG, "mode index out of range");
      return 0; ///\retval 0 if the mode doesn't exist.
   }
   
   _gains(0,i) = g;
   
   return 0;
}
 
template<typename realT>
int leakyIntegrator<realT>::gains(realT g)
{
   for(int i=0;i<_gains.cols(); ++i) 
   {
      _gains(0,i) = g;
   }
   
   return 0;
}
 
template<typename realT>
int leakyIntegrator<realT>::gains( const std::vector<realT> & gains )
{
   
   if( gains.size() != _gains.cols())
   {
      mxError("leakyIntegrator::gain", MXE_SIZEERR, "input gain vector not same size as number of modes");
      return -1; ///\retval -1 on vector size mismatch
   }
   
   for(int i=0;i<_gains.cols(); ++i) 
   {
      _gains(0,i) = gains[i];
   }
   
   return 0;
}
   
template<typename realT>
int leakyIntegrator<realT>::gains( const std::string & ogainf )
{
   std::ifstream fin;
   fin.open(ogainf);
   
   if(!fin.good())
   {
      mxError("leakyIntegrator::gains", MXE_FILEOERR, "could not open gan file");
      return -1; /// \retval -1 if file open fails
   }
   
   std::string tmpstr;
   realT g;
   for(int i=0;i<_gains.cols();++i)
   {
      fin >> tmpstr;
      g = mx::convertFromString<realT>(tmpstr);
      gain(i, g);
 
   }
   
   return 0;
   
}
 
template<typename realT>
int leakyIntegrator<realT>::leak(int i, realT l)
{
   _leaks(0,i) = l;
}
 
template<typename realT>
int leakyIntegrator<realT>::leaks(realT l)
{
   for(int i=0;i<_leaks.cols(); ++i) _leaks(0,i) = l;
   
   return 0;
}
 
template<class realT>
int leakyIntegrator<realT>::initMeasurements(commandT & filtAmps, commandT & rawAmps)
{
   filtAmps.measurement.resize(1, _nModes);
   filtAmps.measurement.setZero();
      
   rawAmps.measurement.resize(1, _nModes);
   rawAmps.measurement.setZero();
    
   return 0;
}
 
template<class realT>
int leakyIntegrator<realT>::filterCommands( commandT & filtAmps, 
                                            commandT & rawAmps,
                                            int iterNo )
{
   filtAmps.iterNo = rawAmps.iterNo;
   
   if(_openLoop) 
   {
      filtAmps.measurement.setZero();
      return 0;
   }
   
   int topN = rawAmps.measurement.cols();
   
   
   
   realT gainF = 1.0; 
   realT HOgainF = 0.0;
   
   //leaks(0.01);
   
   if( iterNo < _closingDelay) gainF = ((realT) iterNo)/_closingDelay;
      
   //if( iterNo < 0.55*_closingDelay) leaks(0.1);
   
   if(_lowOrders > 0)
   {
      if( iterNo >= _closingDelay+_lowOrderDelay)
      {
         _lowOrders = 0;
      }
      else
      {
         topN = _lowOrders;   
         
         if( iterNo >= _closingDelay )
         {
            HOgainF = ( (realT) (iterNo - _closingDelay) )/( _lowOrderDelay );
            
         }
         
      }
   }
   
   
   for(int i=0; i< topN; ++i)
   {
      if( std::isnan( rawAmps.measurement(0,i) ) || !std::isfinite(rawAmps.measurement(0,i))) rawAmps.measurement(0,i) = 0.0; 
 
      _commands(0,i) = (1.0-_leaks(0,i))*_commands(0,i) + gainF*_gains(0,i)*rawAmps.measurement(0,i);
      
      filtAmps.measurement(0,i) = _commands(0,i);
   }
   
   for(int i=topN; i< rawAmps.measurement.cols(); ++i)
   {      
      if( std::isnan( rawAmps.measurement(0,i) ) || !std::isfinite(rawAmps.measurement(0,i))) rawAmps.measurement(0,i) = 0.0; 
 
      _commands(0,i) = (1.0-_leaks(0,i))*_commands(0,i) + HOgainF*_gains(0,i)*rawAmps.measurement(0,i);
      
      filtAmps.measurement(0,i) = _commands(0,i);
      
      //filtAmps.measurement(0,i) = 0;
   }
   
   
   return 0;
   
}
 
 
 
} //namespace sim 
} //namespace AO
} //namespace mx 
 
#endif //__leakyIntegrator_hpp__