Generated: Mon Aug 29 19:34:32 2016 from fg_telnet02.pl 2016/06/23 27.9 KB. text copy
#!/usr/bin/perl -w # NAME: fg_telnet02.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. # 23/06/2016 - Revisit... # 20/10/2014 - Revisit and add some 'keys' # 19/04/2011 - Switched to this version 02, which no longer 'runs' the fg # EXE, and removed the ANSI coloring attempt! # # 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 IO::Select; 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 =~ /(\\|\/)/) { my @tmpsp = split(/(\\|\/)/,$pgmname); $pgmname = $tmpsp[-1]; } my $outfile = $perl_sdir."\\temp.$pgmname.txt"; open_log($outfile); my $be_quiet = 0; my $dbg_on = 0; # force DEBUG VERB5 on my $cwd = cwd(); my $VERS = "0.0.4 2016-06-23"; ###my $VERS = "0.0.3 2014-10-20"; my $HOST = "192.168.1.33"; # Win7-PC #my $HOST = "192.168.1.105"; # Dell02 machine #my $HOST = "localhost"; my $PORT = 5557; # 5501; #5555; my $TIMEOUT = 2; # second to wait for a connect. #my $HOST = "localhost"; #my $PORT = 5555; #my $TIMEOUT = 5; # 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 $PI = 3.1415926535897932384626433832795029; my $D2R = $PI / 180; my $R2D = 180 / $PI; my $ERAD = 6378138.12; my $FGFS_IO = undef; my $read_handles; # = IO::Select->new(); $read_handles->add($cfg::listen_socket); my $m_timeout = 50; my $VERBOSITY = 0; sub VERB1() { return ($VERBOSITY >= 9); } sub VERB2() { return ($VERBOSITY >= 9); } sub VERB5() { return ($VERBOSITY >= 9); } sub VERB9() { return ($VERBOSITY >= 9); } my $load_log = 0; my $test_node_list = 0; my $HTZ = 5; my $g_parking_on = 0; 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); my $got_usr_home = 0; my ($g_home_lat,$g_home_lon); my $def_home_lat = -33.949273; my $def_home_lon = 151.181346833333; my $set_dbg_home = 0; sub fatal($) { my $txt = shift; prt($txt); exit 1; } sub pgm_exit($$) { my ($val,$msg) = @_; if (length($msg)) { prt("$msg\n"); } close_log($outfile,$load_log); exit($val); } 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 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_secs($) { my $secs = shift; # = $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 keyboard_help() { prt("Keyboard Help\n"); prt(" ESC = Exit loop\n"); prt(" ? = Repeat of this help.\n"); prt(" B = Toggle parking brake ($g_parking_on)\n"); prt(" a = Show autopilot status\n"); prt(" R = Increase heading bug by 90 degrees\n"); prt(" L = Decrease heading bug by 90 degrees\n"); } sub check_key_sleep($) { my $secs = shift; my ($char,$val,$pmsg); while ($secs > 0) { if ( got_keyboard(\$char) ) { $val = ord($char); $pmsg = sprintf( "%02X", $val ); return 1 if ($val == 27); # ESC key to EXIT if ($char eq '?') { keyboard_help(); } elsif ($char eq 'a') { show_autopilot(); } elsif ($char eq 'B') { if ($g_parking_on) { $val = 0; prt("Set Parking OFF\n"); $g_parking_on = 0; } else { $val = 1; prt("Set Parking ON\n"); $g_parking_on = 1; } set_parking_break($val); } elsif ($char eq 'R') { fgfs_R_hb(); } elsif ($char eq 'L') { fgfs_L_hb(); } elsif ($char eq 'r') { fgfs_r_hb(); } elsif ($char eq 'l') { fgfs_l_hb(); } else { prt("Got unused keyboard input hex[$pmsg]...\n" ); } } usleep(150 * 1000); $secs -= 0.2; } } sub fgfs_get_raw($) { return 0 if (!defined $FGFS_IO); return 0 if (eof $FGFS_IO); my $val = shift; my ($res); #my $rh = IO::Select->new(); #$rh->add($FGFS_IO); #if ( IO::Select->select($read_handles, undef, undef, $m_timeout ) > 0 ) { #if ($rh->can_read(5)) { $res = <$FGFS_IO>; return 0 if ( ! defined $res ); $res =~ s/\015?\012$//; return 0 if (length($res) == 0); $res =~ /^-ERR (.*)/ and (prt("$1") and return 0); ${$val} = $res; return 1; #} #return 0; } # IO::Select->select($read_handles, undef, undef, $cfg::params{'LISTEN_WAIT'} ); sub get_all_nodes() { my ($txt,$cnt,$res,$node,$cmd,$ncnt); fgfs_send("ls"); $txt = ''; $cnt = 0; my @nodes = (); while (fgfs_get_raw(\$txt) && (defined $txt) && length($txt)) { $cnt++; prt("$cnt: $txt\n"); if ($txt =~ /\/$/) { push(@nodes,$txt); } $txt = ''; last if ($cnt == 30); } $ncnt = scalar @nodes; prt("Now the $ncnt nodes...\n"); foreach $node (@nodes) { $node =~ s/\/$//; $cmd = "cd $node"; prt("Doing cmd [$cmd]\n"); fgfs_send($cmd); #if (fgfs_get_raw(\$txt) && (defined $txt) && length($txt)) { # prt("reply: $txt\n"); #} fgfs_send("ls"); while (fgfs_get_raw(\$txt) && (defined $txt) && length($txt)) { $cnt++; prt("$cnt: $txt\n"); push(@nodes,$txt); $txt = ''; last if ($cnt == 30); } } pgm_exit(1,"Temp exit"); } sub get_all_nodes_bad() { my ($txt,$cnt,$res); fgfs_send("ls"); $txt = ''; $cnt = 0; while ($read_handles->can_read(5)) { $res = fgfs_get_raw(\$txt); #while ($res && (defined $txt) && length($txt)) { if ($res && (defined $txt) && length($txt)) { $cnt++; prt("$cnt: $txt\n"); $txt = ''; #$res = fgfs_get_raw(\$txt); } else { last; } } pgm_exit(1,"Temp exit"); } # This is for the beech99-yasim aircraft sub fgfs_get_ap_ah($) { my $ref = shift; fgfs_get("/autopilot/locks/altitude", $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_get_ap_gs($) { my $ref = shift; fgfs_get("/autopilot/locks/glide-slope", $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_get_ap_hh($) { my $ref = shift; fgfs_get("/autopilot/locks/heading", $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_get_ap_nav($) { my $ref = shift; fgfs_get("/autopilot/locks/nav", $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_get_ap_pm($) { my $ref = shift; fgfs_get("/autopilot/locks/passive-mode", $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_get_ap_sh($) { my $ref = shift; fgfs_get("/autopilot/locks/speed", $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_get_ap_wl($) { my $ref = shift; fgfs_get("/autopilot/locks/wing-leveler", $ref) or get_exit(-2); # = true/false return 1; } my $hdg_bug_stg = "/autopilot/settings/heading-bug-deg"; sub fgfs_get_ap_hb($) { my $ref = shift; fgfs_get($hdg_bug_stg, $ref) or get_exit(-2); # = true/false return 1; } sub fgfs_R_hb() { my ($hb,$msg); fgfs_get_ap_hb(\$hb); if (defined $hb) { if ($hb =~ /^\d+$/) { my $nb = $hb + 90; $nb -= 360 if ($nb >= 360); fgfs_set($hdg_bug_stg, $nb); prt("Changed autopilot hdg bug from $hb to $nb\n"); return; } else { $msg = "hb not digits '$hb'"; } } else { $msg = "fgfs_get_ap_hb failed"; } prt("Failed to get or set autopilot hdg bug! $msg\n"); } sub fgfs_r_hb() { my ($hb,$msg); fgfs_get_ap_hb(\$hb); if (defined $hb) { if ($hb =~ /^\d+$/) { my $nb = $hb + 5; $nb -= 360 if ($nb >= 360); fgfs_set($hdg_bug_stg, $nb); prt("Changed autopilot hdg bug from $hb to $nb\n"); return; } else { $msg = "hb not digits '$hb'"; } } else { $msg = "fgfs_get_ap_hb failed"; } prt("Failed to get or set autopilot hdg bug! $msg\n"); } sub fgfs_L_hb() { my ($hb); fgfs_get_ap_hb(\$hb); if (defined $hb) { if ($hb =~ /^\d+$/) { my $nb = $hb - 90; $nb += 360 if ($nb < 0); fgfs_set($hdg_bug_stg, $nb); prt("Changed autopilot hdg bug from $hb to $nb\n"); return; } } prt("Failed to get or set autopilot hdg bug!\n"); } sub fgfs_l_hb() { my ($hb); fgfs_get_ap_hb(\$hb); if (defined $hb) { if ($hb =~ /^\d+$/) { my $nb = $hb - 5; $nb += 360 if ($nb < 0); fgfs_set($hdg_bug_stg, $nb); prt("Changed autopilot hdg bug from $hb to $nb\n"); return; } } prt("Failed to get or set autopilot hdg bug!\n"); } my %m_curr_locks = (); sub get_curr_locks() { return \%m_curr_locks; } sub fgfs_get_ap_locks() { my ($ah,$gs,$hh,$nav,$pm,$sh,$wl,$hb); fgfs_get_ap_ah(\$ah); fgfs_get_ap_gs(\$gs); fgfs_get_ap_hh(\$hh); fgfs_get_ap_nav(\$nav); fgfs_get_ap_pm(\$pm); fgfs_get_ap_sh(\$sh); fgfs_get_ap_wl(\$wl); fgfs_get_ap_hb(\$hb); $ah = 'none' if ((!defined $ah)||(length($ah)==0)); $gs = 'none' if ((!defined $gs)||(length($gs)==0)); $hh = 'none' if ((!defined $hh)||(length($hh)==0)); $nav = 'none' if ((!defined $nav)||(length($nav)==0)); $pm = 'none' if ((!defined $pm)||(length($pm)==0)); $sh = 'none' if ((!defined $sh)||(length($sh)==0)); $wl = 'none' if ((!defined $wl)||(length($wl)==0)); $hb = 'none' if ((!defined $hb)||(length($hb)==0)); my $rh = get_curr_locks(); ${$rh}{'time'} = time(); ${$rh}{'ah'} = $ah; ${$rh}{'gs'} = $gs; ${$rh}{'hh'} = $hh; ${$rh}{'nav'} = $nav; ${$rh}{'pm'} = $pm; ${$rh}{'sh'} = $sh; ${$rh}{'wl'} = $wl; ${$rh}{'hb'} = $hb; return $rh; } sub show_autopilot() { my ($ah,$gs,$hh,$nav,$pm,$sh,$wl,$hb); my $rh = fgfs_get_ap_locks(); $ah = ${$rh}{'ah'}; $gs = ${$rh}{'gs'}; $hh = ${$rh}{'hh'}; $nav = ${$rh}{'nav'}; $pm = ${$rh}{'pm'}; $sh = ${$rh}{'sh'}; $wl = ${$rh}{'wl'}; $hb = ${$rh}{'hb'}; prt("autopilot: "); prt("ah $ah ") if ($ah ne 'none'); prt("gs $gs ") if ($gs ne 'none'); prt("hh $hh ") if ($hh ne 'none'); prt("nav $nav ") if ($nav ne 'none'); prt("pm $pm ") if ($pm ne 'none'); prt("sh $sh ") if ($sh ne 'none'); prt("wl $wl ") if ($wl ne 'none'); prt("hb $hb ") if ($hb ne 'none'); prt("\n"); } 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) if (VERB5()) { prt("[v5] Connecting to $HOST, on $PORT... to $TIMEOUT\n"); } $FGFS_IO = fgfs_connect($HOST, $PORT, $TIMEOUT) || fatal( "\nERROR: Can't open a socket to FG!\n" ); ReadMode('cbreak'); # not sure this is required, or what it does exactly get_all_nodes() if ($test_node_list); fgfs_send("data"); # switch exchange to data mode fgfs_get_coord(\$oldlon, \$oldlat) or return 0; fgfs_get_parking(\$g_parking_on); keyboard_help(); $t0 = [gettimeofday]; # set START TIME ($x, $y, $z) = ll2xyz($oldlon, $oldlat); prt("Initial: Lat=$oldlat, Lon=$oldlon"); prt(", xyz=($x,$y,$z)") if (VERB9()); prt("\n"); $px = $x; $py = $y; $pz = $z; $dtot = 0; $lastilat = 0; $lastilon = 0; # ### FOREVER ### $i = 0; while (1) { # to exit, just EXIT FG should work last if (check_key_sleep($INTERVAL)); #my_sleep_secs($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; if ($got_usr_home) { $first_lat = $g_home_lat; $first_lon = $g_home_lon; } else { $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 #prt("$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); prt("$msg\n" ); $oldlat = $lat; $oldlon = $lon; $last_lat = $lat; $last_lon = $lon; $px = $x; $py = $y; $pz = $z; } } ###prt("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); prt("Closing telnet IO ...\n" ); close $FGFS_IO; undef $FGFS_IO; ReadMode('normal'); # not sure this is required, or what it does exactly } # Blocking => 0 sub fgfs_connect_no_better() { my $host = shift; my $port = shift; my $timeout = (shift || 120); my $socket; my $nonblocking = 1; STDOUT->autoflush(1); print "connect $host, $port, timeout $timeout secs "; while ($timeout--) { if ($socket = IO::Socket::INET->new( Proto => 'tcp', PeerAddr => $host, PeerPort => $port, Blocking => 0 )) { print ".. done.\n"; $socket->autoflush(1); $socket->blocking(0); #ioctl($socket, 0x8004667e, $nonblocking); $read_handles = IO::Select->new(); $read_handles->add($socket); sleep 1; return $socket; } print "."; sleep(1); } return 0; } sub fgfs_connect() { my $host = shift; my $port = shift; my $timeout = (shift || 120); my $socket; my $nonblocking = 1; 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); $socket->blocking(0); ioctl($socket, 0x8004667e, \$nonblocking); 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 (prt("$1") and return 0); return 1; } END { if (defined $FGFS_IO) { prt("$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; } # ### MAIN ### # ============================================ sub main() { $VERBOSITY = 5 if ($dbg_on); parse_args(@ARGV); my $dir = cwd(); prt("Current work directory = $dir\n" ); main_loop(); # main processing loop prt("Returned from main_loop();\n" ); prt("Closing output LOG [$outfile]...\n" ); pgm_exit(0,""); } main; # =============================================== sub set_verbosity { my (@av) = @_; while (@av) { my $arg = $av[0]; if ($arg =~ /^-(v+)$/) { $VERBOSITY += length($1); } elsif ($arg =~ /^-q$/) { $VERBOSITY = 0; $be_quiet = 1; } shift @av; } prt("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 ($arg,$len,$sarg); my $cnt = 0; while (@av) { $arg = $av[0]; $len = length($arg); $cnt++; prt("$cnt: $arg($len)\n" ) if (VERB9()); if (($arg =~ /^-h$/)|| ($arg =~ /^-\?$/)) { give_help(); exit(0); } elsif ($arg =~ /^-(v+)$/) { # done - $VERBOSITY += length($1); } elsif ($arg =~ /^-q$/) { # done $VERBOSITY = 0; } elsif ($arg =~ /^-/) { $sarg = substr($arg,1); $sarg = substr($sarg,1) while (substr($sarg,0,1) eq '-'); if ($sarg =~ /^i/) { chk_arg($arg, @av); shift @av; $sarg = $av[0]; $HOST = $sarg; prt("Set HOST = $HOST...\n") if (VERB1()); } else { prt("Unknown argument! [$arg $sarg] ... -? for help ... aborting ...\n" ); exit(-1); } } else { prt("Unknown argument! [$arg] ... -? for help ... aborting ...\n" ); exit(-1); } shift @av; } if ($set_dbg_home) { if (!$got_usr_home) { $got_usr_home = 1; $g_home_lat = $def_home_lat; $g_home_lon = $def_home_lon; prt("Set default home lat,lon $g_home_lat,$g_home_lon\n"); } } ##print "aborting ...\n"; ##exit(0); } sub give_help() { my $help = <<EOF; Usage: $pgmname $VERSS -h or -? This brief help. -v[vvv...] Add to verbosity level. -q Quiet (verbosity=0). -i <ip> Set HOST IP address. (def=$HOST) -p <port> Set HOST port. (def=$PORT) EOF prt($help); } # eof - fg_telnet02.pl