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