alignLoop.hpp

Go to the documentation of this file.

/** \file alignLoop.hpp
 * \brief The MagAO-X alignment loop application header file.
 *
 * \ingroup alignLoop_files
 */
 
#ifndef alignLoop_hpp
#define alignLoop_hpp
 
#include "../../libMagAOX/libMagAOX.hpp" //Note this is included on command line to trigger pch
#include "../../magaox_git_version.h"
 
/** \defgroup alignLoop
 * \brief The MagAO-X application for alignment-loop control.
 *
 * <a href="../handbook/operating/software/apps/XXXXXX.html">Application Documentation</a>
 *
 * \ingroup apps
 *
 */
 
/** \defgroup alignLoop_files
 * \ingroup alignLoop
 */
 
namespace MagAOX
{
namespace app
{
 
/// The MagAO-X alignment-loop controller.
/**
 * \ingroup alignLoop
 */
class alignLoop : public MagAOXApp<true>, public dev::shmimMonitor<alignLoop>
{
    // Give the test harness access.
    friend class alignLoop_test;
 
    friend class dev::shmimMonitor<alignLoop>;
 
  public:
    // The base shmimMonitor type
    typedef dev::shmimMonitor<alignLoop> shmimMonitorT;
 
  protected:
    /** \name Configurable Parameters
     *@{
     */
 
    std::vector<std::string> m_ctrlDevices;    ///< Device names of the controlled axes.
    std::vector<std::string> m_ctrlProperties; ///< INDI properties used to send loop commands.
    std::vector<std::string> m_ctrlCurrents;   ///< INDI elements holding the current controller values.
    std::vector<std::string> m_ctrlTargets;    ///< INDI elements to which updated commands are written.
 
    std::vector<float> m_currents; ///< Most recently received controller values.
 
    std::vector<pcf::IndiProperty> m_indiP_ctrl; ///< Registered controller properties for each axis.
 
    std::string m_intMatFile; ///< Interaction matrix file name from configuration.
 
    std::vector<float> m_defaultGains; ///< Default per-axis gains loaded from configuration.
 
    std::string m_upstreamDevice;                   ///< Upstream loop device that this loop can follow.
    std::string m_upstreamProperty{ "loop_state" }; ///< Toggle property on the upstream loop to monitor.
    bool        m_upstreamFollowClosed{ false };    ///< If true, this loop closes when the upstream loop closes.
 
    ///@}
 
    mx::improc::eigenImage<float> m_intMat; ///< Interaction matrix mapping measurements to commands.
 
    mx::improc::eigenImage<float> m_measurements; ///< Most recent measured loop disturbance vector.
    float                         m_delta0{ 0 };  ///< Latest first-axis residual.
    float                         m_delta1{ 0 };  ///< Latest second-axis residual.
 
    mx::improc::eigenImage<float> m_commands; ///< Most recent command delta vector.
 
    float m_ggain{ 0 }; ///< Global loop gain applied to all axes.
 
    std::vector<float> m_gains; ///< Active per-axis gains.
 
    bool m_ctrlEnabled{ false }; ///< True when the loop is closed and may send controller commands.
 
  public:
    /// Default c'tor.
    alignLoop();
 
    /// D'tor, declared and defined for noexcept.
    ~alignLoop() noexcept
    {
    }
 
    /// Set up the app configuration interface.
    virtual void setupConfig();
 
    /// Implementation of loadConfig logic, separated for testing.
    /** This is called by loadConfig().
     */
    int loadConfigImpl(
        mx::app::appConfigurator &_config /**< [in] an application configuration from which to load values*/ );
 
    /// Load the application configuration.
    virtual void loadConfig();
 
    /// Startup function
    /**
     *
     */
    virtual int appStartup();
 
    /// Implementation of the FSM for alignLoop.
    /**
     * \returns 0 on no critical error
     * \returns -1 on an error requiring shutdown
     */
    virtual int appLogic();
 
    /// Shutdown the app.
    /**
     *
     */
    virtual int appShutdown();
 
    /// Change the loop state.
    int toggleLoop( bool onoff /**< [in] true to close the loop and false to open it */ );
 
    // shmimMonitor interface:
    /// Allocate working memory for the monitored image stream.
    int allocate( const dev::shmimT &dummy /**< [in] ignored shmim descriptor */ );
 
    /// Process a new measurement image from the monitored stream.
    int processImage( void              *curr_src /**< [in] pointer to the current image buffer */,
                      const dev::shmimT &dummy /**< [in] ignored shmim descriptor */
    );
 
    /// Send the newly computed commands to the controller devices.
    int sendCommands( std::vector<float> &commands /**< [in] command values to transmit */ );
 
    // INDI
 
    pcf::IndiProperty m_indiP_deltas; ///< Published loop residual values.
 
    pcf::IndiProperty m_indiP_ggain;       ///< INDI property exposing the global loop gain.
    pcf::IndiProperty m_indiP_ctrlEnabled; ///< INDI property exposing the loop open/closed state.
 
    INDI_NEWCALLBACK_DECL( alignLoop, m_indiP_ggain );
    INDI_NEWCALLBACK_DECL( alignLoop, m_indiP_ctrlEnabled );
 
    /// Route controller property updates into the instance callback.
    static int st_setCallBack_ctrl( void                    *app /**< [in] application instance pointer */,
                                    const pcf::IndiProperty &ipRecv /**< [in] controller property update */
    )
    {
        return static_cast<alignLoop *>( app )->setCallBack_ctrl( ipRecv );
    }
 
    /// Update cached controller current values from an INDI property callback.
    int setCallBack_ctrl( const pcf::IndiProperty &ipRecv /**< [in] controller property update */ );
 
    int               m_upstreamState{ 0 }; ///< Cached upstream state indicator.
    pcf::IndiProperty m_indiP_upstream;     ///< Property used to monitor the upstream loop state.
 
    INDI_SETCALLBACK_DECL( alignLoop, m_indiP_upstream );
};
 
alignLoop::alignLoop() : MagAOXApp( MAGAOX_CURRENT_SHA1, MAGAOX_REPO_MODIFIED )
{
 
    return;
}
 
void alignLoop::setupConfig()
{
    shmimMonitorT::setupConfig( config );
 
    config.add( "ctrl.devices",
                "",
                "ctrl.devices",
                argType::Required,
                "ctrl",
                "devices",
                false,
                "string",
                "Device names of the controller(s) (one per element)." );
    config.add( "ctrl.properties",
                "",
                "ctrl.properties",
                argType::Required,
                "ctrl",
                "properties",
                false,
                "string",
                "Properties of the ctrl devices to which to give the commands. One per element" );
    config.add( "ctrl.currents",
                "",
                "ctrl.currents",
                argType::Required,
                "ctrl",
                "currents",
                false,
                "vector<string>",
                "current elements of the properties on which base the commands." );
    config.add( "ctrl.targets",
                "",
                "ctrl.targets",
                argType::Required,
                "ctrl",
                "targets",
                false,
                "vector<string>",
                "target elements of the properties to which to send the commands." );
 
    config.add(
        "loop.gain", "", "loop.gain", argType::Required, "loop", "gain", false, "float", "default global loop gain." );
    config.add( "loop.intMat",
                "",
                "loop.intMat",
                argType::Required,
                "loop",
                "intMat",
                false,
                "string",
                "file name of the interaction matrix." );
    config.add( "loop.gains",
                "",
                "loop.gains",
                argType::Required,
                "loop",
                "gains",
                false,
                "vector<float>",
                "default loop gains.  If single number, it is applied to all axes." );
    config.add( "loop.upstream",
                "",
                "loop.upstream",
                argType::Required,
                "loop",
                "upstream",
                false,
                "string",
                "Upstream loop device name.  This loop will open, and optionally close, with the upstream loop.  "
                "Default none." );
    config.add( "loop.upstreamProperty",
                "",
                "loop.upstreamProperty",
                argType::Required,
                "loop",
                "upstreamProperty",
                false,
                "string",
                "Property of upstream loop device to follow.  Must be a toggle.  Default is loop_state." );
    config.add( "loop.upstreamFollowClosed",
                "",
                "loop.upstreamFollowClosed",
                argType::Required,
                "loop",
                "upstreamFollowClosed",
                false,
                "bool",
                "If true, this loop also closes when the upstream loop closes. Default is false." );
}
 
int alignLoop::loadConfigImpl( mx::app::appConfigurator &_config )
{
    shmimMonitorT::loadConfig( _config );
 
    _config( m_ctrlDevices, "ctrl.devices" );
    _config( m_ctrlProperties, "ctrl.properties" );
    _config( m_ctrlCurrents, "ctrl.currents" );
    _config( m_ctrlTargets, "ctrl.targets" );
    _config( m_ggain, "loop.gain" );
    _config( m_intMatFile, "loop.intMat" );
    _config( m_defaultGains, "loop.gains" );
    _config( m_upstreamDevice, "loop.upstream" );
    _config( m_upstreamProperty, "loop.upstreamProperty" );
    _config( m_upstreamFollowClosed, "loop.upstreamFollowClosed" );
    return 0;
}
 
void alignLoop::loadConfig()
{
    loadConfigImpl( config );
}
 
int alignLoop::appStartup()
{
    if( shmimMonitorT::appStartup() < 0 )
    {
        return log<software_error, -1>( { __FILE__, __LINE__ } );
    }
 
    if( m_ctrlTargets.size() != m_ctrlDevices.size() )
    {
        return log<software_error, -1>( { __FILE__, __LINE__, "ctrl.Targets and ctrl.devices are not the same size" } );
    }
 
    if( m_ctrlTargets.size() != m_ctrlProperties.size() )
    {
        return log<software_error, -1>(
            { __FILE__, __LINE__, "ctrl.Targets and ctrl.properties are not the same size" } );
    }
 
    if( m_ctrlTargets.size() != m_ctrlCurrents.size() )
    {
        return log<software_error, -1>(
            { __FILE__, __LINE__, "ctrl.Currents and ctrl.properties are not the same size" } );
    }
 
    if( m_ctrlTargets.size() != m_defaultGains.size() )
    {
        if( m_defaultGains.size() == 1 )
        {
            float g = m_defaultGains[0];
            m_defaultGains.resize( m_ctrlTargets.size(), g );
            log<text_log>( "Setting loop.gains gains to be same size as ctrl.Targets", logPrio::LOG_NOTICE );
        }
        else
        {
            return log<software_error, -1>(
                { __FILE__, __LINE__, "ctrl.Targets and loop.gains are not the same size" } );
        }
    }
 
    CREATE_REG_INDI_RO_NUMBER( m_indiP_deltas, "deltas", "Deltas", "Deltas" );
    m_indiP_deltas.add( pcf::IndiElement( "delta0" ) );
    m_indiP_deltas["delta0"] = 0;
    m_indiP_deltas.add( pcf::IndiElement( "delta1" ) );
    m_indiP_deltas["delta1"] = 0;
 
    m_gains.resize( m_defaultGains.size() );
    for( size_t n = 0; n < m_defaultGains.size(); ++n )
        m_gains[n] = m_defaultGains[n];
 
    createStandardIndiNumber<unsigned>( m_indiP_ggain, "loop_gain", 0, 1, 0, "%0.2f" );
    m_indiP_ggain["current"] = m_ggain;
    m_indiP_ggain["target"]  = m_ggain;
    if( registerIndiPropertyNew( m_indiP_ggain, INDI_NEWCALLBACK( m_indiP_ggain ) ) < 0 )
    {
        log<software_error>( { __FILE__, __LINE__ } );
        return -1;
    }
 
    createStandardIndiToggleSw( m_indiP_ctrlEnabled, "loop_state" );
    if( registerIndiPropertyNew( m_indiP_ctrlEnabled, INDI_NEWCALLBACK( m_indiP_ctrlEnabled ) ) < 0 )
    {
        log<software_error>( { __FILE__, __LINE__ } );
        return -1;
    }
 
    m_currents.resize( m_ctrlDevices.size(), -1e15 );
 
    m_indiP_ctrl.resize( m_ctrlDevices.size() );
 
    for( size_t n = 0; n < m_ctrlDevices.size(); ++n )
    {
        registerIndiPropertySet( m_indiP_ctrl[n], m_ctrlDevices[n], m_ctrlProperties[n], &st_setCallBack_ctrl );
    }
 
    m_commands.resize( m_ctrlTargets.size(), m_ctrlTargets.size() );
    m_commands.setZero();
 
    m_intMat.resize( m_ctrlTargets.size(), m_ctrlTargets.size() );
    m_intMat.setZero();
    m_intMat( 1, 0 ) = 0.926;
    m_intMat( 1, 1 ) = -0.370;
    m_intMat( 0, 0 ) = 0.185;
    m_intMat( 0, 1 ) = 0.926;
 
    /*
       std::string intMatPath = m_calibDir + "/" + m_intMatFile;
 
       mx::fits::fitsFile<float> ff;
       try
       {
          ff.read(m_intMat, intMatPath);
       }
       catch(...)
       {
          return log<software_error, -1>({__FILE__, __LINE__, "error reading loop.intMatFile.  Does it exist?"});
       }
 
       if(m_intMat.rows() != m_ctrlTargets.size())
       {
          return log<software_error, -1>({__FILE__, __LINE__, "interaction matrix wrong size: rows do not match sensor
       Targets"});
       }
 
       if(m_intMat.cols() != m_correctorTargets.size())
       {
          return log<software_error, -1>({__FILE__, __LINE__, "interaction matrix wrong size: cols do not match
       corrector Targets"});
       }
    */
 
    // Get the loop state for managing offloading
    if( m_upstreamDevice != "" )
    {
        REG_INDI_SETPROP( m_indiP_upstream, m_upstreamDevice, m_upstreamProperty );
    }
 
    return 0;
}
 
int alignLoop::appLogic()
{
    if( shmimMonitorT::appLogic() < 0 )
    {
        return log<software_error, -1>( { __FILE__, __LINE__ } );
    }
 
    state( stateCodes::OPERATING );
 
    return 0;
}
 
int alignLoop::appShutdown()
{
    shmimMonitorT::appShutdown();
 
    return 0;
}
 
int alignLoop::toggleLoop( bool onoff )
{
    if( !m_ctrlEnabled && onoff ) // not enabled so change
    {
        m_ctrlEnabled = true;
        log<loop_closed>();
        updateSwitchIfChanged( m_indiP_ctrlEnabled, "toggle", pcf::IndiElement::On, INDI_OK );
        return 0;
    }
 
    if( m_ctrlEnabled && !onoff )
    {
        m_ctrlEnabled = false;
        log<loop_open>();
        updateSwitchIfChanged( m_indiP_ctrlEnabled, "toggle", pcf::IndiElement::Off, INDI_IDLE );
 
        return 0;
    }
 
    return 0;
}
 
inline int alignLoop::allocate( const dev::shmimT &dummy )
{
    static_cast<void>( dummy );
 
    std::lock_guard<std::mutex> guard( m_indiMutex );
 
    std::cerr << shmimMonitorT::m_width << shmimMonitorT::m_height << "\n";
    m_measurements.resize( shmimMonitorT::m_width, shmimMonitorT::m_height );
 
    return 0;
}
 
inline int alignLoop::processImage( void *curr_src, const dev::shmimT &dummy )
{
    static_cast<void>( dummy );
 
    for( unsigned nn = 0; nn < shmimMonitorT::m_width * shmimMonitorT::m_height; ++nn )
    {
        m_measurements.data()[nn] = ( static_cast<float *>( curr_src ) )[nn];
    }
 
    m_delta0 = m_measurements( 0, 0 );
    m_delta1 = m_measurements( 1, 0 );
 
    std::cout << "measurements: ";
    for( int cc = 0; cc < m_measurements.rows(); ++cc )
    {
        std::cout << m_measurements( cc, 0 ) << " ";
    }
    std::cout << "\n";
 
    if( m_currents[0] < -1e14 )
        return 0;
 
    m_commands.matrix() = m_intMat.matrix() * m_measurements.matrix();
 
    std::cout << "delta commands:    ";
    for( int cc = 0; cc < m_measurements.rows(); ++cc )
    {
        std::cout << -m_commands( cc, 0 ) << " ";
    }
    std::cout << "\n";
 
    std::vector<float> commands;
    commands.resize( m_measurements.rows() );
 
    std::cout << "commands:    ";
    for( int cc = 0; cc < m_measurements.rows(); ++cc )
    {
        commands[cc] = m_currents[cc] - m_ggain * m_gains[cc] * m_commands( cc, 0 );
        std::cout << commands[cc] << " ";
    }
    std::cout << "\n";
 
    // And send commands.
    int rv;
    if( m_ctrlEnabled )
    {
        rv = sendCommands( commands );
    }
    else
    {
        rv = 0;
    }
 
    updateIfChanged( m_indiP_deltas,
                     std::vector<std::string>( { "delta0", "delta1" } ),
                     std::vector<float>( { m_delta0, m_delta1 } ) );
    return rv;
}
 
inline int alignLoop::sendCommands( std::vector<float> &commands )
{
    for( size_t n = 0; n < m_ctrlDevices.size(); ++n )
    {
        pcf::IndiProperty ip( pcf::IndiProperty::Number );
 
        ip.setDevice( m_ctrlDevices[n] );
        ip.setName( m_ctrlProperties[n] );
        ip.add( pcf::IndiElement( m_ctrlTargets[n] ) );
        ip[m_ctrlTargets[n]] = commands[n];
 
        sendNewProperty( ip );
    }
 
    return 0;
}
 
INDI_NEWCALLBACK_DEFN( alignLoop, m_indiP_ggain )( const pcf::IndiProperty &ipRecv )
{
    if( ipRecv.getName() != m_indiP_ggain.getName() )
    {
        log<software_error>( { __FILE__, __LINE__, "invalid indi property received" } );
        return -1;
    }
 
    float target;
 
    if( indiTargetUpdate( m_indiP_ggain, target, ipRecv, true ) < 0 )
    {
        log<software_error>( { __FILE__, __LINE__ } );
        return -1;
    }
 
    m_ggain = target;
 
    updateIfChanged( m_indiP_ggain, "current", m_ggain );
    updateIfChanged( m_indiP_ggain, "target", m_ggain );
 
    log<text_log>( "set global gain to " + std::to_string( m_ggain ), logPrio::LOG_NOTICE );
 
    return 0;
}
 
INDI_NEWCALLBACK_DEFN( alignLoop, m_indiP_ctrlEnabled )( const pcf::IndiProperty &ipRecv )
{
    if( ipRecv.getName() != m_indiP_ctrlEnabled.getName() )
    {
        log<software_error>( { __FILE__, __LINE__, "invalid indi property received" } );
        return -1;
    }
 
    // switch is toggled to on
    if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On )
    {
        return toggleLoop( true );
    }
 
    // switch is toggle to off
    if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::Off )
    {
        return toggleLoop( false );
    }
 
    return 0;
}
 
inline int alignLoop::setCallBack_ctrl( const pcf::IndiProperty &ipRecv )
{
    for( size_t n = 0; n < m_ctrlDevices.size(); ++n )
    {
        if( ipRecv.getDevice() == m_ctrlDevices[n] )
        {
            if( ipRecv.getName() == m_ctrlProperties[n] && ipRecv.find( m_ctrlCurrents[n] ) )
            {
                m_currents[n] = ipRecv[m_ctrlCurrents[n]].get<float>();
            }
        }
    }
 
    return 0;
}
 
INDI_SETCALLBACK_DEFN( alignLoop, m_indiP_upstream )( const pcf::IndiProperty &ipRecv )
{
    if( ipRecv.getName() != m_indiP_upstream.getName() )
    {
        return log<software_error>( { __FILE__, __LINE__, "wrong INDI property received" } );
    }
 
    if( !ipRecv.find( "toggle" ) )
        return 0;
 
    if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::On && m_upstreamFollowClosed )
    {
        std::cerr << "upstream on\n";
        return toggleLoop( true );
    }
    else if( ipRecv["toggle"].getSwitchState() == pcf::IndiElement::Off )
    {
        std::cerr << "upstream off\n";
        return toggleLoop( false );
    }
 
    return 0;
}
 
} // namespace app
} // namespace MagAOX
 
#endif // alignLoop_hpp