1
0
Fork 0
flightgear/src/FDM/ExternalPipe/ExternalPipe.cxx

573 lines
18 KiB
C++
Raw Normal View History

// ExternalPipe.cxx -- a "pipe" interface to an external flight dynamics model
//
// Written by Curtis Olson, started March 2003.
//
// 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
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
2006-02-21 01:16:04 +00:00
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
2003-06-08 12:01:43 +00:00
#ifdef HAVE_MKFIFO
# include <sys/types.h> // mkfifo() umask()
# include <sys/stat.h> // mkfifo() umask()
# include <errno.h> // perror()
# include <unistd.h> // unlink()
#endif
#include <stdio.h> // FILE*, fopen(), fread(), fwrite(), et. al.
#include <iostream> // for cout, endl
#include <simgear/debug/logstream.hxx>
#include <simgear/io/lowlevel.hxx> // endian tests
#include <simgear/misc/strutils.hxx> // split()
#include <Main/fg_props.hxx>
#include <Network/native_ctrls.hxx>
#include <Network/native_fdm.hxx>
#include <Scenery/scenery.hxx>
#include "ExternalPipe.hxx"
using std::cout;
using std::endl;
static const int MAX_BUF = 32768;
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();
2003-06-08 12:01:43 +00:00
#ifdef HAVE_MKFIFO
fifo_name_1 = name + "1";
fifo_name_2 = name + "2";
SG_LOG( SG_IO, SG_ALERT, "ExternalPipe Inited with " << name );
// Make the named pipe
umask(0);
int result;
result = mkfifo( fifo_name_1.c_str(), 0644 );
if ( result == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
<< fifo_name_1 );
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 );
perror( "ExternalPipe()" );
}
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 = 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;
SG_LOG( SG_IO, SG_INFO, "Closing up the ExternalPipe." );
2003-06-08 12:01:43 +00:00
#ifdef HAVE_MKFIFO
// close
int result;
result = fclose( pd1 );
if ( result ) {
SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
<< fifo_name_1 );
perror( "~FGExternalPipe()" );
}
result = fclose( pd2 );
if ( result ) {
SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
<< fifo_name_2 );
perror( "~FGExternalPipe()" );
}
#endif
}
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
2005-02-15 18:07:06 +00:00
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' ) {
2005-08-23 21:36:05 +00:00
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' ) {
2005-08-22 23:47:17 +00:00
// cout << "writing controls packet" << endl;
} else {
cout << "writing unknown command?" << endl;
}
// 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
}
// 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 = 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, "longitude-deg=%.8f", lon );
result = write_binary( '1', pd1, cmd, strlen(cmd) );
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;
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;
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) );
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;
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 ) {
2003-06-08 12:01:43 +00:00
#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, true, false );
char *ptr = buf;
*((int *)ptr) = iterations;
// cout << "iterations = " << iterations << endl;
ptr += sizeof(int);
memcpy( ptr, (char *)(&ctrls), length );
2005-08-22 23:47:17 +00:00
// cout << "writing control structure, size = "
// << length + sizeof(int) << endl;
result = write_binary( '2', pd1, buf, length + sizeof(int) );
fflush( pd1 );
// Read fdm values
length = sizeof(fdm);
2005-08-22 23:47:17 +00:00
// 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 << " expected 1 item, but got " << result );
} else {
2005-08-22 23:47:17 +00:00
// cout << " read successful." << endl;
2005-08-23 21:02:44 +00:00
FGNetFDM2Props( &fdm, false );
}
#endif
}
// Process remote FDM "set" commands
static void 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() );
cur_fdm_state->_updateGeodeticPosition( lat_rad, lon_rad,
alt_m * SG_METER_TO_FEET );
Mathias Frhlich: I have introduced the posibility to start directly on the carrier. With that patch you will have a --carrrier=id argument where id can either be the pennant number configured in the nimitz scenario or the carriers name also configured in the carriers scenario. Additionaly you can use --parkpos=id to select different positions on the carrier. They are also configured in the scenario file. That includes the switch of the whole FGInterface class to make use of the groundcache. That means that an aircraft no longer uses the current elevation value from the scenery class. It rather has its own local cache of the aircrafts environment which is setup in the common_init method of FGInterface and updated either manually by calling FGInterface::get_groundlevel_m(lat, lon, alt_m); or implicitly by calling the above method in the FGInterface::_updateGeo*Position(lat, lon, alt); methods. A call get_groundlevel_m rebuilds the groundcache if the request is outside the range of the cache. Note that for the real usage of the groundcache including the correct information about the movement of objects and the velocity information, you still need to set up the groundcache in the usual way like YASim and JSBSim currently does. If you use the native interface, you will get only static objects correctly. But for FDM's only using one single ground level for a whole step this is IMO sufficient. The AIManager gets a way to return the location of a object which is placed wrt an AI Object. At the moment it only honours AICarriers for that. That method is a static one, which loads the scenario file for that reason and throws it away afterwards. This looked like the aprioriate way, because the AIManager is initialized much later in flightgears bootstrap, and I did not find an easy way to reorder that for my needs. Since this additional load is very small and does only happen if such a relative location is required, I think that this is ok. Note that moving on the carrier will only work correctly for JSBSim and YASim, but you should now be able to start and move on every not itself moving object with any FDM.
2005-07-03 09:39:14 +00:00
double agl_m = alt_m - cur_fdm_state->get_Runway_altitude_m();
cur_fdm_state->_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() );
cur_fdm_state->_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() );
cur_fdm_state->_set_Euler_Rates( phidot, thetadot, psidot );
} else if ( tokens[1] == "alpha" ) {
cur_fdm_state->_set_Alpha( atof(tokens[2].c_str()) );
} else if ( tokens[1] == "beta" ) {
cur_fdm_state->_set_Beta( atof(tokens[2].c_str()) );
#if 0
cur_fdm_state->_set_V_calibrated_kts( net->vcas );
cur_fdm_state->_set_Climb_Rate( net->climb_rate );
cur_fdm_state->_set_Velocities_Local( net->v_north,
net->v_east,
net->v_down );
cur_fdm_state->_set_Velocities_Wind_Body( net->v_wind_body_north,
net->v_wind_body_east,
net->v_wind_body_down );
cur_fdm_state->_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(true);
} 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
}