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() umask()
  29 #  include <sys/stat.h>         // mkfifo() umask()
  30 #  include <errno.h>            // perror()
  31 #  include <unistd.h>           // unlink()
  32 #endif
  33
  34 #include <stdio.h>              // FILE*, fopen(), fread(), fwrite(), et. al.
  35
  36 #include <simgear/debug/logstream.hxx>
  37 #include <simgear/io/lowlevel.hxx> // endian tests
  38 #include <simgear/misc/strutils.hxx> // split()
  39
  40 #include <Main/fg_props.hxx>
  41 #include <Network/native_ctrls.hxx>
  42 #include <Network/native_fdm.hxx>
  43 #include <Scenery/scenery.hxx>
  44
  45 #include "ExternalPipe.hxx"
  46
  47
  48 static const int MAX_BUF = 32768;
  49
  50 FGExternalPipe::FGExternalPipe( double dt, string name, string protocol ) {
  51     valid = true;
  52     last_weight = 0.0;
  53     last_cg_offset = -9999.9;
  54
  55     buf = new char[MAX_BUF];
  56
  57     // clear property request list
  58     property_names.clear();
  59     nodes.clear();
  60
  61 #ifdef HAVE_MKFIFO
  62     fifo_name_1 = name + "1";
  63     fifo_name_2 = name + "2";
  64
  65     SG_LOG( SG_IO, SG_ALERT, "ExternalPipe Inited with " << name );
  66
  67     // Make the named pipe
  68     umask(0);
  69     int result;
  70     result = mkfifo( fifo_name_1.c_str(), 0644 );
  71     if ( result == -1 ) {
  72         SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
  73                 << fifo_name_1 );
  74         perror( "ExternalPipe()" );
  75     }
  76     result = mkfifo( fifo_name_2.c_str(), 0644 );
  77     if ( result == -1 ) {
  78         SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
  79                 << fifo_name_2 );
  80         perror( "ExternalPipe()" );
  81     }
  82
  83     pd1 = fopen( fifo_name_1.c_str(), "w" );
  84     if ( pd1 == NULL ) {
  85         SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_1 );
  86         valid = false;
  87     }
  88     pd2 = fopen( fifo_name_2.c_str(), "r" );
  89     if ( pd2 == NULL ) {
  90         SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_2 );
  91         valid = false;
  92     }
  93 #endif
  94
  95     _protocol = protocol;
  96
  97     if ( _protocol != "binary" && _protocol != "property" ) {
  98         SG_LOG( SG_IO, SG_ALERT, "Constructor(): Unknown ExternalPipe protocol."
  99                 << "  Must be 'binary' or 'property'."
 100                 << "  (assuming binary)" );
 101         _protocol = "binary";
 102     }
 103 }
 104
 105
 106 FGExternalPipe::~FGExternalPipe() {
 107     delete [] buf;
 108
 109     SG_LOG( SG_IO, SG_INFO, "Closing up the ExternalPipe." );
 110
 111 #ifdef HAVE_MKFIFO
 112     // close
 113     int result;
 114     result = fclose( pd1 );
 115     if ( result ) {
 116         SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
 117                 << fifo_name_1 );
 118         perror( "~FGExternalPipe()" );
 119     }
 120     result = fclose( pd2 );
 121     if ( result ) {
 122         SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
 123                 << fifo_name_2 );
 124         perror( "~FGExternalPipe()" );
 125     }
 126 #endif
 127 }
 128
 129
 130 static int write_binary( char cmd_type, FILE *pd, char *cmd, int len ) {
 131 #ifdef HAVE_MKFIFO
 132     char *buf = new char[len + 3];
 133
 134     // write 2 byte command length + command type + command
 135     unsigned char hi = (len + 1) / 256;
 136     unsigned char lo = (len + 1) - (hi * 256);
 137
 138     // cout << "len = " << len << " hi = " << (int)hi << " lo = "
 139     //      << (int)lo << endl;
 140
 141     buf[0] = hi;
 142     buf[1] = lo;
 143     buf[2] = cmd_type;
 144
 145     memcpy( buf + 3, cmd, len );
 146
 147     if ( cmd_type == '1' ) {
 148         cout << "writing ";
 149         cout << (int)hi << " ";
 150         cout << (int)lo << " '";
 151         for ( int i = 2; i < len + 3; ++i ) {
 152             cout << buf[i];
 153         }
 154         cout << "' (" << cmd << ")" << endl;
 155     } else if ( cmd_type == '2' ) {
 156         // cout << "writing controls packet" << endl;
 157     } else {
 158         cout << "writing unknown command?" << endl;
 159     }
 160
 161     // for ( int i = 0; i < len + 3; ++i ) {
 162     //     cout << " " << (int)buf[i];
 163     // }
 164     // cout << endl;
 165
 166     int result = fwrite( buf, len + 3, 1, pd  );
 167     if ( result != 1 ) {
 168         perror( "write_binary()" );
 169         SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << pd );
 170     }
 171     // cout << "wrote " << len + 3 << " bytes." << endl;
 172
 173     delete [] buf;
 174
 175     return result;
 176 #else
 177     return 0;
 178 #endif
 179 }
 180
 181
 182 static int write_property( FILE *pd, char *cmd ) {
 183     int len = strlen(cmd);
 184
 185 #ifdef HAVE_MKFIFO
 186     char *buf = new char[len + 1];
 187
 188     memcpy( buf, cmd, len );
 189     buf[len] = '\n';
 190
 191     int result = fwrite( buf, len + 1, 1, pd );
 192     if ( result == len + 1 ) {
 193         perror( "write_property()" );
 194         SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << pd );
 195     }
 196     // cout << "wrote " << len + 1 << " bytes." << endl;
 197
 198     delete [] buf;
 199
 200     return result;
 201 #else
 202     return 0;
 203 #endif
 204 }
 205
 206
 207 // Wrapper for the ExternalPipe flight model initialization.  dt is
 208 // the time increment for each subsequent iteration through the EOM
 209 void FGExternalPipe::init() {
 210     // Explicitly call the superclass's
 211     // init method first.
 212     common_init();
 213
 214     if ( _protocol == "binary" ) {
 215         init_binary();
 216     } else if ( _protocol == "property" ) {
 217         init_property();
 218     } else {
 219         SG_LOG( SG_IO, SG_ALERT, "Init():  Unknown ExternalPipe protocol."
 220                 << "  Must be 'binary' or 'property'."
 221                 << "  (assuming binary)" );
 222     }
 223 }
 224
 225
 226 // Initialize the ExternalPipe flight model using the binary protocol,
 227 // dt is the time increment for each subsequent iteration through the
 228 // EOM
 229 void FGExternalPipe::init_binary() {
 230     cout << "init_binary()" << endl;
 231
 232     double lon = fgGetDouble( "/sim/presets/longitude-deg" );
 233     double lat = fgGetDouble( "/sim/presets/latitude-deg" );
 234     double alt = fgGetDouble( "/sim/presets/altitude-ft" );
 235     double ground = get_Runway_altitude_m();
 236     double heading = fgGetDouble("/sim/presets/heading-deg");
 237     double speed = fgGetDouble( "/sim/presets/airspeed-kt" );
 238     double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
 239     double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
 240
 241     char cmd[256];
 242     int result;
 243
 244     sprintf( cmd, "longitude-deg=%.8f", lon );
 245     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 246
 247     sprintf( cmd, "latitude-deg=%.8f", lat );
 248     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 249
 250     sprintf( cmd, "altitude-ft=%.8f", alt );
 251     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 252
 253     sprintf( cmd, "ground-m=%.8f", ground );
 254     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 255
 256     sprintf( cmd, "speed-kts=%.8f", speed );
 257     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 258
 259     sprintf( cmd, "heading-deg=%.8f", heading );
 260     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 261
 262     if ( weight > 1000.0 ) {
 263         sprintf( cmd, "aircraft-weight-lbs=%.2f", weight );
 264         result = write_binary( '1', pd1, cmd, strlen(cmd) );
 265     }
 266     last_weight = weight;
 267
 268     if ( cg_offset > -5.0 || cg_offset < 5.0 ) {
 269         sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset );
 270         result = write_binary( '1', pd1, cmd, strlen(cmd) );
 271     }
 272     last_cg_offset = cg_offset;
 273
 274     SG_LOG( SG_IO, SG_ALERT, "before sending reset command." );
 275
 276     if( fgGetBool("/sim/presets/onground") ) {
 277         sprintf( cmd, "reset=ground" );
 278     } else {
 279         sprintf( cmd, "reset=air" );
 280     }
 281     result = write_binary( '1', pd1, cmd, strlen(cmd) );
 282
 283     fflush( pd1 );
 284
 285     SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." );
 286 }
 287
 288
 289 // Initialize the ExternalPipe flight model using the property
 290 // protocol, dt is the time increment for each subsequent iteration
 291 // through the EOM
 292 void FGExternalPipe::init_property() {
 293     cout << "init_property()" << endl;
 294
 295     double lon = fgGetDouble( "/sim/presets/longitude-deg" );
 296     double lat = fgGetDouble( "/sim/presets/latitude-deg" );
 297     double alt = fgGetDouble( "/sim/presets/altitude-ft" );
 298     double ground = get_Runway_altitude_m();
 299     double heading = fgGetDouble("/sim/presets/heading-deg");
 300     double speed = fgGetDouble( "/sim/presets/airspeed-kt" );
 301     double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
 302     double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
 303
 304     char cmd[256];
 305     int result;
 306
 307     sprintf( cmd, "init longitude-deg=%.8f", lon );
 308     result = write_property( pd1, cmd );
 309
 310     sprintf( cmd, "init latitude-deg=%.8f", lat );
 311     result = write_property( pd1, cmd );
 312
 313     sprintf( cmd, "init altitude-ft=%.8f", alt );
 314     result = write_property( pd1, cmd );
 315
 316     sprintf( cmd, "init ground-m=%.8f", ground );
 317     result = write_property( pd1, cmd );
 318
 319     sprintf( cmd, "init speed-kts=%.8f", speed );
 320     result = write_property( pd1, cmd );
 321
 322     sprintf( cmd, "init heading-deg=%.8f", heading );
 323     result = write_property( pd1, cmd );
 324
 325     if ( weight > 1000.0 ) {
 326         sprintf( cmd, "init aircraft-weight-lbs=%.2f", weight );
 327         result = write_property( pd1, cmd );
 328     }
 329     last_weight = weight;
 330
 331     if ( cg_offset > -5.0 || cg_offset < 5.0 ) {
 332         sprintf( cmd, "init aircraft-cg-offset-inches=%.2f", cg_offset );
 333         result = write_property( pd1, cmd );
 334     }
 335     last_cg_offset = cg_offset;
 336
 337     SG_LOG( SG_IO, SG_ALERT, "before sending reset command." );
 338
 339     if( fgGetBool("/sim/presets/onground") ) {
 340         sprintf( cmd, "reset ground" );
 341     } else {
 342         sprintf( cmd, "reset air" );
 343     }
 344     result = write_property( pd1, cmd );
 345
 346     fflush( pd1 );
 347
 348     SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." );
 349 }
 350
 351
 352 // Wrapper for the ExternalPipe update routines.  dt is the time
 353 // increment for each subsequent iteration through the EOM
 354 void FGExternalPipe::update( double dt ) {
 355     if ( _protocol == "binary" ) {
 356         update_binary(dt);
 357     } else if ( _protocol == "property" ) {
 358         update_property(dt);
 359     } else {
 360         SG_LOG( SG_IO, SG_ALERT, "Init():  Unknown ExternalPipe protocol."
 361                 << "  Must be 'binary' or 'property'."
 362                 << "  (assuming binary)" );
 363     }
 364 }
 365
 366
 367 // Run an iteration of the EOM.
 368 void FGExternalPipe::update_binary( double dt ) {
 369 #ifdef HAVE_MKFIFO
 370     SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate_binary()" );
 371
 372     int length;
 373     int result;
 374
 375     if ( is_suspended() ) {
 376         return;
 377     }
 378
 379     int iterations = _calc_multiloop(dt);
 380
 381     double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
 382     static double last_weight = 0.0;
 383     if ( fabs( weight - last_weight ) > 0.01 ) {
 384         char cmd[256];
 385         sprintf( cmd, "aircraft-weight-lbs=%.2f", weight );
 386         result = write_binary( '1', pd1, cmd, strlen(cmd) );
 387     }
 388     last_weight = weight;
 389
 390     double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
 391     if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) {
 392         char cmd[256];
 393         sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset );
 394         result = write_binary( '1', pd1, cmd, strlen(cmd) );
 395     }
 396     last_cg_offset = cg_offset;
 397
 398     // Send control positions to remote fdm
 399     length = sizeof(ctrls);
 400     FGProps2NetCtrls( &ctrls, true, false );
 401     char *ptr = buf;
 402     *((int *)ptr) = iterations;
 403     // cout << "iterations = " << iterations << endl;
 404     ptr += sizeof(int);
 405     memcpy( ptr, (char *)(&ctrls), length );
 406     // cout << "writing control structure, size = "
 407     //      << length + sizeof(int) << endl;
 408
 409     result = write_binary( '2', pd1, buf, length + sizeof(int) );
 410     fflush( pd1 );
 411
 412     // Read fdm values
 413     length = sizeof(fdm);
 414     // cout << "about to read fdm data from remote fdm." << endl;
 415     result = fread( (char *)(& fdm), length, 1, pd2 );
 416     if ( result != 1 ) {
 417         SG_LOG( SG_IO, SG_ALERT, "Read error from named pipe: "
 418                 << fifo_name_2 << " expected 1 item, but got " << result );
 419     } else {
 420         // cout << "  read successful." << endl;
 421         FGNetFDM2Props( &fdm, false );
 422     }
 423 #endif
 424 }
 425
 426
 427 // Process remote FDM "set" commands
 428 static void process_set_command( const string_list &tokens ) {
 429     if ( tokens[1] == "geodetic_position" ) {
 430         double lat_rad = atof( tokens[2].c_str() );
 431         double lon_rad = atof( tokens[3].c_str() );
 432         double alt_m   = atof( tokens[4].c_str() );
 433         cur_fdm_state->_updateGeodeticPosition( lat_rad, lon_rad,
 434                                                 alt_m * SG_METER_TO_FEET );
 435
 436         double agl_m = alt_m - cur_fdm_state->get_Runway_altitude_m();
 437         cur_fdm_state->_set_Altitude_AGL( agl_m * SG_METER_TO_FEET );
 438     } else if ( tokens[1] == "euler_angles" ) {
 439         double phi_rad   = atof( tokens[2].c_str() );
 440         double theta_rad = atof( tokens[3].c_str() );
 441         double psi_rad   = atof( tokens[4].c_str() );
 442         cur_fdm_state->_set_Euler_Angles( phi_rad, theta_rad, psi_rad );
 443     } else if ( tokens[1] == "euler_rates" ) {
 444         double phidot   = atof( tokens[2].c_str() );
 445         double thetadot = atof( tokens[3].c_str() );
 446         double psidot   = atof( tokens[4].c_str() );
 447         cur_fdm_state->_set_Euler_Rates( phidot, thetadot, psidot );
 448     } else if ( tokens[1] == "alpha" ) {
 449         cur_fdm_state->_set_Alpha( atof(tokens[2].c_str()) );
 450     } else if ( tokens[1] == "beta" ) {
 451         cur_fdm_state->_set_Beta( atof(tokens[2].c_str()) );
 452
 453 #if 0
 454     cur_fdm_state->_set_V_calibrated_kts( net->vcas );
 455     cur_fdm_state->_set_Climb_Rate( net->climb_rate );
 456     cur_fdm_state->_set_Velocities_Local( net->v_north,
 457                                           net->v_east,
 458                                           net->v_down );
 459     cur_fdm_state->_set_Velocities_Wind_Body( net->v_wind_body_north,
 460                                               net->v_wind_body_east,
 461                                               net->v_wind_body_down );
 462
 463     cur_fdm_state->_set_Accels_Pilot_Body( net->A_X_pilot,
 464                                            net->A_Y_pilot,
 465                                            net->A_Z_pilot );
 466 #endif
 467     } else {
 468         fgSetString( tokens[1].c_str(), tokens[2].c_str() );
 469     }
 470 }
 471
 472
 473 // Run an iteration of the EOM.
 474 void FGExternalPipe::update_property( double dt ) {
 475     // cout << "update_property()" << endl;
 476
 477 #ifdef HAVE_MKFIFO
 478     // SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate()" );
 479
 480     int result;
 481     char cmd[256];
 482
 483     if ( is_suspended() ) {
 484         return;
 485     }
 486
 487     int iterations = _calc_multiloop(dt);
 488
 489     double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
 490     static double last_weight = 0.0;
 491     if ( fabs( weight - last_weight ) > 0.01 ) {
 492         sprintf( cmd, "init aircraft-weight-lbs=%.2f", weight );
 493         result = write_property( pd1, cmd );
 494     }
 495     last_weight = weight;
 496
 497     double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
 498     if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) {
 499         sprintf( cmd, "init aircraft-cg-offset-inches=%.2f", cg_offset );
 500         result = write_property( pd1, cmd );
 501     }
 502     last_cg_offset = cg_offset;
 503
 504     // Send requested property values to fdm
 505     for ( unsigned int i = 0; i < nodes.size(); i++ ) {
 506         sprintf( cmd, "set %s %s", property_names[i].c_str(),
 507                  nodes[i]->getStringValue() );
 508         // cout << "  sending " << cmd << endl;
 509         result = write_property( pd1, cmd );
 510     }
 511
 512     sprintf( cmd, "update %d", iterations );
 513     write_property( pd1, cmd );
 514
 515     fflush( pd1 );
 516
 517     // Read FDM response
 518     // cout << "ready to read fdm response" << endl;
 519     bool done = false;
 520     while ( !done ) {
 521         if ( fgets( cmd, 256, pd2 ) == NULL ) {
 522             cout << "Error reading data" << endl;
 523         } else {
 524             // cout << "  read " << strlen(cmd) << " bytes" << endl;
 525         }
 526
 527         // chop trailing newline
 528         cmd[strlen(cmd)-1] = '\0';
 529
 530         // cout << cmd << endl;
 531         string_list tokens = simgear::strutils::split( cmd, " " );
 532
 533         if ( tokens[0] == "request" ) {
 534             // save the long form name
 535             property_names.push_back( tokens[1] );
 536
 537             // now do the property name lookup and cache the pointer
 538             SGPropertyNode *node = fgGetNode( tokens[1].c_str() );
 539             if ( node == NULL ) {
 540                 // node doesn't exist so create with requested type
 541                 node = fgGetNode( tokens[1].c_str(), true );
 542                 if ( tokens[2] == "bool" ) {
 543                     node->setBoolValue(true);
 544                 } else if ( tokens[2] == "int" ) {
 545                     node->setIntValue(0);
 546                 } else if ( tokens[2] == "double" ) {
 547                     node->setDoubleValue(0.0);
 548                 } else if ( tokens[2] == "string" ) {
 549                     node->setStringValue("");
 550                 } else {
 551                     cout << "Unknown data type: " << tokens[2]
 552                          << " for " << tokens[1] << endl;
 553                 }
 554             }
 555             nodes.push_back( node );
 556         } else if ( tokens[0] == "set" ) {
 557             process_set_command( tokens );
 558         } else if ( tokens[0] == "update" ) {
 559             done = true;
 560         } else {
 561             cout << "unknown command = " << cmd << endl;
 562         }
 563     }
 564
 565 #endif
 566 }
 567
 568