X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FExternalPipe%2FExternalPipe.cxx;h=abe6569601a4a98b81d943647a23eeff558c398d;hb=2ea50c63bbb95e143662de2f136c79898236e38d;hp=dd2920db48f05dc029364a859f24f11ccbf23ed8;hpb=944a82b57617fa5a9fda4cc3f279c210b3f0d15e;p=flightgear.git diff --git a/src/FDM/ExternalPipe/ExternalPipe.cxx b/src/FDM/ExternalPipe/ExternalPipe.cxx index dd2920db4..abe656960 100644 --- a/src/FDM/ExternalPipe/ExternalPipe.cxx +++ b/src/FDM/ExternalPipe/ExternalPipe.cxx @@ -2,7 +2,7 @@ // // Written by Curtis Olson, started March 2003. // -// Copyright (C) 2003 Curtis L. Olson - curt@flightgear.org +// Copyright (C) 2003 Curtis L. Olson - http://www.flightgear.org/~curt // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License as @@ -16,7 +16,7 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // // $Id$ @@ -25,32 +25,43 @@ #endif #ifdef HAVE_MKFIFO -# include // mkfifo() open() umask() -# include // mkfifo() open() umask() +# include // mkfifo() umask() +# include // mkfifo() umask() # include // perror() -# include // open() # include // unlink() #endif +#include +#include // FILE*, fopen(), fread(), fwrite(), et. al. +#include // for cout, endl + #include #include // endian tests +#include // split() #include
#include #include +#include #include "ExternalPipe.hxx" +using std::cout; +using std::endl; static const int MAX_BUF = 32768; -FGExternalPipe::FGExternalPipe( double dt, string name ) { +FGExternalPipe::FGExternalPipe( double dt, string name, string protocol ) { valid = true; last_weight = 0.0; last_cg_offset = -9999.9; buf = new char[MAX_BUF]; + // clear property request list + property_names.clear(); + nodes.clear(); + #ifdef HAVE_MKFIFO fifo_name_1 = name + "1"; fifo_name_2 = name + "2"; @@ -73,17 +84,26 @@ FGExternalPipe::FGExternalPipe( double dt, string name ) { perror( "ExternalPipe()" ); } - pd1 = open( fifo_name_1.c_str(), O_RDWR ); - if ( pd1 == -1 ) { + pd1 = fopen( fifo_name_1.c_str(), "w" ); + if ( pd1 == NULL ) { SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_1 ); valid = false; } - pd2 = open( fifo_name_2.c_str(), O_RDWR ); - if ( pd2 == -1 ) { + pd2 = fopen( fifo_name_2.c_str(), "r" ); + if ( pd2 == NULL ) { SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_2 ); valid = false; } #endif + + _protocol = protocol; + + if ( _protocol != "binary" && _protocol != "property" ) { + SG_LOG( SG_IO, SG_ALERT, "Constructor(): Unknown ExternalPipe protocol." + << " Must be 'binary' or 'property'." + << " (assuming binary)" ); + _protocol = "binary"; + } } @@ -95,14 +115,14 @@ FGExternalPipe::~FGExternalPipe() { #ifdef HAVE_MKFIFO // close int result; - result = close( pd1 ); - if ( result == -1 ) { + result = fclose( pd1 ); + if ( result ) { SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: " << fifo_name_1 ); perror( "~FGExternalPipe()" ); } - result = close( pd2 ); - if ( result == -1 ) { + result = fclose( pd2 ); + if ( result ) { SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: " << fifo_name_2 ); perror( "~FGExternalPipe()" ); @@ -111,27 +131,35 @@ FGExternalPipe::~FGExternalPipe() { } -static int write_fifo( char cmd_type, int pd, char *cmd, int len ) { +static int write_binary( char cmd_type, FILE *pd, char *cmd, int len ) { #ifdef HAVE_MKFIFO char *buf = new char[len + 3]; // write 2 byte command length + command type + command - char hi = (len + 1) / 256; - char lo = (len + 1) - (hi * 256); + unsigned char hi = (len + 1) / 256; + unsigned char lo = (len + 1) - (hi * 256); + + // cout << "len = " << len << " hi = " << (int)hi << " lo = " + // << (int)lo << endl; buf[0] = hi; buf[1] = lo; buf[2] = cmd_type; - // strncpy( buf + 3, cmd, len ); memcpy( buf + 3, cmd, len ); if ( cmd_type == '1' ) { - // cout << "writing '" << cmd << "'" << endl; + cout << "writing "; + cout << (int)hi << " "; + cout << (int)lo << " '"; + for ( int i = 2; i < len + 3; ++i ) { + cout << buf[i]; + } + cout << "' (" << cmd << ")" << endl; } else if ( cmd_type == '2' ) { // cout << "writing controls packet" << endl; } else { - // cout << "writing unknown command?" << endl; + cout << "writing unknown command?" << endl; } // for ( int i = 0; i < len + 3; ++i ) { @@ -139,9 +167,9 @@ static int write_fifo( char cmd_type, int pd, char *cmd, int len ) { // } // cout << endl; - int result = ::write( pd, buf, len + 3 ); - if ( result == -1 ) { - perror( "write_fifo()" ); + int result = fwrite( buf, len + 3, 1, pd ); + if ( result != 1 ) { + perror( "write_binary()" ); SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << pd ); } // cout << "wrote " << len + 3 << " bytes." << endl; @@ -155,54 +183,95 @@ static int write_fifo( char cmd_type, int pd, char *cmd, int len ) { } -// Initialize the ExternalPipe flight model, dt is the time increment -// for each subsequent iteration through the EOM +static int write_property( FILE *pd, char *cmd ) { + int len = strlen(cmd); + +#ifdef HAVE_MKFIFO + char *buf = new char[len + 1]; + + memcpy( buf, cmd, len ); + buf[len] = '\n'; + + int result = fwrite( buf, len + 1, 1, pd ); + if ( result == len + 1 ) { + perror( "write_property()" ); + SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << pd ); + } + // cout << "wrote " << len + 1 << " bytes." << endl; + + delete [] buf; + + return result; +#else + return 0; +#endif +} + + +// Wrapper for the ExternalPipe flight model initialization. dt is +// the time increment for each subsequent iteration through the EOM void FGExternalPipe::init() { // Explicitly call the superclass's // init method first. common_init(); + if ( _protocol == "binary" ) { + init_binary(); + } else if ( _protocol == "property" ) { + init_property(); + } else { + SG_LOG( SG_IO, SG_ALERT, "Init(): Unknown ExternalPipe protocol." + << " Must be 'binary' or 'property'." + << " (assuming binary)" ); + } +} + + +// Initialize the ExternalPipe flight model using the binary protocol, +// dt is the time increment for each subsequent iteration through the +// EOM +void FGExternalPipe::init_binary() { + cout << "init_binary()" << endl; + double lon = fgGetDouble( "/sim/presets/longitude-deg" ); double lat = fgGetDouble( "/sim/presets/latitude-deg" ); double alt = fgGetDouble( "/sim/presets/altitude-ft" ); - double ground = fgGetDouble( "/environment/ground-elevation-m" ); + double ground = get_Runway_altitude_m(); double heading = fgGetDouble("/sim/presets/heading-deg"); double speed = fgGetDouble( "/sim/presets/airspeed-kt" ); double weight = fgGetDouble( "/sim/aircraft-weight-lbs" ); double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" ); -#ifdef HAVE_MKFIFO - char cmd[256]; int result; sprintf( cmd, "longitude-deg=%.8f", lon ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); sprintf( cmd, "latitude-deg=%.8f", lat ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); sprintf( cmd, "altitude-ft=%.8f", alt ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); sprintf( cmd, "ground-m=%.8f", ground ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); sprintf( cmd, "speed-kts=%.8f", speed ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); sprintf( cmd, "heading-deg=%.8f", heading ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); if ( weight > 1000.0 ) { sprintf( cmd, "aircraft-weight-lbs=%.2f", weight ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); } last_weight = weight; if ( cg_offset > -5.0 || cg_offset < 5.0 ) { sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); } last_cg_offset = cg_offset; @@ -213,17 +282,96 @@ void FGExternalPipe::init() { } else { sprintf( cmd, "reset=air" ); } - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + + fflush( pd1 ); SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." ); -#endif } -// Run an iteration of the EOM. +// Initialize the ExternalPipe flight model using the property +// protocol, dt is the time increment for each subsequent iteration +// through the EOM +void FGExternalPipe::init_property() { + cout << "init_property()" << endl; + + double lon = fgGetDouble( "/sim/presets/longitude-deg" ); + double lat = fgGetDouble( "/sim/presets/latitude-deg" ); + double alt = fgGetDouble( "/sim/presets/altitude-ft" ); + double ground = get_Runway_altitude_m(); + double heading = fgGetDouble("/sim/presets/heading-deg"); + double speed = fgGetDouble( "/sim/presets/airspeed-kt" ); + double weight = fgGetDouble( "/sim/aircraft-weight-lbs" ); + double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" ); + + char cmd[256]; + int result; + + sprintf( cmd, "init longitude-deg=%.8f", lon ); + result = write_property( pd1, cmd ); + + sprintf( cmd, "init latitude-deg=%.8f", lat ); + result = write_property( pd1, cmd ); + + sprintf( cmd, "init altitude-ft=%.8f", alt ); + result = write_property( pd1, cmd ); + + sprintf( cmd, "init ground-m=%.8f", ground ); + result = write_property( pd1, cmd ); + + sprintf( cmd, "init speed-kts=%.8f", speed ); + result = write_property( pd1, cmd ); + + sprintf( cmd, "init heading-deg=%.8f", heading ); + result = write_property( pd1, cmd ); + + if ( weight > 1000.0 ) { + sprintf( cmd, "init aircraft-weight-lbs=%.2f", weight ); + result = write_property( pd1, cmd ); + } + last_weight = weight; + + if ( cg_offset > -5.0 || cg_offset < 5.0 ) { + sprintf( cmd, "init aircraft-cg-offset-inches=%.2f", cg_offset ); + result = write_property( pd1, cmd ); + } + last_cg_offset = cg_offset; + + SG_LOG( SG_IO, SG_ALERT, "before sending reset command." ); + + if( fgGetBool("/sim/presets/onground") ) { + sprintf( cmd, "reset ground" ); + } else { + sprintf( cmd, "reset air" ); + } + result = write_property( pd1, cmd ); + + fflush( pd1 ); + + SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." ); +} + + +// Wrapper for the ExternalPipe update routines. dt is the time +// increment for each subsequent iteration through the EOM void FGExternalPipe::update( double dt ) { + if ( _protocol == "binary" ) { + update_binary(dt); + } else if ( _protocol == "property" ) { + update_property(dt); + } else { + SG_LOG( SG_IO, SG_ALERT, "Init(): Unknown ExternalPipe protocol." + << " Must be 'binary' or 'property'." + << " (assuming binary)" ); + } +} + + +// Run an iteration of the EOM. +void FGExternalPipe::update_binary( double dt ) { #ifdef HAVE_MKFIFO - // SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate()" ); + SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate_binary()" ); int length; int result; @@ -239,7 +387,7 @@ void FGExternalPipe::update( double dt ) { if ( fabs( weight - last_weight ) > 0.01 ) { char cmd[256]; sprintf( cmd, "aircraft-weight-lbs=%.2f", weight ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); } last_weight = weight; @@ -247,7 +395,7 @@ void FGExternalPipe::update( double dt ) { if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) { char cmd[256]; sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset ); - result = write_fifo( '1', pd1, cmd, strlen(cmd) ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); } last_cg_offset = cg_offset; @@ -262,18 +410,170 @@ void FGExternalPipe::update( double dt ) { // cout << "writing control structure, size = " // << length + sizeof(int) << endl; - result = write_fifo( '2', pd1, buf, length + sizeof(int) ); + result = write_binary( '2', pd1, buf, length + sizeof(int) ); + fflush( pd1 ); // Read fdm values length = sizeof(fdm); // cout << "about to read fdm data from remote fdm." << endl; - result = read( pd2, (char *)(& fdm), length ); - if ( result == -1 ) { + result = fread( (char *)(& fdm), length, 1, pd2 ); + if ( result != 1 ) { SG_LOG( SG_IO, SG_ALERT, "Read error from named pipe: " - << fifo_name_2 ); + << fifo_name_2 << " expected 1 item, but got " << result ); } else { // cout << " read successful." << endl; + FGNetFDM2Props( &fdm, false ); + } +#endif +} + + +// Process remote FDM "set" commands + +void FGExternalPipe::process_set_command( const string_list &tokens ) { + if ( tokens[1] == "geodetic_position" ) { + double lat_rad = atof( tokens[2].c_str() ); + double lon_rad = atof( tokens[3].c_str() ); + double alt_m = atof( tokens[4].c_str() ); + _updateGeodeticPosition( lat_rad, lon_rad, + alt_m * SG_METER_TO_FEET ); + + double agl_m = alt_m - get_Runway_altitude_m(); + _set_Altitude_AGL( agl_m * SG_METER_TO_FEET ); + } else if ( tokens[1] == "euler_angles" ) { + double phi_rad = atof( tokens[2].c_str() ); + double theta_rad = atof( tokens[3].c_str() ); + double psi_rad = atof( tokens[4].c_str() ); + _set_Euler_Angles( phi_rad, theta_rad, psi_rad ); + } else if ( tokens[1] == "euler_rates" ) { + double phidot = atof( tokens[2].c_str() ); + double thetadot = atof( tokens[3].c_str() ); + double psidot = atof( tokens[4].c_str() ); + _set_Euler_Rates( phidot, thetadot, psidot ); + } else if ( tokens[1] == "ned" ) { + double north_fps = atof( tokens[2].c_str() ); + double east_fps = atof( tokens[3].c_str() ); + double down_fps = atof( tokens[4].c_str() ); + _set_Velocities_Local( north_fps, east_fps, down_fps ); + } else if ( tokens[1] == "alpha" ) { + _set_Alpha( atof(tokens[2].c_str()) ); + } else if ( tokens[1] == "beta" ) { + _set_Beta( atof(tokens[2].c_str()) ); + +#if 0 + _set_V_calibrated_kts( net->vcas ); + _set_Climb_Rate( net->climb_rate ); + _set_Velocities_Local( net->v_north, + net->v_east, + net->v_down ); + _set_Velocities_Wind_Body( net->v_wind_body_north, + net->v_wind_body_east, + net->v_wind_body_down ); + + _set_Accels_Pilot_Body( net->A_X_pilot, + net->A_Y_pilot, + net->A_Z_pilot ); +#endif + } else { + fgSetString( tokens[1].c_str(), tokens[2].c_str() ); + } +} + + +// Run an iteration of the EOM. +void FGExternalPipe::update_property( double dt ) { + // cout << "update_property()" << endl; + +#ifdef HAVE_MKFIFO + // SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate()" ); + + int result; + char cmd[256]; + + if ( is_suspended() ) { + return; + } + + int iterations = _calc_multiloop(dt); + + double weight = fgGetDouble( "/sim/aircraft-weight-lbs" ); + static double last_weight = 0.0; + if ( fabs( weight - last_weight ) > 0.01 ) { + sprintf( cmd, "init aircraft-weight-lbs=%.2f", weight ); + result = write_property( pd1, cmd ); + } + last_weight = weight; + + double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" ); + if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) { + sprintf( cmd, "init aircraft-cg-offset-inches=%.2f", cg_offset ); + result = write_property( pd1, cmd ); + } + last_cg_offset = cg_offset; + + // Send requested property values to fdm + for ( unsigned int i = 0; i < nodes.size(); i++ ) { + sprintf( cmd, "set %s %s", property_names[i].c_str(), + nodes[i]->getStringValue() ); + // cout << " sending " << cmd << endl; + result = write_property( pd1, cmd ); + } + + sprintf( cmd, "update %d", iterations ); + write_property( pd1, cmd ); + + fflush( pd1 ); + + // Read FDM response + // cout << "ready to read fdm response" << endl; + bool done = false; + while ( !done ) { + if ( fgets( cmd, 256, pd2 ) == NULL ) { + cout << "Error reading data" << endl; + } else { + // cout << " read " << strlen(cmd) << " bytes" << endl; + // cout << cmd << endl; + } + + // chop trailing newline + cmd[strlen(cmd)-1] = '\0'; + + // cout << cmd << endl; + string_list tokens = simgear::strutils::split( cmd, " " ); + + if ( tokens[0] == "request" ) { + // save the long form name + property_names.push_back( tokens[1] ); + + // now do the property name lookup and cache the pointer + SGPropertyNode *node = fgGetNode( tokens[1].c_str() ); + if ( node == NULL ) { + // node doesn't exist so create with requested type + node = fgGetNode( tokens[1].c_str(), true ); + if ( tokens[2] == "bool" ) { + node->setBoolValue(false); + } else if ( tokens[2] == "int" ) { + node->setIntValue(0); + } else if ( tokens[2] == "double" ) { + node->setDoubleValue(0.0); + } else if ( tokens[2] == "string" ) { + node->setStringValue(""); + } else { + cout << "Unknown data type: " << tokens[2] + << " for " << tokens[1] << endl; + } + } + nodes.push_back( node ); + } else if ( tokens[0] == "set" ) { + process_set_command( tokens ); + } else if ( tokens[0] == "update" ) { + done = true; + } else { + cout << "unknown command = " << cmd << endl; + } } - FGNetFDM2Props( &fdm, false ); + #endif } + +