1 // ExternalPipe.cxx -- a "pipe" interface to an external flight dynamics model
3 // Written by Curtis Olson, started March 2003.
5 // Copyright (C) 2003 Curtis L. Olson - http://www.flightgear.org/~curt
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.
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.
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.
28 # include <sys/types.h> // mkfifo() umask()
29 # include <sys/stat.h> // mkfifo() umask()
30 # include <errno.h> // perror()
31 # include <unistd.h> // unlink()
34 #include <stdio.h> // FILE*, fopen(), fread(), fwrite(), et. al.
35 #include <iostream> // for cout, endl
37 #include <simgear/debug/logstream.hxx>
38 #include <simgear/io/lowlevel.hxx> // endian tests
39 #include <simgear/misc/strutils.hxx> // split()
41 #include <Main/fg_props.hxx>
42 #include <Network/native_ctrls.hxx>
43 #include <Network/native_fdm.hxx>
44 #include <Scenery/scenery.hxx>
46 #include "ExternalPipe.hxx"
51 static const int MAX_BUF = 32768;
53 FGExternalPipe::FGExternalPipe( double dt, string name, string protocol ) {
56 last_cg_offset = -9999.9;
58 buf = new char[MAX_BUF];
60 // clear property request list
61 property_names.clear();
65 fifo_name_1 = name + "1";
66 fifo_name_2 = name + "2";
68 SG_LOG( SG_IO, SG_ALERT, "ExternalPipe Inited with " << name );
70 // Make the named pipe
73 result = mkfifo( fifo_name_1.c_str(), 0644 );
75 SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
77 perror( "ExternalPipe()" );
79 result = mkfifo( fifo_name_2.c_str(), 0644 );
81 SG_LOG( SG_IO, SG_ALERT, "Unable to create named pipe: "
83 perror( "ExternalPipe()" );
86 pd1 = fopen( fifo_name_1.c_str(), "w" );
88 SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_1 );
91 pd2 = fopen( fifo_name_2.c_str(), "r" );
93 SG_LOG( SG_IO, SG_ALERT, "Unable to open named pipe: " << fifo_name_2 );
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";
109 FGExternalPipe::~FGExternalPipe() {
112 SG_LOG( SG_IO, SG_INFO, "Closing up the ExternalPipe." );
117 result = fclose( pd1 );
119 SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
121 perror( "~FGExternalPipe()" );
123 result = fclose( pd2 );
125 SG_LOG( SG_IO, SG_ALERT, "Unable to close named pipe: "
127 perror( "~FGExternalPipe()" );
133 static int write_binary( char cmd_type, FILE *pd, char *cmd, int len ) {
135 char *buf = new char[len + 3];
137 // write 2 byte command length + command type + command
138 unsigned char hi = (len + 1) / 256;
139 unsigned char lo = (len + 1) - (hi * 256);
141 // cout << "len = " << len << " hi = " << (int)hi << " lo = "
142 // << (int)lo << endl;
148 memcpy( buf + 3, cmd, len );
150 if ( cmd_type == '1' ) {
152 cout << (int)hi << " ";
153 cout << (int)lo << " '";
154 for ( int i = 2; i < len + 3; ++i ) {
157 cout << "' (" << cmd << ")" << endl;
158 } else if ( cmd_type == '2' ) {
159 // cout << "writing controls packet" << endl;
161 cout << "writing unknown command?" << endl;
164 // for ( int i = 0; i < len + 3; ++i ) {
165 // cout << " " << (int)buf[i];
169 int result = fwrite( buf, len + 3, 1, pd );
171 perror( "write_binary()" );
172 SG_LOG( SG_IO, SG_ALERT, "Write error to named pipe: " << pd );
174 // cout << "wrote " << len + 3 << " bytes." << endl;
185 static int write_property( FILE *pd, char *cmd ) {
186 int len = strlen(cmd);
189 char *buf = new char[len + 1];
191 memcpy( buf, cmd, len );
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 );
199 // cout << "wrote " << len + 1 << " bytes." << endl;
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.
217 if ( _protocol == "binary" ) {
219 } else if ( _protocol == "property" ) {
222 SG_LOG( SG_IO, SG_ALERT, "Init(): Unknown ExternalPipe protocol."
223 << " Must be 'binary' or 'property'."
224 << " (assuming binary)" );
229 // Initialize the ExternalPipe flight model using the binary protocol,
230 // dt is the time increment for each subsequent iteration through the
232 void FGExternalPipe::init_binary() {
233 cout << "init_binary()" << endl;
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" );
247 sprintf( cmd, "longitude-deg=%.8f", lon );
248 result = write_binary( '1', pd1, cmd, strlen(cmd) );
250 sprintf( cmd, "latitude-deg=%.8f", lat );
251 result = write_binary( '1', pd1, cmd, strlen(cmd) );
253 sprintf( cmd, "altitude-ft=%.8f", alt );
254 result = write_binary( '1', pd1, cmd, strlen(cmd) );
256 sprintf( cmd, "ground-m=%.8f", ground );
257 result = write_binary( '1', pd1, cmd, strlen(cmd) );
259 sprintf( cmd, "speed-kts=%.8f", speed );
260 result = write_binary( '1', pd1, cmd, strlen(cmd) );
262 sprintf( cmd, "heading-deg=%.8f", heading );
263 result = write_binary( '1', pd1, cmd, strlen(cmd) );
265 if ( weight > 1000.0 ) {
266 sprintf( cmd, "aircraft-weight-lbs=%.2f", weight );
267 result = write_binary( '1', pd1, cmd, strlen(cmd) );
269 last_weight = weight;
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) );
275 last_cg_offset = cg_offset;
277 SG_LOG( SG_IO, SG_ALERT, "before sending reset command." );
279 if( fgGetBool("/sim/presets/onground") ) {
280 sprintf( cmd, "reset=ground" );
282 sprintf( cmd, "reset=air" );
284 result = write_binary( '1', pd1, cmd, strlen(cmd) );
288 SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." );
292 // Initialize the ExternalPipe flight model using the property
293 // protocol, dt is the time increment for each subsequent iteration
295 void FGExternalPipe::init_property() {
296 cout << "init_property()" << endl;
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" );
310 sprintf( cmd, "init longitude-deg=%.8f", lon );
311 result = write_property( pd1, cmd );
313 sprintf( cmd, "init latitude-deg=%.8f", lat );
314 result = write_property( pd1, cmd );
316 sprintf( cmd, "init altitude-ft=%.8f", alt );
317 result = write_property( pd1, cmd );
319 sprintf( cmd, "init ground-m=%.8f", ground );
320 result = write_property( pd1, cmd );
322 sprintf( cmd, "init speed-kts=%.8f", speed );
323 result = write_property( pd1, cmd );
325 sprintf( cmd, "init heading-deg=%.8f", heading );
326 result = write_property( pd1, cmd );
328 if ( weight > 1000.0 ) {
329 sprintf( cmd, "init aircraft-weight-lbs=%.2f", weight );
330 result = write_property( pd1, cmd );
332 last_weight = weight;
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 );
338 last_cg_offset = cg_offset;
340 SG_LOG( SG_IO, SG_ALERT, "before sending reset command." );
342 if( fgGetBool("/sim/presets/onground") ) {
343 sprintf( cmd, "reset ground" );
345 sprintf( cmd, "reset air" );
347 result = write_property( pd1, cmd );
351 SG_LOG( SG_IO, SG_ALERT, "Remote FDM init() finished." );
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" ) {
360 } else if ( _protocol == "property" ) {
363 SG_LOG( SG_IO, SG_ALERT, "Init(): Unknown ExternalPipe protocol."
364 << " Must be 'binary' or 'property'."
365 << " (assuming binary)" );
370 // Run an iteration of the EOM.
371 void FGExternalPipe::update_binary( double dt ) {
373 SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate_binary()" );
378 if ( is_suspended() ) {
382 int iterations = _calc_multiloop(dt);
384 double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
385 static double last_weight = 0.0;
386 if ( fabs( weight - last_weight ) > 0.01 ) {
388 sprintf( cmd, "aircraft-weight-lbs=%.2f", weight );
389 result = write_binary( '1', pd1, cmd, strlen(cmd) );
391 last_weight = weight;
393 double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
394 if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) {
396 sprintf( cmd, "aircraft-cg-offset-inches=%.2f", cg_offset );
397 result = write_binary( '1', pd1, cmd, strlen(cmd) );
399 last_cg_offset = cg_offset;
401 // Send control positions to remote fdm
402 length = sizeof(ctrls);
403 FGProps2NetCtrls( &ctrls, true, false );
405 *((int *)ptr) = iterations;
406 // cout << "iterations = " << iterations << endl;
408 memcpy( ptr, (char *)(&ctrls), length );
409 // cout << "writing control structure, size = "
410 // << length + sizeof(int) << endl;
412 result = write_binary( '2', pd1, buf, length + sizeof(int) );
416 length = sizeof(fdm);
417 // cout << "about to read fdm data from remote fdm." << endl;
418 result = fread( (char *)(& fdm), length, 1, pd2 );
420 SG_LOG( SG_IO, SG_ALERT, "Read error from named pipe: "
421 << fifo_name_2 << " expected 1 item, but got " << result );
423 // cout << " read successful." << endl;
424 FGNetFDM2Props( &fdm, false );
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 );
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] == "ned" ) {
452 double north_fps = atof( tokens[2].c_str() );
453 double east_fps = atof( tokens[3].c_str() );
454 double down_fps = atof( tokens[4].c_str() );
455 cur_fdm_state->_set_Velocities_Local( north_fps, east_fps, down_fps );
456 } else if ( tokens[1] == "alpha" ) {
457 cur_fdm_state->_set_Alpha( atof(tokens[2].c_str()) );
458 } else if ( tokens[1] == "beta" ) {
459 cur_fdm_state->_set_Beta( atof(tokens[2].c_str()) );
462 cur_fdm_state->_set_V_calibrated_kts( net->vcas );
463 cur_fdm_state->_set_Climb_Rate( net->climb_rate );
464 cur_fdm_state->_set_Velocities_Local( net->v_north,
467 cur_fdm_state->_set_Velocities_Wind_Body( net->v_wind_body_north,
468 net->v_wind_body_east,
469 net->v_wind_body_down );
471 cur_fdm_state->_set_Accels_Pilot_Body( net->A_X_pilot,
476 fgSetString( tokens[1].c_str(), tokens[2].c_str() );
481 // Run an iteration of the EOM.
482 void FGExternalPipe::update_property( double dt ) {
483 // cout << "update_property()" << endl;
486 // SG_LOG( SG_IO, SG_INFO, "Start FGExternalPipe::udpate()" );
491 if ( is_suspended() ) {
495 int iterations = _calc_multiloop(dt);
497 double weight = fgGetDouble( "/sim/aircraft-weight-lbs" );
498 static double last_weight = 0.0;
499 if ( fabs( weight - last_weight ) > 0.01 ) {
500 sprintf( cmd, "init aircraft-weight-lbs=%.2f", weight );
501 result = write_property( pd1, cmd );
503 last_weight = weight;
505 double cg_offset = fgGetDouble( "/sim/aircraft-cg-offset-inches" );
506 if ( fabs( cg_offset - last_cg_offset ) > 0.01 ) {
507 sprintf( cmd, "init aircraft-cg-offset-inches=%.2f", cg_offset );
508 result = write_property( pd1, cmd );
510 last_cg_offset = cg_offset;
512 // Send requested property values to fdm
513 for ( unsigned int i = 0; i < nodes.size(); i++ ) {
514 sprintf( cmd, "set %s %s", property_names[i].c_str(),
515 nodes[i]->getStringValue() );
516 // cout << " sending " << cmd << endl;
517 result = write_property( pd1, cmd );
520 sprintf( cmd, "update %d", iterations );
521 write_property( pd1, cmd );
526 // cout << "ready to read fdm response" << endl;
529 if ( fgets( cmd, 256, pd2 ) == NULL ) {
530 cout << "Error reading data" << endl;
532 // cout << " read " << strlen(cmd) << " bytes" << endl;
533 // cout << cmd << endl;
536 // chop trailing newline
537 cmd[strlen(cmd)-1] = '\0';
539 // cout << cmd << endl;
540 string_list tokens = simgear::strutils::split( cmd, " " );
542 if ( tokens[0] == "request" ) {
543 // save the long form name
544 property_names.push_back( tokens[1] );
546 // now do the property name lookup and cache the pointer
547 SGPropertyNode *node = fgGetNode( tokens[1].c_str() );
548 if ( node == NULL ) {
549 // node doesn't exist so create with requested type
550 node = fgGetNode( tokens[1].c_str(), true );
551 if ( tokens[2] == "bool" ) {
552 node->setBoolValue(false);
553 } else if ( tokens[2] == "int" ) {
554 node->setIntValue(0);
555 } else if ( tokens[2] == "double" ) {
556 node->setDoubleValue(0.0);
557 } else if ( tokens[2] == "string" ) {
558 node->setStringValue("");
560 cout << "Unknown data type: " << tokens[2]
561 << " for " << tokens[1] << endl;
564 nodes.push_back( node );
565 } else if ( tokens[0] == "set" ) {
566 process_set_command( tokens );
567 } else if ( tokens[0] == "update" ) {
570 cout << "unknown command = " << cmd << endl;