remove README.Protocol and add a README that refers to the "real"
[fg:toms-fgdata.git] / Nasal / lead_target.nas
1 # This is a script that controls a target aircraft for various target/tracking
2 # tasks.
3
4
5 # print("Target Lead script loading ...");
6
7 # script defaults (configurable if you like)
8 default_update_period = 0.05;
9 default_goal_range_nm = 0.2;
10 default_target_root = "/ai/models/aircraft[0]";
11 default_target_task = "straight";
12
13 # master enable switch
14 lead_target_enable = 0;
15
16 # update period
17 update_period = default_update_period;
18
19 # goal range to acheive when following target
20 goal_range_nm = 0;
21
22 # Target property tree root
23 target_root = "";
24
25 # Target task (straight, turns, pitch, both)
26 target_task = default_target_task;
27 base_alt = 3000;
28 tgt_alt = base_alt;
29 tgt_hdg = 20;
30 tgt_pitch = 0;
31 tgt_roll = 0;
32 tgt_speed = 280;
33 tgt_lat_mode = "roll";
34 tgt_lon_mode = "alt";
35 next_turn = 0;
36 next_pitch = 0;
37 last_time = 0;
38
39
40 # Calculate new lon/lat given starting lon/lat, and offset radial, and
41 # distance.  NOTE: starting point is specifed in radians, distance is
42 # specified in meters (and converted internally to radians)
43 # ... assumes a spherical world.
44 # @param orig lon specified in polar coordinates
45 # @param orig lat specified in polar coordinates
46 # @param course offset radial
47 # @param dist offset distance
48 # @return destination point in polar coordinates
49
50 # define some global constants
51 SG_METER_TO_NM = 0.0005399568034557235;
52 SG_NM_TO_RAD   = 0.00029088820866572159;
53 SG_EPSILON     = 0.0000001;
54 SGD_PI         = 3.14159265358979323846;
55 SGD_2PI        = 6.28318530717958647692;
56 SGDTR          = SGD_PI / 180.0;
57 fmod = func(x, y) { x - y * int(x/y) }
58 asin = func(y) { math.atan2(y, math.sqrt(1-y*y)) }
59
60 CalcGClonlat = func( lon_rad, lat_rad, hdg_rad, dist_m ) {
61     result = [0, 0];
62
63     # sanity check
64     while ( hdg_rad < 0 ) { hdg_rad += SGD_2PI; }
65     while ( hdg_rad >= SGD_2PI ) { hdg_rad -= SGD_2PI; }
66
67     print("lon = ", lon_rad, " lat = ", lat_rad, " hdg = ", hdg_rad, " dist = ",
68           dist_m);
69
70     # lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
71     # IF (cos(lat)=0)
72     #   lon=lon1      // endpoint a pole
73     # ELSE
74     #   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
75     # ENDIF
76
77     dist_rad = dist_m * SG_METER_TO_NM * SG_NM_TO_RAD;
78
79     result[1] = asin( math.sin(lat_rad) * math.cos(dist_rad) +
80                       math.cos(lat_rad) * math.sin(dist_rad) *
81                       math.cos(hdg_rad) );
82
83     if ( math.cos(result[1]) < SG_EPSILON ) {
84         result[0] = lon_rad;      # endpoint a pole
85     } else {
86         result[0] = 
87             fmod(lon_rad - asin( (math.sin(hdg_rad) *
88                                  math.sin(dist_rad)) /
89                                  math.cos(result[1]) )
90                       + SGD_PI, SGD_2PI) - SGD_PI;
91     }
92
93     print("new lon = ", result[0], " new lat = ", result[1]);
94
95     return result;
96 }
97
98
99 # Initialize target tracking
100 LeadTargetInit = func {
101     # seed the random number generator with time
102     srand();
103
104     lead_target_enable = getprop("/autopilot/lead-target/enable");
105     if ( lead_target_enable == nil ) {
106         lead_target_enable = 0;
107         setprop("/autopilot/lead-target/enable", lead_target_enable);
108     }
109
110     target_task = getprop("/autopilot/lead-target/task");
111     if ( target_task == nil ) {
112         target_task = default_target_task;
113         setprop("/autopilot/lead-target/task", target_task);
114     }
115
116     update_period = getprop("/autopilot/lead-target/update-period");
117     if ( update_period == nil ) {
118         update_period = default_update_period;
119         setprop("/autopilot/lead-target/update-period", update_period);
120     }
121
122     goal_range_nm = getprop("/autopilot/lead-target/goal-range-nm");
123     if ( goal_range_nm == nil ) {
124         goal_range_nm = default_goal_range_nm;
125         setprop("/autopilot/lead-target/goal-range-nm", goal_range_nm);
126     }
127
128     target_root = getprop("/autopilot/lead-target/target-root");
129     if ( target_root == nil ) {
130         target_root = default_target_root;
131         setprop("/autopilot/lead-target/target-root", target_root);
132     }
133 }
134 settimer(LeadTargetInit, 0);
135
136
137 # update target task
138 do_target_task = func {
139     time = getprop("/sim/time/elapsed-sec");
140     dt = time - last_time;
141     last_time = time;
142
143     target_task = getprop("/autopilot/lead-target/task");
144
145     if ( target_task == "straight" ) {
146         tgt_lat_mode = "roll";
147         tgt_lon_mode = "alt";
148         tgt_roll = 0;
149         tgt_alt = base_alt;
150     } else {
151         if ( target_task == "turns" ) {
152             next_turn -= dt;
153             if ( next_turn < 0 ) {
154                 if ( tgt_roll > 0 ) {
155                     # new roll 
156                     tgt_roll = -30.0;
157                 } else {
158                     # new roll 
159                     tgt_roll = 30.0;
160                 }
161                 # next turn in 15 seconds
162                 next_turn = 15.0;
163
164                 # roll to zero if tgt_speed < 50 so we don't spin in mid air
165                 if ( tgt_speed < 50 ) { tgt_roll = 0; }
166             }
167             tgt_lat_mode = "roll";
168         } elsif ( target_task == "turns-rand" or target_task == "both-rand" ) {
169             next_turn -= dt;
170             if ( next_turn < 0 ) {
171                 if ( tgt_roll > 0 ) {
172                     # new roll between [-45 - -15]
173                     tgt_roll = -45.0 + rand() * 30.0;
174                 } else {
175                     # new roll between [15 - 45]
176                     tgt_roll = rand() * 30.0 + 15.0;
177                 }
178                 # next turn between 5 - 15 seconds
179                 next_turn = rand() * 10.0 + 5.0;
180
181                 # roll to zero if tgt_speed < 50 so we don't spin in mid air
182                 if ( tgt_speed < 50 ) { tgt_roll = 0; }
183             }
184             tgt_lat_mode = "roll";
185         }
186         if ( target_task == "pitch" or target_task == "both" ) {
187             next_pitch -= dt;
188             if ( next_pitch < 0 ) {
189                 if ( tgt_alt > base_alt ) {
190                     # new alt base - 1000
191                     tgt_alt = base_alt - 1000.0;
192                 } else {
193                     # new alt base + 1000
194                     tgt_alt = base_alt + 1000.0;
195                 }
196                 # next pitch in 15 seconds
197                 next_pitch = 15.0;
198
199                 # ??? (fixme?)
200                 # pitch to zero if tgt_speed < 50 so we don't porpoise in
201                 # mid air
202                 # if ( tgt_speed < 50 ) { tgt_pitch = 0; }
203             }
204             tgt_lon_mode = "alt";
205         } elsif ( target_task == "pitch-rand" or target_task == "both-rand" ) {
206             next_pitch -= dt;
207             if ( next_pitch < 0 ) {
208                 if ( tgt_alt > base_alt ) {
209                     # new alt between [2000 - 2500]
210                     tgt_alt = base_alt - 1000.0 + rand() * 500.0;
211                 } else {
212                     # new alt between [3500 - 4000]
213                     tgt_alt = base_alt + 500.0 + rand() * 500.0;
214                 }
215                 # next pitch between 5 - 15 seconds
216                 next_pitch = rand() * 10.0 + 5.0;
217                 print("--> alt = ", tgt_alt, " next in ", next_pitch, " secs");
218
219                 # ??? (fixme?)
220                 # pitch to zero if tgt_speed < 50 so we don't porpoise in
221                 # mid air
222                 # if ( tgt_speed < 50 ) { tgt_pitch = 0; }
223             }
224             tgt_lon_mode = "alt";
225         } elsif ( target_task == "lazy-eights" ) {
226             tgt_lat_mode = "roll";
227             tgt_lon_mode = "alt";
228             next_turn -= dt;
229
230             if ( next_turn < 0 ) {
231                 next_turn = 32.0;
232             }
233             if ( next_turn > 16.0 ) {
234                 # rolling right
235                 tgt_roll = 30.0;
236             } else {
237                 # rolling left
238                 tgt_roll = -30.0;
239             }
240             # roll to zero if tgt_speed < 50 so we don't spin in mid air
241             if ( tgt_speed < 50 ) { tgt_roll = 0; }
242
243             if ( next_turn > 24.0 ) {
244                 # climbing
245                 tgt_alt = base_alt + 1000.0;
246             } elsif ( next_turn > 16.0 ) {
247                 # decending to original altitude
248                 tgt_alt = base_alt;
249             } elsif ( next_turn > 8.0 ) {
250                 # climbing
251                 tgt_alt = base_alt + 1000.0;
252             } else {
253                 # decending to original altitude
254                 tgt_alt = base_alt;
255             }
256         }
257         # fixed altitude if in turns mode
258         if ( target_task == "turns" or target_task == "turns-rand" ) {
259             tgt_lon_mode = "alt";
260             tgt_alt = base_alt;
261         }
262         # zero roll if in pitch mode
263         if ( target_task == "pitch" or target_task == "pitch-rand" ) {
264             tgt_lat_mode = "roll";
265             tgt_roll = 0;
266         }
267     }
268
269     tgt_lat_prop = sprintf("%s/controls/flight/lateral-mode",
270                            target_root );
271     setprop(tgt_lat_prop, tgt_lat_mode);
272
273     tgt_lon_prop = sprintf("%s/controls/flight/longitude-mode",
274                            target_root );
275     setprop(tgt_lon_prop, tgt_lon_mode);
276
277
278     if ( tgt_lat_mode == "roll" ) {
279         tgt_roll_prop = sprintf("%s/controls/flight/target-roll", target_root );
280         setprop(tgt_roll_prop, tgt_roll);
281     } else {
282         tgt_hdg_prop = sprintf("%s/controls/flight/target-hdg", target_root );
283         setprop(tgt_hdg_prop, tgt_hdg);
284     }
285
286     if ( tgt_lon_mode == "alt" ) {
287         tgt_alt_prop = sprintf("%s/controls/flight/target-alt", target_root );
288         setprop(tgt_alt_prop, tgt_alt);
289     } else {
290         tgt_pitch_prop = sprintf("%s/controls/flight/target-pitch",
291                                  target_root );
292         setprop(tgt_pitch_prop, tgt_pitch);
293     }
294 }
295
296
297 # reset target aircraft position and heading relative to the "ownship".
298 reset_target_aircraft = func {
299     print("Reseting target aircraft position");
300
301     my_lon = getprop("/position/longitude-deg");
302     my_lat = getprop("/position/latitude-deg");
303     my_alt = getprop("/position/altitude-ft");
304     my_hdg = getprop("/orientation/heading-deg");
305     my_spd = getprop("/velocities/airspeed-kt");
306
307     result = CalcGClonlat( my_lon*SGDTR, my_lat*SGDTR, -my_hdg*SGDTR,
308                            0.5/SG_METER_TO_NM );
309
310     target_root = getprop("/autopilot/lead-target/target-root");
311
312     target_prop = sprintf("%s/position/longitude-deg", target_root );
313     setprop(target_prop, result[0]/SGDTR);
314
315     target_prop = sprintf("%s/position/latitude-deg", target_root );
316     setprop(target_prop, result[1]/SGDTR);
317
318     target_prop = sprintf("%s/position/altitude-ft", target_root );
319     setprop(target_prop, my_alt);
320
321     target_prop = sprintf("%s/orientation/true-heading-deg", target_root );
322     setprop(target_prop, my_hdg);
323
324     target_prop = sprintf("%s/velocities/true-airspeed-kt", target_root );
325     setprop(target_prop, my_spd);
326
327     base_alt = my_alt;
328 }
329
330
331 # If enabled, update our AP target values based on the target range,
332 # bearing, and speed
333 LeadTargetUpdate = func {
334     lead_target_enable = props.globals.getNode("/autopilot/lead-target/enable");
335     update_period = getprop("/autopilot/lead-target/update-period");
336
337     if ( lead_target_enable.getBoolValue() ) {
338         # refresh user configurable values
339         goal_range_nm = getprop("/autopilot/lead-target/goal-range-nm");
340         target_root = getprop("/autopilot/lead-target/target-root");
341
342         # force radar debug-mode on (forced radar calculations even if
343         # no radar instrument and ai aircraft are out of range
344         setprop("/instrumentation/radar/debug-mode", 1);
345
346         # update target task
347         do_target_task();
348
349         my_hdg = getprop("/orientation/heading-deg");
350
351         alt = getprop("/position/altitude-ft");
352         if ( alt == nil ) { alt = 0; }
353     
354         speed = getprop("/velocities/airspeed-kt");
355         if ( speed == nil ) { speed = 0; }
356     
357         range_prop = sprintf("%s/radar/range-nm", target_root );
358         range = getprop(range_prop);
359         if ( range == nil ) {
360             print("bad property path: ", range_prop);
361             return;
362         }
363         if ( range > 3.0 ) {
364             # if range is greater than threshold, then reset the target
365             # aircraft back in range/view.
366             reset_target_aircraft();
367         }
368     
369         h_offset_prop = sprintf("%s/radar/h-offset", target_root );
370         h_offset = getprop(h_offset_prop);
371         if ( h_offset == nil ) {
372             print("bad property path: ", h_offset_prop);
373             return;
374         }
375
376         if ( h_offset > -90 and h_offset < 90 ) {
377             # in front of us
378             range_error = goal_range_nm - range;
379         } else {
380             # behind us
381             range_error = goal_range_nm + range;
382         }
383         if ( range_error < 0 ) {
384             # slow the target down
385             tgt_speed = speed + range_error * 300.0;
386         } else {
387             # speed the target up agressively
388             tgt_speed = speed + range_error * 2000.0;
389         }
390         if ( tgt_speed < 0 ) { tgt_speed = 0; }
391
392         # print("speed = ", speed, " range err = ", range_error,
393         #       " target spd = ", tgt_speed);
394
395         speed_prop = sprintf("%s/controls/flight/target-spd", target_root );
396         setprop( speed_prop, tgt_speed );
397     }
398
399     # last thing to do before we return from this function
400     registerTimer();
401 }
402
403
404 select_task_dialog = func {
405     dialog.load();  # load every time?
406     dialog.open();
407 }
408
409
410 var dialog = nil;
411 settimer(func {
412     dialog = gui.Dialog.new("/sim/gui/dialogs/NTPS/config/dialog", "gui/dialogs/NTPS_target_task.xml");
413 }, 0);
414
415
416 # timer handling to cause our update function to be called periodially
417 registerTimer = func {
418     settimer(LeadTargetUpdate, update_period );
419 }
420 registerTimer();
421