WpHeadingController.h

#pragma once
 
#include <string>
 
#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; 
        void ComputeHeadingError(const double T, const double* const X) 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 zeta = 0.0;                      
 
        double m_adTorqueBODY[3] = {0.0, 0.0, 0.0};             
        mutable double heading_error = 0.0;                 
        double m_dRudderangle = 0.0;                    
        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
 
    };
}