src/Network/mini_fdm.cxx

   1 // native_fdm.cxx -- FGFS "Native" flight dynamics protocal class
   2 //
   3 // Written by Curtis Olson, started September 2001.
   4 //
   5 // Copyright (C) 2001  Curtis L. Olson - curt@flightgear.org
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <simgear/debug/logstream.hxx>
  29 #include <simgear/io/lowlevel.hxx> // endian tests
  30 #include <simgear/io/iochannel.hxx>
  31 #include <simgear/timing/sg_time.hxx>
  32
  33 #include <FDM/flight.hxx>
  34 #include <Time/tmp.hxx>
  35 #include <Main/fg_props.hxx>
  36 #include <Main/globals.hxx>
  37
  38 #include "mini_fdm.hxx"
  39
  40 // FreeBSD works better with this included last ... (?)
  41 #if defined(WIN32) && !defined(__CYGWIN__)
  42 #  include <windows.h>
  43 #else
  44 #  include <netinet/in.h>       // htonl() ntohl()
  45 #endif
  46
  47
  48 // The function htond is defined this way due to the way some
  49 // processors and OSes treat floating point values.  Some will raise
  50 // an exception whenever a "bad" floating point value is loaded into a
  51 // floating point register.  Solaris is notorious for this, but then
  52 // so is LynxOS on the PowerPC.  By translating the data in place,
  53 // there is no need to load a FP register with the "corruped" floating
  54 // point value.  By doing the BIG_ENDIAN test, I can optimize the
  55 // routine for big-endian processors so it can be as efficient as
  56 // possible
  57 static void htond (double &x)
  58 {
  59     if ( sgIsLittleEndian() ) {
  60         int    *Double_Overlay;
  61         int     Holding_Buffer;
  62
  63         Double_Overlay = (int *) &x;
  64         Holding_Buffer = Double_Overlay [0];
  65
  66         Double_Overlay [0] = htonl (Double_Overlay [1]);
  67         Double_Overlay [1] = htonl (Holding_Buffer);
  68     } else {
  69         return;
  70     }
  71 }
  72
  73
  74 FGMiniFDM::FGMiniFDM() {
  75 }
  76
  77 FGMiniFDM::~FGMiniFDM() {
  78 }
  79
  80
  81 // open hailing frequencies
  82 bool FGMiniFDM::open() {
  83     if ( is_enabled() ) {
  84         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
  85                 << "is already in use, ignoring" );
  86         return false;
  87     }
  88
  89     SGIOChannel *io = get_io_channel();
  90
  91     if ( ! io->open( get_direction() ) ) {
  92         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
  93         return false;
  94     }
  95
  96     set_enabled( true );
  97
  98     cur_fdm_state->_set_Sea_level_radius( SG_EQUATORIAL_RADIUS_FT );
  99     return true;
 100 }
 101
 102
 103 void FGProps2NetMiniFDM( FGNetMiniFDM *net ) {
 104     int i;
 105
 106     // Version sanity checking
 107     net->version = FG_NET_FDM_MINI_VERSION;
 108
 109     // Aero parameters
 110     net->longitude = cur_fdm_state->get_Longitude();
 111     net->latitude = cur_fdm_state->get_Latitude();
 112     net->altitude = cur_fdm_state->get_Altitude() * SG_FEET_TO_METER;
 113     net->phi = cur_fdm_state->get_Phi();
 114     net->theta = cur_fdm_state->get_Theta();
 115     net->psi = cur_fdm_state->get_Psi();
 116
 117     // Consumables
 118     net->num_tanks = FGNetMiniFDM::FG_MAX_TANKS;
 119     for ( i = 0; i < net->num_tanks; ++i ) {
 120         SGPropertyNode *node = fgGetNode("/consumables/fuel/tank", i, true);
 121         net->fuel_quantity[i] = node->getDoubleValue("level-gal_us");
 122     }
 123
 124     // the following really aren't used in this context
 125     net->cur_time = globals->get_time_params()->get_cur_time();
 126     net->warp = globals->get_warp();
 127
 128     // Convert the net buffer to network format
 129     net->version = htonl(net->version);
 130
 131     htond(net->longitude);
 132     htond(net->latitude);
 133     htond(net->altitude);
 134     htond(net->phi);
 135     htond(net->theta);
 136     htond(net->psi);
 137
 138     for ( i = 0; i < net->num_tanks; ++i ) {
 139         htond(net->fuel_quantity[i]);
 140     }
 141     net->num_tanks = htonl(net->num_tanks);
 142
 143     net->cur_time = htonl( net->cur_time );
 144     net->warp = htonl( net->warp );
 145 }
 146
 147
 148 void FGNetMiniFDM2Props( FGNetMiniFDM *net ) {
 149     int i;
 150
 151     // Convert to the net buffer from network format
 152     net->version = ntohl(net->version);
 153
 154     htond(net->longitude);
 155     htond(net->latitude);
 156     htond(net->altitude);
 157     htond(net->phi);
 158     htond(net->theta);
 159     htond(net->psi);
 160
 161     net->num_tanks = htonl(net->num_tanks);
 162     for ( i = 0; i < net->num_tanks; ++i ) {
 163         htond(net->fuel_quantity[i]);
 164     }
 165
 166     net->cur_time = ntohl(net->cur_time);
 167     net->warp = ntohl(net->warp);
 168
 169     if ( net->version == FG_NET_FDM_MINI_VERSION ) {
 170         // cout << "pos = " << net->longitude << " " << net->latitude << endl;
 171         // cout << "sea level rad = " << cur_fdm_state->get_Sea_level_radius()
 172         //      << endl;
 173         cur_fdm_state->_updateGeodeticPosition( net->latitude,
 174                                                 net->longitude,
 175                                                 net->altitude
 176                                                   * SG_METER_TO_FEET );
 177         cur_fdm_state->_set_Euler_Angles( net->phi,
 178                                           net->theta,
 179                                           net->psi );
 180
 181         for (i = 0; i < net->num_tanks; ++i ) {
 182             SGPropertyNode * node
 183                 = fgGetNode("/consumables/fuel/tank", i, true);
 184             node->setDoubleValue("level-gal_us", net->fuel_quantity[i] );
 185         }
 186
 187         if ( net->cur_time ) {
 188             fgSetLong("/sim/time/cur-time-override", net->cur_time);
 189         }
 190
 191         globals->set_warp( net->warp );
 192     } else {
 193         SG_LOG( SG_IO, SG_ALERT,
 194                 "Error: version mismatch in FGNetMiniFDM2Props()" );
 195         SG_LOG( SG_IO, SG_ALERT,
 196                 "\tread " << net->version << " need " << FG_NET_FDM_MINI_VERSION );
 197         SG_LOG( SG_IO, SG_ALERT,
 198                 "\tNeed to upgrade net_fdm.hxx and recompile." );
 199     }
 200 }
 201
 202
 203 // process work for this port
 204 bool FGMiniFDM::process() {
 205     SGIOChannel *io = get_io_channel();
 206     int length = sizeof(buf);
 207
 208     if ( get_direction() == SG_IO_OUT ) {
 209         // cout << "size of cur_fdm_state = " << length << endl;
 210         FGProps2NetMiniFDM( &buf );
 211         if ( ! io->write( (char *)(& buf), length ) ) {
 212             SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
 213             return false;
 214         }
 215     } else if ( get_direction() == SG_IO_IN ) {
 216         if ( io->get_type() == sgFileType ) {
 217             if ( io->read( (char *)(& buf), length ) == length ) {
 218                 SG_LOG( SG_IO, SG_DEBUG, "Success reading data." );
 219                 FGNetMiniFDM2Props( &buf );
 220             }
 221         } else {
 222             while ( io->read( (char *)(& buf), length ) == length ) {
 223                 SG_LOG( SG_IO, SG_DEBUG, "Success reading data." );
 224                 FGNetMiniFDM2Props( &buf );
 225             }
 226         }
 227     }
 228
 229     return true;
 230 }
 231
 232
 233 // close the channel
 234 bool FGMiniFDM::close() {
 235     SGIOChannel *io = get_io_channel();
 236
 237     set_enabled( false );
 238
 239     if ( ! io->close() ) {
 240         return false;
 241     }
 242
 243     return true;
 244 }