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