HorizontalControl.h

Go to the documentation of this file.

#ifndef HORIZONTALCONTROL_H_
#define HORIZONTALCONTROL_H_

#include "component/SyncComponent.h"
#include "utils/Trajectory.h"

class Timestamp;
class UniversalDataReader;

class HorizontalControl : public SyncControlComponent
{
public:

    HorizontalControl( const Module* module );

    virtual ~HorizontalControl();

    void initialize( void );

    void run();

    void uninitialize( void );

protected:

    float headingIntegral_;
    float xteIntegral_;

private:

    // Note that the copy constructor below is private and not given a body.
    // Any attempt to call it will return a compiler error.
    HorizontalControl( const HorizontalControl& old ); // disallow copy constructor

    //Command Generator states
    Trajectory headingTrajectory_;

    bool ok_;


    float kdHeading_;
    float kiHeading_;
    float kpHeading_;
    float kwpHeading_;
    float kiwpHeading_;
    float maxKxte_;
    float maxHeadingInt_;
    float rudderLimit_;
    float headingRate_;
    float maxHeadingRateCfg_;
    float maxHdgAccel_;
    float xte_;

    float rudDeadband_;


    bool readConfig( void );

    void controlHeading();

    void setHeading( float desiredHeading, bool useInt = true );

    void setHeadingRate( float desiredheadingRate );

    void setRudderAngle( float desiredRudderAngle );

    float headingControl( float heading, float headingCmd, float yawRate, bool useInt = true );

    // Slate input setting variables

    DataReader *horizontalModeReader_;

    DataReader *latitudeCmdReader_;

    DataReader *longitudeCmdReader_;

    DataReader *bearingCmdReader_;

    DataReader *headingCmdReader_;

    DataReader *headingRateCmdReader_;

    DataReader *rudderAngleCmdReader_;

    // Configuration readers
    ConfigReader* kdHeadingCfgReader_;   // Derivative gain
    ConfigReader* kiHeadingCfgReader_;   // Integral gain
    ConfigReader* kpHeadingCfgReader_;   // Proportional gain
    ConfigReader* kwpHeadingCfgReader_;  // Cross-track error gain
    ConfigReader* kiwpHeadingCfgReader_; // Cross-track integral error gain
    ConfigReader* maxHdgAccelCfgReader_; // Max turn accel
    ConfigReader* maxHdgIntCfgReader_;   // Max cmded rudder from hdg int
    ConfigReader* maxHdgRateCfgReader_;  // Max turn rate
    ConfigReader* maxKxteCfgReader_;     // Max heading correction due to kwpHeading
    ConfigReader* rudDeadbandCfgReader_; // Degree of rounding in output values
    ConfigReader* rudLimitCfgReader_;    // Max rudder angle


    // Slate input measurements

    UniversalDataReader *headingReader_;

    DataWriter *headingInternalWriter_;

    DataWriter *smoothHeadingInternalWriter_;

    DataWriter *headingIntegralInternalWriter_;
    DataWriter *xteIntegralInternalWriter_;

    DataWriter *xteInternalWriter_;

    DataWriter *kxteInternalWriter_;

    DataWriter *bearingInternalWriter_;

    UniversalDataReader *headingRateReader_;

    UniversalDataReader *latitudeReader_;

    UniversalDataReader *longitudeReader_;

    UniversalDataReader *rudderAngleReader_;

    // Slate output variables

    //Output *outputRudderAngle_;
    DataWriter *rudderAngleActionWriter_;

    DataReader *rudderAngleActionReader_;

};

#endif /*SAMPLECOMPONENT_H_*/