WpHeadingController.h

#pragma once
 
#include <Eigen/Eigen>
using namespace Eigen;
 
#include "ControlAlgorithm.h"
 
 
namespace Ship{
    class WpHeadingController : public ControlAlgorithm
    {
    public:
        WpHeadingController(const std::string& simObjectName, ISimObjectCreator* const creator); 
        const double* TorqueBODY(const double T, const double* const X);
        void OdeFcn(const double T, const double* const X, double* const XDot) const; 
        const double* RudderAngle(const double T,const double* const X);
        const double* HeadingError(const double T,const double* const X);
 
 
    #ifdef FH_VISUALIZATION
        virtual void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator){} 
 
        virtual void RenderUpdate(const double T, const double* const X){} 
    #endif
 
    private:
        double Kp_yaw;                          
        double Ki_yaw;                          
        double Kd_yaw;                          
        double Kdelta;                          
        double m_dM;                            
        double m_dD;                            
        double m_dOmega_b;                      
        double m_dRelative_damping;             
        double zeta;                            
 
        double m_adTorqueBODY[3];               
        mutable double heading_error;                   
        double m_dRudderangle;                  
        double directionparameter;              
 
        int m_headingErrorIntegralIndex;        
 
        ISignalPort* adStateRotation;           
        ISignalPort* adStateOmega;
        ISignalPort* crossTrackErrorPort;       
        ISignalPort* desiredHeadingPort;        
 
 
    #ifdef FH_VISUALIZATION
        Ogre::SceneNode*    m_RenderNode;   
        double              m_Radius;       
        std::string             m_Material;     
        double              m_Color[3];     
    #endif
 
    };
}