GoToSurface.h

Go to the documentation of this file.

#ifndef GOTOSURFACE_H_
#define GOTOSURFACE_H_

#include "component/Behavior.h"

class UniversalDataReader;

class GoToSurface : public Behavior
{
public:

    GoToSurface( const Str& prefix, const Module* module );

    virtual ~GoToSurface();

    void initialize( void );

    bool readParams( float& depth );

    bool calcSatisfied( const float depth );

    void run();

    bool isSatisfied();

    bool runIfUnsatisfied();

    void uninitialize( void );

    static Behavior* CreateBehavior( const Str& prefix, const Module* module );

protected:

    // Slate input setting variables

    SettingReader* depthRateSettingReader_;
    SettingReader* pitchSettingReader_;
    SettingReader* speedSettingReader_;
    SettingReader* pitchTimeoutSettingReader_;
    SettingReader* surfaceTimeoutSettingReader_;

    // Slate input measurements

    // Current depth (in meter from sea surface -- positive down)
    UniversalDataReader* depthReader_;

    // Current height above sea floor
    UniversalDataReader *altitudeReader_;

    // Current pitch of vehicle
    UniversalDataReader *pitchReader_;

    // Slate output variables

    // Sets the depth mode for dynamic control
    DataWriter* verticalModeWriter_;
    DataWriter* elevatorAngleCmdWriter_;
    DataWriter* massPositionCmdWriter_;

    // Sets Control variables
    DataWriter* depthRateCmdWriter_;
    DataWriter* pitchCmdWriter_;
    DataWriter* speedCmdWriter_;

    // Configuration readers
    ConfigReader* surfaceThresholdCfgReader_;
    ConfigReader* pitchLimitCfgReader_;

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

    // indicates whether things are ok to run
    bool ok_;
    bool surfaceTimeoutReported_, pitchTimeoutReported_; // used to only report failures once

    float surfaceThreshold_;
    float pitchLimit_;

    float depthRateSetting_;
    float pitchSetting_;
    float speedSetting_;
    float pitchTimeoutSetting_;
    float surfacingTimeoutSetting_;

    // Time that behavior was started
    Timestamp surfaceStartTime_, pitchStartTime_;

    // Timeout for pitching up and overall attempt at surfacing before calc is satisfied and allowed to fall through.
    Timespan pitchTimeout_, surfacingTimeout_;

    // initialize vehicle config variables
    bool loadParams( void );

    // Returns true if there is sufficient altitude to turn up and pitch is responding within the timeout.
    bool checkPitchAndAltitude( void );

    void resetTimeouts( void );

};

#endif /*GOTOSURFACE_H_*/