Generated: Sun Apr 15 11:46:11 2012 from fg_f14b.pl 2011/09/30 14.6 KB.
#!/usr/bin/perl -w # NAME: fg_f14b.pl # AIM: Add auto pilot to f14b - first attempt # 24/09/2011 geoff mclane http://geoffair.net/mperl use strict; use warnings; use File::Basename; # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] ) use Cwd; use Term::ReadKey; use Time::HiRes qw( usleep gettimeofday tv_interval ); use GeoCoord; # ================================== my $perl_dir = 'C:\GTools\perl'; unshift(@INC, $perl_dir); require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl' Check paths in \@INC...\n"; require 'lib_fgio.pl' or die "Unable to load 'lib_fgio.pl'! Chech paths in \@INC...\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_dir."\\temp.$pgmname.txt"; open_log($outfile); # user variables my $VERS = "0.0.1 2011-09-26"; my $load_log = 0; my $in_file = ''; my $verbosity = 0; my $debug_on = 0; my $def_file = 'def_file'; # constants my $MPS2KNOT = 1.9438444924406; # Knots # defaults my $HOST = "192.168.1.105"; # Dell02 machine #my $HOST = "localhost"; my $PORT = 5555; my $TIMEOUT = 2; # second to wait for a connect. my $auto_state = "/auto/state"; my $uas_state = "/uas/state"; my $last_state = ""; my $last_time = time(); ### program variables my @warnings = (); my $cwd = cwd(); my $os = $^O; sub VERB1() { return $verbosity >= 1; } sub VERB2() { return $verbosity >= 2; } sub VERB5() { return $verbosity >= 5; } sub VERB9() { return $verbosity >= 9; } sub show_warnings($) { my ($val) = @_; if (@warnings) { prt( "\nGot ".scalar @warnings." WARNINGS...\n" ); foreach my $itm (@warnings) { prt("$itm\n"); } prt("\n"); } else { ### prt( "\nNo warnings issued.\n\n" ); } } sub pgm_exit($$) { my ($val,$msg) = @_; fgfs_disconnect(); if (length($msg)) { $msg .= "\n" if (!($msg =~ /\n$/)); prt($msg); } show_warnings($val); close_log($outfile,$load_log); exit($val); } sub prtw($) { my ($tx) = shift; $tx =~ s/\n$//; prt("$tx\n"); push(@warnings,$tx); } sub prtt($) { my $txt = shift; prt(lu_get_hhmmss_UTC(time()).": $txt"); } sub local_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 key_help() { prt("Keyboard HELP\n"); prt(" ESC = Exit main loop.\n"); prt(" ? = This help output.\n"); prt(" a = Output A/P Locks.\n"); prt(" e = Show engines.\n"); prt(" g = Show gear.\n"); prt(" i = Get all info.\n"); } sub show_aplocks() { show_ap_locks(fgfs_get_ap_locks()); } my $last_pos_time = 0; my $delay_secs = 5 - 1; # each 5 seconds sub show_position() { my $rlp = get_curr_posit(); if (!defined ${$rlp}{'time'}) { $rlp = fgfs_get_position(); return if (!defined ${$rlp}{'time'}); } my $tm = time(); if ($tm > $last_pos_time) { $last_pos_time = $tm + $delay_secs; # get previous values my ($llat,$llon); $llon = ${$rlp}{'lon'}; $llat = ${$rlp}{'lat'}; my $tb = ${$rlp}{'gettimeofday'}; # get NEW values my $rp = fgfs_get_position(); my $tnow = [gettimeofday]; my $elap_secs = tv_interval( $tb, $tnow ); my $ctm = lu_get_hhmmss_UTC($tm); my ($lon,$lat,$alt,$hdg,$agl,$hb,$mag,$aspd,$gspd,$cpos); $lon = ${$rp}{'lon'}; $lat = ${$rp}{'lat'}; $alt = ${$rp}{'alt'}; $hdg = ${$rp}{'hdg'}; $agl = ${$rp}{'agl'}; $hb = ${$rp}{'bug'}; $mag = ${$rp}{'mag'}; $aspd = ${$rp}{'aspd'}; # Knots $gspd = ${$rp}{'gspd'}; # Knots my ($az1,$az2,$dist,$mps,$speed,$secs); # calculate distance travelled in meters since last fg_geo_inverse_wgs_84 ($llat,$llon,$lat,$lon,\$az1,\$az2,\$dist); $mps = 0; $speed = 0; if ($elap_secs > 0) { $mps = ($dist / $elap_secs); $speed = $mps * $MPS2KNOT; } # note values DESTROYED for display # ================================= set_int_stg(\$alt); set_lat_stg(\$lat); set_lon_stg(\$lon); set_int_stg(\$speed); set_int_stg(\$aspd); set_int_stg(\$gspd); $elap_secs = (int(($elap_secs * 100)+0.5) / 100); $secs = sprintf("%4.2f",$elap_secs); $speed = sprintf("%4d",$speed); $cpos = "$lat,$lon,$alt $speed $secs $aspd $gspd"; prtt("$cpos\n"); if (${$rp}{'gps-update'} == 1) { # maybe in display, show difference, if ANY... my $rg = get_curr_gps(); if (defined ${$rg}{'time'}) { ${$rp}{'gps-update'} = 0; my $gctm = lu_get_hhmmss_UTC(${$rg}{'time'}); my $glon = ${$rg}{'lon'}; my $glat = ${$rg}{'lat'}; my $galt = ${$rg}{'alt'}; my $ghdg = ${$rg}{'hdg'}; # $agl = ${$rp}{'agl'}; # $hb = ${$rp}{'bug'}; my $gmag = ${$rg}{'mag'}; #fgfs_get_mag_var(\$mag_variation); # display mess set_lat_stg(\$glat); set_lon_stg(\$glon); set_hdg_stg(\$ghdg); set_hdg_stg(\$gmag); set_int_stg(\$galt); prt("$gctm: GPS $glat,$glon,$galt ft, hdg=${ghdg}T/${gmag}M \n"); #show_environ(fgfs_get_environ()); } } } } my ($m_curr_lon,$m_curr_lat,$m_curr_alt,$m_curr_hdg,$m_curr_tim); sub set_init_position($$$$) { my ($lon,$lat,$alt,$hdg) = @_; $m_curr_lon = $lon; $m_curr_lat = $lat; $m_curr_alt = $alt; $m_curr_hdg = $hdg; $m_curr_tim = time(); } sub check_auto_mode() { #my $state = getprop($uas_state); my $state = getprop($auto_state); if (!defined $state || (length($state)==0)) { return; } my ($lon,$lat,$alt,$hdg); if ($state ne $last_state) { $last_state = $state; if ($state eq 'ready') { # clear any locks setprop("/autopilot/locks/heading",""); setprop("/autopilot/locks/altitude",""); setprop("/autopilot/locks/speed",""); setprop("/controls/gear/brake-parking",1); setprop("/controls/engines/engine[0]/throttle",0.0); setprop("/controls/engines/engine[1]/throttle",0.0); setprop("/controls/engines/engine[2]/throttle",0.0); setprop("/controls/engines/engine[3]/throttle",0.0); setprop("/controls/flight/elevator",0.0); setprop("/controls/flight/elevator-trim",0.0); } elsif ($state eq "init") { prt("Initialise autoflight system\n"); setprop("/controls/gear/brake-parking",1); my $rc = fgfs_get_position(); $lon = ${$rc}{'lon'}; $lat = ${$rc}{'lat'}; $alt = ${$rc}{'alt'}; $hdg = ${$rc}{'hdg'}; #$hdg = getprop("/orientation/heading-deg"); set_init_position($lon,$lat,$alt,$hdg); } elsif ($state eq "init-settle") { my $talt = setprop("/autopilot/settings/target-bank-deg",0); setprop("/autopilot/locks/heading","bank-hold"); setprop("/autopilot/locks/altitude","altitude-hold"); } elsif ($state eq "pretakoff") { } elsif ($state eq "climbout") { } elsif ($state eq "circle") { my $hxDP=getprop("/uas/flight-altitude-ft"); my $talt=getprop("/autopilot/settings/target-altitude-ft"); $lon = getprop("/sim/input/click/longitude-deg"); $lat = getprop("/sim/input/click/latitude-deg"); $alt = getprop("/sim/input/click/elevation-ft"); #var OTy8=geo.aircraft_position(); #var b9oZ=OTy8.course_to(Iqvt); #var RwYr=OTy8.distance_to(Iqvt); #var WJDx=b9oZ+90; #if(WJDx>360.0){WJDx-=360.0;} #var AATK=(weRf*weRf)/(11.23*math.tan(0.01745*XJtG)); #var yOmS=AATK*FT2M; #var jRnP=WJDx; #if(RwYr<yOmS){ #var vjvt=90*(1.0-RwYr/yOmS); #jRnP+=vjvt; #if(jRnP>360.0){ jRnP-=360.0; } #}elsif(RwYr>yOmS){ #var Cclx=RwYr-yOmS; #if(Cclx>yOmS){ Cclx=yOmS; } #var vjvt=90*Cclx/yOmS; #jRnP-=vjvt; #if(jRnP<0.0){ jRnP+=360.0;} #} my $f8Ep=getprop("/instrumentation/airspeed-indicator/true-speed-kt"); #var Qxt9=VZeb(jRnP,f8Ep,0); my $head=getprop("/autopilot/settings/true-heading-deg"); #,Qxt9.heading); my $tspd=getprop("/autopilot/settings/target-speed-kt"); #,weRf); #lOnA(weRf,5); my $zhU7=getprop("/velocities/airspeed-kt"); #if(zhU7<=GQOf){ # setprop("/controls/flight/flaps",1); # setprop("/controls/flight/flapscommand",1); #}else if(zhU7>GQOf+10){ # setprop("/controls/flight/flaps",0); # setprop("/controls/flight/flapscommand",0); #} prt("NEW $state: $hxDP $talt $lon $lat $alt $f8Ep $head $tspd $zhU7\n"); } elsif ($state eq "gohome") { } elsif ($state eq "downwind") { } elsif ($state eq "base") { } elsif ($state eq "final") { } elsif ($state eq "flare") { } elsif ($state eq "touchdown") { } } else { my $tm = time(); if ($tm > ($last_time + 4)) { if ($state eq 'circle') { #my $hxDP=getprop("/uas/flight-altitude-ft"); #my $talt=getprop("/autopilot/settings/target-altitude-ft"); $lon = getprop("/sim/input/click/longitude-deg"); $lat = getprop("/sim/input/click/latitude-deg"); $alt = getprop("/sim/input/click/elevation-ft"); my $f8Ep=getprop("/instrumentation/airspeed-indicator/true-speed-kt"); #var Qxt9=VZeb(jRnP,f8Ep,0); my $head=getprop("/autopilot/settings/true-heading-deg"); #,Qxt9.heading); my $tspd=getprop("/autopilot/settings/target-speed-kt"); #,weRf); #lOnA(weRf,5); my $zhU7=getprop("/velocities/airspeed-kt"); #prt("$state: $hxDP $talt $lon $lat $alt $f8Ep $head $tspd\n"); prt("$state: $head $lon $lat $alt $f8Ep $tspd\n"); } $last_time = $tm; } } } # ==================================================== sub main_loop() { my $ok = 1; my $char = ''; prtt("Entering MAIN loop...\n"); my ($val,$pmsg); # FOREVER, until ESC = exit while ($ok) { if ( got_keyboard(\$char) ) { $val = ord($char); $pmsg = sprintf( "%02X", $val ); if ($val == 27) { prt("ESC key... Exiting...\n"); $ok = 0; last; } elsif ($char eq '?') { key_help(); } elsif ($char eq 'a') { show_aplocks(); } elsif ($char eq 'e') { show_engines(); } elsif ($char eq 'g') { fgfs_show_gear_wow(); } elsif ($char eq 'i') { get_all_info(); } else { prtt("Unused keyboard $val\n"); } } show_position(); check_auto_mode(); } prtt("Exit MAIN loop...\n"); } # ==================================================== sub get_all_info() { prtt("Get 'sim' information...\n"); show_sim_info(fgfs_get_sim_info()); prtt("Get Fuel - comsumables...\n"); show_consumables(fgfs_get_consumables()); prtt("Getting current environment...\n"); show_environ(fgfs_get_environ()); prtt("Getting current COMS...\n"); show_comms(fgfs_get_comms()); prtt("Getting current autopilot settings...\n"); show_autopilot_settings(fgfs_get_aps()); #my ($alt); #fgfs_get_aps_targ_alt(\$alt); #prt("Target altitude = $alt"); #fgfs_get("/autopilot/settings/target-altitude-ft",\$alt); #prt(" $alt\n"); show_aplocks(); show_engines(); fgfs_show_gear_wow(); } sub init() { fgfs_connect($HOST, $PORT, $TIMEOUT) || pgm_exit(1,"ERROR: can't open socket! Is FG running, with TELNET enabled?\n"); ReadMode('cbreak'); # not sure this is required, or what it does exactly fgfs_send("data"); # switch exchange to data mode set_wait_ms_get(-1); #setprop($auto_state,"ready"); # this will this be created get_all_info(); } ######################################### ### MAIN ### parse_args(@ARGV); init(); main_loop(); pgm_exit(0,""); ######################################## sub give_help { prt("$pgmname: version $VERS\n"); prt("Usage: $pgmname [options] in-file\n"); prt("Options:\n"); prt(" --help (-h or -?) = This help, and exit 0.\n"); prt(" --verb[n] (-v) = Bump [or set] verbosity. def=$verbosity\n"); } sub need_arg { my ($arg,@av) = @_; pgm_exit(1,"ERROR: [$arg] must have a following argument!\n") if (!@av); } sub parse_args { my (@av) = @_; my ($arg,$sarg); while (@av) { $arg = $av[0]; if ($arg =~ /^-/) { $sarg = substr($arg,1); $sarg = substr($sarg,1) while ($sarg =~ /^-/); if (($sarg =~ /^h/i)||($sarg eq '?')) { give_help(); pgm_exit(0,"Help exit(0)"); } elsif ($sarg =~ /v/) { if ($sarg =~ /v.*(\d+)$/) { $verbosity = $1; } else { while ($sarg =~ /v/) { $verbosity++; $sarg = substr($sarg,1); } } prt("Verbosity = $verbosity\n") if (VERB1()); } else { pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n"); } } else { $in_file = $arg; pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n"); } shift @av; } } # eof - fg_f14b.pl