X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FExternalPipe%2FExternalPipe.cxx;h=abe6569601a4a98b81d943647a23eeff558c398d;hb=2ea50c63bbb95e143662de2f136c79898236e38d;hp=da73ca06921826d8e1c5afd999dd0af1d14ad484;hpb=dbf7218c638b7b8a9dfaed5fe605d27ebcf7e3c9;p=flightgear.git diff --git a/src/FDM/ExternalPipe/ExternalPipe.cxx b/src/FDM/ExternalPipe/ExternalPipe.cxx index da73ca069..abe656960 100644 --- a/src/FDM/ExternalPipe/ExternalPipe.cxx +++ b/src/FDM/ExternalPipe/ExternalPipe.cxx @@ -1,8 +1,8 @@ -// ExternalPipe.hxx -- a "pipe" interface to an external flight dynamics model +// ExternalPipe.cxx -- a "pipe" interface to an external flight dynamics model // // 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$ @@ -24,31 +24,45 @@ # include #endif -#if defined( HAVE_SYS_TYPES_H ) && defined( HAVE_SYS_STAT_H ) -# include // mkfifo() open() umask() -# include // mkfifo() open() umask() -# include // open() +#ifdef HAVE_MKFIFO +# include // mkfifo() umask() +# include // mkfifo() umask() +# include // perror() # 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[sizeof(ctrls) + 1]; + buf = new char[MAX_BUF]; - cout << "dt = " << dt << endl; + // clear property request list + property_names.clear(); + nodes.clear(); -#if defined( HAVE_SYS_TYPES_H ) && defined( HAVE_SYS_STAT_H ) +#ifdef HAVE_MKFIFO fifo_name_1 = name + "1"; fifo_name_2 = name + "2"; @@ -61,171 +75,505 @@ FGExternalPipe::FGExternalPipe( double dt, string name ) { if ( result == -1 ) { SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: " << fifo_name_1 ); - valid = false; + perror( "ExternalPipe()" ); } result = mkfifo( fifo_name_2.c_str(), 0644 ); if ( result == -1 ) { SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: " << fifo_name_2 ); - valid = false; + perror( "ExternalPipe()" ); } - pd1 = open( fifo_name_1.c_str(), O_RDWR ); - cout << "pd1 = " << pd1 << endl; - 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 ); - cout << "pd2 = " << pd2 << endl; - 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"; + } } FGExternalPipe::~FGExternalPipe() { delete [] buf; -#if defined( HAVE_SYS_TYPES_H ) && defined( HAVE_SYS_STAT_H ) + SG_LOG( SG_IO, SG_INFO, "Closing up the ExternalPipe." ); + +#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()" ); } +#endif +} - // remove the file system entry - result = unlink( fifo_name_1.c_str() ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Unable to remove named pipe: " - << fifo_name_1 ); + +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 + 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; + + memcpy( buf + 3, cmd, len ); + + if ( cmd_type == '1' ) { + 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; } - result = unlink( fifo_name_2.c_str() ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Unable to remove named pipe: " - << fifo_name_2 ); + + // for ( int i = 0; i < len + 3; ++i ) { + // cout << " " << (int)buf[i]; + // } + // cout << endl; + + 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; + + delete [] buf; + + return result; +#else + return 0; +#endif +} + + +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 } -// Initialize the ExternalPipe flight model, dt is the time increment -// for each subsequent iteration through the EOM +// 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" ); - -#if defined( HAVE_SYS_TYPES_H ) && defined( HAVE_SYS_STAT_H ) + double weight = fgGetDouble( "/sim/aircraft-weight-lbs" ); + double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" ); char cmd[256]; int result; - sprintf( cmd, "1longitude-deg=%.8f", lon ); - result = std::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); - } + sprintf( cmd, "longitude-deg=%.8f", lon ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); - sprintf( cmd, "1latitude-deg=%.8f", lat ); - result = ::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); + sprintf( cmd, "latitude-deg=%.8f", lat ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + + sprintf( cmd, "altitude-ft=%.8f", alt ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + + sprintf( cmd, "ground-m=%.8f", ground ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + + sprintf( cmd, "speed-kts=%.8f", speed ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + + sprintf( cmd, "heading-deg=%.8f", heading ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + + if ( weight > 1000.0 ) { + sprintf( cmd, "aircraft-weight-lbs=%.2f", weight ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); } + last_weight = weight; - sprintf( cmd, "1altitude-ft=%.8f", alt ); - result = std::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); + if ( cg_offset > -5.0 || cg_offset < 5.0 ) { + sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); } + last_cg_offset = cg_offset; - sprintf( cmd, "1ground-m=%.8f", ground ); - result = std::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); + 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_binary( '1', pd1, cmd, strlen(cmd) ); - sprintf( cmd, "1speed-kts=%.8f", speed ); - result = std::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); + fflush( pd1 ); + + SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." ); +} + + +// 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; - sprintf( cmd, "1heading-deg=%.8f", heading ); - result = std::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); + 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_INFO, "before sending reset command." ); + SG_LOG( SG_IO, SG_ALERT, "before sending reset command." ); if( fgGetBool("/sim/presets/onground") ) { - sprintf( cmd, "1reset=ground" ); + sprintf( cmd, "reset ground" ); } else { - sprintf( cmd, "1reset=air" ); - } - result = std::write( pd1, cmd, strlen(cmd) ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << fifo_name_1 ); + sprintf( cmd, "reset air" ); } + result = write_property( pd1, cmd ); - SG_LOG( SG_IO, SG_INFO, "Remote FDM init() finished." ); -#endif + fflush( pd1 ); + + SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." ); } -// Run an iteration of the EOM. +// Wrapper for the ExternalPipe update routines. dt is the time +// increment for each subsequent iteration through the EOM void FGExternalPipe::update( double dt ) { -#if defined( HAVE_SYS_TYPES_H ) && defined( HAVE_SYS_STAT_H ) + 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_binary()" ); + int length; int result; - + 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 ) { + char cmd[256]; + sprintf( cmd, "aircraft-weight-lbs=%.2f", weight ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + } + last_weight = weight; + + double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" ); + if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) { + char cmd[256]; + sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset ); + result = write_binary( '1', pd1, cmd, strlen(cmd) ); + } + last_cg_offset = cg_offset; + // Send control positions to remote fdm length = sizeof(ctrls); - FGProps2NetCtrls( &ctrls, false ); + FGProps2NetCtrls( &ctrls, true, false ); char *ptr = buf; - *ptr = '2'; - ptr++; *((int *)ptr) = iterations; + // cout << "iterations = " << iterations << endl; ptr += sizeof(int); memcpy( ptr, (char *)(&ctrls), length ); - result = std::write( pd1, buf, length + 1 ); - if ( result == -1 ) { - SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " - << fifo_name_1 ); - } - // cout << "wrote to pipe" << endl; + // cout << "writing control structure, size = " + // << length + sizeof(int) << endl; - // Read next set of FDM data (blocking enabled to maintain 'sync') + result = write_binary( '2', pd1, buf, length + sizeof(int) ); + fflush( pd1 ); + + // Read fdm values length = sizeof(fdm); - result = std::read( pd2, (char *)(& fdm), length ); - if ( result == -1 ) { + // cout << "about to read fdm data from remote fdm." << endl; + 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 ); } - FGNetFDM2Props( &fdm, false ); - // cout << "read from pipe" << endl; #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; + } + } + +#endif +} + +