Simulator.h

Go to the documentation of this file.

#ifndef SIMULATOR_H_
#define SIMULATOR_H_

#include "SimInitStruct.h"
#include "SimResultStruct.h"
#include "SimRunStruct.h"

#include "data/Location.h"
#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "ActuatorModel.h"
#include "ThrusterModel.h"
#include "utils/AuvMath.h"

class Simulator
{

public:
    Simulator( bool simulateSensors, int nSteps );

    virtual ~Simulator();

    void initialize( const SimInitStruct& initParams, SimResultStruct& results );

    void run( const SimRunStruct& runParams, SimResultStruct& results );

    virtual void uninitialize() {}

    void motion( float propOmegaAction, float rudderAngleAction,
                 float elevatorAngleAction, float massPositionAction,
                 float buoyancyAction, int dropWeightState,
                 float density, float dt,
                 SimResultStruct& results );

    long setLocation( const double& latitude, const double& longitude );

    void setDepth( float depth );

    void setHeading( float heading );

    void setPitch( float pitch );

    void setRoll( float roll );

    void setPropOmega( float propOmega );

    void setRudderAngle( float rudderAngle );

    void setElevatorAngle( float elevatorAngle );

    void setMassPosition( float massPosition );

    void setBuoyancyPosition( float buoyancyPosition );

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

protected:

    virtual void publishState( SimResultStruct& results );

    virtual void configureSensors() {}

    virtual void simulateSensors( double latDeg, double lonDeg, SimResultStruct& results ) {}

    bool loadParams( const SimInitStruct& initParams, SimResultStruct& results );

    //ReturnMatrix invertMass( void );
    void invertMass( SimResultStruct& results );

    void calcForce( Point6D &rate, Point6D &pos, float propOmegaAction,
                    float rudderAngleAction, float elevatorAngleAction,
                    float massPositionAction, float buoyancyAction,
                    int dropWeightState, float density, float dt, float smallDt );

    //------------------ Helper functions --------------------------------

    void calcFinForces( Point6D &rate, float ruddAngleProj, float elevAngleProj,
                        Point3D finForces[], Point3D finMoments[], float density );

    //*------------------- static constant variables ----------------------------*/

    enum
    {
        LO_RUDD,   // Lower Rudder
        PO_ELEV,   // Port Elevator
        UP_RUDD,   // Upper Rudder
        ST_ELEV,   // Starboard Elevator
    };

#define EFF_FACTOR 0.90

    //*------------------- other member variables ----------------------------*/

    bool simulateSensors_;

    Str oceanModelData_;

    int minNSteps_;

    float maxDt_;

    bool ok_;

    ActuatorModel elevatorModel_;
    ActuatorModel rudderModel_;
    ActuatorModel movableMassModel_;
    ActuatorModel buoyancyModel_;
    ThrusterModel thrusterModel_;

    Matrix6x6 massInvert_;

    float netBuoy_;

    Point6D force_;

    float propThrust_, propTorque_;

    Point6D pos_;
    Point6D posDot_;

    Point6D rate_;
    Point6D rateDot_;

    //unsigned int utmZone_;
    //bool northernHemi_;

    // WGS84 position
    double latitude_;
    double longitude_;

    Point3D current_;

    float northCurrent_; 
    float eastCurrent_; 
    float vertCurrent_; 
    float magneticVariation_; // Overrides value calculated from lat & lon
    float soundSpeed_;        // Overrides value calculated from density, pressure
    float density_;           // Overrides sea water density from ocean model.
    float sst_; // Overrides sea surface temperature from ocean model
    float tMixed_; // Overrides mixed layer depth temperature from ocean model
    float t300_; // Overrides temperature at 300 meters from ocean model
    float sss_; // Overrides sea surface salinity from ocean model
    float sMixed_; // Overrides mixed layer depth salinity from ocean model
    float s300_; // Overrides salinity at 300 meters from ocean model
    float mixedLayerDepth_; // Overrides mixed layer depth from ocean model

    float defaultDensity_;

    // Variable buoyancy setting
    float buoyancyNeutral_;     // cc    // volume in buoyancy "bag" at neutral tri,.

    float lastDensity_;

    //
    //   x is fwd, y stbd, z down.
    //

    //
    //
    float mass_;
    float movableMass_;
    float volume_;
    Point3D cOfMass_;
    Point3D cOfMovableMass_;
    Point3D cOfBuoy_;
    Point6D moment_;

    // Dropweight1 parameters
    float dropWt1Volume_;    /* volume of drop weight #1 */
    float dropWt1Mass_; /* mass of drop weight #1 */
    float dropWt1X_; /* location of drop weight #1 */
    float dropWt1Y_;
    float dropWt1Z_;

    // Simple geometry model of vehicle
    float cylinderLength_;
    float cylinderRadius_;

    //
    //
    //
    /* added mass */
    float kpDot_;
    float mqDot_;
    float nrDot_;
    float xuDot_;
    float yvDot_;
    float zwDot_;

    /* added mass cross terms */
    float kvDot_;
    float mwDot_;
    float nvDot_;
    float yrDot_;
    float ypDot_;
    float zqDot_;

    //                /* quadratic drag */
    float kpabp_;  // Kp|p| , kg-m^2
    float mqabq_;  // Mq|q| , kg-m^2
    float nrabr_;  // Nr|r| , kg-m^2
    float xuabu_;
    float yvabv_;  // Yv|v| , kg/m
    float zwabw_;  // Zw|w| , kg/m
    //
    //                /* quadratic drag cross terms */
    float mwabw_;  // Mw|w| , kg
    float nvabv_;  // Nv|v| , kg
    float yrabr_;  // Yr|r| , ?
    float zqabq_;  // Zq|q| , ?
    //
    //                /* in-line lift and drag */
    float muq_;  // Muq   , kg-m
    float muw_;  // Muw   , kg
    float mpr_;  // Mpr   , kg-m^2
    //
    float nur_;  // Nur   , kg-m
    float nuv_;  // Nuv   , kg
    float npq_;  // Npq   , kg-m^2
    //
    float xvv_;  // Xvv   , kg/m
    float xww_;  // Xww   , kg/m
    float xvr_;  // Xvr   , kg
    float xwq_;  // Xwq   , kg
    float xrr_;  // Xrr   , kg-m
    float xqq_;  // Xqq   , kg-m

    float yur_;  // Yur   , kg
    float yuv_;  // Yuv   , kg/m
    float ywp_;  // Ywp   , kg-m

    float zuq_;  // Zuq   , kg
    float zuw_;  // Zuw   , kg/m
    float zvp_;  // Zvp   , kg

    float kvt2_;  // ?     , ?

    float aspectRatio_;  //   , Unitless
    float stallAngle_;  //   , Deg.

    float finArea_;  //   , m^2
    float coeffDrag_;  // Coef. of Drag  , Unitless
    float coeffLift_;  // Coef. of Lift  , Unitless

    //
    //   upper rudder, #4 is stbd elevator.  This fin model is taken
    //   directly from PNA, which cites Whicker and Fehlner, 1958.  */
    //
    Point3D fins_[4];

};

#endif /*SIMULATOR_H_*/