#include #include #include #include #include #include "Simulator.h" #include "System.h" #include "MathP.h" #include "FastTime.h" #include "VehicleConfigurationIF.h" #include "IntegerAttribute.h" #include "FloatAttribute.h" #include "AngleAttribute.h" #include "StringAttribute.h" #include "BooleanAttribute.h" #include "AttributeParser.h" #include "Syslog.h" #include "System.h" #include "WorkSiteIF.h" #include "NavUtils.h" #include "matrixMath.h" // Number of simulation steps per command #define NSteps 10 #define CONTROL_LOOP_PERIOD 200 // Milliseconds /*-----------------------------------------------------------------------* | structure of this file: | global variables declarations (there are a lot of them) | main() simple interface to test (for now) | start_sub(), calls lhs | lhs() precompute lhs of hydro equations | motion() main computation | hydro() calcs rhs of hydro equations | fins() computes forces caused by fins | act_model() fin actuator model (just a delay) | thruster_force(),thruster_torque() thruster model | matinv() | *-----------------------------------------------------------------------*/ /*------------------- generic constants -------------------------------*/ #define FTOM 0.3048 /* from feet to meters */ #define FTOM2 (0.3048 * 0.3048) #define FTOM3 (0.3048 * 0.3048 * 0.3048) #define STOKG 14.59 #define TWOPI (2*PI) // Simulation time increment, in seconds const double Simulator::_timeIncr = (NavigationPeriodMillisec / 1000.) / NSteps; /* coordinate transformation matrix, we use only indices 1-6 */ double Simulator::ctrn[7][7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; double Simulator::_position[7] = {0., 0., 0., 0., 0., 0., 0.}; double Simulator::_position0[7] = {0., 0., 0., 0., 0., 0., 0.}; double Simulator::_rate[7] = {0., 0., 0., 0., 0., 0., 0.}; // //global pointer to current simulator object. This is for the call to bisect. Simulator *psim; /*----------------------- vehicle parameters ------------------------------*/ /* Note that some of the parameters are defined in the data structure. If READ_HYDRO is 1 (in sim_main.c), most of the following parameters get overwritten from get_am.tex and get_drag.tex, which are generated by running get_params.m in the matlab environment. The default parameters listed here are current 5/25/95, based on Draper's Maneuvering Tool. If there is a chance we'll overwrite the value, it is NOT declared a constant. x is fwd, y stbd, z down. The following values give a cg-cb separation that accounts for zero static pitch, 0 degrees static stb roll, and an undamped roll period of approximately 3 seconds */ const double Simulator::Ixy = 0.; const double Simulator::Izx = 0.; const double Simulator::Iyz = 0.; const double Simulator::fin_speed = Math::degToRad(35.); const double Simulator::cdo = 0.0065; /* Cd from NACA 0015 section */ const double Simulator::ec = 0.90; /* "Oswald" efficiency factor */ const int Simulator::delay = 30; /* 10 int. steps per sample prd*/ static FastTime* Simulator::_fastTime=NULL; /* THRUSTER. ------------------------------------------------------------*/ /* torque constant, in Nm/A - inc gearbox */ const double Simulator::Kt = .456; Simulator::Simulator(const char *configFileName, const char *outputFileName) : SimulatorIF_SK(), _attributes("Simulator"), _eventLog("eventLog"), _useOctrees(True), _fastSim(False) { Boolean debug = True; _log = new SimulationLog(this, DataLog::BinaryFormat); _nBatts = 3; _battLog = new BluefinBattLog(this, DataLog::BinaryFormat, _nBatts); _missionClock = new MissionClock(); // // Get flight-code vehicle parameters from vehicle.cfg // dprintf("Create VehicleConfigurationIF...\n"); _configuration = new VehicleConfigurationIF("configuration"); // dprintf("Create DropWeightIF...\n"); // _dropWeight = new DropWeightIF("dropWeight"); dprintf("Okay...\n"); _input.vehicleMass = _configuration->mass(); _configuration->centerOfMass(_centerOfMass); _configuration->centerOfBuoyancy(_centerOfBuoyancy); Pit = _configuration->propPitch(); Area = _configuration->ductArea(); Eta = _configuration->propEfficiency(); Xuabu = _configuration->xuabu(); mTs = _configuration->Ts(); mTsMsec = (long) mTs * 1000; // // On-board hack: Fixed OdysseyTailcone.cc; but the Pit number // doesn't match the sim. Hardcode a temporary fix 2.403; // FIX THIS LATER rsm 00/1/18 // Pit *= 2.403; VehicleConfigurationIF::Vector location; _configuration->dropWeight(1, &Wdw1, &Bdw1, location); Xdw1 = location[X]; Ydw1 = location[Y]; Zdw1 = location[Z]; _configuration->dropWeight(2, &Wdw2, &Bdw2, location); Xdw2 = location[X]; Ydw2 = location[Y]; Zdw2 = location[Z]; // Get fin locations VehicleConfigurationIF::Vector lowerRudder, portElevator; VehicleConfigurationIF::Vector upperRudder, stbdElevator; _configuration->finLocations(lowerRudder, upperRudder, portElevator, stbdElevator); // Copy into an array that starts at one (for example r1[0] is unused). for (int i = X; i <= Z; i++) { r1[i+1] = lowerRudder[i]; r2[i+1] = portElevator[i]; r3[i+1] = upperRudder[i]; r4[i+1] = stbdElevator[i]; } dprintf("Create attributes...\n"); // Define attributes and default values. // Set some attribute defaults to Odyssey values for now! /* translational and rotational body mass */ _attributes.add(new FloatAttribute("Ixx", "?", &Ixx)); _attributes.add(new FloatAttribute("Iyy", "?", &Iyy)); _attributes.add(new FloatAttribute("Izz", "?", &Izz)); /* hydrodynamic parameters ----------------------------------------------*/ _attributes.add(new FloatAttribute("Kpabp", "?", &Kpabp)); _attributes.add(new FloatAttribute("Mqabq", "?", &Mqabq)); _attributes.add(new FloatAttribute("Nrabr", "?", &Nrabr)); _attributes.add(new FloatAttribute("Yvabv", "?", &Yvabv)); _attributes.add(new FloatAttribute("Zwabw", "?", &Zwabw)); /* added mass */ _attributes.add(new FloatAttribute("Kpdot", "?", &Kpdot)); _attributes.add(new FloatAttribute("Mqdot", "?", &Mqdot)); _attributes.add(new FloatAttribute("Nrdot", "?", &Nrdot)); _attributes.add(new FloatAttribute("Xudot", "?", &Xudot)); _attributes.add(new FloatAttribute("Yvdot", "?", &Yvdot)); _attributes.add(new FloatAttribute("Zwdot", "?", &Zwdot)); /* added mass cross terms */ _attributes.add(new FloatAttribute("Kvdot", "?", &Kvdot)); _attributes.add(new FloatAttribute("Mwdot", "?", &Mwdot)); _attributes.add(new FloatAttribute("Nvdot", "?", &Nvdot)); _attributes.add(new FloatAttribute("Yrdot", "?", &Yrdot)); _attributes.add(new FloatAttribute("Ypdot", "?", &Ypdot)); _attributes.add(new FloatAttribute("Zqdot", "?", &Zqdot)); /* quadratic drag cross terms */ _attributes.add(new FloatAttribute("Mwabw", "?", &Mwabw)); _attributes.add(new FloatAttribute("Nvabv", "?", &Nvabv)); _attributes.add(new FloatAttribute("Yrabr", "?", &Yrabr)); _attributes.add(new FloatAttribute("Zqabq", "?", &Zqabq)); /* in-line lift and drag */ _attributes.add(new FloatAttribute("Muq", "?", &Muq)); _attributes.add(new FloatAttribute("Nuv", "?", &Nuv)); _attributes.add(new FloatAttribute("Muw", "?", &Muw)); _attributes.add(new FloatAttribute("Mpr", "?", &Mpr)); _attributes.add(new FloatAttribute("Nur", "?", &Nur)); _attributes.add(new FloatAttribute("Npq", "?", &Npq)); _attributes.add(new FloatAttribute("Xvv", "?", &Xvv)); _attributes.add(new FloatAttribute("Xww", "?", &Xww)); _attributes.add(new FloatAttribute("Xvr", "?", &Xvr)); _attributes.add(new FloatAttribute("Xwq", "?", &Xwq)); _attributes.add(new FloatAttribute("Xrr", "?", &Xrr)); _attributes.add(new FloatAttribute("Xqq", "?", &Xqq)); _attributes.add(new FloatAttribute("Yur", "?", &Yur)); _attributes.add(new FloatAttribute("Yuv", "?", &Yuv)); _attributes.add(new FloatAttribute("Ywp", "?", &Ywp)); _attributes.add(new FloatAttribute("Zuq", "?", &Zuq)); _attributes.add(new FloatAttribute("Zuw", "?", &Zuw)); _attributes.add(new FloatAttribute("Zvp", "?", &Zvp)); _attributes.add(new FloatAttribute("Kvt2", "?", &Kvt2)); // // Tailcone and fin model // _attributes.add(new FloatAttribute("aspectRatio", "?", &aspect_ratio)); _attributes.add(new AngleAttribute("stallAngle", "?", &stall_angle)); _attributes.add(new AngleAttribute("wideHystRud", "?", &wide_hyst_rud)); _attributes.add(new AngleAttribute("centerHystRud", "?", ¢er_hyst_rud)); _attributes.add(new AngleAttribute("wideHystElev", "?", &wide_hyst_elev)); _attributes.add(new AngleAttribute("centerHystElev", "?", ¢er_hyst_elev)); _attributes.add(new FloatAttribute("finArea", "?", &S)); _attributes.add(new FloatAttribute("CDc", "?", &CDc)); _attributes.add(new FloatAttribute("dCL", "?", &dCL)); // // Initial Vehicle State // _attributes.add(new FloatAttribute("initX", "?", &_position[1], 0.)); _attributes.add(new FloatAttribute("initY", "?", &_position[2], 0.)); _attributes.add(new FloatAttribute("initZ", "?", &_position[3], 0.)); _attributes.add(new AngleAttribute("initPhi", "?", &_position[4], 0.)); _attributes.add(new AngleAttribute("initTheta", "?", &_position[5], 0.)); _attributes.add(new AngleAttribute("initPsi", "?", &_position[6], 0.)); _attributes.add(new FloatAttribute("initU", "?", &_rate[1], 0.)); _attributes.add(new FloatAttribute("initV", "?", &_rate[2], 0.)); _attributes.add(new FloatAttribute("initW", "?", &_rate[3], 0.)); _attributes.add(new AngleAttribute("initP", "?", &_rate[4], 0.)); _attributes.add(new AngleAttribute("initQ", "?", &_rate[5], 0.)); _attributes.add(new AngleAttribute("initR", "?", &_rate[6], 0.)); _attributes.add(new FloatAttribute("zMax" , "?", &zMax, 210.)); _attributes.add(new FloatAttribute("northCurrent", "?", &northCurrent, 0.)); _attributes.add(new FloatAttribute("eastCurrent", "?", &eastCurrent, 0.)); _attributes.add(new IntegerAttribute("msSimDelay", "ms Delay between FastSim GNC iterations", &_msSimDelay, 0)); Syslog::write("Simulator::Simulator() - msSimDelay = %d", _msSimDelay); char depthName[32], yName[32]; for( i=0; i< MAXNDEPTHS; i++ ) { sprintf(depthName, "depth%d", i+1); _attributes.add(new FloatAttribute(depthName, depthName, &_depth[i], -1)); sprintf(yName, "ydepth%d",i+1); _attributes.add(new FloatAttribute(yName, yName, &_ydepth[i], -1)); } _attributes.add(new FloatAttribute("kWhr", "?", &_energy0, 2.)); _attributes.add(new FloatAttribute("hotel_W", "?", &_Ph, 30.)); char nameStr[64]; sprintf( nameStr, "mapFileName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_mapFileName, "NotSpecified" ) ); _attributes.add( new BooleanAttribute("verticalMount", "verticalMount", &_vertMt, 1 ) ); _attributes.add( new BooleanAttribute("fwdEnabled", "fwdEnabled", &_fwdEnabled, 1 ) ); // Move this to vehicle.cfg rho = 1.025e3; /* kg/m^3 density of seawater */ dprintf("Simulator:: initPsi = %f\n", _position[6]*180/PI); dprintf("Done creating attributes. Parse 'em...\n"); AttributeParser::parse(configFileName, &_attributes); // // Copy the config file (usually simulator.cfg) to the log dir: System::copyToLogDir( configFileName ); dprintf("Simulator:: initPsi = %f\n", _position[6]*180/PI); dprintf("Simulator:: zMax = %f\n", zMax); // Convert the initial lat/long provided in worksite.cfg // WorkSiteIF workSite("workSite"); // Initialize x-y to worksite location NavUtils::geoToUtm(workSite.latitude(), workSite.longitude(), workSite.utmZone(), &_position[1], &_position[2]); // // Save the initial position _position0[1] = _position[1]; _position0[2] = _position[2]; // Initialize the state. // dprintf(" The utm zone is %d\n", workSite.utmZone()); dprintf(" Simulator: The lat/long in workSite.cfg is %g, %g", workSite.latitude(), workSite.longitude()); dprintf(" Simulator: The lat/long in workSite.cfg, cvt'ed to UTM is %g, %g", _position[1], _position[2]); for( i=0; i= ydepth%d + 1 meter. " "Exiting.", i+2, i+1); exit(1); } // _slope[i] = (_depth[i+1] - _depth[i]) / (_ydepth[i+1] - _ydepth[i]); _yinter[i] = _depth[i] - _slope[i]*_ydepth[i]; // // See p. 364 of CRC Standard Mathematical Tables, 18th Ed. alpha = atan(_slope[i]); // // Put the normal vector in Newtonian (N) coordinates, which are NED. _normalArray[0][i] = 0.; _normalArray[1][i] = -sin(alpha); _normalArray[2][i] = cos(alpha); } dprintf("Simulator: _nydepths = %d", _nydepths); for( i=0; i= %11d bytes.", _map.GetTreeSize() ); } } catch(...) { Syslog::write("Simulator:: Octree couldn't read %s. Use simple map.", _mapFile); } _lastBeamRange = 1; } init_simulation(); _firstCallback = True; } Simulator::~Simulator() { Boolean debug=False; delete _configuration; // delete _dropWeight; delete _layeredControl; delete _missionClock; delete _log; delete _battLog; if (Simulator::_fastTime!=NULL) { dprintf("\n****Simulator::~Simulator dtor [%p] deleting FastTime [%p]\n",this,Simulator::_fastTime); delete Simulator::_fastTime; } /* original code delete _configuration; // delete _dropWeight; delete _layeredControl; delete _missionClock; delete _log; delete _battLog; */ } long Simulator::setCurrent(double north, double east) { _input.northCurrent = north; _input.eastCurrent = east; return 0; } long Simulator::command(double propSpeedCmd, double elevator, double rudder) { Boolean debug = False; double LHS; static Boolean first=True; if (!_missionStarted) return 0; if ( first ) { _missionClock->reset(); first = False; printf("Simulator: Got here!\n"); } // // We've replaced uC with propSpeedCmd as the first argument above. // propSpeedCmd (omega_P) is now computed as a function of uC in // DynamicControlServer.cc // // Also, thrAmps used to be computed here, and passed to motion(), // and then to thruster_force() and thruster_torque(). These have been // changed to take omega_P as the input. // omega_P = propSpeedCmd; dprintf(" Simulator: propSpeedCmd = %.2f\n", propSpeedCmd ); #if 0 double uC = 1.54; // // Convert commanded vehicle speed, uC, to commanded // propeller speed, using the Momentum Theory Equations in Franz's // Modeling report, Eqns. 30 - 32. Also see my notes, book #3, p.5 // The implementation here is a cleaned up version of that in Odyssey. // // Although the model is unrealistic at slow or negative speeds, I've // fixed the implementation so that a negative speed command results in // the prop reversing direction, and has a negative current command. // // I've also made the emperical Cdf1 symmetric for +/- speed, because it // surely is not valid in the reverse direction, and this way it // at least gives a reasonable answer. // // Beware that the basic force derivative relation is // // Xuabu = - 1/2 rho Af Cdf0 // // Here, they use a more accurate version of Xuabu; // // Xuabu1 = Xuabu Cdf1 // // Cdf0 is the coeffecient of drag, and Cdf1 is an emperical modifying // factor that depends on speed. See Franz's Ody. Sys. Id. report, Eqn. 4. // // Kthr is not constant here since Cdf = Cdf0*Cdf1 contains uC. // // The sign on thrAmps must be explicitly set for reverse, since this // equation contains only an omega^2 and cannot implicitly know the sign. // // Beware that this model is NOT used in the actual tailcone algorithm, the // cubic fix is. // Cdf1 = ( 2.455 - 2.175*fabs(uC) + 0.75*pow(uC,2.) ); Kthr = ( 1. + sqrt( 1. - 4.*Xuabu*Cdf1/rho/Area) )/Eta/Pit/2.; omega_P = Kthr * uC; thrAmps = .5 * rho * Area * Eta * Pit * ( pow( Eta*Pit*omega_P, 2. ) - pow( uC, 2. ) )/Kt; // // Retain forward motion only until the fins() routine is checked out // for reverse. // //thrAmps *= Math::sgn(uC); // omega_P = fabs(omega_P); thrAmps = Math::limit( thrAmps, 5.0, -5.0 ); dprintf(" Simulator: Computed propSpeedCmd = %.2f\n", omega_P ); // //This was the original version of the above: // // LHS = -Xuabu*pow(uC,2.)*( 2.455 - 2.175*uC + // 0.75*pow(uC,2.) ) ; // omega_P = ( ductArea*rho*propEfficiency*propPitch*uC + // sqrt( pow(ductArea*rho*propEfficiency*propPitch* // uC,2.) + // 4.*LHS*ductArea*rho*propEfficiency*propEfficiency* // propPitch*propPitch ) ) / // (2.*ductArea*rho*pow(propEfficiency*propPitch,2.)); // // OK, we'll do it the old way. Compute thruster current here. // // thrAmps = // (.5*rho*ductArea * pow(propEfficiency * propPitch,3.) * pow(omega_P, 2.)- // .5*rho*ductArea * propEfficiency * propPitch * pow(uC,2.))/Kt; // /* make sign same as desired speed */ // thrAmps *= Math::sgn(uC); // /* limit to +- 5 amps, update */ // thrAmps = Math::limit( thrAmps, 5.0, -5.0 ); #endif Boolean timeDebug = False; // _simTime = _missionClock->seconds(); // dprintf("Simulator::command() - t=%.2f elevator=%.7f\n", // _simTime, elevator); struct timespec startTime, endTime; // Get starting time clock_gettime(CLOCK_REALTIME, &startTime); _input.propOmega = omega_P; _input.elevator = elevator; _input.rudder = rudder; long simTimeMsec; // // This should be simTimeMsec = (long) ( _simTime * 1000.); // but this compiles and then omits many "write"s. There is a // rounding problem; so I'll try counting with kk. // simTimeMsec = _simTime * 1000.; // // I'd like to use mTsMsec instead of hardcoding 1000 below, but that // causes a floating point exception. // dprintf("DEBUG - simTime = %f, simTimeMsec=%d, modulo 1000=%d\n", _simTime, simTimeMsec, simTimeMsec % 1000); // if( (simTimeMsec % 1000) == 0 ) // Syslog::write("\n simTime = %.2f\n", _simTime); // // Save and log water depth. _waterDepth = waterDepth(0., 0.); if( (kk % 500) == 0 ) { Syslog::write("Simulator:: Before the call to motion(): \n" " simTime = %.2f, (simTime - realTime) = %.2f\n" "\tHeading = %.2f \n\tDepth = %.2f\n\tSpeed = %.2f\n" "\tPitch = %.2f", _simTime, _simTime-_missionClock->seconds(), _position[6]*180/PI, _position[3], _rate[1], _position[5]*180/PI); } kk++; for (int i = 0; i < NSteps; i++) { motion(_rate, _position, omega_P, _input.rudder, _input.elevator, _timeIncr); _simTime += _timeIncr; } clock_gettime(CLOCK_REALTIME, &endTime); _dynT = (endTime.tv_sec * 1000 + endTime.tv_nsec / 1000000) - (startTime.tv_sec * 1000 + startTime.tv_nsec / 1000000); if( _firstCallback ) { _firstCallback = False; _simLoopInterval = 0.; } else { _simLoopInterval = (endTime.tv_sec * 1000 + endTime.tv_nsec / 1000000) - (_lastTime.tv_sec * 1000 + _lastTime.tv_nsec / 1000000); } _lastTime.tv_sec = endTime.tv_sec; _lastTime.tv_nsec = endTime.tv_nsec; //Syslog::write("Simulator::simTime = %.2f; realTime = %.3f", _simTime, // _missionClock->seconds()); // // Compute battery energy remaining and voltage: battery(); // // Log data at every sampling instant. Move this into motion() to log // at every integration step. // _log->write(); // // Copy the direction cosine into an array that works with TVMult. // // Had to move this back into beamAlt and also usbl(). I don't know why it // didn't work here. // for( i=0; i<3; i++ ) // for( int j=0; j<3; j++ ) T_N_B[i][j] = psim->ctrn[i+1][j+1]; // Update fast time if necessary // if (_fastSim) { #ifdef FASTTIME updateFastTime(); // Use non-zero delay to slow the fastsim // if (_msSimDelay > 0) System::milliSleep(_msSimDelay); #endif } // Trigger NewOutput event to subscribers // triggerEvent(SimulatorIF::NewOutput); return 0; } // Update simulation time // void Simulator::updateFastTime() { Boolean debug=False; #ifdef FASTTIME FastTime* st = Simulator::_fastTime; st->_time.read(); dprintf("\t\t\tSimulator::updateFastTime - testing FastTime before: %ld %ld", st->_time.data.ts.seconds, st->_time.data.ts.nanoSeconds); st->incrFastTime(CONTROL_LOOP_PERIOD); st->_time.read(); dprintf("\t\t\tSimulator::updateFastTime - testing FastTime after: %ld %ld", st->_time.data.ts.seconds, st->_time.data.ts.nanoSeconds); #endif } void Simulator::battery( void ) { //////////////////////////////////////////// // // Purpose: Simulate decreasing battery energy and voltage. // Original Author: R. McEwen, 17 Oct 2007. // Change log: (1) // //////////////////////////////////////////// // // Power consumption from mapping vehicle speed step run 2004.120.06. Propulsion // is modeled as P = K1*omega^3 + K0, NOT including hotel. // [K1 K0]' = // [(pi/30)^3*[288^3 228^3 188^3 150^3 130^3] ; 1 1 1 1 1]'\... // [129 75.0 50.5 33.1 28]' double K1 = 4.0595e-003; // double K0 = 1.8406e+001; // Watts // // Page 6 of the BF battery manual, 8/14/2002. The voltage decreases to 29 // at 90% discharge. double yInt = 32.0; // Voltage at full charge. double y90 = 29.0; // Voltage at 90% discharge. double maxDis= .9; // Battery dies at 90% discharge. double slope = (y90 - yInt)/maxDis; double voltage, power, discharge; double omega = fabs(_output.propOmega); double Ts = _simTime - _lastSimTime; _lastSimTime = _simTime; // // Compute instantaneous power consumption: power = ( K1 * pow(omega,3.) + K0 + _Ph ); if( omega < .1 ) power = _Ph; // // Compute energy left. This is a state. The equation below is an // accumulator. //printf("power = %.2f, energy = %.2f, Ts = %.2f\n", power, _energy, Ts); _energy -= power*Ts; if( _energy <= 0. ) _energy = 0.; discharge = 1. - _energy/(_energy0*3600.*1000.); voltage = slope*discharge + yInt; if( voltage > yInt ) voltage = yInt; if( voltage < y90 ) voltage = y90; // // Assume we're using a single battery. if( discharge <= maxDis ) { _battData.state = BluefinBatteryIF::Discharging; _battData.fault = BluefinBatteryIF::Clear; _battData.voltage = voltage; _battData.current = power/voltage; } else { _battData.state = BluefinBatteryIF::Shutdown; _battData.fault = BluefinBatteryIF::UnderVolt; _battData.voltage = 28.; _battData.current = 0.; } // // Log every two seconds. It's pretty slow. if( (kk % 10) == 0 ) _battLog->write(); } void Simulator::getSimBatteryData(BluefinBatteryIF::BfBattData *simBattData) { memcpy( simBattData, &_battData, sizeof(BluefinBatteryIF::BfBattData) ); } void Simulator::getSimDeltaTData( Boolean *_vertMtOut, Boolean *_fwdEnabledOut) { *_vertMtOut = _vertMt; *_fwdEnabledOut = _fwdEnabled; } #if 0 void Simulator::hydroscat( void ) { _hydroscatData.snorm1 = (short) 2*_position[3]; _hydroscatData.snorm2 = (short) 5*_position[3]; _hydroscatData.snorm3 = (short) 10*_position[3]; _hydroscatData.calculated.fl676_uncorr = 15.*_position[3]; memcpy( &(_hydroscatOutput->data), &_hydroscatData, sizeof(HydroscatIF::Data)); _hydroscatOutput->data.deviceReady = True; _hydroscatOutput->write(); } void Simulator::isus( void ) { _isusOutput->data._deviceReady = True; _isusOutput->data._nitrate = 5*_position[3]; _isusOutput->write(); } #endif #if 0 //***********DEBUG Remove this void Simulator::getSimBatteryData(BluefinBatteryIF::BfBattData *simBattData) { return; } //***********END DEBUG #endif long Simulator::rotationalVelocity(SimulatorIF::Vector velocity, SimulatorIF::Vector backDiff) { memcpy((void *)velocity, (void *)_output.rotationalVelocity, sizeof(SimulatorIF::Vector)); memcpy((void *)backDiff, (void *)_output.rotationalBackDiff, sizeof(SimulatorIF::Vector)); return 0; } long Simulator::translationalVelocity(SimulatorIF::Vector velocity, SimulatorIF::Vector backDiff) { memcpy((void *)velocity, (void *)_output.translationalVelocity, sizeof(SimulatorIF::Vector)); memcpy((void *)backDiff, (void *)_output.translationalBackDiff, sizeof(SimulatorIF::Vector)); return 0; } long Simulator::translationalBottomVelocity(SimulatorIF::Vector vel_Bo_N_B) { memcpy((void *)vel_Bo_N_B, (void *)_output.vel_Bo_N_B, sizeof(SimulatorIF::Vector)); return 0; } long Simulator::state(SimulatorIF::Vector position, SimulatorIF::Vector positionRate, SimulatorIF::Vector eulerAngles, SimulatorIF::Vector rotationRate) { memcpy((void *)position, (void *)_output.position, sizeof(SimulatorIF::Vector)); memcpy((void *)positionRate, (void *)_output.translationalVelocity, sizeof(SimulatorIF::Vector)); memcpy((void *)eulerAngles, (void *)_output.eulerAngles, sizeof(SimulatorIF::Vector)); memcpy((void *)rotationRate, (void *)_output.rotationalVelocity, sizeof(SimulatorIF::Vector)); return 0; } long Simulator::controlSurfaces(double *rudder, double *elevator) { // printf("Begin controlSurfaces()\n"); *rudder = _output.rudder; *elevator = _output.elevator; // printf("End controlSurfaces()\n"); return 0; } long Simulator::force(SimulatorIF::Vector vector) { memcpy((void *)vector, (void *)_output.force, sizeof(SimulatorIF::Vector)); return 0; } long Simulator::torque(SimulatorIF::Vector vector) { memcpy((void *)vector, (void *)_output.torque, sizeof(SimulatorIF::Vector)); return 0; } long Simulator::propOmega(double *omega) { // printf("Begin propOmega()\n"); *omega = _output.propOmega; // printf("End propOmega()\n"); return 0; } long Simulator::motorCurrent(double *amps) { *amps = thrAmps; return 0; } double Simulator::simTime() { // return _missionClock->seconds(); return _simTime; } // // Compute depth as a function of _position + (dx,dy). // Parameters not local here are initialized or computed in the constructor. double Simulator::waterDepth( double dx, double dy) { double depth, posx, posy; int i, j; posx = _position[1] + dx; posy = _position[2] + dy; double nadir_N[3] = {0., 0., 1.}; long counter; double grazing; double offset_B[3] = {0., 0., 0.}; double rangeTol = 0.; double maxRange = 10000.; if( _useOctrees ) { Vector startpoint, dir_N; dir_N.SetValues( 0., 0., 1.); startpoint.SetValues( posx, posy, 0. ); return depth = _map.RayTrace( startpoint, dir_N ); } if( _ydepth[0] == -1 || posy < _ydepth[0] ) { depth = _depth[0]; if( _computeGrazing ) { _bottomNormal[0]=0.; _bottomNormal[1]=0.; _bottomNormal[2]=1.; } } else if( posy< _ydepth[_nydepths-1] ) { for( i=0; i< _nydepths-1; i++ ) { if( posy >= _ydepth[i] && posy < _ydepth[i+1] ) { depth = _slope[i]*posy + _yinter[i]; if( _computeGrazing ) for( j=0; j<3; j++ ) _bottomNormal[j] = _normalArray[j][i]; break; } if( i >= _nydepths-1 ) { Syslog::write("waterDepth() ERROR - Shouldn't get here. Depth = %.2f " "posy = %.2f, _ydepth[%d]=%.2f, simTime= %.2f", depth, posy, i+1, _ydepth[i+1], _simTime); exit(1); } } } else { depth = _depth[ _nydepths-1 ]; if( _computeGrazing ) for( j=0; j<3; j++ ) _bottomNormal[j] = _normalArray[j][_nydepths-1]; } return depth; } // // PURPOSE: Return the altitude of a point rho along the beam. This is // called by beamRange() further below. The business with the psim pointer // below allows this function to be passed as a function pointer to the // bisect() routine in utils/Math.cc, by beamRange(). // double Simulator::beamAlt( double rho ) { // // p_N is the vector from inertial origin (No) to a point p along the Dvl // line-of-sight (los), coordinatized in the Earth-fixed inertial frame N. // The depth is the third component, p_N[Z]. Then, p_N is // // p_N = r_No_Bo_N + r_Bo_p_N. (0) // // r_Bo_p_N is the vector from the body-fixed frame Bo to a point p that lies // along the Dvl los, coordinatized in the inertially fixed N frame. // // p_N = r_No_Bo_N + T_N_B*( r_Bo_Dvlo_B + los_B*rho ). (1) // // r_No_Bo_N is the vector from No to the origin of the body-fixed frame Bo, // coordinatized in the inertially fixed N frame. // // This reduces to a scalar equation for the third component: // // p_N[Z] = r_No_Bo_N[Z] + [0 0 1]*( T_N_B*( r_Bo_Dvlo_B + los_B*rho ) ). (2) // // The altitude of p_N is then // // waterDepth( r_Bo_p_N[X], r_Bo_p_N[Y]) - p_N[Z]. (3) // // Bo is the origin of the body-fixed reference frame. // In Simulator.cc; r_No_Bo_N = _position, and T_N_B = ctrn[1:3][1:3]. (4) // Also, r_Bo_Dvlo_B = _offset_B. (4') // // NOTE: For the simple case of the Dvl los, r_Bo_Dvlo_B = [0 0 0]', and // los_B = [0 0 1]'; so Eqns. 2 and 3 reduce to // // altitude = waterDepth(p_N[X], p_N[Y]) - r_No_Bo_N[Z] - T_N_B[3][3]*rho.(5) // // NOTE: The Dvl is used for example here. This algo works for any sonar. // // First compute r_Bo_p_N from Eqns. 0 and 1. double r_Bo_p_B[3], r_Bo_p_N[3], T_N_B[3][3]; double altitude, los_N[3]; //for( int i = 0; i<3; i++ ) r_Bo_p_N[i] = r_Bo_Dvlo_B[i] + los_B[i]*rho ) for( int i = 0; i<3; i++ ) r_Bo_p_B[i] = psim->_offset_B[i] + psim->_los_B[i]*rho; // // Copy the direction cosine into an array that works with TVMult. // for( i=0; i<3; i++ ) for( int j=0; j<3; j++ ) T_N_B[i][j] = psim->ctrn[i+1][j+1]; TVMult( r_Bo_p_N, T_N_B, r_Bo_p_B ); altitude = psim->waterDepth( r_Bo_p_N[0], r_Bo_p_N[1] ) - (psim->_position[3] + r_Bo_p_N[2] ); if( !psim->_computeGrazing ) return altitude; // // Grazing angle computation: // //for( i=0; i<3; i++) los_N[i] = r_Bo_p_N[i]; TVMult( los_N, T_N_B, psim->_los_B ); Vnormalize( los_N ); // // Compute the grazing angle double losCrossNormal[3], norm; Vcross( losCrossNormal, los_N, psim->_bottomNormal ); norm = Vnorm( losCrossNormal); if( norm > 1. ) norm = 1.; psim->_grazing = PI/2. - asin( norm ); #if 0 Syslog::write("beamAlt - los_B = [%9.3f %9.3f %9.3f]", psim->_los_B[0], psim->_los_B[1], psim->_los_B[2]); Syslog::write("beamAlt - los_N = [%9.3f %9.3f %9.3f]", los_N[0],los_N[1],los_N[2]); Syslog::write("beamAlt - bottomNormal = [%9.3f %9.3f %9.3f]", psim->_bottomNormal[0],psim->_bottomNormal[1],psim->_bottomNormal[2]); Syslog::write("beamAlt - norm = %9.3f", norm); Syslog::write("beamAlt - grazing = %9.3f deg.", psim->_grazing*180./PI); Syslog::write("_simTime=%.2f", psim->_simTime); #endif return altitude; } // // PURPOSE: Compute range to the bottom along the specified line-of-sight. // Return: // // maxRange: If there is no intersection with the bottom or // surface, or when the range exceeds maxRange. // zero: _position + offset lands us beneath the bottom, or // range < minRange. // #define MAXINC 16 double Simulator::beamRange( SimulatorIF::Vector offset_B, SimulatorIF::Vector los_B, double minRange, double maxRange, double rangeTol, long *simCntr, double *grazing, Boolean *surfaceDetect) { double los_N[3], offset_N[3], T_N_B[3][3]; double location_N[3]; int i; for( i=0; i<3; i++ ) for( int j=0; j<3; j++ ) T_N_B[i][j] = psim->ctrn[i+1][j+1]; double norm = Vnorm( los_B ); for( i=0; i<3; i++ ) los_B[i] = los_B[i]/norm; TVMult( los_N, T_N_B, los_B ); TVMult( offset_N, T_N_B, offset_B ); norm = Vnorm( los_N ); for( i=0; i<3; i++ ) los_N[i] = los_N[i]/norm; // // Location of the sonar in N, accounting for lever arm: for( i=0; i<3; i++ ) location_N[i] = _position[i+1]+offset_N[i]; if(_useOctrees) { Vector startpoint, dirVec; dirVec.SetValues( los_N[0], los_N[1], los_N[2]); startpoint.SetValues( location_N[0], location_N[1], location_N[2]); // // 0 => Inside a solid voxel. // -1 => No intersection with a voxel. _beamRange = _map.RayTrace( startpoint, dirVec ); if( _beamRange == -1. ) { if( _lastBeamRange != -1.) _missCntr = 0; _missCntr++; _lastMissTime = _simTime; } else { if( _lastBeamRange == -1.) { //Syslog::write("Simulator::Octree:: %d misses at simtime = %.2f.", // _missCntr, _lastMissTime); } } if( _beamRange == 0. && _lastBeamRange != 0.) { Syslog::write("Simulator::Octree:: Inside a solid voxel at " "simtime = %.2f.", _simTime); } _lastBeamRange = _beamRange; *simCntr = 0; *grazing = PI/2.; if( _beamRange > maxRange || _beamRange == -1.) _beamRange = maxRange; } else { double rho = 0.; double drho = maxRange /( (double) MAXINC ); _computeGrazing = False; // // Pass los_B to beamAlt() through member variables. It can't be in // beamAlt's argument list because bisect requires only the independent // variable. for( int i=0; i<3; i++ ) { _los_B[i] = los_B[i]; _offset_B[i] = offset_B[i]; } // if( beamAlt( minRange ) < 0. ) { //Syslog::write("Simulator::beamRange. Error. Negative beam range.\n"); _beamRange = 0.; } else { double maxRangeLocal = maxRange; double minRangeLocal = minRange; // // First, ensure that the bottom is within range. This immediately // brings up the issue of multiple zeros, because we can't simply // check beamAlt(maxRange). If the vehicle is pitched up, the beam // may pass through a hill and still have beamAlt(maxRange) > 0. // So, a very crude way to avoid this is to walk down the beam in // fixed increments and check beamAlt at each increment. for( i=1; i<=MAXINC; i++ ) { //rho = ( (double) i)/( (double) MAXINC) * maxRange; rho = ( (double) i )*drho; if( beamAlt( rho ) < 0. ) { maxRangeLocal = rho; minRangeLocal = rho - drho; break; } } //if( beamAlt( maxRangeLocal ) > 0. ) return 0.; //This is what RDI does. if( beamAlt( maxRangeLocal ) > 0. ) { *grazing = 0.0; _beamRange = maxRangeLocal; } else { // // Use bisect to find the intersection of the beam with the // bottom: _beamRange = Math::bisect( beamAlt, minRangeLocal, maxRangeLocal, rangeTol, count ); _computeGrazing = True; double altitude; altitude = beamAlt(_beamRange); _computeGrazing = False; *grazing = _grazing; *simCntr = (long) *count + (long) i; } } } // if(_useOctrees) else use PWL bottom. if( _beamRange < minRange ) _beamRange = 0.; // // Now check for water-surface intersection. if( los_N[2] < 0. && _beamRange == maxRange && (location_N[2] + los_N[2]*_beamRange <= 0. ) ) { _beamRange = -location_N[2]/los_N[2]; *surfaceDetect = True; } else *surfaceDetect = False; return _beamRange; } double Simulator::usbl(SimulatorIF::Vector usblOffset_B, SimulatorIF::Vector transponder, double minRange, double maxRange, SimulatorIF::Vector usblRange ) { double r_N[3], T_B_N[3][3], usblOffset_N[3]; int i; // // Copy the direction cosine into an array that works with TVMult. // for( i=0; i<3; i++ ) for( int j=0; j<3; j++ ) T_N_B[i][j] = psim->ctrn[i+1][j+1]; TTransp(T_B_N,T_N_B); TVMult(usblOffset_N, T_N_B, usblOffset_B); // // _position[0] is unused, x is in _position[1], etc. for( i=0; i<3; i++ ) r_N[i] = transponder[i] - psim->_position[i+1] - usblOffset_N[i]; TVMult(usblRange, T_B_N, r_N); return 0.; } long Simulator::nTs() { return kk; } int Simulator::spawnAuxTasks() { return 0; } Simulator::InputData::InputData() { vehicleMass = propOmega = rudder = elevator = northCurrent = eastCurrent = 0.; } Simulator::OutputData::OutputData() { initialize(); } void Simulator::OutputData::initialize() { for (int i = 0; i < 3; i++) { position[i] = eulerAngles[i] = rotationalVelocity[i] = rotationalBackDiff[i] = translationalVelocity[i] = translationalBackDiff[i] = force[i] = torque[i] = 0.; } rudder = elevator = propOmega = 0.; M1_1 = M2_1 = M3_1 = M4_1 = M1_2 = M2_2 = M3_2 = M4_2 = 0.; } /*-----------------------------------------------------------------------* Copyright (C) 1994-1998, Massachusetts Institute of Technology. Proprietary to Sea Grant AUV Laboratory. All rights reserved. $Id: Simulator.cc,v 1.50.2.20 2016/11/17 21:06:57 rob Exp $ *-----------------------------------------------------------------------*/ /*-----------------------------------------------------------------------* | sim.c Odyssey II hydrodynamic simulator with Franz Hover's updates | | | | Last mod: 6/9/95 Franz Hover | | History: copied to SGI from Mac by June 1, 1994 by jleonard | | compilation of vehicle simulation routines written by | | Jim Bellingham and other Sea Grant staff (198?-1994) | | 90% of the code came from Draper, a student of Abkowitz. | | | | From March to June, 1995 this code was significantly | | improved by Franz Hover. Errors were removed and the | | model was updated based on theoretical considerations, | | Draper's modeling program, and analysis of mission data | | | | NOTE: this program reads hydro coefficients from two files, | | get_am.tex and get_drag.tex; The coefficients given in the source | | code here are from Draper and are not as accurate as the coefficients | | in the files | *-----------------------------------------------------------------------*/ char *s; /*----------------------------------------------------------------------*/ void Simulator::init_simulation( void ) { Boolean debug=True; _simTime = 0.; _output.initialize(); lhs(); #ifdef FASTTIME static int fastTimeState=-1; if (fastTimeState<0) { char *fast_time_en=getenv(FAST_TIME_ENV); if (fast_time_en && (strcmp(fast_time_en,FAST_TIME_EN)==0) && Simulator::_fastTime==NULL) { fastTimeState=1; // create a FastTime instance Simulator::_fastTime=new FastTime(); dprintf("\n****Simulator::init_simulation [%p] using FastTime instance [%p]\n",this,_fastTime); _fastSim=True; }else{ fastTimeState=0; _fastSim=False; } } #endif // Old Odyssey simulation starts by doing this motion(_rate, _position, 0., 0., 0., _timeIncr); _simTime += _timeIncr; // command(0., 0., 0.); } /*----------------------------------------------------------------------*/ void Simulator::lhs() /* calculates left-hand-side of the eqn */ { double a[7][7],work[7][7]; int i, j, debug=0; /* fsh checked inertia matrix 3/22/94 */ a[1][1] = _input.vehicleMass - Xudot; /* surge force */ a[1][2] = 0.; a[1][3] = 0.; a[1][4] = 0.; a[1][5] = _input.vehicleMass * _centerOfMass[Z]; a[1][6] = -_input.vehicleMass * _centerOfMass[Y]; a[2][1] = 0.; /* sway force */ a[2][2] = _input.vehicleMass - Yvdot ; a[2][3] = 0.; a[2][4] = -_input.vehicleMass * _centerOfMass[Z] - Ypdot; a[2][5] = 0.; a[2][6] = _input.vehicleMass * _centerOfMass[X] - Yrdot; a[3][1] = 0.; /* heave force */ a[3][2] = 0.; a[3][3] = _input.vehicleMass - Zwdot; a[3][4] = _input.vehicleMass * _centerOfMass[Y]; a[3][5] = -_input.vehicleMass * _centerOfMass[X] - Zqdot; a[3][6] = 0.; a[4][1] = 0.; /* roll moment */ a[4][2] = -_input.vehicleMass * _centerOfMass[Z] - Kvdot; a[4][3] = _input.vehicleMass * _centerOfMass[Y]; a[4][4] = Ixx - Kpdot; a[4][5] = -Ixy; a[4][6] = -Izx; a[5][1] = _input.vehicleMass * _centerOfMass[Z]; /* pitch moment */ a[5][2] = 0.; a[5][3] = -_input.vehicleMass * _centerOfMass[X] - Mwdot; a[5][4] = -Ixy; a[5][5] = Iyy - Mqdot; a[5][6] = -Iyz; a[6][1] = -_input.vehicleMass* _centerOfMass[Y]; /* yaw moment */ a[6][2] = _input.vehicleMass* _centerOfMass[X] - Nvdot; a[6][3] = 0.; a[6][4] = -Izx; a[6][5] = -Iyz; a[6][6] = Izz - Nrdot; if(debug) { printf("Inertia matrix: ------------ \n") ; for(i=1;i<=6;i++){ for(j=1;j<=6;j++) printf("%6.2f ", a[i][j]) ; printf("\n"); } printf("\n"); } if ( matinv(a,ainv,6,work) == 1 ) { strcpy( _eventMsg, "Singular matrix in equations of motion LHS\n"); _eventLog.write(EventLogIF::Info, _eventMsg); } } void Simulator::missionStatusCallback(TaskInterface *taskInterface, EventCode eventCode) { Boolean debug = True; dprintf("!!!! Simulator::missionStatusCallback() - Got event %d from \"%s\"\n", eventCode, taskInterface->name()); dprintf("Event received at _simTime=%.2f", _simTime); if (taskInterface == _layeredControl) { switch (eventCode) { case LayeredControlIF::MissionStarted: _missionStarted = True; dprintf("Simulator::missionStatusCallback() - mission started\n"); // Synchronize MissionClock with LayeredControl // _missionClock->reset(); _simTime = 0.; break; default: Syslog::write("Simulator::missionStatusCallback() - unknown event=%d!\n", eventCode); } } return; }