src/FDM/ExternalPipe/ExternalPipe.cxx

   1 // ExternalPipe.cxx -- a "pipe" interface to an external flight dynamics model
   2 //
   3 // Written by Curtis Olson, started March 2003.
   4 //
   5 // Copyright (C) 2003  Curtis L. Olson  - http://www.flightgear.org/~curt
   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 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #ifdef HAVE_MKFIFO
  28 #  include <sys/types.h>        // mkfifo() open() umask()
  29 #  include <sys/stat.h>         // mkfifo() open() umask()
  30 #  include <errno.h>            // perror()
  31 #  include <fcntl.h>            // open()
  32 #  include <unistd.h>           // unlink()
  33 #endif
  34
  35 #include <simgear/debug/logstream.hxx>
  36 #include <simgear/io/lowlevel.hxx> // endian tests
  37
  38 #include <Main/fg_props.hxx>
  39 #include <Network/native_ctrls.hxx>
  40 #include <Network/native_fdm.hxx>
  41
  42 #include "ExternalPipe.hxx"
  43
  44
  45 static const int MAX_BUF = 32768;
  46
  47 FGExternalPipe::FGExternalPipe( double dt, string name ) {
  48     valid = true;
  49     last_weight = 0.0;
  50     last_cg_offset = -9999.9;
  51
  52     buf = new char[MAX_BUF];
  53
  54 #ifdef HAVE_MKFIFO
  55     fifo_name_1 = name + "1";
  56     fifo_name_2 = name + "2";
  57
  58     SG_LOG( SG_IO, SG_ALERT, "ExternalPipe Inited with " << name );
  59
  60     // Make the named pipe
  61     umask(0);
  62     int result;
  63     result = mkfifo( fifo_name_1.c_str(), 0644 );
  64     if ( result == -1 ) {
  65         SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
  66                 << fifo_name_1 );
  67         perror( "ExternalPipe()" );
  68     }
  69     result = mkfifo( fifo_name_2.c_str(), 0644 );
  70     if ( result == -1 ) {
  71         SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
  72                 << fifo_name_2 );
  73         perror( "ExternalPipe()" );
  74     }
  75
  76     pd1 = open( fifo_name_1.c_str(), O_RDWR );
  77     if ( pd1 == -1 ) {
  78         SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_1 );
  79         valid = false;
  80     }
  81     pd2 = open( fifo_name_2.c_str(), O_RDWR );
  82     if ( pd2 == -1 ) {
  83         SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_2 );
  84         valid = false;
  85     }
  86 #endif
  87 }
  88
  89
  90 FGExternalPipe::~FGExternalPipe() {
  91     delete [] buf;
  92
  93     SG_LOG( SG_IO, SG_INFO, "Closing up the ExternalPipe." );
  94
  95 #ifdef HAVE_MKFIFO
  96     // close
  97     int result;
  98     result = close( pd1 );
  99     if ( result == -1 ) {
 100         SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
 101                 << fifo_name_1 );
 102         perror( "~FGExternalPipe()" );
 103     }
 104     result = close( pd2 );
 105     if ( result == -1 ) {
 106         SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
 107                 << fifo_name_2 );
 108         perror( "~FGExternalPipe()" );
 109     }
 110 #endif
 111 }
 112
 113
 114 static int write_fifo( char cmd_type, int pd, char *cmd, int len ) {
 115 #ifdef HAVE_MKFIFO
 116     char *buf = new char[len + 3];
 117
 118     // write 2 byte command length + command type + command
 119     unsigned char hi = (len + 1) / 256;
 120     unsigned char lo = (len + 1) - (hi * 256);
 121
 122     // cout << "len = " << len << " hi = " << (int)hi << " lo = " << (int)lo << endl;
 123
 124     buf[0] = hi;
 125     buf[1] = lo;
 126     buf[2] = cmd_type;
 127
 128     // strncpy( buf + 3, cmd, len );
 129     memcpy( buf + 3, cmd, len );
 130
 131     if ( cmd_type == '1' ) {
 132         // cout << "writing '" << cmd << "'" << endl;
 133     } else if ( cmd_type == '2' ) {
 134         // cout << "writing controls packet" << endl;
 135     } else {
 136         // cout << "writing unknown command?" << endl;
 137     }
 138
 139     // for ( int i = 0; i < len + 3; ++i ) {
 140     //     cout << " " << (int)buf[i];
 141     // }
 142     // cout << endl;
 143
 144     int result = ::write( pd, buf, len + 3 );
 145     if ( result == -1 ) {
 146         perror( "write_fifo()" );
 147         SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << pd );
 148     }
 149     // cout << "wrote " << len + 3 << " bytes." << endl;
 150
 151     delete [] buf;
 152
 153     return result;
 154 #else
 155     return 0;
 156 #endif
 157 }
 158
 159
 160 // Initialize the ExternalPipe flight model, dt is the time increment
 161 // for each subsequent iteration through the EOM
 162 void FGExternalPipe::init() {
 163     // Explicitly call the superclass's
 164     // init method first.
 165     common_init();
 166
 167     double lon = fgGetDouble( "/sim/presets/longitude-deg" );
 168     double lat = fgGetDouble( "/sim/presets/latitude-deg" );
 169     double alt = fgGetDouble( "/sim/presets/altitude-ft" );
 170     double ground = fgGetDouble( "/environment/ground-elevation-m" );
 171     double heading = fgGetDouble("/sim/presets/heading-deg");
 172     double speed = fgGetDouble( "/sim/presets/airspeed-kt" );
 173     double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
 174     double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
 175
 176 #ifdef HAVE_MKFIFO
 177
 178     char cmd[256];
 179     int result;
 180
 181     sprintf( cmd, "longitude-deg=%.8f", lon );
 182     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 183
 184     sprintf( cmd, "latitude-deg=%.8f", lat );
 185     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 186
 187     sprintf( cmd, "altitude-ft=%.8f", alt );
 188     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 189
 190     sprintf( cmd, "ground-m=%.8f", ground );
 191     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 192
 193     sprintf( cmd, "speed-kts=%.8f", speed );
 194     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 195
 196     sprintf( cmd, "heading-deg=%.8f", heading );
 197     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 198
 199     if ( weight > 1000.0 ) {
 200         sprintf( cmd, "aircraft-weight-lbs=%.2f", weight );
 201         result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 202     }
 203     last_weight = weight;
 204
 205     if ( cg_offset > -5.0 || cg_offset < 5.0 ) {
 206         sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset );
 207         result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 208     }
 209     last_cg_offset = cg_offset;
 210
 211     SG_LOG( SG_IO, SG_ALERT, "before sending reset command." );
 212
 213     if( fgGetBool("/sim/presets/onground") ) {
 214         sprintf( cmd, "reset=ground" );
 215     } else {
 216         sprintf( cmd, "reset=air" );
 217     }
 218     result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 219
 220     SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." );
 221 #endif
 222 }
 223
 224
 225 // Run an iteration of the EOM.
 226 void FGExternalPipe::update( double dt ) {
 227 #ifdef HAVE_MKFIFO
 228     // SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate()" );
 229
 230     int length;
 231     int result;
 232
 233     if ( is_suspended() ) {
 234         return;
 235     }
 236
 237     int iterations = _calc_multiloop(dt);
 238
 239     double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
 240     static double last_weight = 0.0;
 241     if ( fabs( weight - last_weight ) > 0.01 ) {
 242         char cmd[256];
 243         sprintf( cmd, "aircraft-weight-lbs=%.2f", weight );
 244         result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 245     }
 246     last_weight = weight;
 247
 248     double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
 249     if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) {
 250         char cmd[256];
 251         sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset );
 252         result = write_fifo( '1', pd1, cmd, strlen(cmd) );
 253     }
 254     last_cg_offset = cg_offset;
 255
 256     // Send control positions to remote fdm
 257     length = sizeof(ctrls);
 258     FGProps2NetCtrls( &ctrls, true, false );
 259     char *ptr = buf;
 260     *((int *)ptr) = iterations;
 261     // cout << "iterations = " << iterations << endl;
 262     ptr += sizeof(int);
 263     memcpy( ptr, (char *)(&ctrls), length );
 264     // cout << "writing control structure, size = "
 265     //      << length + sizeof(int) << endl;
 266
 267     result = write_fifo( '2', pd1, buf, length + sizeof(int) );
 268
 269     // Read fdm values
 270     length = sizeof(fdm);
 271     // cout << "about to read fdm data from remote fdm." << endl;
 272     result = read( pd2, (char *)(& fdm), length );
 273     if ( result == -1 ) {
 274         SG_LOG( SG_IO, SG_ALERT, "Read error from named pipe: "
 275                 << fifo_name_2 );
 276     } else {
 277         // cout << "  read successful." << endl;
 278     }
 279     FGNetFDM2Props( &fdm, false );
 280 #endif
 281 }