fly/fgdata
1
0
Fork 0
Code Activity

This should never really have been in the central nasal area.

Browse source
This commit is contained in:
curt 2009-01-03 01:23:47 +00:00
parent 997cbfc50a
commit 278b0a7262
1 changed files with 0 additions and 421 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

421
Nasal/lead_target.nas
View file

@ -1,421 +0,0 @@
# This is a script that controls a target aircraft for various target/tracking
# tasks.
# print("Target Lead script loading ...");
# script defaults (configurable if you like)
default_update_period = 0.05;
default_goal_range_nm = 0.2;
default_target_root = "/ai/models/aircraft[0]";
default_target_task = "straight";
# master enable switch
lead_target_enable = 0;
# update period
update_period = default_update_period;
# goal range to acheive when following target
goal_range_nm = 0;
# Target property tree root
target_root = "";
# Target task (straight, turns, pitch, both)
target_task = default_target_task;
base_alt = 3000;
tgt_alt = base_alt;
tgt_hdg = 20;
tgt_pitch = 0;
tgt_roll = 0;
tgt_speed = 280;
tgt_lat_mode = "roll";
tgt_lon_mode = "alt";
next_turn = 0;
next_pitch = 0;
last_time = 0;
# Calculate new lon/lat given starting lon/lat, and offset radial, and
# distance. NOTE: starting point is specifed in radians, distance is
# specified in meters (and converted internally to radians)
# ... assumes a spherical world.
# @param orig lon specified in polar coordinates
# @param orig lat specified in polar coordinates
# @param course offset radial
# @param dist offset distance
# @return destination point in polar coordinates
# define some global constants
SG_METER_TO_NM = 0.0005399568034557235;
SG_NM_TO_RAD = 0.00029088820866572159;
SG_EPSILON = 0.0000001;
SGD_PI = 3.14159265358979323846;
SGD_2PI = 6.28318530717958647692;
SGDTR = SGD_PI / 180.0;
fmod = func(x, y) { x - y * int(x/y) }
asin = func(y) { math.atan2(y, math.sqrt(1-y*y)) }
CalcGClonlat = func( lon_rad, lat_rad, hdg_rad, dist_m ) {
result = [0, 0];
# sanity check
while ( hdg_rad < 0 ) { hdg_rad += SGD_2PI; }
while ( hdg_rad >= SGD_2PI ) { hdg_rad -= SGD_2PI; }
print("lon = ", lon_rad, " lat = ", lat_rad, " hdg = ", hdg_rad, " dist = ",
dist_m);
# lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
# IF (cos(lat)=0)
# lon=lon1 // endpoint a pole
# ELSE
# lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
# ENDIF
dist_rad = dist_m * SG_METER_TO_NM * SG_NM_TO_RAD;
result[1] = asin( math.sin(lat_rad) * math.cos(dist_rad) +
math.cos(lat_rad) * math.sin(dist_rad) *
math.cos(hdg_rad) );
if ( math.cos(result[1]) < SG_EPSILON ) {
result[0] = lon_rad; # endpoint a pole
} else {
result[0] =
fmod(lon_rad - asin( (math.sin(hdg_rad) *
math.sin(dist_rad)) /
math.cos(result[1]) )
+ SGD_PI, SGD_2PI) - SGD_PI;
}
print("new lon = ", result[0], " new lat = ", result[1]);
return result;
}
# Initialize target tracking
LeadTargetInit = func {
# seed the random number generator with time
srand();
lead_target_enable = getprop("/autopilot/lead-target/enable");
if ( lead_target_enable == nil ) {
lead_target_enable = 0;
setprop("/autopilot/lead-target/enable", lead_target_enable);
}
target_task = getprop("/autopilot/lead-target/task");
if ( target_task == nil ) {
target_task = default_target_task;
setprop("/autopilot/lead-target/task", target_task);
}
update_period = getprop("/autopilot/lead-target/update-period");
if ( update_period == nil ) {
update_period = default_update_period;
setprop("/autopilot/lead-target/update-period", update_period);
}
goal_range_nm = getprop("/autopilot/lead-target/goal-range-nm");
if ( goal_range_nm == nil ) {
goal_range_nm = default_goal_range_nm;
setprop("/autopilot/lead-target/goal-range-nm", goal_range_nm);
}
target_root = getprop("/autopilot/lead-target/target-root");
if ( target_root == nil ) {
target_root = default_target_root;
setprop("/autopilot/lead-target/target-root", target_root);
}
}
settimer(LeadTargetInit, 0);
# update target task
do_target_task = func {
time = getprop("/sim/time/elapsed-sec");
dt = time - last_time;
last_time = time;
target_task = getprop("/autopilot/lead-target/task");
if ( target_task == "straight" ) {
tgt_lat_mode = "roll";
tgt_lon_mode = "alt";
tgt_roll = 0;
tgt_alt = base_alt;
} else {
if ( target_task == "turns" ) {
next_turn -= dt;
if ( next_turn < 0 ) {
if ( tgt_roll > 0 ) {
# new roll
tgt_roll = -30.0;
} else {
# new roll
tgt_roll = 30.0;
}
# next turn in 15 seconds
next_turn = 15.0;
# roll to zero if tgt_speed < 50 so we don't spin in mid air
if ( tgt_speed < 50 ) { tgt_roll = 0; }
}
tgt_lat_mode = "roll";
} elsif ( target_task == "turns-rand" or target_task == "both-rand" ) {
next_turn -= dt;
if ( next_turn < 0 ) {
if ( tgt_roll > 0 ) {
# new roll between [-45 - -15]
tgt_roll = -45.0 + rand() * 30.0;
} else {
# new roll between [15 - 45]
tgt_roll = rand() * 30.0 + 15.0;
}
# next turn between 5 - 15 seconds
next_turn = rand() * 10.0 + 5.0;
# roll to zero if tgt_speed < 50 so we don't spin in mid air
if ( tgt_speed < 50 ) { tgt_roll = 0; }
}
tgt_lat_mode = "roll";
}
if ( target_task == "pitch" or target_task == "both" ) {
next_pitch -= dt;
if ( next_pitch < 0 ) {
if ( tgt_alt > base_alt ) {
# new alt base - 1000
tgt_alt = base_alt - 1000.0;
} else {
# new alt base + 1000
tgt_alt = base_alt + 1000.0;
}
# next pitch in 15 seconds
next_pitch = 15.0;
# ??? (fixme?)
# pitch to zero if tgt_speed < 50 so we don't porpoise in
# mid air
# if ( tgt_speed < 50 ) { tgt_pitch = 0; }
}
tgt_lon_mode = "alt";
} elsif ( target_task == "pitch-rand" or target_task == "both-rand" ) {
next_pitch -= dt;
if ( next_pitch < 0 ) {
if ( tgt_alt > base_alt ) {
# new alt between [2000 - 2500]
tgt_alt = base_alt - 1000.0 + rand() * 500.0;
} else {
# new alt between [3500 - 4000]
tgt_alt = base_alt + 500.0 + rand() * 500.0;
}
# next pitch between 5 - 15 seconds
next_pitch = rand() * 10.0 + 5.0;
print("--> alt = ", tgt_alt, " next in ", next_pitch, " secs");
# ??? (fixme?)
# pitch to zero if tgt_speed < 50 so we don't porpoise in
# mid air
# if ( tgt_speed < 50 ) { tgt_pitch = 0; }
}
tgt_lon_mode = "alt";
} elsif ( target_task == "lazy-eights" ) {
tgt_lat_mode = "roll";
tgt_lon_mode = "alt";
next_turn -= dt;
if ( next_turn < 0 ) {
next_turn = 32.0;
}
if ( next_turn > 16.0 ) {
# rolling right
tgt_roll = 30.0;
} else {
# rolling left
tgt_roll = -30.0;
}
# roll to zero if tgt_speed < 50 so we don't spin in mid air
if ( tgt_speed < 50 ) { tgt_roll = 0; }
if ( next_turn > 24.0 ) {
# climbing
tgt_alt = base_alt + 1000.0;
} elsif ( next_turn > 16.0 ) {
# decending to original altitude
tgt_alt = base_alt;
} elsif ( next_turn > 8.0 ) {
# climbing
tgt_alt = base_alt + 1000.0;
} else {
# decending to original altitude
tgt_alt = base_alt;
}
}
# fixed altitude if in turns mode
if ( target_task == "turns" or target_task == "turns-rand" ) {
tgt_lon_mode = "alt";
tgt_alt = base_alt;
}
# zero roll if in pitch mode
if ( target_task == "pitch" or target_task == "pitch-rand" ) {
tgt_lat_mode = "roll";
tgt_roll = 0;
}
}
tgt_lat_prop = sprintf("%s/controls/flight/lateral-mode",
target_root );
setprop(tgt_lat_prop, tgt_lat_mode);
tgt_lon_prop = sprintf("%s/controls/flight/longitude-mode",
target_root );
setprop(tgt_lon_prop, tgt_lon_mode);
if ( tgt_lat_mode == "roll" ) {
tgt_roll_prop = sprintf("%s/controls/flight/target-roll", target_root );
setprop(tgt_roll_prop, tgt_roll);
} else {
tgt_hdg_prop = sprintf("%s/controls/flight/target-hdg", target_root );
setprop(tgt_hdg_prop, tgt_hdg);
}
if ( tgt_lon_mode == "alt" ) {
tgt_alt_prop = sprintf("%s/controls/flight/target-alt", target_root );
setprop(tgt_alt_prop, tgt_alt);
} else {
tgt_pitch_prop = sprintf("%s/controls/flight/target-pitch",
target_root );
setprop(tgt_pitch_prop, tgt_pitch);
}
}
# reset target aircraft position and heading relative to the "ownship".
reset_target_aircraft = func {
print("Reseting target aircraft position");
my_lon = getprop("/position/longitude-deg");
my_lat = getprop("/position/latitude-deg");
my_alt = getprop("/position/altitude-ft");
my_hdg = getprop("/orientation/heading-deg");
my_spd = getprop("/velocities/airspeed-kt");
result = CalcGClonlat( my_lon*SGDTR, my_lat*SGDTR, -my_hdg*SGDTR,
0.5/SG_METER_TO_NM );
target_root = getprop("/autopilot/lead-target/target-root");
target_prop = sprintf("%s/position/longitude-deg", target_root );
setprop(target_prop, result[0]/SGDTR);
target_prop = sprintf("%s/position/latitude-deg", target_root );
setprop(target_prop, result[1]/SGDTR);
target_prop = sprintf("%s/position/altitude-ft", target_root );
setprop(target_prop, my_alt);
target_prop = sprintf("%s/orientation/true-heading-deg", target_root );
setprop(target_prop, my_hdg);
target_prop = sprintf("%s/velocities/true-airspeed-kt", target_root );
setprop(target_prop, my_spd);
base_alt = my_alt;
}
# If enabled, update our AP target values based on the target range,
# bearing, and speed
LeadTargetUpdate = func {
lead_target_enable = props.globals.getNode("/autopilot/lead-target/enable");
update_period = getprop("/autopilot/lead-target/update-period");
if ( lead_target_enable.getBoolValue() ) {
# refresh user configurable values
goal_range_nm = getprop("/autopilot/lead-target/goal-range-nm");
target_root = getprop("/autopilot/lead-target/target-root");
# force radar debug-mode on (forced radar calculations even if
# no radar instrument and ai aircraft are out of range
setprop("/instrumentation/radar/debug-mode", 1);
# update target task
do_target_task();
my_hdg = getprop("/orientation/heading-deg");
alt = getprop("/position/altitude-ft");
if ( alt == nil ) { alt = 0; }
speed = getprop("/velocities/airspeed-kt");
if ( speed == nil ) { speed = 0; }
range_prop = sprintf("%s/radar/range-nm", target_root );
range = getprop(range_prop);
if ( range == nil ) {
print("bad property path: ", range_prop);
return;
}
if ( range > 3.0 ) {
# if range is greater than threshold, then reset the target
# aircraft back in range/view.
reset_target_aircraft();
}
h_offset_prop = sprintf("%s/radar/h-offset", target_root );
h_offset = getprop(h_offset_prop);
if ( h_offset == nil ) {
print("bad property path: ", h_offset_prop);
return;
}
if ( h_offset > -90 and h_offset < 90 ) {
# in front of us
range_error = goal_range_nm - range;
} else {
# behind us
range_error = goal_range_nm + range;
}
if ( range_error < 0 ) {
# slow the target down
tgt_speed = speed + range_error * 300.0;
} else {
# speed the target up agressively
tgt_speed = speed + range_error * 2000.0;
}
if ( tgt_speed < 0 ) { tgt_speed = 0; }
# print("speed = ", speed, " range err = ", range_error,
# " target spd = ", tgt_speed);
speed_prop = sprintf("%s/controls/flight/target-spd", target_root );
setprop( speed_prop, tgt_speed );
}
# last thing to do before we return from this function
registerTimer();
}
select_task_dialog = func {
dialog.load(); # load every time?
dialog.open();
}
var dialog = nil;
settimer(func {
dialog = gui.Dialog.new("/sim/gui/dialogs/NTPS/config/dialog", "gui/dialogs/NTPS_target_task.xml");
}, 0);
# timer handling to cause our update function to be called periodially
registerTimer = func {
settimer(LeadTargetUpdate, update_period );
}
registerTimer();