src/FDM/JSBSim/input_output/FGOutputFG.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGOutputFG.cpp
   4  Author:       Bertrand Coconnier
   5  Date started: 09/10/11
   6  Purpose:      Manage output of sim parameters to FlightGear
   7  Called by:    FGOutput
   8
   9  ------------- Copyright (C) 2011 Bertrand Coconnier -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU Lesser General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30 This is the place where you create output routines to dump data for perusal
  31 later.
  32
  33 HISTORY
  34 --------------------------------------------------------------------------------
  35 11/09/07   HDW   Added FlightGear Socket Interface
  36 09/10/11   BC    Moved the FlightGear socket in a separate class
  37
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  39 INCLUDES
  40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  41
  42 #include <cstring>
  43 #include <cstdlib>
  44
  45 #include "FGOutputFG.h"
  46 #include "FGFDMExec.h"
  47 #include "models/FGAerodynamics.h"
  48 #include "models/FGAuxiliary.h"
  49 #include "models/FGPropulsion.h"
  50 #include "models/FGMassBalance.h"
  51 #include "models/FGPropagate.h"
  52 #include "models/FGGroundReactions.h"
  53 #include "models/FGFCS.h"
  54 #include "models/propulsion/FGPiston.h"
  55 #include "models/propulsion/FGTank.h"
  56
  57 #if defined(WIN32) && !defined(__CYGWIN__)
  58 #  include <windows.h>
  59 #else
  60 #  include <netinet/in.h>       // htonl() ntohl()
  61 #endif
  62
  63 #if !defined (min)
  64 #  define min(X,Y) X<Y?X:Y
  65 #endif
  66
  67 static const int endianTest = 1;
  68 #define isLittleEndian (*((char *) &endianTest ) != 0)
  69
  70 using namespace std;
  71
  72 namespace JSBSim {
  73
  74 IDENT(IdSrc,"$Id: FGOutputFG.cpp,v 1.8 2014/01/13 10:46:00 ehofman Exp $");
  75 IDENT(IdHdr,ID_OUTPUTFG);
  76
  77 // (stolen from FGFS native_fdm.cxx)
  78 // The function htond is defined this way due to the way some
  79 // processors and OSes treat floating point values.  Some will raise
  80 // an exception whenever a "bad" floating point value is loaded into a
  81 // floating point register.  Solaris is notorious for this, but then
  82 // so is LynxOS on the PowerPC.  By translating the data in place,
  83 // there is no need to load a FP register with the "corruped" floating
  84 // point value.  By doing the BIG_ENDIAN test, I can optimize the
  85 // routine for big-endian processors so it can be as efficient as
  86 // possible
  87 static void htond (double &x)
  88 {
  89     if ( isLittleEndian ) {
  90         int    *Double_Overlay;
  91         int     Holding_Buffer;
  92
  93         Double_Overlay = (int *) &x;
  94         Holding_Buffer = Double_Overlay [0];
  95
  96         Double_Overlay [0] = htonl (Double_Overlay [1]);
  97         Double_Overlay [1] = htonl (Holding_Buffer);
  98     } else {
  99         return;
 100     }
 101 }
 102
 103 // Float version
 104 static void htonf (float &x)
 105 {
 106     if ( isLittleEndian ) {
 107         int    *Float_Overlay;
 108         int     Holding_Buffer;
 109
 110         Float_Overlay = (int *) &x;
 111         Holding_Buffer = Float_Overlay [0];
 112
 113         Float_Overlay [0] = htonl (Holding_Buffer);
 114     } else {
 115         return;
 116     }
 117 }
 118
 119
 120 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 121 CLASS IMPLEMENTATION
 122 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 123
 124 FGOutputFG::FGOutputFG(FGFDMExec* fdmex) :
 125   FGOutputSocket(fdmex)
 126 {
 127   memset(&fgSockBuf, 0x0, sizeof(fgSockBuf));
 128 }
 129
 130 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 131
 132 void FGOutputFG::SocketDataFill(FGNetFDM* net)
 133 {
 134   unsigned int i;
 135
 136   // Version
 137   net->version = FG_NET_FDM_VERSION;
 138
 139   // Positions
 140   net->longitude = Propagate->GetLocation().GetLongitude(); // geodetic (radians)
 141   net->latitude  = Propagate->GetLocation().GetLatitude(); // geodetic (radians)
 142   net->altitude  = Propagate->GetAltitudeASL()*0.3048; // altitude, above sea level (meters)
 143   net->agl       = (float)(Propagate->GetDistanceAGL()*0.3048); // altitude, above ground level (meters)
 144
 145   net->phi       = (float)(Propagate->GetEuler(ePhi)); // roll (radians)
 146   net->theta     = (float)(Propagate->GetEuler(eTht)); // pitch (radians)
 147   net->psi       = (float)(Propagate->GetEuler(ePsi)); // yaw or true heading (radians)
 148
 149   net->alpha     = (float)(Auxiliary->Getalpha()); // angle of attack (radians)
 150   net->beta      = (float)(Auxiliary->Getbeta()); // side slip angle (radians)
 151
 152   // Velocities
 153   net->phidot     = (float)(Auxiliary->GetEulerRates(ePhi)); // roll rate (radians/sec)
 154   net->thetadot   = (float)(Auxiliary->GetEulerRates(eTht)); // pitch rate (radians/sec)
 155   net->psidot     = (float)(Auxiliary->GetEulerRates(ePsi)); // yaw rate (radians/sec)
 156   net->vcas       = (float)(Auxiliary->GetVcalibratedKTS()); // VCAS, knots
 157   net->climb_rate = (float)(Propagate->Gethdot());           // altitude rate, ft/sec
 158   net->v_north    = (float)(Propagate->GetVel(eNorth));      // north vel in NED frame, fps
 159   net->v_east     = (float)(Propagate->GetVel(eEast));       // east vel in NED frame, fps
 160   net->v_down     = (float)(Propagate->GetVel(eDown));       // down vel in NED frame, fps
 161 //---ADD METHOD TO CALCULATE THESE TERMS---
 162   net->v_body_u = (float)(Propagate->GetUVW(1)); // ECEF speed in body axis
 163   net->v_body_v = (float)(Propagate->GetUVW(2)); // ECEF speed in body axis
 164   net->v_body_w = (float)(Propagate->GetUVW(3)); // ECEF speed in body axis
 165
 166   // Accelerations
 167   net->A_X_pilot   = (float)(Auxiliary->GetPilotAccel(1));    // X body accel, ft/s/s
 168   net->A_Y_pilot   = (float)(Auxiliary->GetPilotAccel(2));    // Y body accel, ft/s/s
 169   net->A_Z_pilot   = (float)(Auxiliary->GetPilotAccel(3));    // Z body accel, ft/s/s
 170
 171   // Stall
 172   net->stall_warning = 0.0;  // 0.0 - 1.0 indicating the amount of stall
 173   net->slip_deg    = (float)(Auxiliary->Getbeta(inDegrees));  // slip ball deflection, deg
 174
 175   // Engine status
 176   if (Propulsion->GetNumEngines() > FGNetFDM::FG_MAX_ENGINES && FDMExec->GetSimTime() == 0.0)
 177     cerr << "This vehicle has " << Propulsion->GetNumEngines() << " engines, but the current " << endl
 178          << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_ENGINES << " engines." << endl
 179          << "Only the first " << FGNetFDM::FG_MAX_ENGINES << " engines will be used." << endl;
 180
 181   net->num_engines = min(FGNetFDM::FG_MAX_ENGINES,Propulsion->GetNumEngines()); // Number of valid engines
 182
 183   for (i=0; i<net->num_engines; i++) {
 184     if (Propulsion->GetEngine(i)->GetRunning())
 185       net->eng_state[i] = 2;       // Engine state running
 186     else if (Propulsion->GetEngine(i)->GetCranking())
 187       net->eng_state[i] = 1;       // Engine state cranking
 188     else
 189       net->eng_state[i] = 0;       // Engine state off
 190
 191     switch (Propulsion->GetEngine(i)->GetType()) {
 192     case (FGEngine::etRocket):
 193       break;
 194     case (FGEngine::etPiston):
 195       net->rpm[i]       = (float)(((FGPiston *)Propulsion->GetEngine(i))->getRPM());
 196       net->fuel_flow[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getFuelFlow_gph());
 197       net->fuel_px[i]   = 0; // Fuel pressure, psi  (N/A in current model)
 198       net->egt[i]       = (float)(((FGPiston *)Propulsion->GetEngine(i))->GetEGT());
 199       net->cht[i]       = (float)(((FGPiston *)Propulsion->GetEngine(i))->getCylinderHeadTemp_degF());
 200       net->mp_osi[i]    = (float)(((FGPiston *)Propulsion->GetEngine(i))->getManifoldPressure_inHg());
 201       net->oil_temp[i]  = (float)(((FGPiston *)Propulsion->GetEngine(i))->getOilTemp_degF());
 202       net->oil_px[i]    = (float)(((FGPiston *)Propulsion->GetEngine(i))->getOilPressure_psi());
 203       net->tit[i]       = 0; // Turbine Inlet Temperature  (N/A for piston)
 204       break;
 205     case (FGEngine::etTurbine):
 206       break;
 207     case (FGEngine::etTurboprop):
 208       break;
 209     case (FGEngine::etElectric):
 210       break;
 211     case (FGEngine::etUnknown):
 212       break;
 213     }
 214   }
 215
 216   // Consumables
 217   if (Propulsion->GetNumTanks() > FGNetFDM::FG_MAX_TANKS && FDMExec->GetSimTime() == 0.0)
 218     cerr << "This vehicle has " << Propulsion->GetNumTanks() << " tanks, but the current " << endl
 219          << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_TANKS << " tanks." << endl
 220          << "Only the first " << FGNetFDM::FG_MAX_TANKS << " tanks will be used." << endl;
 221
 222   net->num_tanks = min(FGNetFDM::FG_MAX_TANKS, Propulsion->GetNumTanks());   // Max number of fuel tanks
 223
 224   for (i=0; i<net->num_tanks; i++) {
 225     net->fuel_quantity[i] = (float)(((FGTank *)Propulsion->GetTank(i))->GetContents());
 226   }
 227
 228   // Gear status
 229   if (GroundReactions->GetNumGearUnits() > FGNetFDM::FG_MAX_WHEELS && FDMExec->GetSimTime() == 0.0)
 230     cerr << "This vehicle has " << GroundReactions->GetNumGearUnits() << " bogeys, but the current " << endl
 231          << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_WHEELS << " bogeys." << endl
 232          << "Only the first " << FGNetFDM::FG_MAX_WHEELS << " bogeys will be used." << endl;
 233
 234   net->num_wheels  = min(FGNetFDM::FG_MAX_WHEELS, GroundReactions->GetNumGearUnits());
 235
 236   for (i=0; i<net->num_wheels; i++) {
 237     net->wow[i]              = GroundReactions->GetGearUnit(i)->GetWOW();
 238     if (GroundReactions->GetGearUnit(i)->GetGearUnitDown())
 239       net->gear_pos[i]      = 1;  //gear down, using FCS convention
 240     else
 241       net->gear_pos[i]      = 0;  //gear up, using FCS convention
 242     net->gear_steer[i]       = (float)(GroundReactions->GetGearUnit(i)->GetSteerNorm());
 243     net->gear_compression[i] = (float)(GroundReactions->GetGearUnit(i)->GetCompLen());
 244   }
 245
 246   // Environment
 247   net->cur_time    = (long int)1234567890;    // Friday, Feb 13, 2009, 23:31:30 UTC (not processed by FGFS anyway)
 248   net->warp        = 0;                       // offset in seconds to unix time
 249   net->visibility  = 25000.0;                 // visibility in meters (for env. effects)
 250
 251   // Control surface positions (normalized values)
 252   net->elevator          = (float)(FCS->GetDePos(ofNorm));    // Norm Elevator Pos, --
 253   net->elevator_trim_tab = (float)(FCS->GetPitchTrimCmd());   // Norm Elev Trim Tab Pos, --
 254   net->left_flap         = (float)(FCS->GetDfPos(ofNorm));    // Norm Flap Pos, --
 255   net->right_flap        = (float)(FCS->GetDfPos(ofNorm));    // Norm Flap Pos, --
 256   net->left_aileron      = (float)(FCS->GetDaLPos(ofNorm));   // Norm L Aileron Pos, --
 257   net->right_aileron     = (float)(FCS->GetDaRPos(ofNorm));   // Norm R Aileron Pos, --
 258   net->rudder            = (float)(FCS->GetDrPos(ofNorm));    // Norm Rudder Pos, --
 259   net->nose_wheel        = (float)(FCS->GetDrPos(ofNorm));    // *** FIX ***  Using Rudder Pos for NWS, --
 260   net->speedbrake        = (float)(FCS->GetDsbPos(ofNorm));   // Norm Speedbrake Pos, --
 261   net->spoilers          = (float)(FCS->GetDspPos(ofNorm));   // Norm Spoiler Pos, --
 262
 263   // Convert the net buffer to network format
 264   if ( isLittleEndian ) {
 265     net->version = htonl(net->version);
 266
 267     htond(net->longitude);
 268     htond(net->latitude);
 269     htond(net->altitude);
 270     htonf(net->agl);
 271     htonf(net->phi);
 272     htonf(net->theta);
 273     htonf(net->psi);
 274     htonf(net->alpha);
 275     htonf(net->beta);
 276
 277     htonf(net->phidot);
 278     htonf(net->thetadot);
 279     htonf(net->psidot);
 280     htonf(net->vcas);
 281     htonf(net->climb_rate);
 282     htonf(net->v_north);
 283     htonf(net->v_east);
 284     htonf(net->v_down);
 285     htonf(net->v_body_u);
 286     htonf(net->v_body_v);
 287     htonf(net->v_body_w);
 288
 289     htonf(net->A_X_pilot);
 290     htonf(net->A_Y_pilot);
 291     htonf(net->A_Z_pilot);
 292
 293     htonf(net->stall_warning);
 294     htonf(net->slip_deg);
 295
 296     for (i=0; i<net->num_engines; ++i ) {
 297       net->eng_state[i] = htonl(net->eng_state[i]);
 298       htonf(net->rpm[i]);
 299       htonf(net->fuel_flow[i]);
 300       htonf(net->fuel_px[i]);
 301       htonf(net->egt[i]);
 302       htonf(net->cht[i]);
 303       htonf(net->mp_osi[i]);
 304       htonf(net->tit[i]);
 305       htonf(net->oil_temp[i]);
 306       htonf(net->oil_px[i]);
 307     }
 308     net->num_engines = htonl(net->num_engines);
 309
 310     for (i=0; i<net->num_tanks; ++i ) {
 311       htonf(net->fuel_quantity[i]);
 312     }
 313     net->num_tanks = htonl(net->num_tanks);
 314
 315     for (i=0; i<net->num_wheels; ++i ) {
 316       net->wow[i] = htonl(net->wow[i]);
 317       htonf(net->gear_pos[i]);
 318       htonf(net->gear_steer[i]);
 319       htonf(net->gear_compression[i]);
 320     }
 321     net->num_wheels = htonl(net->num_wheels);
 322
 323     net->cur_time = htonl( net->cur_time );
 324     net->warp = htonl( net->warp );
 325     htonf(net->visibility);
 326
 327     htonf(net->elevator);
 328     htonf(net->elevator_trim_tab);
 329     htonf(net->left_flap);
 330     htonf(net->right_flap);
 331     htonf(net->left_aileron);
 332     htonf(net->right_aileron);
 333     htonf(net->rudder);
 334     htonf(net->nose_wheel);
 335     htonf(net->speedbrake);
 336     htonf(net->spoilers);
 337   }
 338 }
 339
 340 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 341
 342 void FGOutputFG::Print(void)
 343 {
 344   int length = sizeof(fgSockBuf);
 345
 346   if (socket == 0) return;
 347   if (!socket->GetConnectStatus()) return;
 348
 349   SocketDataFill(&fgSockBuf);
 350   socket->Send((char *)&fgSockBuf, length);
 351 }
 352 }