#include #include #include #include #include #include "Simulator.h" #include "System.h" #include "MathP.h" #include "VehicleConfigurationIF.h" #include "FloatAttribute.h" #include "AngleAttribute.h" #include "AttributeParser.h" #include "Syslog.h" #include "System.h" #include "WorkSiteIF.h" #include "NavUtils.h" // Number of simulation steps per command #define NSteps 10 /*-----------------------------------------------------------------------* | 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::_rate[7] = {0., 0., 0., 0., 0., 0., 0.}; /*----------------------- 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 = 20; /* 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") { Boolean debug = True; _log = new SimulationLog(this, DataLog::BinaryFormat); _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 retarded/idiotic "1-based" array (jeezus...) 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.)); // 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); 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]); // 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]); // ATTACH DEBUGGER // sleep(20); _layeredControl = new LayeredControlIF("layeredControl"); // Want to be notified when mission status changes subscribe(_layeredControl, LayeredControlIF::MissionStarted, (EventCallback )Simulator::missionStatusCallback); _missionStarted = False; kk = 0; //Sample period counter; called "k" in discrete-time analysis init_simulation(); } Simulator::~Simulator() { delete _configuration; delete _dropWeight; delete _layeredControl; delete _missionClock; delete _log; } 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; } // // 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 if (timeDebug) 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); if( (kk % 5) == 0 ) { Syslog::write(" simTime = %.2f\n", _simTime); Syslog::write(" realTime = %.2f\n", _missionClock->seconds()); } kk++; for (int i = 0; i < NSteps; i++) { motion(_rate, _position, omega_P, _input.rudder, _input.elevator, _timeIncr); _simTime += _timeIncr; } // // Log data at every sampling instant. Move this into motion() to log // at every integration step. // _log->write(); // Trigger NewOutput event to subscribers triggerEvent(SimulatorIF::NewOutput); if (timeDebug) { // Get ending time clock_gettime(CLOCK_REALTIME, &endTime); dprintf("*** startTime: %d sec, %d nsec\nendTime: %d sec, %d nsec\n", startTime.tv_sec, startTime.tv_nsec, endTime.tv_sec, endTime.tv_nsec); double deltaT = (endTime.tv_sec * 1000 + endTime.tv_nsec / 1000000) - (startTime.tv_sec * 1000 + startTime.tv_nsec / 1000000); dprintf("*** deltaT = %.0f millisec\n", deltaT); } return 0; } 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) { *rudder = _output.rudder; *elevator = _output.elevator; 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) { *omega = _output.propOmega; return 0; } long Simulator::motorCurrent(double *amps) { *amps = thrAmps; return 0; } double Simulator::simTime() { // return _missionClock->seconds(); return _simTime; } double Simulator::waterDepth() { return zMax; } 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.29 2001/06/02 22:12:02 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 ) { _simTime = 0.; _output.initialize(); lhs(); // 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; }