Generated: Sun Aug 21 11:10:58 2011 from fg_telnet.pl 2011/01/02 20.7 KB.
#!/perl -w # NAME: fg_telnet.pl # AIM: To RUN FlightGear, and get/send information to it using TELNET # With much thanks to Franz Melchior for the 'signs' perl script, # on which this is based. # # Note, although Term::ReadKey is used to CHECK for any keyboard input, # and the main_loop() is terminated on an ESC keyboard input, the process # will NOT exit, due to the nature of fork() and exec() as implemented in WIN32 # The secondary process of fork() will WAIT until exec(FG) exits, and at present it appears # sending the command "quit" is ignored by FG - found sending 'run exit' worked ;=)) # 10/12/2010 Review - Updated to FG 2.0 (C:\FG\28) - Showed a/c position... # 13/12/2008 geoff mclane http://geoffair.net/mperl use strict; use warnings; use IO::Socket; use Cwd; use Win32::Console::ANSI; # for WIN32 use Term::ReadKey; use Time::HiRes qw( usleep gettimeofday tv_interval ); my $perl_sdir = 'C:\GTools\perl'; unshift(@INC, $perl_sdir); require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl'! Check location and \@INC content.\n"; require 'fg_wsg84.pl' or die "Unable to load fg_wsg84.pl ...\n"; require "Bucket2.pm" or die "Unable to load Bucket2.pm ...\n"; # log file stuff our ($LF); my $pgmname = $0; if ($pgmname =~ /\w{1}:\\.*/) { my @tmpsp = split(/\\/,$pgmname); $pgmname = $tmpsp[-1]; } my $outfile = $perl_sdir."\\temp.$pgmname.txt"; open_log($outfile); my $dbg_on = 1; # force DEBUG on my $def_fg_root = "C:\\FG\\28\\data"; my $def_fg_rt = 'C:\FG\28\bin'; #my $def_fg_root = "C:\\FG\\27\\data"; #my $def_fg_rt = 'C:\FG\27\bin'; my $def_fg_binary = 'flightgear'; my $cwd = cwd(); my $HOME = $ENV{HOME} || "."; my $FG_HOME = $ENV{FG_HOME} || $HOME . "/.fgfs"; my $FG_ROOT = $ENV{FG_ROOT} || $def_fg_root; my $BASEDIR = "$FG_ROOT/Local/signs"; my $FGFS = $def_fg_binary; my $HOST = "localhost"; my $PORT = 5500; my $TIMEOUT = 120; # second to wait for a connect. my $INTERVAL = 1; # get postion EACH second my $USECOLOR = 1; my $MIN_CHANGE = 0.00001; my $SG_EPSILON = 0.0000001; my $CLR_ERR = "41m\033[33;1"; my $CLR_WARN = "47m\033[31;1"; my $CLR_INFO = "32"; my $CLR_BULK = ""; my $CLR_DEBUG = "36;1"; # $ERR $WARN $INFO $BULK $DEBUG my @COLOR = ( $CLR_ERR, $CLR_WARN, $CLR_INFO, $CLR_BULK, $CLR_DEBUG ); my $PI = 3.1415926535897932384626433832795029; my $D2R = $PI / 180; my $R2D = 180 / $PI; my $ERAD = 6378138.12; my $FGFS_IO; my $ERR = 0; my $WARN = 1; my $INFO = 2; my $BULK = 3; my $DEBUG = 4; my $VERBOSITY = $INFO; my $help = <<EOF; Usage: $pgmnamee -h or -? This brief help. -v[vvv...] Add to verbosity level. -q Quiet (verbosity=0). -rt=path Runtime folder. (Def=$def_fg_rt). NOTE: Will change to this directory to run FG! -binary=name FG binary EXE. (Def=$def_fg_binary). -root=path Set FG root. (Def=$def_fg_root). --add-fg-params Ends internal commands, and this, plus any following, will be passed to FG. EOF my $HTZ = 5; my $DAT_FILE = 'C:\FG\27\bin\records\temp-tn.txt'; my $g_parking_on = 0; # command arguments to FG my @fgfsargs = ( "--fg-root=$FG_ROOT", # "--aircraft=c182rg", "--aircraft=pa24-250", "--fdm=yasim", # "--atlas=socket,out,5,localhost,5500,udp", "--generic=file,out,$HTZ,$DAT_FILE,playback", "--fg-scenery=C:/FG/Scenery-1.0.1", "--airport=YGIL", "--timeofday=noon", "--disable-random-objects", "--disable-ai-models", "--fog-disable", "--disable-real-weather-fetch" ); my @fgfsargs2 = ( "--fg-root=$FG_ROOT", "--aircraft=ufo", "--fdm=ufo", "--prop:/sim/rendering/fps-display=true", "--timeofday=noon", "--disable-random-objects", "--disable-ai-models", "--fog-disable", "--disable-real-weather-fetch", "--altitude=1000", "--lon=-122.33276046", "--lat=37.60364931", "--heading=297" ); my $t0 = [gettimeofday]; my $done_ll_chg = 0; my $g_total_dist = 0; my ($first_lat,$first_lon); my ($last_lat,$last_lon); my $g_curr_heading = 0; my ($g_sg_az1,$g_sg_az2,$g_sg_dist); sub get_playback_nodes() { my @ppns = qw( /accelerations/ned/east-accel-fps_sec /accelerations/ned/north-accel-fps_sec /accelerations/nlf /accelerations/pilot/x-accel-fps_sec /accelerations/pilot/y-accel-fps_sec /accelerations/pilot/z-accel-fps_sec /controls/autoflight/altitude-select /controls/autoflight/autopilot[0]/engage /controls/autoflight/bank-angle-select /controls/autoflight/heading-select /controls/autoflight/speed-select /controls/autoflight/vertical-speed-select /controls/electric/APU-generator /controls/electric/battery-switch /controls/electric/external-power /controls/engines/engine[0]/cutoff /controls/engines/engine[0]/fuel-pump /controls/engines/engine[0]/ignition /controls/engines/engine[0]/magnetos /controls/engines/engine[0]/mixture /controls/engines/engine[0]/propeller-pitch /controls/engines/engine[0]/starter /controls/engines/engine[0]/throttle /controls/engines/engine[1]/cutoff /controls/engines/engine[1]/fuel-pump /controls/engines/engine[1]/ignition /controls/engines/engine[1]/magnetos /controls/engines/engine[1]/mixture /controls/engines/engine[1]/propeller-pitch /controls/engines/engine[1]/starter /controls/engines/engine[1]/throttle /controls/flight/aileron-trim /controls/flight/aileron[0] /controls/flight/elevator /controls/flight/elevator-trim /controls/flight/flaps /controls/flight/rudder /controls/flight/rudder-trim /controls/flight/slats /controls/flight/speedbrake /controls/gear/brake-left /controls/gear/brake-parking /controls/gear/brake-right /controls/gear/gear-down /controls/gear/steering /controls/hydraulic/system[0]/electric-pump /controls/hydraulic/system[0]/engine-pump /controls/hydraulic/system[1]/electric-pump /controls/hydraulic/system[1]/engine-pump /gear/gear/position-norm /gear/gear[1]/position-norm /gear/gear[2]/position-norm /gear/gear[3]/position-norm /gear/gear[4]/position-norm /orientation/heading-deg /orientation/pitch-deg /orientation/roll-deg /orientation/side-slip-deg /position/altitude-ft /position/latitude-deg /position/longitude-deg /surface-positions/elevator-pos-norm[0] /surface-positions/flap-pos-norm[0] /surface-positions/left-aileron-pos-norm[0] /surface-positions/right-aileron-pos-norm[0] /surface-positions/rudder-pos-norm[0] /velocities/airspeed-kt /velocities/glideslope /velocities/mach /velocities/speed-down-fps /velocities/speed-east-fps /velocities/speed-north-fps /velocities/uBody-fps /velocities/vBody-fps /velocities/vertical-speed-fps /velocities/wBody-fps ); return \@ppns; } sub set_verbosity { my (@av) = @_; while (@av) { my $arg = $av[0]; if ($arg =~ /^-(v+)$/) { $VERBOSITY += length($1); } elsif ($arg =~ /^-q$/) { $VERBOSITY = 0; } shift @av; } mylog( $DEBUG, "Verbosity set to $VERBOSITY\n" ); } sub chk_arg { my ($arg, @av) = @_; fatal( "Invalid $arg - needs value ... -? for help ... aborting!\n" ) if !(@av); } sub parse_args { my (@av) = @_; set_verbosity(@av); # parse only for verbosity my $cnt = 0; while (@av) { my $arg = $av[0]; my $len = length($arg); $cnt++; mylog( $DEBUG, "$cnt: $arg($len)\n" ); last if (($len > 2)&&(substr($arg,0,2) eq '--')); # assume this is a FG argument if (($arg =~ /^-h$/)|| ($arg =~ /^-\?$/)) { print $help; if ($VERBOSITY > $WARN) { print "Arguments to FG are :\n"; print join("\n", @fgfsargs); } print "\n"; exit(0); } elsif ($arg =~ /^-(v+)$/) { # done - $VERBOSITY += length($1); } elsif ($arg =~ /^-q$/) { # done $VERBOSITY = 0; } elsif ($arg =~ /^-rt=(.+)$/) { $def_fg_rt = $1; mylog( $DEBUG, "Runtime folder to [$def_fg_rt].\n" ); } elsif ($arg =~ /^-rt$/) { chk_arg(@av); shift @av; $arg = $av[0]; $def_fg_rt = $arg; mylog( $DEBUG, "Runtime folder to [$def_fg_rt].\n" ); } elsif ($arg =~ /^-binary=(.+)$/) { $def_fg_binary = $1; mylog( $DEBUG, "FG binary EXE to [$def_fg_binary].\n" ); } elsif ($arg =~ /^-binary$/) { chk_arg(@av); shift @av; $arg = $av[0]; $def_fg_binary = $arg; mylog( $DEBUG, "FG binary EXE to [$def_fg_binary].\n" ); } elsif ($arg =~ /^-root=(.+)$/) { $def_fg_root = $1; mylog( $DEBUG, "FG root to [$def_fg_root].\n" ); } elsif ($arg =~ /^-root$/) { chk_arg(@av); shift @av; $arg = $av[0]; $def_fg_root = $arg; mylog( $DEBUG, "FG root to [$def_fg_root].\n" ); } else { mylog( $ERR, "Unknown argument! [$arg] ... -? for help ... aborting ...\n" ); exit(-1); } shift @av; } if (@av) { mylog( $DEBUG, "Adding ".join(" ",@av)." to exec commands\n" ); push(@fgfsargs, @av); } ##print "aborting ...\n"; ##exit(0); } sub main() { $VERBOSITY = $DEBUG if ($dbg_on); parse_args(@ARGV); if (! chdir($def_fg_rt) ) { fatal( "ERROR: Unable to change to $def_fg_rt ...\n" ); } my $dir = cwd(); mylog( $BULK, "Current work directory = $dir\n" ); if (my $pid = fork) { main_loop(); # main processing loop mylog( $DEBUG, "Returned from main_loop();\n" ); } else { defined $pid or fatal("cannot fork: $!"); #exec("$FGFS --telnet=$PORT --config=$BASEDIR/signs.xml @fgfsargs"); exec("$FGFS --telnet=$PORT @fgfsargs") or mylog( $WARN, "Exec FAILED ...\n" ); } mylog( $DEBUG, "Closing output LOG ...\n" ); close_log($outfile,0); mylog( $DEBUG, "Exit, returning zero ...\n" ); exit(0); } main; sub got_keyboard { my ($rc) = shift; if (defined (my $char = ReadKey(-1)) ) { # input was waiting and it was $char $$rc = $char; return 1; } return 0; } sub my_sleep($) { my $secs = @_; # = $INTERVAL sleep $secs; # sampling interval } sub secs2minsecs($) { my ($secs) = @_; my $mins = int($secs / 60); $secs = $secs - ($mins * 60); $mins = "0$mins" if ($mins < 10); $secs = sprintf("%02.2f", $secs); # $secs = (int($secs * 100) / 100); $secs = "0$secs" if ($secs < 10); return "$mins:$secs"; } sub get_decimal_stg($$$) { my ($dec,$il,$dl) = @_; my (@arr); if ($dec =~ /\./) { @arr = split(/\./,$dec); if (scalar @arr == 2) { $arr[0] = " ".$arr[0] while (length($arr[0]) < $il); $dec = $arr[0]; if ($dl > 0) { $dec .= "."; $arr[1] = substr($arr[1],0,$dl) if (length($arr[1]) > $dl); $dec .= $arr[1]; } } } else { $dec = " $dec" while (length($dec) < $il); if ($dl) { $dec .= "."; while ($dl--) { $dec .= "0"; } } } return $dec; } sub get_heading_stg($) { my ($hdg) = @_; return get_decimal_stg($hdg,3,1); } sub get_sg_dist_stg($) { my ($sg_dist) = @_; my $sg_km = $sg_dist / 1000; my $sg_im = int($sg_dist); my $sg_ikm = int($sg_km + 0.5); # if (abs($sg_pdist) < $CP_EPSILON) my $sg_dist_stg = ""; if (abs($sg_km) > $SG_EPSILON) { # = 0.0000001; # EQUALS SG_EPSILON 20101121 if ($sg_ikm && ($sg_km >= 1)) { $sg_km = int(($sg_km * 10) + 0.05) / 10; $sg_dist_stg .= get_decimal_stg($sg_km,5,1)." km"; } else { #$sg_dist_stg .= "$sg_im m, <1km"; $sg_dist_stg .= get_decimal_stg($sg_im,7,0)." m."; } } else { #$sg_dist_stg .= "0 m"; $sg_dist_stg .= get_decimal_stg('0',7,0)." m."; } return $sg_dist_stg; } sub get_dist_stg($$) { my ($dlat,$dlon) = @_; my ($sg_az1,$sg_az2,$sg_dist); my $res = fg_geo_inverse_wgs_84 ($first_lat,$first_lon,$dlat,$dlon,\$g_sg_az1,\$g_sg_az2,\$g_sg_dist); $res = fg_geo_inverse_wgs_84 ($last_lat,$last_lon,$dlat,$dlon,\$sg_az1,\$sg_az2,\$sg_dist); $g_total_dist += $sg_dist; $g_sg_az1 = int(($g_sg_az1 * 10) + 0.05) / 10; $g_sg_az2 = $g_sg_az1 + 180; $g_sg_az2 -= 360 if ($g_sg_az2 >= 360); # $g_sg_az2 = int(($g_sg_az2 * 10) + 0.05) / 10; my $sg_km = $sg_dist / 1000; my $sg_im = int($sg_dist); my $sg_ikm = int($sg_km + 0.5); # if (abs($sg_pdist) < $CP_EPSILON) my $dist_hdg = " "; #my $dist_hdg = " (SGD: "; $sg_az1 = int(($sg_az1 * 10) + 0.05) / 10; $g_curr_heading = $sg_az1; # only keep to first decimal place if (abs($sg_km) > $SG_EPSILON) { # = 0.0000001; # EQUALS SG_EPSILON 20101121 if ($sg_ikm && ($sg_km >= 1)) { $sg_km = int(($sg_km * 10) + 0.05) / 10; $dist_hdg .= "$sg_km km"; } else { $dist_hdg .= "$sg_im m, <1km"; } } else { $dist_hdg .= "0 m"; } $dist_hdg .= " on $sg_az1"; # $dist_hdg .= ")"; return $dist_hdg; } sub main_loop() { my ($x, $y, $z); my ($px, $py, $pz); my ($lon, $lat, $i, $i2); my ($oldlon, $oldlat); my ($dist, $dtot); my ($alt, $agl, $hdg); # alititude and heading my ($pmsg, $amsg); my ($char, $val); my ($elap, $ms, $sg_dist, $msg); my ($ilat,$ilon,$lastilat,$lastilon); # If it takes a WHILE for FG to start, use greater than 2 minutes (120 seconds) $FGFS_IO = fgfs_connect($HOST, $PORT, $TIMEOUT) || die " can't open socket\n"; ReadMode('cbreak'); # not sure this is required, or what it does exactly fgfs_send("data"); # switch exchange to data mode fgfs_get_coord(\$oldlon, \$oldlat) or return 0; fgfs_get_parking(\$g_parking_on); $t0 = [gettimeofday]; # set START TIME ($x, $y, $z) = ll2xyz($oldlon, $oldlat); mylog( $INFO, "Initial: Lat=$oldlat, Lon=$oldlon, xyz=($x,$y,$z)\n" ); $px = $x; $py = $y; $pz = $z; $dtot = 0; $lastilat = 0; $lastilon = 0; # ### FOREVER ### for ($i = 0;; $i++) { # to exit, just EXIT FG should work my_sleep($INTERVAL); fgfs_get_coord(\$lon, \$lat) or last; $i2 = $i + 1; if ((abs($oldlat - $lat) > $MIN_CHANGE)|| (abs($oldlon - $lon) > $MIN_CHANGE)) { fgfs_get_altitude( \$alt ); fgfs_get_agl( \$agl ); fgfs_get_heading( \$hdg ); if (!$done_ll_chg) { $done_ll_chg = 1; $first_lat = $lat; $first_lon = $lon; $last_lat = $lat; $last_lon = $lon; } #($x, $y, $z) = ll2xyz($lon, $lat); #$dist = sqrt( coord_dist_sq( $px, $py, $pz, $x, $y, $z ) ) * 1000; # Km??? maybe??? #$dtot += $dist; $elap = tv_interval ( $t0, [gettimeofday]); $ms = secs2minsecs($elap); $sg_dist = get_dist_stg($lat,$lon); # distance from last lat, lon, accumulated #mylog( $BULK, "$i2: Lat=$lat, Lon=$lon, xyz=($x,$y,$z) d=$dist, t=$dtot\n" ); #$dmsg = sprintf( "d=%0.6f, t=%0.6f", $dist, $dtot ); $ilat = int((abs($lat)+0.0005) * 1000); $ilon = int((abs($lon)+0.0005) * 1000); if (($ilat == $lastilat)&&($ilon == $lastilon)) { $pmsg = " , "; } else { $pmsg = sprintf( "%3.4f,%4.4f", $lat, $lon ); } $lastilat = $ilat; $lastilon = $ilon; #$pmsg = sprintf( "lat=%0.8f, lon=%0.8f", $lat, $lon ); #$amsg = sprintf( "alt=%5d agl=%5d ft", int($alt + 0.5), int($agl + 0.5)); #$amsg = sprintf( "alt=%5d/%5d ft", int($alt + 0.5), int($agl + 0.5)); $amsg = sprintf( "%5d/%5d ft", int($alt + 0.5), int($agl + 0.5)); $msg = "$ms: $pmsg, $amsg"; # $msg = "$ms: $pmsg, $dmsg, alt=".int($alt + 0.5).", agl=".int($agl + 0.5)." ft"; # $msg .= " $sg_dist, total ".get_sg_dist_stg($g_total_dist); $msg .= " ".get_heading_stg($g_curr_heading); $msg .= " ".get_heading_stg($hdg); $msg .= ", home ".get_heading_stg($g_sg_az2)." at ".get_sg_dist_stg($g_sg_dist); $msg .= ", tot ".get_sg_dist_stg($g_total_dist); mylog( $INFO, "$msg\n" ); $oldlat = $lat; $oldlon = $lon; $last_lat = $lat; $last_lon = $lon; $px = $x; $py = $y; $pz = $z; } if ( got_keyboard(\$char) ) { $val = ord($char); $pmsg = sprintf( "%02X", $val ); mylog( $WARN, "Got keyboard input hex[$pmsg]...\n" ); last if ($val == 27); # ESC key to EXIT if ($char eq 'B') { if ($g_parking_on) { $val = 0; mylog($WARN,"Set Parking OFF\n"); $g_parking_on = 0; } else { $val = 1; mylog($WARN,"Set Parking ON\n"); $g_parking_on = 1; } set_parking_break($val); } } } mylog( $DEBUG, "Sending 'quit' to FG ...\n" ); #fgfs_send("quit"); # this ONLY closes the interface #fgfs_send("\033"); # try an ESC key, did nothing fgfs_send("run exit"); # YAHOO! THAT WORKED!!! PHEW!!! sleep(5); mylog( $DEBUG, "Closing telnet IO ...\n" ); close $FGFS_IO; undef $FGFS_IO; ReadMode('normal'); # not sure this is required, or what it does exactly } sub fgfs_connect() { my $host = shift; my $port = shift; my $timeout = (shift || 120); my $socket; STDOUT->autoflush(1); print "connect $host, $port, timeout $timeout secs "; while ($timeout--) { if ($socket = IO::Socket::INET->new( Proto => 'tcp', PeerAddr => $host, PeerPort => $port)) { print ".. done.\n"; $socket->autoflush(1); sleep 1; return $socket; } print "."; sleep(1); } return 0; } sub fgfs_send { print $FGFS_IO shift, "\015\012"; } sub fgfs_get_coord($$) { my $lon = shift; my $lat = shift; fgfs_get("/position/longitude-deg", $lon) or exit -2; fgfs_get("/position/latitude-deg", $lat) or exit -2; return 1; } sub fgfs_get_altitude($) { my $ref_alt = shift; fgfs_get("/position/altitude-ft", $ref_alt) or exit -2; return 1; } sub fgfs_get_heading($) { my $ref_hdg = shift; fgfs_get("/orientation/heading-deg", $ref_hdg) or exit -2; return 1; } sub fgfs_get_agl($) { my $ref_alt = shift; fgfs_get("/position/altitude-agl-ft", $ref_alt) or exit -2; return 1; } sub get_parking_node() { my $node = "/controls/gear/brake-parking"; return $node; } sub fgfs_get_parking($) { my $ref_alt = shift; fgfs_get(get_parking_node(),$ref_alt) or exit -2; # fgfs_get("/controls/gear/brake-parking", $ref_alt) or exit -2; return 1; } sub fgfs_set($$) { my ($node,$val) = @_; fgfs_send("set $node $val"); return 1; } sub set_parking_break($) { my $val = shift; fgfs_set(get_parking_node(), $val); } sub fgfs_get() { fgfs_send("get " . shift); eof $FGFS_IO and return 0; my $val = shift; $$val = <$FGFS_IO>; $$val =~ s/\015?\012$//; $$val =~ /^-ERR (.*)/ and (mylog($WARN, "$1") and return 0); return 1; } END { if (defined $FGFS_IO) { mylog( $WARN, "$pgmname: Ending ...\n\n" ); fgfs_send("run exit"); close $FGFS_IO; undef $FGFS_IO; } print "\n"; } sub ll2xyz($$) { my $lon = (shift) * $D2R; my $lat = (shift) * $D2R; my $cosphi = cos $lat; my $di = $cosphi * cos $lon; my $dj = $cosphi * sin $lon; my $dk = sin $lat; return ($di, $dj, $dk); } sub xyz2ll($$$) { my ($di, $dj, $dk) = @_; my $aux = $di * $di + $dj * $dj; my $lat = atan2($dk, sqrt $aux) * $R2D; my $lon = atan2($dj, $di) * $R2D; return ($lon, $lat); } sub coord_dist_sq($$$$$$) { my ($xa, $ya, $za, $xb, $yb, $zb) = @_; my $x = $xb - $xa; my $y = $yb - $ya; my $z = $zb - $za; return $x * $x + $y * $y + $z * $z; } sub fatal { mylog($ERR, "$pgmname: @_"); exit -1; } sub mylog { my ($v) = shift; my $txt = "@_"; return if $v > $VERBOSITY; $v = 4 if $v > 4; my $msg = ''; $msg .= "\033[$COLOR[$v]m" if $USECOLOR; $msg .= "$txt"; $msg .= "\033[m" if $USECOLOR; if (is_ulog_open()) { print $LF $txt; } print $msg; #prt( "$msg" ); } # eof - fg_telnet.pl