fgoutput.cpp

来自「6 DOF Missle Simulation」· C++ 代码 · 共 1,057 行 · 第 1/3 页

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Module:       FGOutput.cpp
 Author:       Jon Berndt
 Date started: 12/02/98
 Purpose:      Manage output of sim parameters to file or stdout
 Called by:    FGSimExec

 ------------- Copyright (C) 1999  Jon S. Berndt ([e-mail]) -------------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU Lesser General Public License as published by the Free Software
 Foundation; either version 2 of the License, or (at your option) any later
 version.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
 details.

 You should have received a copy of the GNU Lesser General Public License along with
 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 Place - Suite 330, Boston, MA  02111-1307, USA.

 Further information about the GNU Lesser General Public License can also be found on
 the world wide web at http://www.gnu.org.

FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This is the place where you create output routines to dump data for perusal
later.

HISTORY
--------------------------------------------------------------------------------
12/02/98   JSB   Created
11/09/07   HDW   Added FlightGear Socket Interface

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include "FGOutput.h"
#include "FGState.h"
#include "FGFDMExec.h"
#include "FGAtmosphere.h"
#include "FGFCS.h"
#include "FGAerodynamics.h"
#include "FGGroundReactions.h"
#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGPropagate.h"
#include "FGAuxiliary.h"
#include "FGInertial.h"
#include "FGPropulsion.h"   //access to FGEngine, FGTank
#include "models/propulsion/FGPiston.h"
#include <fstream>
#include <iomanip>
#include <cstring>

#include "input_output/net_fdm.hxx"

#if defined(WIN32) && !defined(__CYGWIN__)
#  include <windows.h>
#else
#  include <netinet/in.h>       // htonl() ntohl()
#endif

static const int endianTest = 1;
#define isLittleEndian (*((char *) &endianTest ) != 0)

namespace JSBSim {

static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_OUTPUT;

// (stolen from FGFS native_fdm.cxx)
// The function htond is defined this way due to the way some
// processors and OSes treat floating point values.  Some will raise
// an exception whenever a "bad" floating point value is loaded into a
// floating point register.  Solaris is notorious for this, but then
// so is LynxOS on the PowerPC.  By translating the data in place,
// there is no need to load a FP register with the "corruped" floating
// point value.  By doing the BIG_ENDIAN test, I can optimize the
// routine for big-endian processors so it can be as efficient as
// possible
static void htond (double &x)
{
    if ( isLittleEndian ) {
        int    *Double_Overlay;
        int     Holding_Buffer;

        Double_Overlay = (int *) &x;
        Holding_Buffer = Double_Overlay [0];

        Double_Overlay [0] = htonl (Double_Overlay [1]);
        Double_Overlay [1] = htonl (Holding_Buffer);
    } else {
        return;
    }
}

// Float version
static void htonf (float &x)
{
    if ( isLittleEndian ) {
        int    *Float_Overlay;
        int     Holding_Buffer;

        Float_Overlay = (int *) &x;
        Holding_Buffer = Float_Overlay [0];

        Float_Overlay [0] = htonl (Holding_Buffer);
    } else {
        return;
    }
}


/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex)
{
  Name = "FGOutput";
  sFirstPass = dFirstPass = true;
  socket = 0;
  flightGearSocket = 0;
  Type = otNone;
  SubSystems = 0;
  enabled = true;
  delimeter = ", ";
  Filename = "";
  DirectivesFile = "";
  output_file_name = "";

  memset(&fgSockBuf, 0x00, sizeof(fgSockBuf));

  Debug(0);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FGOutput::~FGOutput()
{
  delete socket;
  delete flightGearSocket;
  OutputProperties.clear();
  Debug(1);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGOutput::InitModel(void)
{
  if (!FGModel::InitModel()) return false;

  return true;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGOutput::Run(void)
{
  if (FGModel::Run()) return true;

  if (enabled && !State->IntegrationSuspended()&& !FDMExec->Holding()) {
    if (Type == otSocket) {
      SocketOutput();
    } else if (Type == otFlightGear) {
      FlightGearSocketOutput();
    } else if (Type == otCSV || Type == otTab) {
      DelimitedOutput(Filename);
    } else if (Type == otTerminal) {
      // Not done yet
    } else if (Type == otNone) {
      // Do nothing
    } else {
      // Not a valid type of output
    }
  }
  return false;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGOutput::SetType(string type)
{
  if (type == "CSV") {
    Type = otCSV;
    delimeter = ", ";
  } else if (type == "TABULAR") {
    Type = otTab;
    delimeter = "\t";
  } else if (type == "SOCKET") {
    Type = otSocket;
  } else if (type == "FLIGHTGEAR") {
    Type = otFlightGear;
  } else if (type == "TERMINAL") {
    Type = otTerminal;
  } else if (type != string("NONE")) {
    Type = otUnknown;
    cerr << "Unknown type of output specified in config file" << endl;
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGOutput::DelimitedOutput(string fname)
{
  streambuf* buffer;
  string scratch = "";

  if (fname == "COUT" || fname == "cout") {
    buffer = cout.rdbuf();
  } else {
    datafile.open(fname.c_str());
    buffer = datafile.rdbuf();
  }

  ostream outstream(buffer);

  outstream.precision(10);

  if (dFirstPass) {
    outstream << "Time";
    if (SubSystems & ssSimulation) {
      // Nothing here, yet
    }
    if (SubSystems & ssAerosurfaces) {
      outstream << delimeter;
      outstream << "Aileron Command (norm)" + delimeter;
      outstream << "Elevator Command (norm)" + delimeter;
      outstream << "Rudder Command (norm)" + delimeter;
      outstream << "Flap Command (norm)" + delimeter;
      outstream << "Left Aileron Position (deg)" + delimeter;
      outstream << "Right Aileron Position (deg)" + delimeter;
      outstream << "Elevator Position (deg)" + delimeter;
      outstream << "Rudder Position (deg)" + delimeter;
      outstream << "Flap Position (deg)";
    }
    if (SubSystems & ssRates) {
      outstream << delimeter;
      outstream << "P (deg/s)" + delimeter + "Q (deg/s)" + delimeter + "R (deg/s)" + delimeter;
      outstream << "P dot (deg/s^2)" + delimeter + "Q dot (deg/s^2)" + delimeter + "R dot (deg/s^2)";
    }
    if (SubSystems & ssVelocities) {
      outstream << delimeter;
      outstream << "q bar (psf)" + delimeter;
      outstream << "V_{Total} (ft/s)" + delimeter;
      outstream << "V_{Inertial} (ft/s)" + delimeter;
      outstream << "UBody" + delimeter + "VBody" + delimeter + "WBody" + delimeter;
      outstream << "Aero V_{X Body} (ft/s)" + delimeter + "Aero V_{Y Body} (ft/s)" + delimeter + "Aero V_{Z Body} (ft/s)" + delimeter;
      outstream << "V_{North} (ft/s)" + delimeter + "V_{East} (ft/s)" + delimeter + "V_{Down} (ft/s)";
    }
    if (SubSystems & ssForces) {
      outstream << delimeter;
      outstream << "F_{Drag} (lbs)" + delimeter + "F_{Side} (lbs)" + delimeter + "F_{Lift} (lbs)" + delimeter;
      outstream << "L/D" + delimeter;
      outstream << "F_X (lbs)" + delimeter + "F_Y (lbs)" + delimeter + "F_Z (lbs)";
    }
    if (SubSystems & ssMoments) {
      outstream << delimeter;
      outstream << "L (ft-lbs)" + delimeter + "M (ft-lbs)" + delimeter + "N (ft-lbs)";
    }
    if (SubSystems & ssAtmosphere) {
      outstream << delimeter;
      outstream << "Rho (slugs/ft^3)" + delimeter;
      outstream << "P_{SL} (psf)" + delimeter;
      outstream << "P_{Ambient} (psf)" + delimeter;
      outstream << "Turbulence Magnitude (ft/sec)" + delimeter;
      outstream << "Turbulence X Direction (rad)" + delimeter + "Turbulence Y Direction (rad)" + delimeter + "Turbulence Z Direction (rad)" + delimeter;
      outstream << "Wind V_{North} (ft/s)" + delimeter + "Wind V_{East} (ft/s)" + delimeter + "Wind V_{Down} (ft/s)";
    }
    if (SubSystems & ssMassProps) {
      outstream << delimeter;
      outstream << "I_xx" + delimeter;
      outstream << "I_xy" + delimeter;
      outstream << "I_xz" + delimeter;
      outstream << "I_yx" + delimeter;
      outstream << "I_yy" + delimeter;
      outstream << "I_yz" + delimeter;
      outstream << "I_zx" + delimeter;
      outstream << "I_zy" + delimeter;
      outstream << "I_zz" + delimeter;
      outstream << "Mass" + delimeter;
      outstream << "X_cg" + delimeter + "Y_cg" + delimeter + "Z_cg";
    }
    if (SubSystems & ssPropagate) {
      outstream << delimeter;
      outstream << "Altitude (ft)" + delimeter;
      outstream << "Phi (deg)" + delimeter + "Theta (deg)" + delimeter + "Psi (deg)" + delimeter;
      outstream << "Alpha (deg)" + delimeter;
      outstream << "Beta (deg)" + delimeter;
      outstream << "Latitude (deg)" + delimeter;
      outstream << "Longitude (deg)" + delimeter;
      outstream << "ECEF X (ft)" + delimeter + "ECEF Y (ft)" + delimeter + "ECEF Z (ft)" + delimeter;
      outstream << "EPA (deg)" + delimeter;
      outstream << "Distance AGL (ft)" + delimeter;
      outstream << "Runway Radius (ft)";
    }
    if (SubSystems & ssCoefficients) {
      scratch = Aerodynamics->GetCoefficientStrings(delimeter);
      if (scratch.length() != 0) outstream << delimeter << scratch;
    }
    if (SubSystems & ssFCS) {
      scratch = FCS->GetComponentStrings(delimeter);
      if (scratch.length() != 0) outstream << delimeter << scratch;
    }
    if (SubSystems & ssGroundReactions) {
      outstream << delimeter;
      outstream << GroundReactions->GetGroundReactionStrings(delimeter);
    }
    if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) {
      outstream << delimeter;
      outstream << Propulsion->GetPropulsionStrings(delimeter);
    }
    if (OutputProperties.size() > 0) {
      for (unsigned int i=0;i<OutputProperties.size();i++) {
        outstream << delimeter << OutputProperties[i]->GetPrintableName();
      }
    }

    outstream << endl;
    dFirstPass = false;
  }

  outstream << State->Getsim_time();
  if (SubSystems & ssSimulation) {
  }
  if (SubSystems & ssAerosurfaces) {
    outstream << delimeter;
    outstream << FCS->GetDaCmd() << delimeter;
    outstream << FCS->GetDeCmd() << delimeter;
    outstream << FCS->GetDrCmd() << delimeter;
    outstream << FCS->GetDfCmd() << delimeter;
    outstream << FCS->GetDaLPos(ofDeg) << delimeter;
    outstream << FCS->GetDaRPos(ofDeg) << delimeter;
    outstream << FCS->GetDePos(ofDeg) << delimeter;
    outstream << FCS->GetDrPos(ofDeg) << delimeter;
    outstream << FCS->GetDfPos(ofDeg);
  }
  if (SubSystems & ssRates) {
    outstream << delimeter;
    outstream << (radtodeg*Propagate->GetPQR()).Dump(delimeter) << delimeter;
    outstream << (radtodeg*Propagate->GetPQRdot()).Dump(delimeter);
  }
  if (SubSystems & ssVelocities) {
    outstream << delimeter;
    outstream << Auxiliary->Getqbar() << delimeter;
    outstream << setprecision(12) << Auxiliary->GetVt() << delimeter;
    outstream << Propagate->GetInertialVelocityMagnitude() << delimeter;
    outstream << setprecision(12) << Propagate->GetUVW().Dump(delimeter) << delimeter;