Computing gun sight code with USAAF K-14A sight.
[fg:toms-fgdata.git] / Aircraft / Instruments-3d / computing-gun-sights / Nasal / lead-computer.nas
1 #****************************************************************************\r
2 #\r
3 #  Derived from \r
4 #  A Digital Lead Computing Optical Sight Model\r
5 #  Anthony L. Leatham, et al\r
6 #  Air Force Academy  \r
7 #  Sept. 1974\r
8 #  http://www.dtic.mil/dtic/tr/fulltext/u2/786464.pdf\r
9 #\r
10 #  Appendex D Air Force Avionics Laboratory digital LCOS\r
11 #  Contains FORTRAN code that is used here converted into Nasal code.\r
12 #\r
13 #  Original source for equations and code was from:\r
14 #\r
15 #  Air-to-Air Gun Fire Control Equations for Digital Lead Computing Optical Sights\r
16 #  R.A. Manske\r
17 #  AFAL-TM-74-8-NVE-1\r
18 #  April 1974\r
19 #  Air Force Avionics Laboratory\r
20 #  Wring-Patterson AFB, Ohio\r
21 #\r
22 #  Document location unknown.\r
23 #\r
24 #  non-static inputs\r
25 #\r
26 #  range                  in feet\r
27 #  rangeRate              range rate in ft/sec\r
28 #  P                      angular pitch rate of aircraft body axis in radians/second\r
29 #  Q                      angular yaw rate of aircraft body axis in radians/second\r
30 #  R                      angular roll rate of aircraft body axis in radians/second\r
31 #  normalAcceleration     normal acceleration in ft/sec/sec (-32.17 straight and level)\r
32 #  alpha                  angle of attack radians\r
33 #  trueAirSpeed           true airspeed - feet per second\r
34 #  RHO                    air density in slugs per cubic foot\r
35 #\r
36 #********************************************************************************\r
37 \r
38 # ================================ Initalize ====================================== \r
39 # Make sure all needed properties are present and accounted \r
40 # for, and that they have sane default values.\r
41 \r
42 # print("gunsight-computer.nas started");\r
43 \r
44 var propertyTreeRoot = "/controls/armament/gunsight/";\r
45 \r
46 var z_offset = 0;\r
47 var y_offset = 0;\r
48 var gunElevationRadians = 0;\r
49 \r
50 var ballisticCoefficienct = 0;\r
51 var muzzleVelocity = 0;\r
52 var harmonizationRange = 0;\r
53 var range = 0;\r
54 \r
55 # constants\r
56 var timeStep = 0.1;\r
57 \r
58 # sight damping factor\r
59 var SIG = 0.4;\r
60 var KSIG = 1.0 / (1.0 + SIG);\r
61 \r
62 var seaLevelAirDensity = 0.00238;\r
63 var radiansPerDrgree = 0.0174532925;\r
64 \r
65 # Ballistic coefficient divided by sea level air density\r
66 var KBRHO = 0;\r
67 \r
68 var cosGunElevation = 0;\r
69 var sinGunElevation = 0;\r
70 \r
71 # Reciprocal of gun harmonization range\r
72 var RRH = 0;\r
73 \r
74 var SQRV = 0;\r
75 var RHO = 0;\r
76 \r
77 var RDC = 0;\r
78 var LA = 0;\r
79 var LE = 0;\r
80 var VLS = 0;\r
81 var rangeRate = 0;\r
82 var VOS = 0; \r
83 var VCM = 0;\r
84 var VMC = 0;\r
85 var rangeRateArray = [0, 0, 0, 0]; \r
86 var rRAindex = 0;\r
87 \r
88 var P = 0;\r
89 var Q = 0;\r
90 var R = 0;\r
91 var normalAcceleration = -32.17;\r
92 \r
93 var maxElevation = getprop(propertyTreeRoot, "maxElevation");\r
94 var maxAzimuth = getprop(propertyTreeRoot, "maxAzimuth");\r
95 \r
96 var initSightComputer = func()\r
97 {\r
98    # print("gunsite-computer.nas init()");\r
99    \r
100    # Get aircraft/sight specific info\r
101    \r
102    # Offset of gun in feet from sight line\r
103    z_offset = getprop(propertyTreeRoot, "z-offsetFeet");\r
104    y_offset = getprop(propertyTreeRoot, "y-offsetFeet");\r
105 \r
106    # Elevation of gun(s) above alpha   \r
107    gunElevationRadians = getprop(propertyTreeRoot, "gunElevationDegrees") * 0.0174532925;\r
108    \r
109    # Projectile ballistic Coefficient and muzzle velocity in feet per second\r
110    ballisticCoefficienct = getprop(propertyTreeRoot, "ballisticCoefficienct");\r
111    muzzleVelocity = getprop(propertyTreeRoot, "muzzleVelocity");\r
112    \r
113    # Range where the sight line is = the guns bore line\r
114    harmonizationRange = getprop(propertyTreeRoot, "gunHarminizationRangeFeet");\r
115    \r
116    # range to target usually from an onboard radar system or a \r
117    # manual system controlled by the aircraft crew   \r
118    range = getprop(propertyTreeRoot, "range");\r
119    \r
120    var seaLevelAirDensity = 0.00238;\r
121 \r
122    # Ballistic coefficient divided by sea level air density\r
123    KBRHO = ballisticCoefficienct / seaLevelAirDensity;\r
124 \r
125    cosGunElevation = math.cos(gunElevationRadians);\r
126    sinGunElevation = math.sin(gunElevationRadians);\r
127 \r
128    # Reciprocal of gun harmonization range\r
129    RRH = 1.0 / harmonizationRange;\r
130 \r
131    SQRV = math.sqrt(muzzleVelocity);\r
132    RHO = getprop("/environment/density-slugft3");\r
133 \r
134    VLS = KBRHO * RHO * SQRV * range;\r
135    rangeRate = 0;\r
136    VOS = muzzleVelocity + VLS + rangeRate; \r
137    VCM = math.sqrt(VOS * VOS - 4.0 * (rangeRate) * VLS);\r
138    VMC = math.sqrt(VOS * VOS - 4.0 * (rangeRate) * VLS);\r
139    \r
140    # print("gunsite-computer.nas initialization done");\r
141 }\r
142 \r
143 var getRangeRate = func()\r
144 {\r
145    # print("gunsite-computer.nas getRangeRate");\r
146    var oldRange = range;\r
147    range = getprop(propertyTreeRoot, "range");\r
148    rangeRateArray[rRAindex] = (oldRange - range) / timeStep;\r
149    rRAindex = rRAindex + 1;\r
150    if (rRAindex > 3) \r
151    {\r
152       rRAindex = 0;\r
153    }\r
154    var rangeRateSum = 0;\r
155    forindex(i; rangeRateArray)\r
156    {\r
157       rangeRateSum = rangeRateSum + rangeRateArray[i];  \r
158    }\r
159    var rangeRateAve = rangeRateSum / 4;\r
160    setprop("/controls/armament/gunsight/rangeRate", rangeRateAve);   \r
161    return -1.0 * rangeRateAve;\r
162 }\r
163 \r
164 var getAccelerationData = func()\r
165 {\r
166    # print("gunsite-computer.nas getAccelerationData()");\r
167    \r
168    P = getprop("/orientation/p-body");\r
169    Q = getprop("/orientation/q-body");\r
170    R = getprop("/orientation/r-body");\r
171    normalAcceleration = getprop("/accelerations/pilot/z-accel-fps_sec");\r
172 }\r
173 \r
174 var getTrueAirSpeed = func()\r
175 {\r
176    # print("gunsite-computer.nas getTrueAirspeed()");\r
177    \r
178    # from http://en.wikipedia.org/wiki/True_airspeed\r
179    # TASKnots = A0 * M * sqrt(T/T0)\r
180    # where A0 = speed of sound a standard sea level (661.47 Kts) \r
181    #       M = MACH speed of aircraft\r
182    #       T = static air temperature in Kelvin at aircraft position\r
183    #       T0 = temperature a standard sea level (288.15 K)\r
184    \r
185    var ambientTemperatureKelvin = getprop("/environment/temperature-degc") + 273.15;\r
186    var MACH = getprop("/velocities/mach");\r
187    var TASKnots = 661.47 * MACH * math.sqrt(ambientTemperatureKelvin / 288.15);\r
188    \r
189    # print("true airspeed = ", TASKnots);\r
190    # return TAS in feet per second.\r
191    return TASKnots * 1.68781;\r
192 }   \r
193 \r
194 var getAzimuthAndElevation = func()\r
195 {   \r
196    # print("gunsite-computer.nas getAzimuthAndElevation");\r
197    \r
198    getAccelerationData();\r
199    # print("normalAcceleration = ", normalAcceleration);\r
200    rangeRate = getRangeRate();\r
201    range = getprop(propertyTreeRoot, "range");\r
202    var alpha = getprop("/orientation/alpha-deg") * radiansPerDrgree;\r
203    \r
204    var trueAirSpeed = getTrueAirSpeed();\r
205    \r
206    var totalInitialVelocity = trueAirSpeed + muzzleVelocity;\r
207    var SQRV = math.sqrt(trueAirSpeed + totalInitialVelocity);\r
208    \r
209    var RHO = getprop("/environment/density-slugft3");\r
210    \r
211    var SLA = LA * (1.0 - 0.16667 * LA * LA);\r
212    var SLE = LE * (1.0 - 0.16667 * LE * LE);\r
213    var CLE = 1.0 - (LE * LE)/2;\r
214    var CLA = 1.0 - (LA * LA)/2;\r
215    var RCOS = 1.0/(CLA * CLE);\r
216    var RDE = (-rangeRate - RDC * SLE) * RCOS;\r
217    var RE = range * RCOS;\r
218    \r
219    # Approximation used for sqrt\r
220    # If y=approx. square root of x, then SQTR(x) = 0.5 * (y + x/y) is very close\r
221    SQRV = 0.5 * (SQRV + (trueAirSpeed + totalInitialVelocity)/SQRV);\r
222    VLS = KBRHO * RHO * RE * SQRV;\r
223    VOS = totalInitialVelocity - RDE - VLS;\r
224    \r
225    # Approximation used for sqrt\r
226    # RDE = -rangeRate makes this calculation identical to other LCOSS\r
227    RDE = -rangeRate;\r
228    VCM = 0.5 * (VCM + (VOS * VOS - 4.0 * (trueAirSpeed + RDE) * VLS) / VCM);\r
229    var VE = 0.5 * (VOS + VCM);\r
230    \r
231    var recipricalTimeOfFlight = VE / RE;\r
232    var averageRelativeVelocity = VE + RDE;\r
233       \r
234    var rRange = 1.0 / range;\r
235    var KH = 1.0 - range * RRH;   \r
236    var VN = averageRelativeVelocity - SIG * RDE * VLS * (averageRelativeVelocity + trueAirSpeed)/(VCM * averageRelativeVelocity);\r
237       \r
238    var PG = P * cosGunElevation - R * sinGunElevation;\r
239    var RG = P * sinGunElevation + R * cosGunElevation;\r
240   \r
241    RDC = trueAirSpeed * (alpha + gunElevationRadians) * \r
242           (totalInitialVelocity - averageRelativeVelocity) / \r
243                    (trueAirSpeed + totalInitialVelocity) + \r
244                    0.5 * normalAcceleration / recipricalTimeOfFlight;\r
245    \r
246    # WJ and WK are computed sight line angular rates\r
247    var WK = (KH * y_offset * CLA * recipricalTimeOfFlight - VN * SLA) * rRange;\r
248    var WJ = ((RDC - KH * z_offset * recipricalTimeOfFlight) * CLE - VN * CLA * SLE) * rRange;\r
249    var LED = (WJ + SLA * PG - CLA * Q) * KSIG;\r
250    var LAD = ((WK - SLE * (CLA * PG + SLA * Q)) / CLE -RG) * KSIG;\r
251    \r
252    LA = LA + LAD * timeStep;\r
253    LE = LE + LED * timeStep;\r
254 }\r
255 \r
256 var gunSightMain = func()\r
257 {\r
258    # print("gunSightMain()");\r
259    if (getprop(propertyTreeRoot, "computer-on") == 1)\r
260    {   \r
261       getAzimuthAndElevation();\r
262       # LA and LE are azimuth and elevation angles of the computed sight line\r
263       # LA and LE are in radians.  \r
264           \r
265           # Compute PLA and PLE as negative mills\r
266       # var PLA = -1000.0 * LA;\r
267       # var PLE = -1000.0 * LE;\r
268                  \r
269       # update reticle position\r
270           # Under extreme conditions such as a spin the algorithm \r
271           # can generate invalid results and return NaN for LA \r
272       # and LE which causes the whole thing to break down.        \r
273           # If that happens set them to 0 (reinitialize LA and LE)\r
274           # and continue on.\r
275           \r
276            # clamp LA and LE to max deflection values for the sight\r
277            var tempLA = LA;\r
278            var tempLE = LE;\r
279 \r
280        if (LA > maxAzimuth/1000)\r
281        {\r
282           tempLA = maxAzimuth/1000;\r
283        }\r
284        else if (LA < -maxAzimuth/1000)\r
285        {\r
286            tempLA = -maxAzimuth/1000;\r
287        }\r
288            if (LE > maxElevation/1000)\r
289        {\r
290           tempLE = maxElevation/1000;\r
291        }\r
292        else if (LE < -maxElevation/1000)\r
293        {\r
294            tempLE = -maxElevation/1000;\r
295        }\r
296       call(func setprop(propertyTreeRoot, "azimuth", tempLA), nil, var LEerr = []);\r
297           if (size(LEerr))\r
298           {\r
299             tempLA = 0;\r
300                 setprop(propertyTreeRoot, "azimuth", tempLA);\r
301           }\r
302           \r
303           call (func setprop(propertyTreeRoot, "elevation", tempLE), nil, var LAerr = []);\r
304           if (size(LAerr))\r
305           {\r
306             tempLE = 0;\r
307                 setprop(propertyTreeRoot, "elevation", tempLE)\r
308           }\r
309           \r
310           # print("gunSightMain loop");\r
311           # print("LA (azimuth) = ", LA);\r
312           # print("LE (elevation)= ", LE);\r
313           # wait a while and do this again\r
314           settimer(gunSightMain, timeStep);\r
315    } \r
316 }\r
317 \r
318 # listener for the the sight computer/gyro to be powered up or down.\r
319 # Will start gunSightMain process when the gun sight computer/gyro is powered on.        \r
320 var listenGunSightGyroPower = func(i) \r
321 {    \r
322     if (getprop(propertyTreeRoot, "computer-on") == 1)\r
323         {\r
324             initSightComputer();\r
325                 setprop(propertyTreeRoot, "gunsightComputerInitialized", 1);\r
326             # print("gunsite-computer.nas power switched");\r
327         gunSightMain();\r
328         }\r
329         else\r
330         {\r
331            setprop(propertyTreeRoot, "gunsightComputerInitialized", 0);\r
332         }\r
333 }\r
334 \r
335 setprop("/controls/armament/gunsight/gunsightComputerInitialized", 0);\r
336 \r
337 var L1 = setlistener("/sim/signals/fdm-initialized", func(i) \r
338 {\r
339     var L = setlistener("/controls/armament/gunsight/computer-on", listenGunSightGyroPower, 1, 0);\r
340 }, 1, 0);\r
341 \r
342       \r