/** \file * * Contains the Simulator class implementation. * * Copyright (c) 2007,2008,2009 MBARI * MBARI Proprietary Information. All Rights Reserved */ #include #include "Simulator.h" //#include "MatrixHelpers.h" #include "data/Location.h" #include "data/Matrix6x6.h" #include "data/Point3D.h" #include "data/Point6D.h" #include "simulatorModule/ParameterHandler.h" #include "units/Units.h" #include "utils/AuvMath.h" #include #include //#include "Tools/newmat-10D/newmatio.h" //// Number of simulation steps per command const float FTOM = 0.3048; /* from feet to meters */ const float FTOM2 = FTOM * FTOM; const float FTOM3 = FTOM * FTOM * FTOM; const float STOKG = 14.59; /* from slug to kg */ const float TWOPI = ( 2 * M_PI ); //== Fin constants == //=== Constructor/destructor == /// Constructor Simulator::Simulator( bool simulateSensors, int nSteps, bool debug ) : simulateSensors_( simulateSensors ), // indicates whether to simulate sensors (salinity, temp, u, v) oceanModelData_( Str::EMPTY_STR ), // filename of file that contains ocean model run oceanModelVarCount_( 0 ), // oceanModelVarNames_( 0 ), // oceanModelVarUnits_( 0 ), // minNSteps_( nSteps ), // The simulator runs at least minNSteps_ times per "timestep" maxDt_( 0.2 ), // Maximum timestep ok_( false ), // indicates whether things are ok to run the simulation elevatorModel_(), // Actuator and thruster models rudderModel_(), movableMassModel_(), buoyancyModel_(), thrusterModel_(), massInvert_( 0.0 ), // Inverse of "mass" matrix. Calculated just once in invertMass() called from initialize() netBuoy_( 0.0 ), force_( 0.0 ), pos_( 0.0 ), // 6-dimensional position vector, body coordinates posDot_( 0.0 ), rate_( 0.0 ), // 6-dimensional rate vector, earth coordinates rateDot_( 0.0 ), //utmZone_( 0 ), // UTM zone gathered from initial position //northernHemi_( false ), latitude_( nanf( "" ) ), // WGS84 location longitude_( nanf( "" ) ), current_( 0.0 ), // Ocean current (m/s north, east, down) northCurrent_( nan( "" ) ), // Storage for a static northward current eastCurrent_( nan( "" ) ), // Storage for a static eastward current vertCurrent_( nan( "" ) ), // Storage for a static vertical current (positive downward) magneticVariation_( nan( "" ) ), // Overrides value calculated from lat & lon soundSpeed_( nan( "" ) ), // Overrides value calculated from density, pressure density_( nan( "" ) ), // Overrides sea water density from ocean model. sst_( nan( "" ) ), // Overrides sea surface temperature from ocean model tMixed_( nan( "" ) ), // Overrides mixed layer depth temperature from ocean model t300_( nan( "" ) ), // Overrides temperature at 300 meters from ocean model sss_( nan( "" ) ), // Overrides sea surface salinity from ocean model sMixed_( nan( "" ) ), // Overrides mixed layer depth salinity from ocean model s300_( nan( "" ) ), // Overrides salinity at 300 meters from ocean model mixedLayerDepth_( nan( "" ) ), // Overrides mixed layer depth from ocean model defaultDensity_( 0.0 ), // Density of water around the vehicle buoyancyNeutral_( 0.0 ), // Volume in buoyancy "bag" at neutral trim lastDensity_( nan( "" ) ), // Last calculated density mass_( 0.0 ), // translational and rotational body mass movableMass_( 0.0 ), volume_( 0.0 ), cOfMass_( 0.0 ), cOfMovableMass_( 0.0 ), cOfBuoy_( 0.0 ), moment_( 0.0 ), dropWt1Volume_( 0.0 ), // volume of drop weight #1 dropWt1Mass_( 0.0 ), // mass of drop weight #1 dropWt1X_( 0.0 ), // location of drop weight #1 dropWt1Y_( 0.0 ), dropWt1Z_( 0.0 ), kpDot_( 0.0 ), /* added mass */ mqDot_( 0.0 ), nrDot_( 0.0 ), xuDot_( 0.0 ), yvDot_( 0.0 ), zwDot_( 0.0 ), kvDot_( 0.0 ), /* added mass cross terms */ mwDot_( 0.0 ), nvDot_( 0.0 ), yrDot_( 0.0 ), ypDot_( 0.0 ), zqDot_( 0.0 ), kpabp_( 0.0 ), /* quadratic drag */ // Kp|p| , kg-m^2 mqabq_( 0.0 ), // Mq|q| , kg-m^2 nrabr_( 0.0 ), // Nr|r| , kg-m^2 xuabu_( 0.0 ), yvabv_( 0.0 ), // Yv|v| , kg/m zwabw_( 0.0 ), // Zw|w| , kg/m mwabw_( 0.0 ), /* quadratic drag cross terms */ // Mw|w| , kg nvabv_( 0.0 ), // Nv|v| , kg yrabr_( 0.0 ), // Yr|r| , ? zqabq_( 0.0 ), // Zq|q| , ? muq_( 0.0 ), /* in-line lift and drag */ // Muq , kg-m muw_( 0.0 ), // Muw , kg mpr_( 0.0 ), // Mpr , kg-m^2 nur_( 0.0 ), // Nur , kg-m nuv_( 0.0 ), // Nuv , kg npq_( 0.0 ), // Npq , kg-m^2 xvv_( 0.0 ), // Xvv , kg/m xww_( 0.0 ), // Xww , kg/m xvr_( 0.0 ), // Xvr , kg xwq_( 0.0 ), // Xwq , kg xrr_( 0.0 ), // Xrr , kg-m xqq_( 0.0 ), // Xqq , kg-m yur_( 0.0 ), // Yur , kg yuv_( 0.0 ), // Yuv , kg/m ywp_( 0.0 ), // Ywp , kg-m zuq_( 0.0 ), // Zuq , kg zuw_( 0.0 ), // Zuw , kg/m zvp_( 0.0 ), // Zvp , kg kvt2_( 0.0 ), // ? , ? aspectRatio_( 0.0 ), // , Unitless stallAngle_( 0.0 ), // , Deg. finArea_( 0.0 ), // , m^2 coeffDrag_( 0.0 ), // Coef. of Drag , Unitless coeffLift_( 0.0 ), // Coef. of Lift , Unitless debug_( debug ) // Set to true for debugging output { } /// Destructor Simulator::~Simulator() { delete[] oceanModelVarNames_; delete[] oceanModelVarUnits_; uninitialize(); } /// Initialize void Simulator::initialize( const SimInitStruct& initParams, SimResultStruct& results ) { ok_ = loadParams( initParams, results ); if( !ok_ || *results.errorMessage_ ) { ok_ = false; return; } // This assigns the variable massInvert_ invertMass( results ); if( *results.errorMessage_ ) { ok_ = false; return; } if( simulateSensors_ && oceanModelData_ != Str::EMPTY_STR ) { configureSensors(); } // Now that the sim has been initialized, publish its state. publishState( initParams.time_, results ); } /// Run void Simulator::run( const SimRunStruct& runParams, SimResultStruct& results ) { if( !ok_ ) { return ; } // These are things that should be loaded from the slate... // (rad/sec) if( !isnan( runParams.propOmega_ ) ) { setPropOmega( runParams.propOmega_ ); } if( !isnan( runParams.rudderAngle_ ) ) { setRudderAngle( runParams.rudderAngle_ ); } if( !isnan( runParams.elevatorAngle_ ) ) { setElevatorAngle( runParams.elevatorAngle_ ); } if( !isnan( runParams.massPosition_ ) ) { setMassPosition( runParams.massPosition_ ); } if( !isnan( runParams.buoyancyPosition_ ) ) { setBuoyancyPosition( runParams.buoyancyPosition_ ); } int dropWeightState( runParams.dropWeightState_ ); float propOmegaAction( runParams.propOmegaAction_ ); if( isnan( propOmegaAction ) ) { propOmegaAction = thrusterModel_.currentPropOmega(); } float rudderAngleAction( runParams.rudderAngleAction_ ); if( isnan( rudderAngleAction ) ) { rudderAngleAction = rudderModel_.getCurrent(); } float elevatorAngleAction( runParams.elevatorAngleAction_ ); if( isnan( elevatorAngleAction ) ) { elevatorAngleAction = elevatorModel_.getCurrent(); } float massPositionAction( runParams.massPositionAction_ ); if( isnan( massPositionAction ) ) { massPositionAction = movableMassModel_.getCurrent(); } float buoyancyAction( runParams.buoyancyAction_ ); if( isnan( buoyancyAction ) ) { buoyancyAction = buoyancyModel_.getCurrent(); } float density( runParams.density_ ); if( isnan( density ) ) { density = defaultDensity_; } else { density = isnan( lastDensity_ ) ? defaultDensity_ : lastDensity_; } float dt( runParams.dt_ ); if( dt <= 0 || dt != dt ) { dt = 0.4; } /// Set the action for the actuator models rudderModel_.setCommand( rudderAngleAction ); elevatorModel_.setCommand( elevatorAngleAction ); movableMassModel_.setCommand( massPositionAction ); buoyancyModel_.setCommand( buoyancyAction ); /// It seems that the maximum allowable step time is about 200 msec. int nSteps = MAX( minNSteps_, ( int )( dt / maxDt_ ) ); float stepTime = dt / nSteps; for( int i = 0; i < nSteps && 0 == *results.errorMessage_; i++ ) { motion( propOmegaAction, rudderAngleAction, elevatorAngleAction, massPositionAction, buoyancyAction, dropWeightState, density, stepTime, results ); } pos_.setHeading( AuvMath::ModPi( pos_.getHeading() ) ); if( *results.errorMessage_ ) { return; } publishState( runParams.time_, results ); } //==== MOTION ======================================================= /// As with others /// member functions, motionAlternate is a rewritten form using the newmat /// Matrix library. motion is a port performing as few changes as necessary. /// /// \param propOmegaAction commanded prop rotation rate (rad/sec) /// \param rudderAngleAction Position of rudders (fins 1 and 3) in radians. /// \param elevatorAngleAction Position of elevators (fins 2 and 4) in radians. /// \param massPositionAction Position of movable mass in meters. /// \param buoyancyAction Position of buoyancy engine in cubic meters. /// \param dropWeightState True if drop weight intact /// \param density Density of sea water in kg/m3. /// \param dt Time step /// \param results struct for providing feedback /// /// As a side note, the fourth order Runge-Kutta method /// (from http://en.wikipedia.org/wiki/Runge-Kutta) is an iterative method /// for solving ODEs. Given an initial value problem /// /// \f{eqnarray*} y^\prime = f( t, y ) \\ \\ y(t_0) = y_0 \f} /// /// Then the fourth order Runge-Kutta solution is: /// /// \f[ getY(){n+1} = y_n + \frac{ \Delta t }{6}\left( k_1 + 2k_2 + 2k_3 + k_4 \right) \f] /// /// Where /// /// \f{eqnarray*} k_1 &=& f(t_n,y_n ) \\ k_2 &=& f(t_n + \frac{\Delta t}{2}, y_n + \frac{\Delta t}{2}k_1 ) \\ k_3 &=& f(t_n + \frac{\Delta t}{2}, y_n + \frac{\Delta t}{2}k_2 ) \\ k_4 &=& f(t_n + \Delta t, y_n + \Delta t \cdot k_3 ) \\ \f} /// /// /// Original comment: /// /// 
///   Subroutine motion() uses 4th-order Runge-Kutta method.  Variables are:
///
///   edot(1)  edot(2)  edot(3)  edot(4)  edot(5)  edot(6)
///     ]        ]        ]        ]        ]        ]
///    udot     vdot     wdot     pdot     qdot     rdot
///
///   rate.getU()  rate.getV()  rate.getW()  rate.getP()  rate.getQ()   rate.getR()
///     u        v        w        p        q        r
///   xDot     yDot     zDot     rollDot  pitchDot  yawDot
///
///   pos.getX()   pos.getY()   pos.getZ()    pos.getRoll()   pos.getPitch()   pos.getHeading()
///     x        y        z        phi     theta     psi
///                                roll    pitch     yaw
///
void Simulator::motion( float propOmegaAction, float rudderAngleAction, float elevatorAngleAction, float massPositionAction, float buoyancyAction, int dropWeightState, float density, float dt, SimResultStruct& results ) { float thisDt( dt ); float dtDiv2 = 0.5 * dt; int jjj; Point6D rateInc; Point6D posInc; Point6D rateTemp( rate_ ); Point6D posTemp( pos_ ); Point6D posDot[ 4 ]; Point6D rateDot[ 4 ]; //coordinate transformation matrices Matrix6x6 xformBody2Earth( pos_ ); // /* start 4th order runga kutta integration */ for( jjj = 0; jjj < 4; ++jjj ) { /// Time step depends on which loop of the Runge-Kutta you're on switch( jjj ) { case 0: case 1: thisDt = dtDiv2; break; case 2: case 3: thisDt = dt; break; } calcForce( rateTemp, posTemp, propOmegaAction, rudderAngleAction, elevatorAngleAction, massPositionAction, buoyancyAction, dropWeightState, density, dt, thisDt ); /* multiply by inverse of inertial matrix */ rateDot[ jjj ].addProduct( massInvert_, force_ ); rateInc = rateDot[ jjj ]; rateInc *= thisDt; rateTemp += rateInc; // Use ctrn to calculate pdot from rate. posDot[ jjj ].addProduct( xformBody2Earth, rateTemp ); posInc = posDot[ jjj ]; posInc *= thisDt; posTemp += posInc; } /// Average the intermediate slopes as per the Runge-Kutta method rateDot[ 0 ] *= 1.0 / 6.0; rateDot[ 1 ] *= 1.0 / 3.0; rateDot[ 2 ] *= 1.0 / 3.0; rateDot[ 3 ] *= 1.0 / 6.0; posDot[ 0 ] *= 1.0 / 6.0; posDot[ 1 ] *= 1.0 / 3.0; posDot[ 2 ] *= 1.0 / 3.0; posDot[ 3 ] *= 1.0 / 6.0; rateDot_ = 0; posDot_ = 0; for( jjj = 0; jjj < 4; ++jjj ) { rateDot_ += rateDot[ jjj ]; posDot_ += posDot[ jjj ]; } /// Add in effect of currents posDot_ += current_; rate_.addProduct( rateDot_, dt ); pos_.addProduct( posDot_, dt ); // Basically catch if any of the state variables has grown wildly out of line if( rate_.absSum() > 10000 ) { snprintf( results.errorMessage_, sizeof( results.errorMessage_ ), "error=UNSTABLE SIMULATION -- ABORTED, rate=%s\n", rate_.toString().cStr() ); return; } /// Advance the actuator and thruster models by the time step rudderModel_.advance( dt ); elevatorModel_.advance( dt ); movableMassModel_.advance( dt ); buoyancyModel_.advance( dt ); thrusterModel_.advance( propOmegaAction, posDot_.getX(), dt ); } /// Set the current location to the indicated value /// Returns error code conversion to UTM is unsuccessful. Otherwise returns 0. /// Use Wgs84::UtmErrorToString( error ) to decode the error. long Simulator::setLocation( const double& latitude, const double& longitude ) { /*double northings; double eastings; unsigned int utmZone; bool northernHemi; long error = Wgs84::LatLonToUtm( latitude, longitude, northings, eastings, utmZone, northernHemi ); if ( error == 0 ) { pos_.setY( eastings ); pos_.setX( northings ); utmZone_ = utmZone; northernHemi_ = northernHemi; } return error;*/ latitude_ = latitude; longitude_ = longitude; return 0; } /// Set the current depth to the indicated value void Simulator::setDepth( float depth ) { pos_.setZ( depth ); } /// Set the current heading to the indicated value void Simulator::setHeading( float heading ) { pos_.setHeading( heading ); } /// Set the current pitch to the indicated value void Simulator::setPitch( float pitch ) { pos_.setPitch( pitch ); } /// Set the current roll to the indicated value void Simulator::setRoll( float roll ) { pos_.setRoll( roll ); } /// Set the base prop omega to the indicated value void Simulator::setPropOmega( float propOmega ) { thrusterModel_.setPrevious( propOmega ); } /// Set the base rudder angle to the indicated value void Simulator::setRudderAngle( float rudderAngle ) { rudderModel_.setPrevious( rudderAngle ); } /// Set the base elevator angle to the indicated value void Simulator::setElevatorAngle( float elevatorAngle ) { elevatorModel_.setPrevious( elevatorAngle ); } /// Set the base mass position to the indicated value void Simulator::setMassPosition( float massPosition ) { movableMassModel_.setPrevious( massPosition ); } /// Set the base buoyancy position to the indicated value void Simulator::setBuoyancyPosition( float buoyancyPosition ) { buoyancyModel_.setPrevious( buoyancyPosition ); } //=== Private Functions ================================================== /// Produces the "mass" matrix in the "F = Ma" ODEs describing the vehicle dynamics. /// As long as the vehicle mass is constant, this only needs to be calculated once /// (though the simulator does not recalculate if a dropweight is dropped). /// The inversion (necessary to solve "a = M^-1 A") is also done in this /// function. /// /// Converted and tested 28/6/07 AMM /// /// \return The inverse of the mass matrix. /// /// The A matrix is assembled as /// /// \f[ A = \left[ \begin{array}{cc} A & B \\ C & D \end{array} \right] \f] /// /// \todo Need to add check for singularity before inverting A /// //ReturnMatrix Simulator::invertMass( void ) void Simulator::invertMass( SimResultStruct& results ) { // /* fsh checked inertia matrix 3/22/94 */ // What follows may look a mess. Matrix a is initialized using Odyssey // code, though it is a Matrix, not a float[7][7] array. Matrix massMatrix // is built by concatenating a series of smaller matrices which should // better illustrate the origins of the A matrix. At the end they // are tested for equivalence ( norm of a-massMatrix ). // Assembles submatrices of massMatrix Matrix3x3 momentsOfRotation( moment_.getX(), -moment_.getHeading(), -moment_.getPitch(), -moment_.getHeading(), moment_.getY(), -moment_.getRoll(), -moment_.getPitch(), -moment_.getRoll(), moment_.getZ() ); Matrix3x3 crossMass( 0.0, cOfMass_.getZ(), -cOfMass_.getY(), -cOfMass_.getZ(), 0.0, cOfMass_.getX(), cOfMass_.getY(), -cOfMass_.getX(), 0.0 ); crossMass *= mass_; Matrix3x3 eyeMass( mass_, 0, 0, 0, mass_, 0, 0, 0, mass_ ); /// Added mass terms // These are both diagonal matrices, so its simple to // load them. Matrix3x3 linearForce( xuDot_, 0, 0, 0, yvDot_, 0, 0, 0, zwDot_ ); Matrix3x3 torques( kpDot_, 0, 0, 0, mqDot_, 0, 0, 0, nrDot_ ); Matrix3x3 crossTermsOne( 0.0, 0.0, 0.0, -ypDot_, 0.0, -yrDot_, 0.0, -zqDot_, 0.0 ); Matrix3x3 crossTermsTwo( 0.0, -kvDot_, 0.0, 0.0, 0.0, -mwDot_, 0.0, -nvDot_, 0.0 ); eyeMass -= linearForce; crossTermsOne += crossMass; crossTermsTwo -= crossMass; momentsOfRotation -= torques; Matrix6x6 massMatrix( eyeMass, crossTermsOne, crossTermsTwo, momentsOfRotation ); // Check for singularity here... if( massMatrix.determinantZero() ) { snprintf( results.errorMessage_, sizeof( results.errorMessage_ ), "error=invertMass: Singular(or ill-conditioned) " "matrix in equations of motion LHS" ); return; } massMatrix.invert(); massInvert_ = massMatrix; } //==== HYDRO ============================================================= /// Generates the "force" matrix in the "F=Ma". Sums the various hydrodynamic /// forces acting on the vehicle (including various body lifts and drags, /// as well as forces from the prop (thrust, torque) and forces from /// the fins (drag and turning moments). /// /// Original Comment: /// /// 
///-------------------------------------------------------------------------
///  Subroutine calcForce() gets the right-hand sides of the dynamic equations.
///-------------------------------------------------------------------------
///
void Simulator::calcForce( Point6D &rate, Point6D &pos, float propOmegaAction, float rudderAngleAction, float elevatorAngleAction, float massPositionAction, float buoyancyAction, int dropWeightState, float density, float dt, float smallDt ) { float pp, qq, rr, pr, pq, qr, uv, uw, uq, ur, /*uu,*/ vv, ww;//, ut; float vp, vr, wp, wq; float cosRoll, cosPitch, sinRoll, sinPitch; float wabw, vabv, uabu, pabp, qabq, rabr; //float Vuw, Vuv; float ruddAngleProj, elevAngleProj; float vt, wt; Point3D cOfMassTot; Point3D cOfBuoyTot; float dropWt1Mass = dropWt1Mass_; float dropWt1Vol = dropWt1Volume_; float dropWt1Wt; float dropWt1Buoy; float massTot = mass_; float weight; float volTot = volume_; float movableWeight; float staticWeight; float staticBuoy; float airHeight; float airVolume; float subRadius; const float g = 9.80665; // Figure out vehicle weight/balance if( dropWeightState == 0 ) { massTot -= dropWt1Mass_; volTot -= dropWt1Volume_; dropWt1Mass = dropWt1Vol = 0; } weight = massTot * g; dropWt1Wt = dropWt1Mass * g; movableWeight = movableMass_ * g; staticWeight = weight - movableWeight - dropWt1Wt; float buoy = ( volTot + buoyancyModel_.getCurrent() - buoyancyNeutral_ ) * density * g; dropWt1Buoy = dropWt1Vol * density * g; // Now deal with the surface. // Just subtract from buoyancy, the amount of weight above the surface // Super simple model -- assume a horizontal cylinder! float depthSensorOffset = 0.0; airHeight = pos.getZ() > depthSensorOffset ? 0 : depthSensorOffset - pos.getZ(); if( airHeight > 0 ) { if( airHeight > cylinderRadius_ ) { // Just assume half of the vehicle is out of the water airHeight = cylinderRadius_; } subRadius = cylinderRadius_ - airHeight; airVolume = cylinderLength_ * cylinderRadius_ * cylinderRadius_ * acos( subRadius / cylinderRadius_ ) - subRadius * sqrt( 2.0 * subRadius * airHeight ); buoy -= airVolume * density * g; } // // The presence or absence of the dropweight does not change // staticBuoy. Before the drop, dropWt1Buoy is > 0. After the drop, the // value of buoy is decreased, and dropWt1Buoy is zeroed such that the // difference below remains constant. // // Consequently, this model assumes that the water that fills the void // left by the weight moves with the vehicle as if it were attached. staticBuoy = buoy - dropWt1Buoy; //printf("wtTot=%f, density=%f, netBuoy_=%f\n", wtTot, density, netBuoy_); cOfMassTot.setX( ( cOfMass_.getX() * staticWeight + ( cOfMovableMass_.getX() + massPositionAction ) * movableWeight + dropWt1X_ * dropWt1Wt ) / weight ); cOfMassTot.setY( ( cOfMass_.getY() * staticWeight + cOfMovableMass_.getY() * movableWeight + dropWt1Y_ * dropWt1Wt ) / weight ); cOfMassTot.setZ( ( cOfMass_.getZ() * staticWeight + cOfMovableMass_.getZ() * movableWeight + dropWt1Z_ * dropWt1Wt ) / weight ); cOfBuoyTot.setX( ( cOfBuoy_.getX() * staticBuoy + dropWt1X_ * dropWt1Buoy ) / buoy ); cOfBuoyTot.setY( ( cOfBuoy_.getY() * staticBuoy + dropWt1Y_ * dropWt1Buoy ) / buoy ); cOfBuoyTot.setZ( ( cOfBuoy_.getZ() * staticBuoy + dropWt1Z_ * dropWt1Buoy ) / buoy ); // == Pre-compute some variables == //uu = rate.getU() * rate.getU(); /* some efficient consolidation */ vv = rate.getV() * rate.getV(); ww = rate.getW() * rate.getW(); uv = rate.getU() * rate.getV(); uw = rate.getU() * rate.getW(); ur = rate.getU() * rate.getR(); uq = rate.getU() * rate.getQ(); vr = rate.getV() * rate.getR(); vp = rate.getV() * rate.getP(); wp = rate.getW() * rate.getP(); wq = rate.getW() * rate.getQ(); pr = rate.getP() * rate.getR(); pq = rate.getP() * rate.getQ(); qr = rate.getQ() * rate.getR(); pp = rate.getP() * rate.getP(); qq = rate.getQ() * rate.getQ(); rr = rate.getR() * rate.getR(); //Vuw = sqrt( uu + ww ); //Vuv = sqrt( uu + vv ); //ut = sqrt( uu + vv + ww ); uabu = rate.getU() * fabs( rate.getU() ); vabv = rate.getV() * fabs( rate.getV() ); wabw = rate.getW() * fabs( rate.getW() ); pabp = rate.getP() * fabs( rate.getP() ); qabq = rate.getQ() * fabs( rate.getQ() ); rabr = rate.getR() * fabs( rate.getR() ); cosRoll = cos( pos.getRoll() ); cosPitch = cos( pos.getPitch() ); sinRoll = sin( pos.getRoll() ); sinPitch = sin( pos.getPitch() ); vt = rate.getV() + fins_[ LO_RUDD ].getX() * rate.getR() ; wt = rate.getW() - fins_[ LO_RUDD ].getX() * rate.getQ() ; // dt will vary (from 0, dt/2, dt) as the Runge-Kutta integration steps // forwards. The models contain functions to "look ahead" by that // time step without changing their state. The advance() functions // called in motion after the integration actually moves them forward // by dt. elevAngleProj = elevatorModel_.project( smallDt ); ruddAngleProj = rudderModel_.project( smallDt ); thrusterModel_.project( propOmegaAction, rate.getU(), smallDt, propThrust_, propTorque_ ); ///---- Forces and moments due to the fins (in body frame) ---- Point3D finForces[ 4 ]; Point3D finMoments[ 4 ]; calcFinForces( rate, ruddAngleProj, elevAngleProj, finForces, finMoments, density ); netBuoy_ = buoy - weight; // Sum of forces in X (fore-aft) direction // Thurster force (TForce) // + (weight-buoyancy_) * sin(pitch) is force from the weight/buoyancy vector // + mass*(vr - wq + cOfMassTot.getX()*(qq + rr) - cOfMassTot.getY()*pq - cOfMassTot.getZ()*pr) // is ... // + the sum of the forces (drag) from the fins // + all of the quadratic force terms i.e. xvv_ is the lift/drag due to // the square of the sway velocity (v^2) force_.setX( - ( weight - buoy ) * sinPitch + propThrust_ + mass_ * ( vr - wq + cOfMassTot.getX() * ( qq + rr ) - cOfMassTot.getY() * pq - cOfMassTot.getZ() * pr ) + finForces[ LO_RUDD ].getX() + finForces[ PO_ELEV ].getX() + finForces[ UP_RUDD ].getX() + finForces[ ST_ELEV ].getX() + xvv_ * vv + xww_ * ww + xvr_ * vr + xwq_ * wq + xrr_ * rr + xqq_ * qq ); /* apply drag */ force_.setX( force_.getX() + xuabu_ * uabu ); // Sum of force in the lateral (port-stbd) direction // (weight-buoyancy)*cos pitch * sin roll // + ... force_.setY( ( weight - buoy ) * cosPitch * sinRoll + yvabv_ * vabv + yuv_ * uv + yur_ * ur + yrabr_ * rabr + ywp_ * wp + mass_ * ( wp - ur + cOfMassTot.getY() * ( rr + pp ) - cOfMassTot.getZ() * qr - cOfMassTot.getX() * pq ) + finForces[ LO_RUDD ].getY() + finForces[ PO_ELEV ].getY() + finForces[ UP_RUDD ].getY() + finForces[ ST_ELEV ].getY() ); // Sum of forces in the normal (up-down) direction // (weight-buoyancy) * cos pitch * cos roll // + ... force_.setZ( ( weight - buoy ) * cosPitch * cosRoll + zwabw_ * wabw + zuq_ * uq + zuw_ * uw + zqabq_ * qabq + zvp_ * vp + mass_ * ( uq - vp + cOfMassTot.getZ() * ( pp + qq ) - cOfMassTot.getX() * pr - cOfMassTot.getY() * qr ) + finForces[ LO_RUDD ].getZ() + finForces[ PO_ELEV ].getZ() + finForces[ UP_RUDD ].getZ() + finForces[ ST_ELEV ].getZ() ); // Sum of moments in roll (about x axis) force_.setRoll( - ( cOfMassTot.getZ() * weight - cOfBuoyTot.getZ() * buoy ) * cosPitch * sinRoll /* ROLL MOMENTS */ + ( cOfMassTot.getY() * weight - cOfBuoyTot.getY() * buoy ) * cosPitch * cosRoll + kpabp_ * pabp - ( moment_.getZ() - moment_.getY() ) * qr + moment_.getPitch() * pq - moment_.getHeading() * pr + moment_.getRoll() * qq - moment_.getRoll() * rr - mass_ * ( cOfMassTot.getY() * ( vp - uq ) + cOfMassTot.getZ() * ( wp - ur ) ) + finMoments[ LO_RUDD ].getX() + finMoments[ PO_ELEV ].getX() + finMoments[ UP_RUDD ].getX() + finMoments[ ST_ELEV ].getX() + propTorque_ + kvt2_ * 4. * vt * wt * ( wt * wt - vt * vt ) / ( vt * vt + wt * wt + .0001 ) ); // Sum of moment in pitch (about y axis) force_.setPitch( - ( cOfMassTot.getZ() * weight - cOfBuoyTot.getZ() * buoy ) * sinPitch - ( cOfMassTot.getX() * weight - cOfBuoyTot.getX() * buoy ) * cosPitch * cosRoll + mwabw_ * wabw + mqabq_ * qabq + muw_ * uw + muq_ * uq + mpr_ * pr + ( moment_.getZ() - moment_.getX() ) * pr - moment_.getPitch() * pp - moment_.getRoll() * pq + moment_.getHeading() * qr + moment_.getPitch() * rr - mass_ * ( cOfMassTot.getZ() * ( wq - vr ) + cOfMassTot.getX() * ( uq - vp ) ) + finMoments[ LO_RUDD ].getY() + finMoments[ PO_ELEV ].getY() + finMoments[ UP_RUDD ].getY() + finMoments[ ST_ELEV ].getY() ); // Sum of moments in yaw (about z axis) force_.setHeading( ( cOfMassTot.getY() * weight - cOfBuoyTot.getY() * buoy ) * sinPitch + ( cOfMassTot.getX() * weight - cOfBuoyTot.getX() * buoy ) * cosPitch * sinRoll + nuv_ * uv + nrabr_ * rabr + nur_ * ur + nvabv_ * vabv + npq_ * pq + ( moment_.getX() - moment_.getY() ) * pq + moment_.getHeading() * pp + moment_.getRoll() * pr - moment_.getHeading() * qq - moment_.getPitch() * qr - mass_ * cOfMassTot.getX() * ( ur - wp ) + mass_ * cOfMassTot.getY() * ( wq - vr ) + finMoments[ LO_RUDD ].getZ() + finMoments[ PO_ELEV ].getZ() + finMoments[ UP_RUDD ].getZ() + finMoments[ ST_ELEV ].getZ() ); } //==== FINS ============================================================== /// Calculates the forces and moments on the vehicle from the fins (in vehicle body frame). /// /// \param rate The "force/moment" state vector /// \param ruddAngleProj The rudder angle /// \param elevAngleProj The elevator angle /// \param finForces The calculated fin forces (will be resized to 3 x 4) /// \param finMoments The calculated fin moments (will be resized to 3 x 4) /// \param density Density of seawater in kg/m3 /// Original comment: ///
///* ------------------------------------------------------------------
///   Subroutine fins() computes forces generated by control surfaces
///   ------------------------------------------------------------------ */
///
void Simulator::calcFinForces( Point6D &rate, float ruddAngleProj, float elevAngleProj, Point3D finForces[], Point3D finMoments[], float density ) { float velNorm[ 4 ]; float alpha[ 4 ]; float cosAlpha[ 4 ]; float sinAlpha[ 4 ]; float lift[ 4 ], drag[ 4 ]; float cons, liftDragCd, aa; float liftFinCoords[ 4 ]; float dragFinCoords[ 4 ]; float liftFlowCoords[ 4 ]; float dragFlowCoords[ 4 ]; // Fin velocity is 3 (dimensions) x 4 (fins) Point3D finVels[ 4 ]; float sRud, sElev, cRud, cElev; sRud = sin( ruddAngleProj ); sElev = sin( elevAngleProj ); cRud = cos( ruddAngleProj ); cElev = cos( elevAngleProj ); /* fin velocities in body coords */ finVels[ LO_RUDD ].setU( rate.getU() + rate.getQ() * fins_[ LO_RUDD ].getW() - rate.getR() * fins_[ LO_RUDD ].getV() ); finVels[ LO_RUDD ].setV( rate.getV() - rate.getP() * fins_[ LO_RUDD ].getW() + rate.getR() * fins_[ LO_RUDD ].getU() ); finVels[ LO_RUDD ].setW( rate.getW() + rate.getP() * fins_[ LO_RUDD ].getV() - rate.getQ() * fins_[ LO_RUDD ].getU() ); finVels[ PO_ELEV ].setU( rate.getU() + rate.getQ() * fins_[ PO_ELEV ].getW() - rate.getR() * fins_[ PO_ELEV ].getV() ); finVels[ PO_ELEV ].setV( rate.getV() - rate.getP() * fins_[ PO_ELEV ].getW() + rate.getR() * fins_[ PO_ELEV ].getU() ); finVels[ PO_ELEV ].setW( rate.getW() + rate.getP() * fins_[ PO_ELEV ].getV() - rate.getQ() * fins_[ PO_ELEV ].getU() ); finVels[ UP_RUDD ].setU( rate.getU() + rate.getQ() * fins_[ UP_RUDD ].getW() - rate.getR() * fins_[ UP_RUDD ].getV() ); finVels[ UP_RUDD ].setV( rate.getV() - rate.getP() * fins_[ UP_RUDD ].getW() + rate.getR() * fins_[ UP_RUDD ].getU() ); finVels[ UP_RUDD ].setW( rate.getW() + rate.getP() * fins_[ UP_RUDD ].getV() - rate.getQ() * fins_[ UP_RUDD ].getU() ); finVels[ ST_ELEV ].setU( rate.getU() + rate.getQ() * fins_[ ST_ELEV ].getW() - rate.getR() * fins_[ ST_ELEV ].getV() ); finVels[ ST_ELEV ].setV( rate.getV() - rate.getP() * fins_[ ST_ELEV ].getW() + rate.getR() * fins_[ ST_ELEV ].getU() ); finVels[ ST_ELEV ].setW( rate.getW() + rate.getP() * fins_[ ST_ELEV ].getV() - rate.getQ() * fins_[ ST_ELEV ].getU() ); /* now get angle of attack for each fin */ velNorm[ LO_RUDD ] = sqrt( finVels[ LO_RUDD ].sumSquared() ); if( velNorm[ LO_RUDD ] < 0.001 ) alpha[ LO_RUDD ] = 0.0; else alpha[ LO_RUDD ] = asin( -finVels[ LO_RUDD ].getV() / velNorm[ LO_RUDD ] ) + ruddAngleProj; velNorm[ PO_ELEV ] = sqrt( finVels[ PO_ELEV ].sumSquared() ); if( velNorm[ PO_ELEV ] < 0.001 ) alpha[ PO_ELEV ] = 0.0; else alpha[ PO_ELEV ] = atan2( finVels[ PO_ELEV ].getW(), finVels[ PO_ELEV ].getU() ) + elevAngleProj; velNorm[ UP_RUDD ] = sqrt( finVels[ UP_RUDD ].sumSquared() ); if( velNorm[ UP_RUDD ] < 0.001 ) alpha[ UP_RUDD ] = 0.0; else alpha[ UP_RUDD ] = asin( -finVels[ UP_RUDD ].getV() / velNorm[ UP_RUDD ] ) + ruddAngleProj; velNorm[ ST_ELEV ] = sqrt( finVels[ ST_ELEV ].sumSquared() ); if( velNorm[ ST_ELEV ] < 0.001 ) alpha[ ST_ELEV ] = 0.0; else alpha[ ST_ELEV ] = atan2( finVels[ ST_ELEV ].getW(), finVels[ ST_ELEV ].getU() ) + elevAngleProj; /* lift and drag coefficients */ liftDragCd = coeffDrag_ / aspectRatio_; aa = 1.0 / ( M_PI * aspectRatio_ * EFF_FACTOR ); for( int i = 0; i < 4; i++ ) { // Calculate lift from each fin lift[ i ] = coeffLift_ * alpha[ i ] + liftDragCd * alpha[ i ] * fabs( alpha[ i ] ); // If a given fin has exceeded the stall angle, // it gives no lift if( fabs( alpha[ i ] ) > stallAngle_ ) { lift[ i ] = 0.0; } drag[ i ] = coeffDrag_ + aa * lift[ i ] * lift[ i ]; } /* lift and drag forces, in flow coords... */ cons = ( density * finArea_ ) / 2.0; float velNormSquared[ 4 ]; for( int i = 0; i < 4; ++i ) { velNormSquared[ i ] = velNorm[ i ] * velNorm[ i ]; liftFlowCoords[ i ] = cons * lift[ i ] * velNormSquared[ i ]; dragFlowCoords[ i ] = cons * drag[ i ] * velNormSquared[ i ]; cosAlpha[ i ] = cos( alpha[ i ] ); sinAlpha[ i ] = sin( alpha[ i ] ); } for( int i = 0; i < 4; ++i ) { liftFinCoords[ i ] = liftFlowCoords[ i ] * cosAlpha[ i ] + dragFlowCoords[ i ] * sinAlpha[ i ]; dragFinCoords[ i ] = dragFlowCoords[ i ] * cosAlpha[ i ] - liftFlowCoords[ i ] * sinAlpha[ i ]; } /* finally, transform into the body frame */ finForces[ LO_RUDD ].setX( -liftFinCoords[ LO_RUDD ] * sRud + ( -dragFinCoords[ LO_RUDD ] ) * cRud ); finForces[ LO_RUDD ].setY( liftFinCoords[ LO_RUDD ] * cRud + ( -dragFinCoords[ LO_RUDD ] ) * sRud ); finForces[ LO_RUDD ].setZ( 0.0 ); finForces[ PO_ELEV ].setX( -liftFinCoords[ PO_ELEV ] * sElev + ( -dragFinCoords[ PO_ELEV ] ) * cElev ); finForces[ PO_ELEV ].setY( 0.0 ); finForces[ PO_ELEV ].setZ( -liftFinCoords[ PO_ELEV ] * cElev - ( -dragFinCoords[ PO_ELEV ] ) * sElev ); finForces[ UP_RUDD ].setX( -liftFinCoords[ UP_RUDD ] * sRud + ( -dragFinCoords[ UP_RUDD ] ) * cRud ); finForces[ UP_RUDD ].setY( liftFinCoords[ UP_RUDD ] * cRud + ( -dragFinCoords[ UP_RUDD ] ) * sRud ); finForces[ UP_RUDD ].setZ( 0.0 ); finForces[ ST_ELEV ].setX( -liftFinCoords[ ST_ELEV ] * sElev + ( -dragFinCoords[ ST_ELEV ] ) * cElev ); finForces[ ST_ELEV ].setY( 0.0 ); finForces[ ST_ELEV ].setZ( -liftFinCoords[ ST_ELEV ] * cElev - ( -dragFinCoords[ ST_ELEV ] ) * sElev ); /* moments induced by these forces */ finMoments[ LO_RUDD ].setX( fins_[ LO_RUDD ].getY() * finForces[ LO_RUDD ].getZ() - fins_[ LO_RUDD ].getZ() * finForces[ LO_RUDD ].getY() ); finMoments[ LO_RUDD ].setY( -fins_[ LO_RUDD ].getX() * finForces[ LO_RUDD ].getZ() + fins_[ LO_RUDD ].getZ() * finForces[ LO_RUDD ].getX() ); finMoments[ LO_RUDD ].setZ( fins_[ LO_RUDD ].getX() * finForces[ LO_RUDD ].getY() - fins_[ LO_RUDD ].getY() * finForces[ LO_RUDD ].getX() ); finMoments[ PO_ELEV ].setX( fins_[ PO_ELEV ].getY() * finForces[ PO_ELEV ].getZ() - fins_[ PO_ELEV ].getZ() * finForces[ PO_ELEV ].getY() ); finMoments[ PO_ELEV ].setY( -fins_[ PO_ELEV ].getX() * finForces[ PO_ELEV ].getZ() + fins_[ PO_ELEV ].getZ() * finForces[ PO_ELEV ].getX() ); finMoments[ PO_ELEV ].setZ( fins_[ PO_ELEV ].getX() * finForces[ PO_ELEV ].getY() - fins_[ PO_ELEV ].getY() * finForces[ PO_ELEV ].getX() ); finMoments[ UP_RUDD ].setX( fins_[ UP_RUDD ].getY() * finForces[ UP_RUDD ].getZ() - fins_[ UP_RUDD ].getZ() * finForces[ UP_RUDD ].getY() ); finMoments[ UP_RUDD ].setY( -fins_[ UP_RUDD ].getX() * finForces[ UP_RUDD ].getZ() + fins_[ UP_RUDD ].getZ() * finForces[ UP_RUDD ].getX() ); finMoments[ UP_RUDD ].setZ( fins_[ UP_RUDD ].getX() * finForces[ UP_RUDD ].getY() - fins_[ UP_RUDD ].getY() * finForces[ UP_RUDD ].getX() ); finMoments[ ST_ELEV ].setX( fins_[ ST_ELEV ].getY() * finForces[ ST_ELEV ].getZ() - fins_[ ST_ELEV ].getZ() * finForces[ ST_ELEV ].getY() ); finMoments[ ST_ELEV ].setY( -fins_[ ST_ELEV ].getX() * finForces[ ST_ELEV ].getZ() + fins_[ ST_ELEV ].getZ() * finForces[ ST_ELEV ].getX() ); finMoments[ ST_ELEV ].setZ( fins_[ ST_ELEV ].getX() * finForces[ ST_ELEV ].getY() - fins_[ ST_ELEV ].getY() * finForces[ ST_ELEV ].getX() ); return ; } /////////////////////////////////////////////////////////////////// // IO Routines //////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////// /// Load simulator parameters bool Simulator::loadParams( const SimInitStruct& initParams, SimResultStruct& results ) { // The following are configuration variables which are loaded from // configuration files // // - Vehicle initial position and velocity (e_naught, p_naught) // - Vehicle mass and volume // - Position of center of mass and center of buoyancy (U_COFMASS[XYZ], U_COFBUOY[XYZ], etc) // - Stall angle, hysteresis center and width for rudder and elevator // - "DG_CROSS_DRAG" = coeffDrag_ // - "DG_ELEV_LIFT" = coeffLift_ // - Fin positions (stored in fins[ LO_RUDD ]-fins[ ST_ELEV ]) // - Cross terms nuv_, nur_, xuabu_, xvv_, xvr_, xrr_, yuv_, yur_, Nrr, Nvv, Yrr, Yvv // (there are other terms i.e. muw_, etc, but the matrix is symmetric, // so they can be derived. See the comment in read_hydro) // - Dropweight masses and positions in the vehicle (the sim allows for two) // - Added mass terms moment_.getX(), moment_.getY() (moment_.getZ() = moment_.getY() for rotational solid) // yvDot_, nvDot_, xuDot_, mqDot_, nrDot_, kpDot_, kvDot_ // (as above, other terms in matrix can be derived, see read_hydro) // Return value bool ok( true ); /* // Initial Position Vector long error = setLocation( AuvMath::DEG_TO_RAD * initParams.initLat_, AuvMath::DEG_TO_RAD * initParams.initLon_ ); if ( error != 0 ) { snprintf( results.errorMessage_, sizeof( results.errorMessage_ ), "Error on conversion of position (%g, %g) to UTM: %s", initParams.initLat_, initParams.initLon_, Wgs84::UtmErrorToString( error ) ); return false; }*/ latitude_ = D2R( initParams.initLat_ ); longitude_ = D2R( initParams.initLon_ ); pos_.setX( 0.0 ); pos_.setY( 0.0 ); pos_.setZ( initParams.initZ_ ); pos_.setPitch( initParams.initPitch_ ); pos_.setRoll( initParams.initRoll_ ); pos_.setHeading( initParams.initHeading_ ); // Initial Rate Vector rate_.setU( initParams.initU_ ); rate_.setV( initParams.initV_ ); rate_.setW( initParams.initW_ ); rate_.setP( initParams.initP_ ); rate_.setQ( initParams.initQ_ ); rate_.setR( initParams.initR_ ); // Initial Vehicle Config movableMassModel_.setPrevious( initParams.initMassPosition_ ); buoyancyModel_.setPrevious( initParams.initBuoyancyPosition_ ); // Optional environmental parameters northCurrent_ = initParams.northCurrent_; if( !isnan( northCurrent_ ) ) { eastCurrent_ = initParams.eastCurrent_; ok &= !isnan( eastCurrent_ ); } vertCurrent_ = initParams.vertCurrent_; magneticVariation_ = initParams.magneticVariation_; soundSpeed_ = initParams.soundSpeed_; density_ = initParams.density_; sst_ = initParams.sst_; if( !isnan( sst_ ) ) { tMixed_ = initParams.tMixed_; ok &= !isnan( tMixed_ ); t300_ = initParams.t300_; ok &= !isnan( t300_ ); } sss_ = initParams.sss_; if( !isnan( sss_ ) ) { sMixed_ = initParams.sMixed_; ok &= !isnan( sMixed_ ); s300_ = initParams.s300_; ok &= !isnan( s300_ ); } if( !isnan( sst_ ) || !isnan( sss_ ) ) { mixedLayerDepth_ = initParams. mixedLayerDepth_; ok &= !isnan( mixedLayerDepth_ ); } Str oceanModelStr = ( char* )initParams.oceanModelData_; int oceanModelStrNParts = 0; Str* oceanModelStrParts = oceanModelStr.split( oceanModelStrNParts, ":" ); if( debug_ ) printf( "oceanModelStr=%s, oceanModelStrParts=%d\n", oceanModelStr.cStr(), oceanModelStrNParts ); oceanModelData_ = oceanModelStrParts[0]; if( oceanModelStrNParts > 2 ) { oceanModelVarCount_ = ( oceanModelStrNParts - 1 ) / 2; oceanModelVarNames_ = new Str[oceanModelVarCount_]; oceanModelVarUnits_ = new Str[oceanModelVarCount_]; for( int i = 0; i < oceanModelVarCount_; ++i ) { oceanModelVarNames_[i] = oceanModelStrParts[i * 2 + 1]; oceanModelVarUnits_[i] = oceanModelStrParts[i * 2 + 2]; } } delete[] oceanModelStrParts; defaultDensity_ = initParams.defaultDensity_; buoyancyNeutral_ = initParams.buoyancyNeutral_; // Mass Properties -- from vehicle.cfg mass_ = initParams.mass_; // mass kg; Vehicle mass movableMass_ = initParams.movableMass_; // mass kg; Vehicle mass volume_ = initParams.volume_; // volume m3 // Stability Derivatives -- from vehicle.cfg //effDragCoef = .21; // effDragCoef, unitless; veh drag in x; related to Xuu xuabu_ = initParams.xuabu_; // Xuabu, Force Derivative, kg/meter // Initialize the center of mass -- from vehicle.cfg cOfMass_.setX( initParams.centerOfMassX_ ); cOfMass_.setY( initParams.centerOfMassY_ ); cOfMass_.setZ( initParams.centerOfMassZ_ ); // Initialize the center of mass -- from vehicle.cfg cOfMovableMass_.setX( initParams.centerOfMovableMassX_ ); cOfMovableMass_.setY( initParams.centerOfMovableMassY_ ); cOfMovableMass_.setZ( initParams.centerOfMovableMassZ_ ); // Initialize the center of buoyancy -- from vehicle.cfg cOfBuoy_.setX( initParams.centerOfBuoyX_ ); cOfBuoy_.setY( initParams.centerOfBuoyY_ ); cOfBuoy_.setZ( initParams.centerOfBuoyZ_ ); /// Initialize the fins locations -- from vehicle.cfg fins_[ LO_RUDD ].setX( initParams.lowerRudX_ ); fins_[ LO_RUDD ].setY( initParams.lowerRudY_ ); fins_[ LO_RUDD ].setZ( initParams.lowerRudZ_ ); fins_[ UP_RUDD ].setX( initParams.upperRudX_ ); fins_[ UP_RUDD ].setY( initParams.upperRudY_ ); fins_[ UP_RUDD ].setZ( initParams.upperRudZ_ ); fins_[ PO_ELEV ].setX( initParams.portElevX_ ); fins_[ PO_ELEV ].setY( initParams.portElevY_ ); fins_[ PO_ELEV ].setZ( initParams.portElevZ_ ); fins_[ ST_ELEV ].setX( initParams.stbdElevX_ ); fins_[ ST_ELEV ].setY( initParams.stbdElevY_ ); fins_[ ST_ELEV ].setZ( initParams.stbdElevZ_ ); thrusterModel_.initialize( initParams.designSpeed_, initParams.designPropEff_, initParams.designOmega_, initParams.designThrust_, initParams.designTorque_ ); // Dropweight1 parameters dropWt1Volume_ = initParams.dropWt1Volume_; // dropWt1Volume dropWt1Mass_ = initParams.dropWt1Mass_; // dropWt1Mass dropWt1X_ = initParams.dropWt1X_; // dropWt1X dropWt1Y_ = initParams.dropWt1Y_; // dropWt1Y dropWt1Z_ = initParams.dropWt1Z_; // dropWt1Z // Simply geometry model of vehicle cylinderLength_ = initParams.cylinderLength_; // cylinderLength cylinderRadius_ = initParams.cylinderRadius_; // cylinderRadius // Intertia terms -- from simulator.cfg moment_.setX( initParams.momentXX_ ); // Ixx, kg-m^2. moment_.setY( initParams.momentYY_ ); // Iyy, kg-m^2. moment_.setZ( initParams.momentZZ_ ); // Izz, kg-m^2. moment_.setRoll( 0.0 ); moment_.setPitch( 0.0 ); moment_.setHeading( 0.0 ); // Added mass terms -- from simulator.cfg kpDot_ = initParams.kpDot_; // Kpdot, kg-m^2. mqDot_ = initParams.mqDot_; // Mqdot, kg-m^2. nrDot_ = initParams.nrDot_; // Nrdot, kg-m^2. xuDot_ = initParams.xuDot_; // Xudot, kg. yvDot_ = initParams.yvDot_; // Yvdot, kg. zwDot_ = initParams.zwDot_; // Zwdot, kg. // // added mass cross terms -- from simulator.cfg kvDot_ = initParams.kvDot_; // Kvdot, kg-m. mwDot_ = initParams.mwDot_; // Mwdot, kg-m. nvDot_ = initParams.nvDot_; // Nvdot, kg-m. yrDot_ = initParams.yrDot_; // Yrdot, kg-m. ypDot_ = initParams.ypDot_; // Ypdot, kg-m. zqDot_ = initParams.zqDot_; // Zqdot, kg-m. // quadratic drag -- from simulator.cfg kpabp_ = initParams.kpabp_; // Kpabp, Kp|p| , kg-m^2 mqabq_ = initParams.mqabq_; // Mqabq, Mq|q| , kg-m^2 nrabr_ = initParams.nrabr_; // Nrabr, Nr|r| , kg-m^2 yvabv_ = initParams.yvabv_; // Yvabv, Yv|v| , kg/m zwabw_ = initParams.zwabw_; // Zwabw, Zw|w| , kg/m // quadratic drag cross terms -- from simulator.cfg mwabw_ = initParams.mwabw_; // Mwabw, Mw|w| , kg nvabv_ = initParams.nvabv_; // Nvabv, Nv|v| , kg yrabr_ = initParams.yrabr_; // Yrabr, Yr|r| , ? zqabq_ = initParams.zqabq_; // Zqabq, Zq|q| , ? // in-line lift and drag -- from simulator.cfg muq_ = initParams.muq_; // Muq, kg-m muw_ = initParams.muw_; // Muw, kg mpr_ = initParams.mpr_; // Mpr, kg-m^2 // nur_ = initParams.nur_; // Nur, kg-m nuv_ = initParams.nuv_; // Nuv, kg npq_ = initParams.npq_; // Npq, kg-m^2 // xvv_ = initParams.xvv_; // Xvv, kg/m xww_ = initParams.xww_; // Xww, kg/m xvr_ = initParams.xvr_; // Xvr, kg xwq_ = initParams.xwq_; // Xwq, kg xrr_ = initParams.xrr_; // Xrr, kg-m xqq_ = initParams.xqq_; // Xqq, kg-m yur_ = initParams.yur_; // Yur, kg yuv_ = initParams.yuv_; // Yuv, kg/m ywp_ = initParams.ywp_; // Ywp, kg-m zuq_ = initParams.zuq_; // Zuq, kg zuw_ = initParams.zuw_; // Zuw, kg/m zvp_ = initParams.zvp_; // Zvp, kg kvt2_ = initParams.kvt2_; // Kvt2, ? aspectRatio_ = initParams.aspectRatio_; // aspectRatio, Unitless stallAngle_ = initParams.stallAngle_; // stallAngle rudderModel_.setHyst( initParams.rudderHystWidth_, initParams.rudderHystCenter_ ); rudderModel_.setSpeed( initParams.rudderSpeed_ ); elevatorModel_.setHyst( initParams.elevatorHystWidth_, initParams.elevatorHystCenter_ ); elevatorModel_.setSpeed( initParams.elevatorSpeed_ ); movableMassModel_.setHyst( initParams.movableMassHystWidth_, initParams.movableMassHystCenter_ ); movableMassModel_.setSpeed( initParams.movableMassSpeed_ ); buoyancyModel_.setHyst( initParams.buoyancyHystWidth_, initParams.buoyancyHystCenter_ ); buoyancyModel_.setSpeed( initParams.buoyancySpeed_ ); finArea_ = initParams.finArea_; // finArea, m^2 coeffDrag_ = initParams.coeffDrag_; // CDc, Coef. of Drag, Unitless coeffLift_ = initParams.coeffLift_; // dCL, Coef. of Lift, Unitless return ok; } void Simulator::publishState( const double time, SimResultStruct& results ) { /* // Calculate the location in lat/lon and update that ///\todo needs to convert position(1|2) in UTM to lat/long // Reminder -- X is northings, Y is eastings double latRad; double lonRad; long error = Wgs84::UtmToLatLon( pos_.getX(), pos_.getY(), utmZone_, northernHemi_, latRad, lonRad ); if ( error != 0 ) { snprintf( results.errorMessage_, sizeof( results.errorMessage_ ), "error=Error on conversion of UTM (%.20g, %.20g, zone %d, North? %d,) to position: %s", pos_.getX(), pos_.getY(), utmZone_, northernHemi_, Wgs84::UtmErrorToString( error ) ); return; } /// Handle zone crossings gracefully error = setLocation( latRad, lonRad ); double latDeg( R2D( latRad ) ); double lonDeg( R2D( lonRad ) ); if ( error != 0 ) { snprintf( results.errorMessage_, sizeof( results.errorMessage_ ), "error=Error on conversion of position to UTM (%.20g, %.20g): %s", latDeg, lonDeg, Wgs84::UtmErrorToString( error ) ); return; } */ double bearing( atan2( pos_.getY(), pos_.getX() ) ); double distance( sqrt( pos_.getX()*pos_.getX() + pos_.getY()*pos_.getY() ) ); Location::AtBearing( bearing, distance, latitude_, longitude_ ); double latDeg( R2D( latitude_ ) ); double lonDeg( R2D( longitude_ ) ); results.propOmega_ = thrusterModel_.currentPropOmega(); results.propThrust_ = propThrust_; results.propTorque_ = propTorque_; results.rudderAngle_ = rudderModel_.getCurrent(); results.elevatorAngle_ = elevatorModel_.getCurrent(); results.massPosition_ = movableMassModel_.getCurrent(); results.buoyancyPosition_ = buoyancyModel_.getCurrent(); results.depth_ = pos_.getZ(); results.roll_ = pos_.getRoll(); results.pitch_ = pos_.getPitch(); results.heading_ = pos_.getHeading(); results.speed_ = rate_.getU(); results.latitudeDeg_ = latDeg; results.longitudeDeg_ = lonDeg; results.netBuoy_ = netBuoy_; results.forceX_ = force_.getX(); results.forceY_ = force_.getY(); results.forceZ_ = force_.getZ(); results.posX_ = pos_.getX(); pos_.setX( 0.0 ); results.posY_ = pos_.getY(); pos_.setY( 0.0 ); results.posZ_ = pos_.getZ(); results.posRoll_ = pos_.getRoll(); results.posPitch_ = pos_.getPitch(); results.posHeading_ = pos_.getHeading(); results.posXDot_ = posDot_.getX(); results.posYDot_ = posDot_.getY(); results.posZDot_ = posDot_.getZ(); results.rateU_ = rate_.getU(); results.rateV_ = rate_.getV(); results.rateW_ = rate_.getW(); results.rateP_ = rate_.getP(); results.rateQ_ = rate_.getQ(); results.rateR_ = rate_.getR(); results.utmZone_ = 0; // utmZone_; results.northernHemi_ = 0; // northernHemi_; if( northCurrent_ == northCurrent_ && eastCurrent_ == eastCurrent_ ) { // The sim definition is U = northward, V = eastward. current_.setU( northCurrent_ ); current_.setV( eastCurrent_ ); } simulateSensors( time, latDeg, lonDeg, results ); // The sim definition is U = northward, V = eastward. results.northCurrent_ = current_.getU(); results.eastCurrent_ = current_.getV(); results.vertCurrent_ = current_.getW(); }