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