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