/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : Simulator.h */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #ifndef _SIMULATOR_H #define _SIMULATOR_H #define CMLEN 256 //UTM Package #include "SimulatorIF_SK.h" #include "VehicleConfigurationIF.h" #include "Attributes.h" #include "DropWeightIF.h" #include "EventLogIF.h" #include "LayeredControlIF.h" #include "MissionClock.h" #include "Exception.h" #include "SimulationLog.h" #include "IsusIF.h" #include "BluefinBattLog.h" #include "BluefinBatteryIF.h" #include "SemaphoreP.h" #include "Octree.hpp" #include "OctreeSupport.hpp" class FastTime; class Simulator : public SimulatorIF_SK { public: friend class SimulationLog; friend class BluefinBattLog; Simulator(const char *configFileName, const char *outputFileName); ~Simulator(); // Exception class Error : public Exception { public: Error(char *errorMsg) : Exception(errorMsg) { } }; protected: static FastTime *_fastTime; virtual long setCurrent(double north, double east); virtual long command(double propOmega, double elevator, double rudder); virtual long state(SimulatorIF::Vector position, SimulatorIF::Vector positionRate, SimulatorIF::Vector eulerAngles, SimulatorIF::Vector rotationRate); virtual long rotationalVelocity(SimulatorIF::Vector velocity, SimulatorIF::Vector backDiff); virtual long translationalVelocity(SimulatorIF::Vector velocity, SimulatorIF::Vector backDiff); virtual long translationalBottomVelocity(SimulatorIF::Vector vel_Bo_N_N); virtual long controlSurfaces(double *rudder, double *elevator); virtual long force(SimulatorIF::Vector vector); virtual long torque(SimulatorIF::Vector vector); virtual long propOmega(double *omega); virtual long motorCurrent(double *motor); virtual long waterSpeed(double *speed); // // Compute depth as a function of _position. virtual double waterDepth( double dx, double dy); virtual double simTime(); virtual int spawnAuxTasks(); virtual long nTs(); virtual double beamRange( SimulatorIF::Vector offset_B, SimulatorIF::Vector los_B, double minRange, double maxRange, double rangeTol, long *simCntr, double *grazing, Boolean *surfaceDetect); virtual double usbl( SimulatorIF::Vector offset_B, SimulatorIF::Vector transponder, double minRange, double maxRange, SimulatorIF::Vector usblRange ); virtual void getSimBatteryData(BluefinBatteryIF::BfBattData *data); virtual void getSimDeltaTData( Boolean *_vertMt, Boolean *_fwdEnabled); long _loop_count, _loops_per_trn; private: struct InputData { InputData(); float vehicleMass; double propOmega; double rudder; double elevator; double northCurrent; double eastCurrent; double vehicleSpeed; }; struct OutputData { OutputData(); void initialize(); double position[3]; double eulerAngles[3]; double rotationalVelocity[3]; double rotationalBackDiff[3]; double translationalVelocity[3]; double vel_Bo_N_B[3]; double translationalBackDiff[3]; double rudder; double elevator; double rudderCmd; double elevatorCmd; double force[3]; double torque[3]; double propOmega; double M1_1; double M2_1; double M3_1; double M4_1; double M1_2; double M2_2; double M3_2; double M4_2; }; // // This is the original logging class, Log. It's been replaced by // SimulationLog. // class Log { public: Log(const char *fileName); ~Log(); int write(double time, OutputData *output); private: FILE *_outputFile; }; //Log *_log; /* The old way */ SimulationLog *_log; /* The new way */ BluefinBattLog *_battLog; VehicleConfigurationIF *_configuration; DropWeightIF *_dropWeight; EventLogIF _eventLog; EventLogIF::Message _eventMsg; LayeredControlIF *_layeredControl; InputData _input; OutputData _output; void init_simulation( void ); MissionClock *_missionClock; void updateFastTime(); // Calculates left-hand-side of the eqn void lhs(); // Subroutine motion() uses 4th-order Runge-Kutta method. Variables are: void motion( double e[], double p[], double Amps, double Del13, double Del24, double dt ); // Get the right-hand sides of the dynamic equations. void hydro( double *e, double *p, double Amps, double Del13, double Del24, double dt ); // Compute forces generated by control surfaces void fins( double *e, double rud_ang, double elev_ang ); // Move at either zero, full forward, or full backward speed. double act_model(double des_angle, double angle, double dt); // Hysteresis: applied to an arbitrary input, probably elevator or // rudder. Width and center of loop are defined with other physical // variables. double hysteresis( double x, double x_last, double y_last, double wide, double center); // Computation of the thruster force from the Amps double thruster_force( double real_amps, double w_speed ); // Computation of the thruster torque from the Amps double thruster_torque( double real_amps, double w_speed ); // Matrix calculations, not sure where their from, probably // numerical recipes in fortran (note array element[0] unused!) int matinv( double a[][7], double ainv[][7], int n, double adum[][7] ); double gasdev( void ); double drand48( void ); // Called when event is received from LayeredControlIF object void missionStatusCallback(TaskInterface *taskInterface, EventCode eventCode); // // PURPOSE: Return the altitude of a point rho along the beam. It must be // static so that it can be passed to bisect as a function pointer. It // uses member variables, but we get around this with Han's "this" pointer // magic. // static double beamAlt( double rho ); // // Compute the remaining charge and voltage. // void battery( void ); //See the .cc for comments. double _energy; //Energy remaining in the battery. double _energy0; //Initial charge. double _Ph; //Hotel load. BluefinBatteryIF::BfBattData _battData; Attributes _attributes; VehicleConfigurationIF::Vector _centerOfMass; VehicleConfigurationIF::Vector _centerOfBuoyancy; VehicleConfigurationIF::Vector _dropWt1Location; VehicleConfigurationIF::Vector _dropWt2Location; double omega_P; double Pit; double Area; double Eta; double Cdf1; /* Emperical Coef. of Drag Modifier */ double Kthr; /* Factor relating commanded speed, uC, to omega_P */ double rho; /* Density of seawater */ double rhs[7]; /* calculated in hydro, used in motion */ double ainv[7][7]; /* calculated by lhs, used in motion */ /* coordinate transformation matrix, we use only indices 1-6 */ static double ctrn[7][7]; int jjj; /* runge-kutta index (shouldn't be global) but is ? */ /* FINS. Note fin #1 is lower rudder, #2 is port elevator, #3 is upper rudder, #4 is stbd elevator. This fin model is taken directly from PNA, which cites Whicker and Fehlner, 1958. */ double r1[4], r2[4], r3[4], r4[4]; /*---- Forces and moments due to the fins ----*/ double F1[4], F2[4], F3[4], F4[4]; double M1[4], M2[4], M3[4], M4[4]; double Wdw1; double Bdw1; double Xdw1; double Ydw1; double Zdw1; double Wdw2; double Bdw2; double Xdw2; double Ydw2; double Zdw2; // Simulation time increment static const double _timeIncr; static const double Ixy; static const double Izx; static const double Iyz; static const double fin_speed; static const double cdo; /* Cd from NACA 0015 section */ static const double ec; /* "Oswald" efficiency factor */ static const int delay; /* THRUSTER. ------------------------------------------------------------*/ static const double Kt; /* torque constant (in Nm/A) - inc gearbox */ double thrAmps; /* Motor current, amps. */ double Ixx; double Iyy; double Izz; /* hydrodynamic parameters ----------------------------------------------*/ /* quadratic drag */ double Kpabp; /* we do fin drag effects separately*/ double Mqabq; /* value from get_drag.tex; G/T had -9800*/ double Nrabr; double Xuabu; double Yvabv; double Zwabw; /* added mass */ double Kpdot; double Mqdot; double Nrdot; double Xudot; double Yvdot; double Zwdot; /* added mass cross terms */ double Kvdot; double Mwdot; double Nvdot; double Yrdot; double Ypdot; double Zqdot; /* quadratic drag cross terms */ double Mwabw; double Nvabv; double Yrabr; double Zqabq; /* in-line lift and drag */ double Muq; double Muw; double Mpr; double Nur; double Nuv; double Npq; double Xvv; double Xww; double Xvr; double Xwq; double Xrr; double Xqq; double Yur; double Yuv; double Ywp; double Zuq; double Zuw; double Zvp; double Kvt2; double aspect_ratio; /* effective aspect ratio */ double stall_angle; /* fin stall angle */ double wide_hyst_rud; /* hysteresis loop double width */ double center_hyst_rud; /* hysteresis loop center */ double wide_hyst_elev; double center_hyst_elev; double S; /* fin area (m^2) */ double CDc; /* crossflow Cd, fsh changed from 0.1 */ double dCL; /* lift coefficient slope */ #define MAXNDEPTHS 10 double _depth [MAXNDEPTHS]; double _ydepth[MAXNDEPTHS]; double _slope [MAXNDEPTHS]; /* The last (MAXNDEPTHS-1) element should be unused*/ double _yinter[MAXNDEPTHS]; double _normalArray[3][MAXNDEPTHS]; double _bottomNormal[3]; short _nydepths; double zMax; /* Bottom Depth */ // In keeping with the custom established by whoever wrote this // simulator, we use an extra dimension in these arrays, so we // can index things starting at 1. static double _position[7]; static double _position0[7]; //Save the position at t=0. static double _rate[7]; double northCurrent, eastCurrent; long _msSimDelay; // During fastsim, this sets the delay before triggering next GNC loop (in ms) double _simTime, _lastSimTime, _lastMissTime; double mTs; //Control system sampling period, seconds. long mTsMsec; long kk; //Control system sample period counter double mgl; //mass * gravity * length // False until we get LayeredControlIF::MissionStarted event Boolean _missionStarted; double _waterDepth; //Store the result of waterDepth() above. int count[1]; //Pointer & memory for int counter for bisect(). double _beamRange; //Meters; stores the result of beamRange(); double _grazing; //Rad; grazing angle of the beam wrto the bottom. Boolean _computeGrazing; double _los_B[3]; //Meters; beam line-of-sight in body-fixed coord. double _offset_B[3]; //Meters; beam offset from body-fixed origin. double T_N_B[3][3]; //Direction cosine from body to inertial. double _lastBeamRange; short _missCntr; short _nBatts; //Number of batteries Boolean _useOctrees; Boolean _firstSurfaceDetect; char _mapFile[128]; char *_mapFileName; Boolean _vertMt, _fwdEnabled; Boolean _firstCallback; double _dynT, _simLoopInterval; struct timespec _lastTime; Octree _map; Boolean _fastSim; }; #endif