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