Hitch/aertow updates fro D-NXKT
[fg:fgdata.git] / Nasal / towing / hitch.nas
1 #
2 # Version: 30. December 2014
3 #
4 # Purpose of this routine:
5 # ------------------------
6 #
7 # - Create visible winch- and towropes for gliders and towplanes
8 # - Support of aerotowing and winch for JSBSim-aircraft (glider and towplanes)
9 #
10 # This routine is very similar to /FDM/YASim/Hitch.cpp
11 # Aerotowing is fully compatible to the YASim functionality.
12 # This means that YASim-gliders could be towed by JSBSim-aircraft and vice versa.
13 # Setup-instructions with copy and paste examples are given below:
14 #
15 #
16 # Setup of visible winch/towropes for Yasim-aircraft:
17 # ----------------------------------------------------
18 #
19 # YASim-aircraft with winch/aerotowing functionality should work out of the box.
20 # Optional you can customize the rope-diameter by adding the following to "your_aircraft-set.xml":
21 # </sim>
22 #  <hitches>
23 #   <aerotow>
24 #    <rope>
25 #     <rope-diameter-mm type ="float">10</rope-diameter-mm>
26 #    </rope>
27 #   </aerotow>
28 #   <winch>
29 #    <rope>
30 #     <rope-diameter-mm type ="float">20</rope-diameter-mm>
31 #    </rope>
32 #   </winch>
33 #  </hitches>
34 # </sim>
35 #
36 # That's all!
37 #
38 #
39 #
40 # Support of aerotowing and winch for JSBSim-aircraft (glider and towplanes):
41 # ----------------------------------------------------------------------------
42 #
43 # 1. Define a hitch in the JSBSim-File. Coordinates according to JSBSims structural frame of reference
44 # (x points to the tail, y points to the right wing, z points upwards).
45 # Unit must be "LBS", frame must be "BODY". The force name is arbitrary.
46 #
47 # <external_reactions>
48 #  <force name="hitch" frame="BODY" unit="LBS" >
49 #   <location unit="M">
50 #    <x>3.65</x>
51 #    <y> 0.0</y>
52 #    <z>-0.12</z>
53 #   </location>
54 #   <direction>
55 #    <x>0.0</x>
56 #    <y>0.0</y>
57 #    <z>0.0</z>
58 #   </direction>
59 #  </force>
60 # </external_reactions>
61
62
63 # 2. Define controls for aerotowing and winch.
64 # Add the following key bindings in "yourAircraft-set.xml":
65 # <input>
66 #  <keyboard>
67 #
68 #   <key n="15">
69 #     <name>Ctrl-o</name>
70 #     <desc>Find aircraft for aerotow</desc>
71 #     <binding>
72 #       <command>nasal</command>
73 #       <script>towing.findBestAIObject()</script>
74 #     </binding>
75 #   </key>
76 #
77 #   <key n="111">
78 #     <name>o</name>
79 #     <desc>Lock aerotow-hook</desc>
80 #     <binding>
81 #       <command>nasal</command>
82 #       <script>towing.closeHitch()</script>
83 #     </binding>
84 #   </key>
85 #
86 #   <key n="79">
87 #     <name>O</name>
88 #     <desc>Open aerotow-hook</desc>
89 #     <binding>
90 #       <command>nasal</command>
91 #       <script>towing.releaseHitch("aerotow")</script>
92 #     </binding>
93 #   </key>
94 #
95 #   <key n="23">
96 #     <name>Ctrl-w</name>
97 #     <desc>Place Winch and hook in</desc>
98 #     <binding>
99 #       <command>nasal</command>
100 #       <script>towing.setWinchPositionAuto()</script>
101 #     </binding>
102 #   </key>
103 #
104 #   <key n="119">
105 #     <name>w</name>
106 #     <desc>Start winch</desc>
107 #     <binding>
108 #       <command>nasal</command>
109 #       <script>towing.runWinch()</script>
110 #     </binding>
111 #   </key>
112 #
113 #   <key n="87">
114 #     <name>W</name>
115 #     <desc>Open winch-hook</desc>
116 #     <binding>
117 #       <command>nasal</command>
118 #       <script>towing.releaseHitch("winch")</script>
119 #     </binding>
120 #   </key>
121 #
122 #  </keyboard>
123 # </input>
124 #
125 # For towplanes only "key n=79" (Open aerotow-hook) is required!
126
127
128 # 3. Set mandatory properties:
129 #<sim>
130 # <hitches>
131 #  <aerotow>
132 #   <force_name_jsbsim type="string">hitch</force_name_jsbsim>
133 #   <force-is-calculated-by-other type="bool">false</force-is-calculated-by-other>
134 #   <mp-auto-connect-period type="float">0.0</mp-auto-connect-period>
135 #   <!-- OPTIONAL
136 #     <decoupled-force-and-rope-locations type="bool">true</decoupled-force-and-rope-locations>
137 #     <local-pos-x type="float">1.5</local-pos-x>
138 #     <local-pos-y type="float"> 0.00</local-pos-y>
139 #     <local-pos-z type="float">-0.3</local-pos-z>
140 #   -->
141 #  </aerotow>
142 #  <winch>
143 #   <force_name_jsbsim type="string">hitch</force_name_jsbsim>
144 #   <!-- OPTIONAL
145 #     <decoupled-force-and-rope-locations type="bool">true</decoupled-force-and-rope-locations>
146 #     <local-pos-x type="float">0.0</local-pos-x>
147 #     <local-pos-y type="float">0.0</local-pos-y>
148 #     <local-pos-z type="float">0.0</local-pos-z>
149 #   -->
150 #  </winch>
151 # </hitches>
152 #</sim>
153 #
154 # "force_name_jsbsim" must be the external force name in JSBSim.
155 # "force-is-calculated-by-other" should be "false" for gliders and "true" for tow planes.
156 # "mp-auto-connect-period" is only needed for tow planes and should be "1".
157 #
158 # IMPORTANT:
159 # The hitch location is stored twice in the property tree (for tow force and for rope animation).
160 # This is necessary to keep the towrope animation compatible to YASim-aircraft.
161 # The hitch location for the tow force is stored in "fdm/jsbsim/external_reactions/hitch/location-x(yz)-in" and for the
162 # animated towrope in "sim/hitches/aerotow(winch)/local-pos-x(yz)".
163 # By default only values for the tow force location have to be defined. The values for the towrope location are set
164 # automatically (decoupled-force-and-rope-locations is "false" by default).
165 # It is feasible to use different locations for the force and rope. In order to do this, you have to set
166 # "decoupled-force-and-rope-locations" to "true" and provide values for "sim/hitches/aerotow(winch)/local-pos-x(yz)".
167 # Note that the frame of reference is different. Here the coordinates for the "YASim-System" are needed
168 # (x points to the nose, y points to the left wing, z points upwards).
169
170
171 # 4. Set optional properties:
172 #<sim>
173 # <hitches>
174 #  <aerotow>
175 #   <tow>
176 #    <brake-force type="float">6000</brake-force>
177 #    <elastic-constant type="float">9000</elastic-constant>
178 #   </tow>
179 #   <rope>
180 #    <rope-diameter-mm type="float">20</rope-diameter-mm>
181 #   </rope>
182 #  </aerotow>
183 #  <winch>
184 #   <automatic-release-angle-deg type="float">70.</automatic-release-angle-deg>
185 #   <winch>
186 #    <initial-tow-length-m type="float">1000.</initial-tow-length-m>
187 #    <max-tow-length-m type="float">1500.</max-tow-length-m>
188 #    <max-force type="float">800.</max-force>
189 #    <max-power-kW type="float">100.</max-power-kW>
190 #    <max-spool-speed-m-s type="float">15.</max-spool-speed-m-s>
191 #    <max-unspool-speed-m-s type="float">20.</max-unspool-speed-m-s>
192 #    <spool-acceleration-m-s-s type="float">8.</spool-acceleration-m-s-s>
193 #    <rel-speed alias="/sim/hitches/winch/winch/actual-spool-speed-m-s"/>
194 #   </winch>
195 #   <tow>
196 #    <break-force type="float">10000</break-force>
197 #    <elastic-constant type="float">40000</elastic-constant>
198 #    <weight-per-m-kg-m type="float">0.01</weight-per-m-kg-m>
199 #   </tow>
200 #   <rope>
201 #    <rope-diameter-mm type="float">40</rope-diameter-mm>
202 #   </rope>
203 #  </winch>
204 # </hitches>
205 #<sim>
206
207
208 # That's it!
209
210
211 ##################################################  general info  ############################################
212 #
213 # 3 different types of towplanes could exist: AI-plane, MP-plane without interaction, MP-plane with interaction.
214 # AI-planes are identified by the node "ai/models/aircraft/".
215 # MP-planes (interactice/non-interactive) are identified by the existence of node "ai/models/multiplayer".
216 # Interactive MP-plane: variables in node "ai/models/multiplayer/sim/hitches/" are updated.
217 # Non-interactive MP-plane: variables are not updated (values are either not defined or have "wrong" values
218 # from a former owner of this node.
219 #
220 # The following properties are transmitted in multiplayer:
221 # "sim/hitches/aerotow/tow/elastic-constant"
222 # "sim/hitches/aerotow/tow/weight-per-m-kg-m"
223 # "sim/hitches/aerotow/tow/dist"
224 # "sim/hitches/aerotow/tow/connected-to-property-node"
225 # "sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign"
226 # "sim/hitches/aerotow/tow/brake-force"
227 # "sim/hitches/aerotow/tow/end-force-x"
228 # "sim/hitches/aerotow/tow/end-force-y"
229 # "sim/hitches/aerotow/tow/end-force-z"
230 # "sim/hitches/aerotow/is-slave"
231 # "sim/hitches/aerotow/speed-in-tow-direction"
232 # "sim/hitches/aerotow/open", open);
233 # "sim/hitches/aerotow/local-pos-x"
234 # "sim/hitches/aerotow/local-pos-y"
235 # "sim/hitches/aerotow/local-pos-z"
236 #
237 ##############################################################################################################
238
239
240
241
242 # ######################################################################################################################
243 #                                            check, if towing support makes sense
244 # ######################################################################################################################
245
246 # Check if node "sim/hitches" is defined. If not, return!
247   if (props.globals.getNode("sim/hitches") == nil ) return;
248   print("towing is active!");
249
250
251 # ######################################################################################################################
252 #                                           set defaults / initialize at startup
253 # ######################################################################################################################
254
255 # set defaults for properties that are NOT already defined
256
257  # yasim properties for aerotow (should be already defined for yasim aircraft but not for JSBSim aircraft
258   if (props.globals.getNode("sim/hitches/aerotow/broken") == nil )
259       props.globals.getNode("sim/hitches/aerotow/broken", 1).setBoolValue(0);
260   if (props.globals.getNode("sim/hitches/aerotow/force") == nil )
261       props.globals.getNode("sim/hitches/aerotow/force", 1).setValue(0.);
262   if (props.globals.getNode("sim/hitches/aerotow/force-is-calculated-by-other") == nil )
263       props.globals.getNode("sim/hitches/aerotow/force-is-calculated-by-other", 1).setBoolValue(0);
264   if (props.globals.getNode("sim/hitches/aerotow/is-slave") == nil )
265       props.globals.getNode("sim/hitches/aerotow/is-slave", 1).setBoolValue(0);
266   if (props.globals.getNode("sim/hitches/aerotow/local-pos-x") == nil )
267       props.globals.getNode("sim/hitches/aerotow/local-pos-x", 1).setValue(0.);
268   if (props.globals.getNode("sim/hitches/aerotow/local-pos-y") == nil )
269       props.globals.getNode("sim/hitches/aerotow/local-pos-y", 1).setValue(0.);
270   if (props.globals.getNode("sim/hitches/aerotow/local-pos-z") == nil )
271       props.globals.getNode("sim/hitches/aerotow/local-pos-z", 1).setValue(0.);
272   if (props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period") == nil )
273       props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period", 1).setValue(0.);
274   if (props.globals.getNode("sim/hitches/aerotow/mp-time-lag") == nil )
275       props.globals.getNode("sim/hitches/aerotow/mp-time-lag", 1).setValue(0.);
276   #if (props.globals.getNode("sim/hitches/aerotow/open") == nil )
277       props.globals.getNode("sim/hitches/aerotow/open", 1).setBoolValue(1);
278   if (props.globals.getNode("sim/hitches/aerotow/speed-in-tow-direction") == nil )
279       props.globals.getNode("sim/hitches/aerotow/speed-in-tow-direction", 1).setValue(0.);
280   if (props.globals.getNode("sim/hitches/aerotow/tow/brake-force") == nil )
281       props.globals.getNode("sim/hitches/aerotow/tow/brake-force", 1).setValue(12345.);
282   if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node") == nil )
283       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node", 1).setBoolValue(0);
284   if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign") == nil )
285       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign", 1).setValue("");
286   if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id") == nil )
287       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id", 1).setIntValue(0);
288   if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node") == nil )
289       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node", 1).setBoolValue(0);
290   if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node") == nil )
291       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node", 1).setBoolValue(0);
292   if (props.globals.getNode("sim/hitches/aerotow/tow/dist") == nil )
293       props.globals.getNode("sim/hitches/aerotow/tow/dist", 1).setValue(0.);
294   if (props.globals.getNode("sim/hitches/aerotow/tow/elastic-constant") == nil )
295       props.globals.getNode("sim/hitches/aerotow/tow/elastic-constant", 1).setValue(9111.);
296   if (props.globals.getNode("sim/hitches/aerotow/tow/end-force-x") == nil )
297       props.globals.getNode("sim/hitches/aerotow/tow/end-force-x", 1).setValue(0.);
298   if (props.globals.getNode("sim/hitches/aerotow/tow/end-force-y") == nil )
299       props.globals.getNode("sim/hitches/aerotow/tow/end-force-y", 1).setValue(0.);
300   if (props.globals.getNode("sim/hitches/aerotow/tow/end-force-z") == nil )
301       props.globals.getNode("sim/hitches/aerotow/tow/end-force-z", 1).setValue(0.);
302   if (props.globals.getNode("sim/hitches/aerotow/tow/length") == nil )
303       props.globals.getNode("sim/hitches/aerotow/tow/length", 1).setValue(60.);
304   if (props.globals.getNode("sim/hitches/aerotow/tow/node") == nil )
305       props.globals.getNode("sim/hitches/aerotow/tow/node", 1).setValue("");
306   if (props.globals.getNode("sim/hitches/aerotow/tow/weight-per-m-kg-m") == nil )
307       props.globals.getNode("sim/hitches/aerotow/tow/weight-per-m-kg-m", 1).setValue(0.35);
308
309  # additional properties
310   if (props.globals.getNode("sim/hitches/aerotow/oldOpen") == nil )
311       props.globals.getNode("sim/hitches/aerotow/oldOpen", 1).setBoolValue(1);
312
313  # new properties for towrope
314   if (props.globals.getNode("sim/hitches/aerotow/rope/exist") == nil )
315       props.globals.getNode("sim/hitches/aerotow/rope/exist", 1).setBoolValue(0);
316   if (props.globals.getNode("sim/hitches/aerotow/rope/model_id") == nil )
317       props.globals.getNode("sim/hitches/aerotow/rope/model_id", 1).setIntValue(-1);
318   if (props.globals.getNode("sim/hitches/aerotow/rope/path_to_model") == nil )
319       props.globals.getNode("sim/hitches/aerotow/rope/path_to_model", 1).setValue("Models/Aircraft/towropes.xml");
320   if (props.globals.getNode("sim/hitches/aerotow/rope/rope-diameter-mm") == nil )
321       props.globals.getNode("sim/hitches/aerotow/rope/rope-diameter-mm", 1).setIntValue(20.);
322
323  # new properties for JSBSim aerotow
324  if ( getprop("sim/flight-model") == "jsb" ) {
325   if (props.globals.getNode("sim/hitches/aerotow/force_name_jsbsim") == nil )
326       props.globals.getNode("sim/hitches/aerotow/force_name_jsbsim", 1).setValue("hitch");
327   if (props.globals.getNode("sim/hitches/aerotow/mp_oldOpen") == nil )
328       props.globals.getNode("sim/hitches/aerotow/mp_oldOpen", 1).setBoolValue(1);
329   if (props.globals.getNode("sim/hitches/aerotow/tow/mp_last_reporded_dist") == nil )
330       props.globals.getNode("sim/hitches/aerotow/tow/mp_last_reported_dist", 1).setValue(0.);
331  }
332
333  # yasim properties for winch (should already be defined for yasim aircraft but not for JSBSim aircraft
334   #if (props.globals.getNode("sim/hitches/winch/open") == nil )
335       props.globals.getNode("sim/hitches/winch/open", 1).setBoolValue(1);
336   if (props.globals.getNode("sim/hitches/winch/broken") == nil )
337       props.globals.getNode("sim/hitches/winch/broken", 1).setBoolValue(0);
338   if (props.globals.getNode("sim/hitches/winch/winch/global-pos-x") == nil )
339       props.globals.getNode("sim/hitches/winch/winch/global-pos-x", 1).setValue(0.);
340   if (props.globals.getNode("sim/hitches/winch/winch/global-pos-y") == nil )
341       props.globals.getNode("sim/hitches/winch/winch/global-pos-y", 1).setValue(0.);
342   if (props.globals.getNode("sim/hitches/winch/winch/global-pos-z") == nil )
343       props.globals.getNode("sim/hitches/winch/winch/global-pos-z", 1).setValue(0.);
344   if (props.globals.getNode("sim/hitches/winch/winch/initial-tow-length-m") == nil )
345       props.globals.getNode("sim/hitches/winch/winch/initial-tow-length-m", 1).setValue(1000.);
346   if (props.globals.getNode("sim/hitches/winch/winch/max-tow-length-m") == nil )
347       props.globals.getNode("sim/hitches/winch/winch/max-tow-length-m", 1).setValue(1500.);
348   if (props.globals.getNode("sim/hitches/winch/winch/min-tow-length-m") == nil )
349       props.globals.getNode("sim/hitches/winch/winch/min-tow-length-m", 1).setValue(1.);
350
351   if (props.globals.getNode("sim/hitches/winch/tow/length") == nil )
352       props.globals.getNode("sim/hitches/winch/tow/length", 1).setValue(0.);
353   if (props.globals.getNode("sim/hitches/winch/tow/dist") == nil )
354       props.globals.getNode("sim/hitches/winch/tow/dist", 1).setValue(0.);
355   if (props.globals.getNode("sim/hitches/winch/tow/elastic-constant") == nil )
356       props.globals.getNode("sim/hitches/winch/tow/elastic-constant", 1).setValue(40001.);
357   if (props.globals.getNode("sim/hitches/winch/tow/weight-per-m-kg-m") == nil )
358       props.globals.getNode("sim/hitches/winch/tow/weight-per-m-kg-m", 1).setValue(0.1);
359
360  # additional properties
361   if (props.globals.getNode("sim/hitches/winch/oldOpen") == nil )
362       props.globals.getNode("sim/hitches/winch/oldOpen", 1).setBoolValue(1);
363   if (props.globals.getNode("sim/hitches/winch/winch/max-spool-speed-m-s") == nil )
364       props.globals.getNode("sim/hitches/winch/winch/max-spool-speed-m-s", 1).setValue(40.);
365
366  # new properties for winch-rope
367   if (props.globals.getNode("sim/hitches/winch/rope/exist") == nil )
368       props.globals.getNode("sim/hitches/winch/rope/exist", 1).setBoolValue(0);
369   if (props.globals.getNode("sim/hitches/winch/rope/model_id") == nil )
370       props.globals.getNode("sim/hitches/winch/rope/model_id", 1).setIntValue(-1);
371   if (props.globals.getNode("sim/hitches/winch/rope/path_to_model") == nil )
372       props.globals.getNode("sim/hitches/winch/rope/path_to_model", 1).setValue("Models/Aircraft/towropes.xml");
373   if (props.globals.getNode("sim/hitches/winch/rope/rope-diameter-mm") == nil )
374       props.globals.getNode("sim/hitches/winch/rope/rope-diameter-mm", 1).setIntValue(20.);
375
376  # new properties for JSBSim winch
377  if ( getprop("sim/flight-model") == "jsb" ) {
378   if (props.globals.getNode("sim/hitches/winch/force_name_jsbsim") == nil )
379       props.globals.getNode("sim/hitches/winch/force_name_jsbsim", 1).setValue("hitch");
380   if (props.globals.getNode("sim/hitches/winch/automatic-release-angle-deg") == nil )
381       props.globals.getNode("sim/hitches/winch/automatic-release-angle-deg", 1).setValue(361.);
382   if (props.globals.getNode("sim/hitches/winch/winch/clutched") == nil )
383       props.globals.getNode("sim/hitches/winch/winch/clutched", 1).setBoolValue(0);
384   if (props.globals.getNode("sim/hitches/winch/winch/actual-spool-speed-m-s") == nil )
385       props.globals.getNode("sim/hitches/winch/winch/actual-spool-speed-m-s", 1).setValue(0.);
386   if (props.globals.getNode("sim/hitches/winch/winch/spool-acceleration-m-s-s") == nil )
387       props.globals.getNode("sim/hitches/winch/winch/spool-acceleration-m-s-s", 1).setValue(8.);
388   if (props.globals.getNode("sim/hitches/winch/winch/max-unspool-speed-m-s") == nil )
389       props.globals.getNode("sim/hitches/winch/winch/max-unspool-speed-m-s", 1).setValue(40.);
390   if (props.globals.getNode("sim/hitches/winch/winch/actual-force-N") == nil )
391       props.globals.getNode("sim/hitches/winch/winch/actual-force-N", 1).setValue(0.);
392   if (props.globals.getNode("sim/hitches/winch/winch/max-force-N") == nil )
393       props.globals.getNode("sim/hitches/winch/winch/max-force-N", 1).setValue(1000.);
394   if (props.globals.getNode("sim/hitches/winch/winch/max-power-kW") == nil )
395       props.globals.getNode("sim/hitches/winch/winch/max-power-kW", 1).setValue(123.);
396   if (props.globals.getNode("sim/hitches/winch/tow/break-force-N") == nil )
397       props.globals.getNode("sim/hitches/winch/tow/break-force-N", 1).setValue(12345.);
398   if (props.globals.getNode("sim/hitches/winch/winch/magic-constant") == nil )
399       props.globals.getNode("sim/hitches/winch/winch/magic-constant", 1).setValue(500.);
400  }
401
402  # new properties for JSBSim aerotow and winch
403  if ( getprop("sim/flight-model") == "jsb" ) {
404   if (props.globals.getNode("sim/hitches/aerotow/decoupled-force-and-rope-locations") == nil )
405       props.globals.getNode("sim/hitches/aerotow/decoupled-force-and-rope-locations", 1).setBoolValue(0);
406   if (props.globals.getNode("sim/hitches/winch/decoupled-force-and-rope-locations") == nil )
407       props.globals.getNode("sim/hitches/winch/decoupled-force-and-rope-locations", 1).setBoolValue(0);
408   # consider older JSBSim-versions which do NOT provide the locations of external_reactions in the property tree
409    var hitchname_aerotow = getprop("sim/hitches/aerotow/force_name_jsbsim");
410    var hitchname_winch   = getprop("sim/hitches/winch/force_name_jsbsim");
411    if (props.globals.getNode("fdm/jsbsim/external_reactions/" ~ hitchname_aerotow ~ "/location-x-in") == nil )
412      props.globals.getNode("sim/hitches/aerotow/decoupled-force-and-rope-locations").setBoolValue(1);
413    if (props.globals.getNode("fdm/jsbsim/external_reactions/" ~ hitchname_winch ~ "/location-x-in") == nil )
414      props.globals.getNode("sim/hitches/winch/decoupled-force-and-rope-locations").setBoolValue(1);
415  }
416
417 # ######################################################################################################################
418 #                                                         main function
419 # ######################################################################################################################
420
421 var towing = func {
422
423   #print("function towing is running");
424
425   var FT2M = 0.30480;
426   var M2FT = 1 / FT2M;
427   var dt = 0;
428
429   # -------------------------------  aerotow part -------------------------------
430
431   var open = getprop("sim/hitches/aerotow/open");
432   var oldOpen = getprop("sim/hitches/aerotow/oldOpen");
433
434   if ( open != oldOpen ) {   # check if my hitch state has changed, if yes: message
435     #print("state has changed: open=",open,"  oldOpen=",oldOpen);
436
437     if ( !open ) {      # my hitch was open and is closed now
438       if ( getprop("sim/flight-model") == "jsb" ) {
439         var distance = getprop("sim/hitches/aerotow/tow/dist");
440         var towlength_m = getprop("sim/hitches/aerotow/tow/length");
441         if ( distance > towlength_m * 1.0001 ) {
442           setprop("sim/messages/pilot", sprintf("Could not lock hitch (tow length is insufficient) on hitch %i!",
443                                                getprop("sim/hitches/aerotow/tow/connected-to-mp-node")));
444           props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(1);  # open my hitch again
445         }  # mp aircraft to far away
446         else {  # my hitch is closed
447           setprop("sim/messages/pilot", sprintf("Locked hitch aerotow %i!",
448                                                  getprop("sim/hitches/aerotow/tow/connected-to-mp-node")));
449         }
450         props.globals.getNode("sim/hitches/aerotow/broken").setBoolValue(0);
451       }  # end: JSBSim
452       if ( !getprop("sim/hitches/aerotow/open") ) {
453         # setup ai-towrope
454         createTowrope("aerotow");
455
456         # set default hitch coordinates (needed for Ai- and non-interactive MP aircraft)
457         setAIObjectDefaults() ;
458       }
459     }  # end hitch is closed
460
461     if ( open ) {   # my hitch is now open
462       if ( getprop("sim/flight-model") == "jsb" ) {
463         if ( getprop("sim/hitches/aerotow/broken") ) {
464           setprop("sim/messages/pilot", sprintf("Oh no, the tow is broken"));
465         }
466         else {
467           setprop("sim/messages/pilot", sprintf("Opened hitch aerotow %i!",
468                                                 getprop("sim/hitches/aerotow/tow/connected-to-mp-node")));
469         }
470       releaseHitch("aerotow"); # open=1 / forces=0
471       }  # end: JSBSim
472       removeTowrope("aerotow");   # remove towrope model
473     }  # end hitch is open
474
475     setprop("sim/hitches/aerotow/oldOpen",open);
476   }  # end hitch state has changed
477
478   if (!open ) {
479     aerotow(open);
480   }  # end hitch is closed (open == 0)
481   else {   # my hitch is open
482     var mp_auto_connect_period = props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period").getValue();
483     if ( mp_auto_connect_period != 0 ) {   # if auto-connect
484       if ( getprop("sim/flight-model") == "jsb" ) {  # only for JSBSim aircraft
485         findBestAIObject();
486       }   # end JSBSim  aircraft
487       dt = mp_auto_connect_period;
488       #print("towing: running as auto connect with period=",dt);
489     }   # end if auto-connect
490     else {  # my hitch is open and not auto-connect
491       dt = 0;
492     }
493   }
494
495
496   # -------------------------------  winch part -------------------------------
497
498   var winchopen = getprop("sim/hitches/winch/open");
499   var wincholdOpen = getprop("sim/hitches/winch/oldOpen");
500
501   if ( winchopen != wincholdOpen ) {   # check if my hitch state has changed, if yes: message
502     #print("winch state has changed: open=",winchopen,"  oldOpen=",wincholdOpen);
503     if ( !winchopen ) {      # my hitch was open and is closed now
504       if ( getprop("sim/flight-model") == "jsb" ) {
505         var distance = getprop("sim/hitches/winch/tow/dist");
506         var towlength_m = getprop("sim/hitches/winch/tow/length");
507         if ( distance > towlength_m ) {
508           setprop("sim/messages/pilot", sprintf("Could not lock hitch (tow length is insufficient) on hitch %i!",
509                                                getprop("sim/hitches/aerotow/tow/connected-to-mp-node")));
510           props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(1);  # open my hitch again
511         }  # mp aircraft to far away
512         else {  # my hitch is closed
513           setprop("sim/messages/pilot", sprintf("Locked hitch winch %i!",
514                                                  getprop("sim/hitches/aerotow/tow/connected-to-mp-node")));
515           setprop("sim/hitches/winch/winch/clutched","false");
516         }
517         props.globals.getNode("sim/hitches/winch/broken").setBoolValue(0);
518         props.globals.getNode("sim/hitches/winch/winch/actual-spool-speed-m-s").setValue(0.);
519       }  # end: JSBSim
520       if ( !getprop("sim/hitches/winch/open") ) {
521         # setup ai-towrope
522         createTowrope("winch");
523
524         # set default hitch coordinates (needed for Ai- and non-interactive MP aircraft)
525         setAIObjectDefaults() ;
526       }
527     }  # end hitch is closed
528
529     if ( winchopen ) {   # my hitch is now open
530       if ( getprop("sim/flight-model") == "jsb" ) {
531         if ( getprop("sim/hitches/winch/broken") ) {
532           setprop("sim/messages/pilot", sprintf("Oh no, the tow is broken"));
533         }
534       releaseHitch("winch");
535       }  # end: JSBSim
536       pull_in_rope();
537     }  # end hitch is open
538
539     setprop("sim/hitches/winch/oldOpen",winchopen);
540   } # end hitch state has changed
541
542   if (!winchopen ) {
543     winch(winchopen);
544   }
545
546   settimer( towing, dt );
547
548 }   # end towing
549
550
551 # ######################################################################################################################
552 #                                                   find best AI object
553 # ######################################################################################################################
554
555 var findBestAIObject = func (){
556
557   # the nearest found plane, that is close enough will be used
558   # set some default variables, needed later to identify if the found object is
559   # an AI-Object, a "non-interactiv MP-Object or an interactive MP-Object
560
561   # local variables
562   var aiobjects = [];                    # keeps the ai-planes from the property tree
563   var aiPosition = geo.Coord.new();      # current processed ai-plane
564   var myPosition = geo.Coord.new();      # coordinates of glider
565   var distance_m = 0;                    # distance to ai-plane
566
567   var FT2M = 0.30480;
568
569   var nodeIsAiAircraft = 0;
570   var nodeIsMpAircraft = 0;
571   var running_as_autoconnect = 0;
572   var mp_open_last_state = 0;
573   var isSlave = 0;
574
575   if ( getprop("sim/flight-model") == "yasim" ) return; # bypass this routine for Yasim-aircraft
576
577   #print("findBestAIObject");
578
579   if (props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period").getValue() != 0 ) {
580     var running_as_autoconnect = 1;
581     #print("findBestAIObject: running as auto connect");
582   }
583
584   var towlength_m = props.globals.getNode("sim/hitches/aerotow/tow/length").getValue();
585
586   var bestdist_m = towlength_m; # initial value
587
588   myPosition = geo.aircraft_position();
589   # todo: calculate exact hitch position
590
591   if( running_as_autoconnect ) {
592     var mycallsign = props.globals.getNode("sim/multiplay/callsign").getValue();
593     #print('mycallsign=',mycallsign);
594   }
595
596   var found = 0;
597   aiobjects = props.globals.getNode("ai/models").getChildren();
598   foreach (var aimember; aiobjects) {
599     if ( (var node = aimember.getName() ) != nil ) {
600       nodeIsAiAircraft = 0;
601       nodeIsMpAircraft = 0;
602       if ( sprintf("%8s",node)  == "aircraft" ) nodeIsAiAircraft = 1;
603       if ( sprintf("%11s",node) == "multiplayer" ) nodeIsMpAircraft = 1;
604       #print("found NodeName=",node,"  nodeIsAiAircraft=",nodeIsAiAircraft,"  nodeIsMpAircraft=",nodeIsMpAircraft  );
605       if ( !nodeIsAiAircraft and !nodeIsMpAircraft ) continue;
606       if ( !aimember.getNode("valid").getValue() )   continue;   # node is invalid
607
608       if( running_as_autoconnect ) {
609         if ( !nodeIsMpAircraft ) continue;
610         #if ( aimember.getValue("sim/hitches/aerotow/open") == nil ) continue; # this node MUST exist for mp-aircraft which want to be towed
611         #if ( aimember.getValue("sim/hitches/aerotow/open") == 1 ) continue;   # if mp hook open, auto-connect is NOT possible
612         if ( aimember.getValue("sim/hitches/aerotow/open") != 0 ) continue;
613         if (mycallsign != aimember.getValue("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign") ) continue ;  # I am the wrong one
614         if ( !getprop("sim/hitches/aerotow/mp_oldOpen") ) continue;  # this prevents an unwanted immediate auto-connect after the dragger
615                                                                      # released its hitch. Firstly wait for a reported "open" hitch from glider
616       }
617
618       var lat_deg = aimember.getNode("position/latitude-deg").getValue();
619       var lon_deg = aimember.getNode("position/longitude-deg").getValue();
620       var alt_m = aimember.getNode("position/altitude-ft").getValue() * FT2M;
621
622       var aiPosition = geo.Coord.set_latlon( lat_deg, lon_deg, alt_m );
623       distance_m = (myPosition.distance_to(aiPosition));
624       #print('distance_m=',distance_m,'  bestdist_m=',bestdist_m);
625       if ( distance_m < bestdist_m ) {
626         bestdist_m = distance_m;
627
628         var towEndNode = node;
629         var nodeID = aimember.getNode("id").getValue();
630         var aicallsign = aimember.getNode("callsign").getValue();
631         #print('nodeId=',nodeID,'   AiCallsign=',aicallsign);
632
633         #set properties
634         props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(0);
635         props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node").setBoolValue(nodeIsAiAircraft);
636         props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node").setBoolValue(nodeIsMpAircraft);
637         props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign").setValue(aicallsign);
638         props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id").setIntValue(nodeID);
639         props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node").setBoolValue(1);
640         props.globals.getNode("sim/hitches/aerotow/tow/node").setValue(towEndNode);
641         props.globals.getNode("sim/hitches/aerotow/tow/dist").setValue(bestdist_m);
642         props.globals.getNode("sim/hitches/aerotow/tow/mp_last_reported_dist", 1).setValue(0.);
643
644         # Set some dummy values. In case of an "interactive"-MP plane
645         # the correct values will be transmitted in the following loop
646         aimember.getNode("sim/hitches/aerotow/local-pos-x",1).setValue(-5.);
647         aimember.getNode("sim/hitches/aerotow/local-pos-y",1).setValue(0.);
648         aimember.getNode("sim/hitches/aerotow/local-pos-z",1).setValue(0.);
649         aimember.getNode("sim/hitches/aerotow/tow/dist",1).setValue(-1.);
650
651         found = 1;
652       }   # end distance_m < bestdist_m
653     }   # end node != nil
654   }   # end loop aiobjects
655   if (found) {
656     if ( !running_as_autoconnect) {
657       setprop("sim/messages/pilot", sprintf("%s, I am on your hook, distance %4.3f meter.",aicallsign,bestdist_m));
658     }
659     else {
660       setprop("sim/messages/ai-plane", sprintf("%s: I am on your hook, distance %4.3f meter.",aicallsign,bestdist_m ));
661     }
662     if ( running_as_autoconnect ) {
663       isSlave = 1;
664       props.globals.getNode("sim/hitches/aerotow/is-slave").setBoolValue(isSlave);
665     }
666
667     props.globals.getNode("sim/hitches/aerotow/mp_oldOpen").setBoolValue(1);
668
669   }   # end: if found
670   else {
671     if (!running_as_autoconnect) {
672       setprop("sim/messages/atc", sprintf("Sorry, no aircraft for aerotow!"));
673     }
674     else{
675       #print("auto-connect: found=0");
676       props.globals.getNode("sim/hitches/aerotow/mp_oldOpen").setBoolValue(1);
677     }
678   }
679
680 } # End function findBestAIObject
681
682
683 # ######################################################################################################################
684
685
686 # Start the towing animation ASAP
687 towing();
688
689
690
691 # ######################################################################################################################
692 #                                                         aerotow function
693 # ######################################################################################################################
694
695 var aerotow = func (open){
696
697    #print("function aerotow is running");
698
699 #  if (!open ) {
700
701   ###########################################  my hitch position  ############################################
702
703   myPosition = geo.aircraft_position();
704   var my_head_deg  = getprop("orientation/heading-deg");
705   var my_roll_deg  = getprop("orientation/roll-deg");
706   var my_pitch_deg = getprop("orientation/pitch-deg");
707
708   # hook coordinates in Yasim-system (x-> nose / y -> left wing / z -> up)
709   assignHitchLocations("aerotow");
710   var x = getprop("sim/hitches/aerotow/local-pos-x");
711   var y = getprop("sim/hitches/aerotow/local-pos-y");
712   var z = getprop("sim/hitches/aerotow/local-pos-z");
713
714   var alpha_deg = my_roll_deg * (1.);   # roll clockwise (looking in x-direction) := +
715   var beta_deg  = my_pitch_deg * (-1.); # pitch clockwise (looking in y-direction) := -
716
717   # transform hook coordinates
718   var Xn = PointRotate3D(x:x,y:y,z:z,xr:0.,yr:0.,zr:0.,alpha_deg:alpha_deg,beta_deg:beta_deg,gamma_deg:0.);
719
720   var install_distance_m = Xn[0]; # in front of ref-point of glider
721   var install_side_m     = Xn[1];
722   var install_alt_m      = Xn[2];
723
724   var myHitch_pos    = myPosition.apply_course_distance( my_head_deg , install_distance_m );
725   var myHitch_pos    = myPosition.apply_course_distance( my_head_deg - 90. , install_side_m );
726   myHitch_pos.set_alt(myPosition.alt() + install_alt_m);
727
728   ###########################################  ai hitch position  ############################################
729
730   #var aiNodeID = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id");   # id of former found ai/mp aircraft
731   #print("aiNodeID=",aiNodeID);
732   var aiCallsign = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign");   # callsign of former found ai/mp aircraft
733
734   var found = 0;
735
736   aiobjects = props.globals.getNode("ai/models").getChildren();
737   foreach (var aimember; aiobjects) {
738     if ( (var c = aimember.getNode("id") ) != nil ) {
739       if ( !aimember.getNode("valid").getValue() ) continue;  # node is invalid
740
741       # Identifying the MP-aircraft by its node-id works fine with JSBSim-aircraft but NOT with YASim.
742       # In YASim the node-id is not updated which could lead to complications (e.g. node-id changes after "Pause" or "Exit").
743       #var testprop = c.getValue();
744       #if ( testprop ==  aiNodeID) {
745
746       # Identifying the MP-aircraft by its callsign works fine with JSBSim AND YASim-aircraft
747       var testprop = aimember.getNode("callsign").getValue();
748       if ( testprop == aiCallsign ) {
749
750         found = found + 1;
751
752         ######################  check status of ai hitch  ######################
753         if ( getprop("sim/flight-model") == "jsb" ) {
754           # check if the multiplayer hitch state has changed
755           # this trick avoids immediate opening after locking because MP-aircraft has not yet reported a locked hitch
756           if ( (var d = aimember.getNode("sim/hitches/aerotow/open") ) != nil ) {
757             var mpOpen = aimember.getNode("sim/hitches/aerotow/open").getValue();
758             var mp_oldOpen = getprop("sim/hitches/aerotow/mp_oldOpen");
759             #print('mpOpen=',mpOpen,'  mp_oldOpen=',mp_oldOpen);
760             if ( mpOpen != mp_oldOpen ) { # state has changed: was open and is now locked OR was locked and is now open
761               if ( mpOpen ) {
762                 setprop("sim/messages/ai-plane", sprintf("%s: I have released the tow!",getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign")) );
763                 releaseHitch("aerotow"); # my open=1 / forces=0 / remove towrope
764               }  # end: open
765               props.globals.getNode("sim/hitches/aerotow/mp_oldOpen").setBoolValue(mpOpen);
766             }  # end: state has changed
767           }  # end: node is available
768         }  #end : JSBSim
769         ########################################################################
770
771         # get coordinates
772         var ai_lat = aimember.getNode("position/latitude-deg").getValue();
773         var ai_lon = aimember.getNode("position/longitude-deg").getValue();
774         var ai_alt = (aimember.getNode("position/altitude-ft").getValue()) * FT2M;
775         #print("ai_lat,lon,alt",ai_lat,ai_lon,ai_alt);
776
777         var ai_pitch_deg = aimember.getNode("orientation/pitch-deg").getValue();
778         var ai_roll_deg = aimember.getNode("orientation/roll-deg").getValue();
779         var ai_head_deg = aimember.getNode("orientation/true-heading-deg").getValue();
780
781         var aiHitchX = aimember.getNode("sim/hitches/aerotow/local-pos-x").getValue();
782         var aiHitchY = aimember.getNode("sim/hitches/aerotow/local-pos-y").getValue();
783         var aiHitchZ = aimember.getNode("sim/hitches/aerotow/local-pos-z").getValue();
784
785         var aiPosition = geo.Coord.set_latlon( ai_lat, ai_lon, ai_alt );
786
787         var alpha_deg = ai_roll_deg * (1.);
788         var beta_deg  = ai_pitch_deg * (-1.);
789
790         # transform hook coordinates
791         var Xn = PointRotate3D(x:aiHitchX,y:aiHitchY,z:aiHitchZ,xr:0.,yr:0.,zr:0.,alpha_deg:alpha_deg,beta_deg:beta_deg,gamma_deg:0.);
792
793         var install_distance_m =  Xn[0]; # in front of ref-point of glider
794         var install_side_m     =  Xn[1];
795         var install_alt_m      =  Xn[2];
796
797         var aiHitch_pos    = aiPosition.apply_course_distance( ai_head_deg , install_distance_m );
798         var aiHitch_pos    = aiPosition.apply_course_distance( ai_head_deg - 90. , install_side_m );
799         aiHitch_pos.set_alt(aiPosition.alt() + install_alt_m);
800
801         ###########################################  distance between hitches  #####################################
802
803         var distance = (myHitch_pos.direct_distance_to(aiHitch_pos));      # distance to plane in meter
804         var aiHitchheadto = (myHitch_pos.course_to(aiHitch_pos));
805         var height = myHitch_pos.alt() - aiHitch_pos.alt();
806
807         var aiHitchpitchto = -math.asin((myHitch_pos.alt()-aiHitch_pos.alt())/distance) / 0.01745;
808         #print("  pitch: ", aiHitchpitchto);
809
810         # update position of rope
811         setprop("ai/models/aerotowrope/position/latitude-deg", myHitch_pos.lat());
812         setprop("ai/models/aerotowrope/position/longitude-deg", myHitch_pos.lon());
813         setprop("ai/models/aerotowrope/position/altitude-ft", myHitch_pos.alt() * M2FT);
814         #print("ai_lat,lon,alt",myHitch_pos.lat(),"   ",myHitch_pos.lon(),"   ",myHitch_pos.alt() );
815
816         # update pitch and heading of rope
817         setprop("ai/models/aerotowrope/orientation/true-heading-deg", aiHitchheadto);
818         setprop("ai/models/aerotowrope/orientation/pitch-deg", aiHitchpitchto);
819
820         # update length of rope
821         setprop("sim/hitches/aerotow/tow/dist", distance);
822         #print("distance=",distance);
823
824
825         #############################################  calc forces  ##################################################
826
827         # calc forces only for JSBSim-aircraft
828
829         # tow-end-forces must be reported in N to be consiststent to Yasim-aircraft
830         # hitch-forces must be LBS to be consistent to the JSBSim "external_forces/.../magnitude" definition
831
832         if ( getprop("sim/flight-model") == "jsb" ) {
833           #print("Force-Routine");
834
835           # check if the MP-aircraft properties have been updated. If not (maybe due to time-lag) bypass force calculation (use previous forces instead)
836           var mp_reported_dist = aimember.getNode("sim/hitches/aerotow/tow/dist").getValue();
837           var mp_last_reported_dist = getprop("sim/hitches/aerotow/tow/mp_last_reported_dist");
838           var mp_delta_reported_dist = mp_reported_dist - mp_last_reported_dist ;
839           setprop("sim/hitches/aerotow/tow/mp_last_reported_dist",mp_reported_dist);
840           var mp_delta_reported_dist2 = mp_delta_reported_dist  * mp_delta_reported_dist ;   # we need the absolute value
841           if ( (mp_delta_reported_dist2 > 0.0000001) or (mp_reported_dist < 0. )){     # we have the updated MP coordinates (no time lag)
842                                                                                        # or the MP-aircraft is a non-interactive mp plane (mp_reported_dist = -1)
843                                                                                        # => update forces else use the old forces!
844
845           var breakforce_N = getprop("sim/hitches/aerotow/tow/brake-force");  # could be different in both aircraft
846
847           var isSlave = getprop("sim/hitches/aerotow/is-slave");
848           if ( !isSlave ){  # if we are master, we have to calculate the forces
849             #print("master: calc forces");
850             var elastic_constant = getprop("sim/hitches/aerotow/tow/elastic-constant");
851             var towlength_m = getprop("sim/hitches/aerotow/tow/length");
852
853             var delta_towlength_m = distance - towlength_m;
854             #print("towlength_m= ", towlength_m , "  elastic_constant= ", elastic_constant,"  delta_towlength_m= ", delta_towlength_m);
855
856             if ( delta_towlength_m < 0. ) {
857               var forcetow_N = 0.;
858             }
859             else{
860               var forcetow_N = elastic_constant * delta_towlength_m / towlength_m;
861             }
862           }  # end !isSlave
863           else {   # we are slave and get the forces from master
864             #print("slave: get forces","    aimember=",aimember.getName());
865             # get forces
866             var forcetowX_N = aimember.getNode("sim/hitches/aerotow/tow/end-force-x").getValue() * 1;
867             var forcetowY_N = aimember.getNode("sim/hitches/aerotow/tow/end-force-y").getValue() * 1;
868             var forcetowZ_N = aimember.getNode("sim/hitches/aerotow/tow/end-force-z").getValue() * 1;
869             var forcetow_N = math.sqrt( forcetowX_N * forcetowX_N + forcetowY_N * forcetowY_N + forcetowZ_N * forcetowZ_N );
870           }  # end isSlave
871
872           var forcetow_LBS = forcetow_N * 0.224809;   # N -> LBF
873           #print(" forcetow_N ", forcetow_N , "  distance ", distance,"  ", breakforce_N);
874
875           if ( forcetow_N < breakforce_N ) {
876
877             var distancepr = (myHitch_pos.distance_to(aiHitch_pos));
878
879             # correct a failure, if the projected length is larger than direct length
880             if (distancepr > distance) { distancepr = distance;}
881
882             var alpha = math.acos( (distancepr / distance) );
883             if ( aiHitch_pos.alt() > myHitch_pos.alt()) alpha = - alpha;
884
885             var beta = ( aiHitchheadto - my_head_deg ) * 0.01745;
886             var gamma = my_pitch_deg * 0.01745;
887             var delta = my_roll_deg * 0.01745;
888
889             var sina = math.sin(alpha);
890             var cosa = math.cos(alpha);
891             var sinb = math.sin(beta);
892             var cosb = math.cos(beta);
893             var sing = math.sin(gamma);
894             var cosg = math.cos(gamma);
895             var sind = math.sin(delta);
896             var cosd = math.cos(delta);
897
898             #var forcetow = forcetow_N;   # we deliver N to JSBSim
899             var forcetow = forcetow_LBS;   # we deliver LBS to JSBSim
900
901             # calculate unit vector of force direction in JSBSim-system
902             var force = 1;
903
904             # global forces: alpha beta
905             var fglobalx = force * cosa * cosb;
906             var fglobaly = force * cosa * sinb;
907             var fglobalz = force * sina;
908
909             # local forces by pitch: gamma
910             var flpitchx = fglobalx * cosg - fglobalz * sing;
911             var flpitchy = fglobaly;
912             var flpitchz = fglobalx * sing + fglobalz * cosg;
913
914             # local forces by roll: delta
915             var flrollx  =   flpitchx;
916             var flrolly  =   flpitchy * cosd + flpitchz * sind;
917             var flrollz  = - flpitchy * sind + flpitchz * cosd;
918
919             # asigning to LOCAL coord of plane
920             var forcex = flrollx;
921             var forcey = flrolly;
922             var forcez = flrollz;
923             #print("fx=",forcex,"  fy=",forcey,"  fz=",forcez);
924
925             # JSBSim-body-frame:  x-> nose / y -> right wing / z -> down
926             # apply forces to hook (forces are in LBS or N see above)
927             var hitchname = getprop("sim/hitches/aerotow/force_name_jsbsim");
928             setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/magnitude", forcetow);
929             setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/x", forcex);
930             setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/y", forcey);
931             setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/z", forcez);
932
933           }  # end force < break force
934           else {  # rope is broken
935             props.globals.getNode("sim/hitches/aerotow/broken").setBoolValue(1);
936             #setprop("sim/messages/atc", sprintf("Oh no, the tow is broken"));
937             releaseHitch("aerotow"); # open=1 / forces=0 / remove towrope
938           }
939
940           #############################################  report forces  ##############################################
941
942           # if we are connected to a MP-aircraft and master
943           var nodeIsMpAircraft = getprop("sim/hitches/aerotow/tow/connected-to-mp-node");
944           if ( nodeIsMpAircraft and !isSlave ){
945             #print("report Forces");
946
947             # transform my hitch coordinates to cartesian earth coordinates
948             var myHitchCartEarth = geodtocart(myHitch_pos.lat(),myHitch_pos.lon(),myHitch_pos.alt() );
949             var myHitchXearth_m = myHitchCartEarth[0];
950             var myHitchYearth_m = myHitchCartEarth[1];
951             var myHitchZearth_m = myHitchCartEarth[2];
952
953             # transform MP hitch coordinates to cartesian earth coordinates
954             var aiHitchCartEarth = geodtocart(aiHitch_pos.lat(),aiHitch_pos.lon(),aiHitch_pos.alt() );
955             var aiHitchXearth_m = aiHitchCartEarth[0];
956             var aiHitchYearth_m = aiHitchCartEarth[1];
957             var aiHitchZearth_m = aiHitchCartEarth[2];
958
959             # calculate normal vector in tow direction in cartesian earth coordinates
960             var dx = aiHitchXearth_m - myHitchXearth_m;
961             var dy = aiHitchYearth_m - myHitchYearth_m;
962             var dz = aiHitchZearth_m - myHitchZearth_m;
963             var dl = math.sqrt( dx * dx + dy * dy + dz * dz );
964
965             var forcetowX_N = forcetow_N * dx / dl;
966             var forcetowY_N = forcetow_N * dy / dl;
967             var forcetowZ_N = forcetow_N * dz / dl;
968
969             setprop("sim/hitches/aerotow/tow/dist", distance);
970             setprop("sim/hitches/aerotow/tow/end-force-x", -forcetowX_N); # force acts in
971             setprop("sim/hitches/aerotow/tow/end-force-y", -forcetowY_N); # opposite direction
972             setprop("sim/hitches/aerotow/tow/end-force-z", -forcetowZ_N); # at tow end
973
974           } # end report forces
975
976           }  # end: timelag
977           else{
978             #print("forces NOT updated!");
979           }
980         }  # end forces/JSBSim
981
982
983       }  # end: aiNodeID
984     }  # end: check id != nil
985   }  # end: loop over aiobjects
986
987   if ( found == 0 ) {
988     if ( getprop("sim/flight-model") == "jsb" ) {
989       setprop("sim/messages/atc", sprintf("MP-aircraft disappeared!" ));
990       props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(1);  # open my hitch
991       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id").setIntValue(0);
992       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign").setValue("");
993       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node").setBoolValue(0);
994       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node").setBoolValue(0);
995       props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node").setBoolValue(0);
996     }
997     #if ( getprop("sim/flight-model") == "yasim" ) removeTowrope("aerotow");   # remove towrope model
998   } # end found=0
999
1000 }   # end function aerotow
1001
1002
1003
1004 # ######################################################################################################################
1005 #                                                         winch function
1006 # ######################################################################################################################
1007
1008 var winch = func (open){
1009
1010   var FT2M = 0.30480;
1011   var M2FT = 1. / FT2M;
1012   var RAD2DEG = 57.29578;
1013   var DEG2RAD = 1. / RAD2DEG;
1014
1015   if (!open ) {
1016
1017   ###########################################  my hitch position  ############################################
1018
1019   myPosition = geo.aircraft_position();
1020   var my_head_deg  = getprop("orientation/heading-deg");
1021   var my_roll_deg  = getprop("orientation/roll-deg");
1022   var my_pitch_deg = getprop("orientation/pitch-deg");
1023
1024   # hitch coordinates in YASim-system (x-> nose / y -> left wing / z -> up)
1025   assignHitchLocations("winch");
1026   var x = getprop("sim/hitches/winch/local-pos-x");
1027   var y = getprop("sim/hitches/winch/local-pos-y");
1028   var z = getprop("sim/hitches/winch/local-pos-z");
1029
1030   var alpha_deg = my_roll_deg * (1.);   # roll clockwise (looking in x-direction) := +
1031   var beta_deg  = my_pitch_deg * (-1.); # pitch clockwise (looking in y-direction) := -
1032
1033   # transform hook coordinates
1034   var Xn = PointRotate3D(x:x,y:y,z:z,xr:0.,yr:0.,zr:0.,alpha_deg:alpha_deg,beta_deg:beta_deg,gamma_deg:0.);
1035
1036   var install_distance_m = Xn[0]; # in front of ref-point of glider
1037   var install_side_m     = Xn[1];
1038   var install_alt_m      = Xn[2];
1039
1040   var myHitch_pos    = myPosition.apply_course_distance( my_head_deg , install_distance_m );
1041   var myHitch_pos    = myPosition.apply_course_distance( my_head_deg - 90. , install_side_m );
1042   myHitch_pos.set_alt(myPosition.alt() + install_alt_m);
1043
1044   ###########################################  winch hitch position  ############################################
1045
1046   # get coordinates
1047   var winch_global_pos_x = getprop("sim/hitches/winch/winch/global-pos-x");
1048   var winch_global_pos_y = getprop("sim/hitches/winch/winch/global-pos-y");
1049   var winch_global_pos_z = getprop("sim/hitches/winch/winch/global-pos-z");
1050
1051   var winch_geod = carttogeod(winch_global_pos_x,winch_global_pos_y,winch_global_pos_z);
1052
1053   var ai_lat = winch_geod[0];
1054   var ai_lon = winch_geod[1];
1055   #var ai_alt = winch_geod[2] * FT2M;
1056   var ai_alt = winch_geod[2];
1057   #print("ai_lat,lon,alt",ai_lat,ai_lon,ai_alt);
1058
1059   var aiHitch_pos = geo.Coord.set_latlon( ai_lat, ai_lon, ai_alt );
1060
1061
1062   ###########################################  distance between hitches  #####################################
1063
1064   var distance = (myHitch_pos.direct_distance_to(aiHitch_pos));    # distance to winch in meter
1065   var aiHitchheadto = (myHitch_pos.course_to(aiHitch_pos));
1066   var height = myHitch_pos.alt() - aiHitch_pos.alt();
1067
1068   var aiHitchpitchto = -math.asin((myHitch_pos.alt()-aiHitch_pos.alt())/distance) / 0.01745;
1069   #print("  pitch: ", aiHitchpitchto);
1070
1071   # update position of rope
1072   setprop("ai/models/winchrope/position/latitude-deg", myHitch_pos.lat());
1073   setprop("ai/models/winchrope/position/longitude-deg", myHitch_pos.lon());
1074   setprop("ai/models/winchrope/position/altitude-ft", myHitch_pos.alt() * M2FT);
1075   #print("ai_lat,lon,alt",myHitch_pos.lat(),"   ",myHitch_pos.lon(),"   ",myHitch_pos.alt() );
1076
1077   # update pitch and heading of rope
1078   setprop("ai/models/winchrope/orientation/true-heading-deg", aiHitchheadto);
1079   setprop("ai/models/winchrope/orientation/pitch-deg", aiHitchpitchto);
1080
1081   # update length of rope
1082   setprop("sim/hitches/winch/tow/dist", distance);
1083   #print("distance=",distance);
1084
1085
1086   #############################################  calc forces  ##################################################
1087
1088   # calc forces only for JSBSim-aircraft
1089
1090   # tow-end-forces must be reported in N to be consiststent to Yasim-aircraft
1091   # hitch-forces must be LBS to be consistent to the JSBSim "external_forces/.../magnitude" definition
1092
1093   if ( getprop("sim/flight-model") == "jsb"  ) {
1094
1095     var spool_max = getprop("sim/hitches/winch/winch/max-spool-speed-m-s");
1096     var unspool_max = getprop("sim/hitches/winch/winch/max-unspool-speed-m-s");
1097     var max_force_N = getprop("sim/hitches/winch/winch/max-force-N");
1098     var max_power_W = getprop("sim/hitches/winch/winch/max-power-kW") * 1000.;
1099     var breakforce_N = getprop("sim/hitches/winch/tow/break-force-N");
1100     var elastic_constant = getprop("sim/hitches/winch/tow/elastic-constant");
1101     var towlength_m = getprop("sim/hitches/winch/tow/length");
1102     var max_tow_length_m = getprop("sim/hitches/winch/winch/max-tow-length-m");
1103     var spoolspeed = getprop("sim/hitches/winch/winch/actual-spool-speed-m-s");
1104     var spool_acceleration = getprop("sim/hitches/winch/winch/spool-acceleration-m-s-s");
1105     var delta_t = getprop("sim/time/delta-sec");
1106
1107     var towlength_new_m = towlength_m - spoolspeed * delta_t;
1108     var delta_towlength_m = distance - towlength_new_m;
1109     #print("towlength_m= ", towlength_m , "  elastic_constant= ", elastic_constant,"  delta_towlength_m= ", delta_towlength_m);
1110
1111     if ( getprop("sim/hitches/winch/winch/clutched") ) {
1112       var delta_spoolspeed =  spool_acceleration * delta_t;
1113       spoolspeed = spoolspeed + delta_spoolspeed ;
1114       if ( spoolspeed > spool_max ) spoolspeed = spool_max;
1115     }
1116     else {   # un-clutched
1117       # --- experimental --- #
1118
1119       # we assume that the the winch-operator avoids tow sagging ( => rigid rope; negativ forces allowed)
1120       var forcetow_N = elastic_constant * delta_towlength_m / towlength_new_m;
1121
1122       # drag of tow-rope ( magic! )
1123       var magic_constant = getprop("sim/hitches/winch/winch/magic-constant");
1124       tow_drag_N = spoolspeed * spoolspeed * math.sqrt( math.sqrt( height * height ) * max_tow_length_m ) / magic_constant ;
1125
1126       # mass = tow-mass only (drum-mass ignored)
1127       var mass_kg = max_tow_length_m * getprop("sim/hitches/winch/tow/weight-per-m-kg-m");
1128
1129       var acceleration = ( forcetow_N - tow_drag_N ) / mass_kg;
1130       var delta_spoolspeed = acceleration * delta_t;
1131       spoolspeed = spoolspeed - delta_spoolspeed;
1132       if ( spoolspeed < - unspool_max ) spoolspeed = - unspool_max;
1133       #print("spoolspeed= ",spoolspeed,"  delta_spoolspeed= ",delta_spoolspeed,"  delta_towlength= ", delta_towlength_m);
1134       #print("forcetow_N= ",forcetow_N,"  tow_drag_N= ",tow_drag_N,"  acceleration= ", acceleration);
1135     }
1136
1137     if ( delta_towlength_m < 0. ) {
1138       var forcetow_N = 0.;
1139     }
1140     else{
1141       var forcetow_N = elastic_constant * delta_towlength_m / towlength_new_m;
1142     }
1143
1144     if ( forcetow_N > max_force_N ) {
1145       forcetow_N = max_force_N;
1146       var towlength_new_m = distance / ( forcetow_N / elastic_constant + 1. );
1147       spoolspeed = (towlength_m - towlength_new_m ) / delta_t;
1148     }
1149
1150     var power = forcetow_N * spoolspeed;
1151     if ( power > max_power_W) {
1152       power = max_power_W;
1153       spoolspeed = power / forcetow_N;
1154       towlength_new_m = towlength_m - spoolspeed * delta_t;
1155     }
1156     #print("power=",power,"  spoolspeed=",spoolspeed,"  force=",forcetow_N);
1157
1158     setprop("sim/hitches/winch/tow/length",towlength_new_m);
1159     setprop("sim/hitches/winch/winch/actual-spool-speed-m-s",spoolspeed);
1160     setprop("sim/hitches/winch/winch/actual-force-N",forcetow_N);
1161
1162     # force due to tow-weight (acts in tow direction at the heigher hitch)
1163     var force_due_to_weight_N = getprop("sim/hitches/winch/tow/weight-per-m-kg-m") * 9.81 * height;
1164     if (height < 0. ) force_due_to_weight_N = 0.;
1165
1166     forcetow_N = forcetow_N + force_due_to_weight_N;
1167     var forcetow_LBS = forcetow_N * 0.224809;   # N -> LBF
1168     #print(" forcetow_N ", forcetow_N , "  distance ", distance,"  ", breakforce_N);
1169     #print(" forcetow_N=", forcetow_N , "  force_due_to_weight_N=", force_due_to_weight_N,"  height=",height);
1170
1171     if ( forcetow_N < breakforce_N ) {
1172
1173       var distancepr = (myHitch_pos.distance_to(aiHitch_pos));
1174
1175       # correct a failure, if the projected length is larger than direct length
1176       if (distancepr > distance) { distancepr = distance;}
1177
1178       var alpha = math.acos( (distancepr / distance) );
1179       if ( aiHitch_pos.alt() > myHitch_pos.alt()) alpha = - alpha;
1180       var beta = ( aiHitchheadto - my_head_deg ) * DEG2RAD;
1181       var gamma = my_pitch_deg * DEG2RAD;
1182       var delta = my_roll_deg * DEG2RAD;
1183
1184       var sina = math.sin(alpha);
1185       var cosa = math.cos(alpha);
1186       var sinb = math.sin(beta);
1187       var cosb = math.cos(beta);
1188       var sing = math.sin(gamma);
1189       var cosg = math.cos(gamma);
1190       var sind = math.sin(delta);
1191       var cosd = math.cos(delta);
1192
1193       #var forcetow = forcetow_N;   # we deliver N to JSBSim
1194       var forcetow = forcetow_LBS;   # we deliver LBS to JSBSim
1195
1196       # calculate unit vector of force direction in JSBSim-system
1197       var force = 1;
1198
1199       # global forces: alpha beta
1200       var fglobalx = force * cosa * cosb;
1201       var fglobaly = force * cosa * sinb;
1202       var fglobalz = force * sina;
1203
1204       # local forces by pitch: gamma
1205       var flpitchx = fglobalx * cosg - fglobalz * sing;
1206       var flpitchy = fglobaly;
1207       var flpitchz = fglobalx * sing + fglobalz * cosg;
1208
1209       # local forces by roll: delta
1210       var flrollx  =   flpitchx;
1211       var flrolly  =   flpitchy * cosd + flpitchz * sind;
1212       var flrollz  = - flpitchy * sind + flpitchz * cosd;
1213
1214       # asigning to LOCAL coord of plane
1215       var forcex = flrollx;
1216       var forcey = flrolly;
1217       var forcez = flrollz;
1218       #print("fx=",forcex,"  fy=",forcey,"  fz=",forcez);
1219
1220       # JSBSim-body-frame:  x-> nose / y -> right wing / z -> down
1221       # apply forces to hook (forces are in LBS or N see above)
1222       var hitchname = getprop("sim/hitches/winch/force_name_jsbsim");
1223       setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/magnitude", forcetow);
1224       setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/x", forcex );
1225       setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/y", forcey );
1226       setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/z", forcez );
1227
1228       # check, if auto-release condition is reached
1229       var rope_angle_deg = math.atan2(forcez , forcex ) * RAD2DEG;
1230       #print("rope_angle_deg=",rope_angle_deg);
1231       if (rope_angle_deg > getprop("sim/hitches/winch/automatic-release-angle-deg") ) releaseWinch();
1232
1233     }  # end force < break force
1234     else {  # rope is broken
1235       props.globals.getNode("sim/hitches/winch/broken").setBoolValue(1);
1236       releaseWinch();
1237     }
1238
1239     if ( towlength_new_m > max_tow_length_m ) {
1240       setprop("sim/messages/atc", sprintf("tow length exceeded!"));
1241       releaseWinch();
1242     }
1243
1244   }  # end forces/JSBSim
1245
1246   }  # end hitch is closed (open == 0)
1247
1248 }  # end function winch
1249
1250
1251 # ######################################################################################################################
1252 #                                                      create towrope
1253 # ######################################################################################################################
1254
1255 var createTowrope = func (device){
1256
1257   # create the towrope in the model property tree
1258   #print("createTowrope for ",device);
1259
1260   if ( getprop("sim/hitches/" ~ device ~ "/rope/exist") == 0 ) {   # does the towrope exist?
1261
1262     # get the next free model id
1263     var freeModelid = getFreeModelID();
1264
1265     props.globals.getNode("sim/hitches/" ~ device ~ "/rope/model_id").setIntValue(freeModelid);
1266     props.globals.getNode("sim/hitches/" ~ device ~ "/rope/exist").setBoolValue(1);
1267
1268     var towrope_ai  = props.globals.getNode("ai/models/" ~ device ~ "rope", 1);
1269     var towrope_mod  = props.globals.getNode("models", 1);
1270
1271     towrope_ai.getNode("id", 1).setIntValue(4711);
1272     towrope_ai.getNode("callsign", 1).setValue("towrope");
1273     towrope_ai.getNode("valid", 1).setBoolValue(1);
1274     towrope_ai.getNode("position/latitude-deg", 1).setValue(0.);
1275     towrope_ai.getNode("position/longitude-deg", 1).setValue(0.);
1276     towrope_ai.getNode("position/altitude-ft", 1).setValue(0.);
1277     towrope_ai.getNode("orientation/true-heading-deg", 1).setValue(0.);
1278     towrope_ai.getNode("orientation/pitch-deg", 1).setValue(0.);
1279     towrope_ai.getNode("orientation/roll-deg", 1).setValue(0.);
1280
1281     towrope_mod.model = towrope_mod.getChild("model", freeModelid, 1);
1282     towrope_mod.model.getNode("path", 1).setValue(getprop("sim/hitches/" ~ device ~ "/rope/path_to_model") );
1283     towrope_mod.model.getNode("longitude-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/position/longitude-deg");
1284     towrope_mod.model.getNode("latitude-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/position/latitude-deg");
1285     towrope_mod.model.getNode("elevation-ft-prop", 1).setValue("ai/models/" ~ device ~ "rope/position/altitude-ft");
1286     towrope_mod.model.getNode("heading-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/orientation/true-heading-deg");
1287     towrope_mod.model.getNode("roll-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/orientation/roll-deg");
1288     towrope_mod.model.getNode("pitch-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/orientation/pitch-deg");
1289     towrope_mod.model.getNode("load", 1).remove();
1290   }  # end towrope exist
1291 }
1292
1293
1294 # ######################################################################################################################
1295 #                                     get the next free id of "models/model" members
1296 # ######################################################################################################################
1297
1298 var getFreeModelID = func {
1299   #print("getFreeModelID");
1300   var modelid = 0;   # next unused id
1301   modelobjects = props.globals.getNode("models", 1).getChildren();
1302   foreach ( var member; modelobjects ) {
1303     if ( (var c = member.getIndex()) != nil) {
1304       modelid = c + 1;
1305     }
1306   }
1307   #print("modelid=",modelid);
1308   return(modelid);
1309 }
1310
1311
1312 # ######################################################################################################################
1313 #                                                   close aerotow hitch
1314 # ######################################################################################################################
1315
1316 var closeHitch = func {
1317
1318   #print("closeHitch");
1319
1320   # close only, if
1321   # - not yet closed
1322   # - connected to property-node
1323   # - distance < towrope length
1324
1325   var open = getprop("sim/hitches/aerotow/open");
1326   if ( !open ) return;
1327
1328   var aiNodeID = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id");   # id of former found ai/mp aircraft
1329   if ( aiNodeID < 1 ) {
1330     setprop("sim/messages/atc", sprintf("No aircraft selected!"));
1331     return;
1332   }
1333
1334   #####################################  calc distance between hitches  ######################
1335
1336   ######################  my hitch position  #######################
1337
1338   myPosition = geo.aircraft_position();
1339   var my_head_deg  = getprop("orientation/heading-deg");
1340   var my_roll_deg  = getprop("orientation/roll-deg");
1341   var my_pitch_deg = getprop("orientation/pitch-deg");
1342
1343   # hook coordinates in Yasim-system (x-> nose / y -> left wing / z -> up)
1344   assignHitchLocations("aerotow");
1345   var x = getprop("sim/hitches/aerotow/local-pos-x");
1346   var y = getprop("sim/hitches/aerotow/local-pos-y");
1347   var z = getprop("sim/hitches/aerotow/local-pos-z");
1348
1349   var alpha_deg = my_roll_deg * (1.);   # roll clockwise (looking in x-direction) := +
1350   var beta_deg  = my_pitch_deg * (-1.); # pitch clockwise (looking in y-direction) := -
1351
1352   # transform hook coordinates
1353   var Xn = PointRotate3D(x:x,y:y,z:z,xr:0.,yr:0.,zr:0.,alpha_deg:alpha_deg,beta_deg:beta_deg,gamma_deg:0.);
1354
1355   var install_distance_m = Xn[0]; # in front of ref-point of glider
1356   var install_side_m     = Xn[1];
1357   var install_alt_m      = Xn[2];
1358
1359   var myHitch_pos    = myPosition.apply_course_distance( my_head_deg , install_distance_m );
1360   var myHitch_pos    = myPosition.apply_course_distance( my_head_deg - 90. , install_side_m );
1361   myHitch_pos.set_alt(myPosition.alt() + install_alt_m);
1362
1363   ######################  ai hitch position  #######################
1364
1365   var found = 0;
1366
1367   aiobjects = props.globals.getNode("ai/models").getChildren();
1368   foreach (var aimember; aiobjects) {
1369     if ( (var c = aimember.getNode("id") ) != nil ) {
1370       var testprop = c.getValue();
1371       if ( testprop ==  aiNodeID) {
1372         found = found + 1;
1373
1374         # get coordinates
1375         var ai_lat = aimember.getNode("position/latitude-deg").getValue();
1376         var ai_lon = aimember.getNode("position/longitude-deg").getValue();
1377         var ai_alt = (aimember.getNode("position/altitude-ft").getValue()) * FT2M;
1378
1379         var ai_pitch_deg = aimember.getNode("orientation/pitch-deg").getValue();
1380         var ai_roll_deg = aimember.getNode("orientation/roll-deg").getValue();
1381         var ai_head_deg = aimember.getNode("orientation/true-heading-deg").getValue();
1382
1383         var aiHitchX = aimember.getNode("sim/hitches/aerotow/local-pos-x").getValue();
1384         var aiHitchY = aimember.getNode("sim/hitches/aerotow/local-pos-y").getValue();
1385         var aiHitchZ = aimember.getNode("sim/hitches/aerotow/local-pos-z").getValue();
1386
1387         var aiPosition = geo.Coord.set_latlon( ai_lat, ai_lon, ai_alt );
1388
1389         var alpha_deg = ai_roll_deg * (1.);
1390         var beta_deg  = ai_pitch_deg * (-1.);
1391
1392         # transform hook coordinates
1393         var Xn = PointRotate3D(x:aiHitchX,y:aiHitchY,z:aiHitchZ,xr:0.,yr:0.,zr:0.,alpha_deg:alpha_deg,beta_deg:beta_deg,gamma_deg:0.);
1394
1395         var install_distance_m =  Xn[0]; # in front of ref-point of glider
1396         var install_side_m     =  Xn[1];
1397         var install_alt_m      =  Xn[2];
1398
1399         var aiHitch_pos    = aiPosition.apply_course_distance( ai_head_deg , install_distance_m );
1400         var aiHitch_pos    = aiPosition.apply_course_distance( ai_head_deg - 90. , install_side_m );
1401         aiHitch_pos.set_alt(aiPosition.alt() + install_alt_m);
1402
1403         var distance = (myHitch_pos.direct_distance_to(aiHitch_pos));
1404
1405         var towlength_m = props.globals.getNode("sim/hitches/aerotow/tow/length").getValue();
1406         if ( distance > towlength_m ) {
1407           var aicallsign = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign");
1408           #setprop("sim/messages/atc", sprintf("Aircraft with callsign %s is too far away (distance is %4.0f meter).",aicallsign, distance));
1409           setprop("sim/messages/atc", sprintf("Selected aircraft is too far away (distance to %s is %4.0f meter).",aicallsign, distance));
1410           return;
1411         }
1412
1413         setprop("sim/hitches/aerotow/tow/dist", distance);
1414
1415       }
1416     }
1417   }
1418
1419   setprop("sim/hitches/aerotow/open", "false");
1420   setprop("sim/hitches/aerotow/mp_oldOpen", "true");
1421
1422 } # End function closeHitch
1423
1424
1425 # ######################################################################################################################
1426 #                                                     release hitch
1427 # ######################################################################################################################
1428
1429 var releaseHitch = func (device){
1430
1431   #print("releaseHitch");
1432
1433   if ( getprop("sim/flight-model") == "yasim" ) return; # bypass this routine for Yasim-aircraft
1434
1435   setprop("sim/hitches/" ~ device ~ "/open", "true");
1436
1437   var hitchname = getprop("sim/hitches/" ~ device ~ "/force_name_jsbsim");
1438   setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/magnitude", 0.);
1439   setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/x", 0.);
1440   setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/y", 0.);
1441   setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/z", 0.);
1442
1443   if ( device == "aerotow" ) {
1444     setprop("sim/hitches/aerotow/tow/end-force-x", 0.);          # MP tow-end forces
1445     setprop("sim/hitches/aerotow/tow/end-force-y", 0.);          #
1446     setprop("sim/hitches/aerotow/tow/end-force-z", 0.);          #
1447   }
1448
1449 } # End function releaseHitch
1450
1451
1452 # ######################################################################################################################
1453 #                                                  remove/delete towrope
1454 # ######################################################################################################################
1455
1456 var removeTowrope = func (device){
1457
1458   # remove the towrope from the property tree ai/models
1459   # remove the towrope from the property tree models/
1460
1461   if ( getprop("sim/hitches/" ~ device ~ "/rope/exist") == 1 ) {   # does the towrope exist?
1462
1463     # remove 3d model from scenery
1464     # identification is /models/model[x] with x=id_model
1465     var id_model = getprop("sim/hitches/" ~ device ~ "/rope/model_id");
1466     var modelsNode = "models/model[" ~ id_model ~ "]";
1467     props.globals.getNode(modelsNode).remove();
1468     props.globals.getNode("ai/models/" ~ device ~ "rope").remove();
1469     #print("towrope removed");
1470     setprop("sim/hitches/" ~ device ~ "/rope/exist", 0);
1471   }
1472
1473 }
1474
1475
1476 # ######################################################################################################################
1477 #                                           pull in towrope after hitch has been opened
1478 # ######################################################################################################################
1479
1480 var pull_in_rope = func {
1481
1482   var deg2rad = math.pi / 180.;
1483   var FT2M = 0.30480;
1484
1485   if ( getprop("sim/hitches/winch/open") ) {
1486
1487     # get length of rope
1488     #var distance = getprop("sim/hitches/winch/tow/dist");
1489
1490     var towlength_m = getprop("sim/hitches/winch/tow/length");
1491     var spoolspeed = getprop("sim/hitches/winch/winch/max-spool-speed-m-s");
1492     var delta_t = getprop("sim/time/delta-sec");
1493
1494     var delta_length_m = spoolspeed * delta_t;
1495     var towlength_new_m = towlength_m - delta_length_m;
1496     var towlength_min_m = getprop("sim/hitches/winch/winch/min-tow-length-m");
1497
1498     if ( towlength_new_m > towlength_min_m ) {
1499       #print("actual towlength= ",towlength_new_m);
1500
1501       # get position of rope end (former myHitch_pos)
1502       var tow_lat = getprop("ai/models/winchrope/position/latitude-deg");
1503       var tow_lon = getprop("ai/models/winchrope/position/longitude-deg");
1504       var tow_alt_m = getprop("ai/models/winchrope/position/altitude-ft") * FT2M;
1505       # get pitch and heading of rope
1506       var tow_heading_deg = getprop("ai/models/winchrope/orientation/true-heading-deg");
1507       var tow_pitch_rad = getprop("ai/models/winchrope/orientation/pitch-deg") * deg2rad;
1508
1509       var aiTow_pos = geo.Coord.set_latlon( tow_lat, tow_lon, tow_alt_m );
1510
1511       var delta_distance_m = delta_length_m * math.cos(tow_pitch_rad);
1512       var delta_alt_m      = delta_length_m * math.sin(tow_pitch_rad);
1513       # vertical sink rate not yet taken into account!
1514       aiTow_pos    = aiTow_pos.apply_course_distance( tow_heading_deg , delta_distance_m );
1515       aiTow_pos.set_alt(tow_alt_m + delta_alt_m);
1516       #print("aiTow_pos.alt()= ",aiTow_pos.alt(),"  ",tow_alt_m + delta_alt_m);
1517
1518       # update position of rope
1519       setprop("ai/models/winchrope/position/latitude-deg", aiTow_pos.lat());
1520       setprop("ai/models/winchrope/position/longitude-deg", aiTow_pos.lon());
1521       setprop("ai/models/winchrope/position/altitude-ft", aiTow_pos.alt() * M2FT);
1522
1523       # update length of rope
1524       setprop("sim/hitches/winch/tow/length",towlength_new_m);
1525
1526       settimer( pull_in_rope , 0 );
1527     }  # end towlength > min
1528     else {
1529       #print("pull in finished!");
1530       setprop("sim/hitches/winch/winch/actual-spool-speed-m-s", 0. );
1531       removeTowrope("winch");   # remove towrope model
1532     }
1533
1534   }  # end if open
1535
1536 }
1537
1538
1539 # ######################################################################################################################
1540 #                                              set some AI-object default values
1541 # ######################################################################################################################
1542
1543 var setAIObjectDefaults = func (){
1544
1545   # set some default variables, needed to identify, if the found object is an AI-object, a "non-interactiv MP-object or
1546   # an interactive MP-object
1547
1548   var aiNodeID = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id");   # id of former found ai/mp aircraft
1549
1550   aiobjects = props.globals.getNode("ai/models").getChildren();
1551   foreach (var aimember; aiobjects) {
1552     if ( (var c = aimember.getNode("id") ) != nil ) {
1553       var testprop = c.getValue();
1554       if ( testprop ==  aiNodeID) {
1555          # Set some dummy values. In case of an "interactive"-MP plane
1556          # the correct values will be transmitted in the following loop.
1557          # Create this variables if not present.
1558          aimember.getNode("sim/hitches/aerotow/local-pos-x",1).setValue(-5.);
1559          aimember.getNode("sim/hitches/aerotow/local-pos-y",1).setValue(0.);
1560          aimember.getNode("sim/hitches/aerotow/local-pos-z",1).setValue(0.);
1561          aimember.getNode("sim/hitches/aerotow/tow/dist",1).setValue(-1.);
1562       }
1563     }
1564   }
1565
1566 }
1567
1568
1569 # ######################################################################################################################
1570 #                                                  place winch model
1571 # ######################################################################################################################
1572
1573 var setWinchPositionAuto = func {
1574
1575   # remove already existing winch model
1576   if ( getprop("/sim/hitches/winch/winch/winch-model-index") != nil ) {
1577     var id_model = getprop("/sim/hitches/winch/winch/winch-model-index");
1578     var modelsNode = "models/model[" ~ id_model ~ "]";
1579     props.globals.getNode(modelsNode).remove();
1580     #print("winch model removed");
1581   }
1582
1583   var initial_length_m = getprop("sim/hitches/winch/winch/initial-tow-length-m");
1584   var ac_pos = geo.aircraft_position();              # get position of aircraft
1585   var ac_hd  = getprop("orientation/heading-deg");   # get heading of aircraft
1586
1587   # setup winch
1588   # get initial runway position
1589   var ipos_lat_deg = getprop("sim/presets/latitude-deg");
1590   var ipos_lon_deg = getprop("sim/presets/longitude-deg");
1591   var ipos_hd_deg  = getprop("sim/presets/heading-deg");
1592   var ipos_alt_m = geo.elevation(ipos_lat_deg,ipos_lon_deg);
1593   var ipos_geo = geo.Coord.new().set_latlon(ipos_lat_deg, ipos_lon_deg, ipos_alt_m);
1594   # offset to initial position
1595   var deviation = (ac_pos.distance_to(ipos_geo));
1596   # if deviation is too much, locate winch in front of glider, otherwise locate winch to end of runway
1597   if ( deviation > 200) {
1598     var w = ac_pos.apply_course_distance( ac_hd , initial_length_m -1. );
1599   }
1600   else {
1601     var w = ipos_geo.apply_course_distance( ipos_hd_deg , initial_length_m - 1. );
1602   }
1603   var wpalt = geo.elevation(w.lat(), w.lon());
1604   w.set_alt(wpalt);
1605
1606   var winchModel = geo.put_model("Models/Airport/supacat_winch.xml", w.lat(), w.lon(), (w.alt()+0.81), (w.course_to(ac_pos) ));
1607
1608   setprop("/sim/hitches/winch/winch/global-pos-x", w.x());
1609   setprop("/sim/hitches/winch/winch/global-pos-y", w.y());
1610   setprop("/sim/hitches/winch/winch/global-pos-z", w.z());
1611
1612   setprop("sim/hitches/winch/tow/dist",initial_length_m - 1.);
1613   setprop("sim/hitches/winch/tow/length",initial_length_m);
1614
1615   #print("name=",winchModel.getName(),"  Index=",winchModel.getIndex(),"  Type=",winchModel.getType() );
1616   #print("val=",winchModel.getValue(),"  children=",winchModel.getChildren(),"  size=",size(winchModel) );
1617   setprop("/sim/hitches/winch/winch/winch-model-index",winchModel.getIndex() );
1618   setprop("sim/messages/pilot", sprintf("Connected to winch!"));
1619
1620   props.globals.getNode("sim/hitches/winch/open").setBoolValue(0);
1621
1622 } # End function setWinchPositionAuto
1623
1624
1625 # ######################################################################################################################
1626 #                                                  clutch / un-clutch winch
1627 # ######################################################################################################################
1628
1629 var runWinch = func {
1630
1631   if ( !getprop("sim/hitches/winch/winch/clutched") ) {
1632     setprop("sim/hitches/winch/winch/clutched","true");
1633     setprop("sim/messages/pilot", sprintf("Winch clutched!"));
1634   }
1635   else {
1636     setprop("sim/hitches/winch/winch/clutched","false");
1637     setprop("sim/messages/pilot", sprintf("Winch un-clutched!"));
1638   }
1639
1640 } # End function runWinch
1641
1642
1643 # ######################################################################################################################
1644 #                                                     release winch
1645 # ######################################################################################################################
1646
1647 var releaseWinch = func {
1648
1649   setprop("sim/hitches/winch/open","true");
1650
1651 } # End function releaseWinch
1652
1653
1654 # ######################################################################################################################
1655 #                                                  assignHitchLocations
1656 # ######################################################################################################################
1657
1658 var assignHitchLocations = func (device){
1659
1660   if ( getprop("sim/flight-model") == "yasim" ) return; # bypass this routine for Yasim-aircraft
1661
1662   if ( getprop("sim/hitches/" ~ device ~ "/decoupled-force-and-rope-locations") ) return; # bypass this routine
1663
1664   #print("assignHitchLocations");
1665
1666   var in2m = 0.0254;
1667
1668   var hitchname = getprop("sim/hitches/" ~ device ~ "/force_name_jsbsim");
1669
1670   # location-x(yz)-in: JSBSim Structural Frame: x points to tail, y points to right wing, z points upward
1671   # local-pos-x(yz):   YaSim frame:             x points to nose, y points to left wing,  z points upward
1672
1673   setprop("sim/hitches/" ~ device ~ "/local-pos-x",
1674     - getprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/location-x-in") * in2m );
1675   setprop("sim/hitches/" ~ device ~ "/local-pos-y",
1676     - getprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/location-y-in") * in2m );
1677   setprop("sim/hitches/" ~ device ~ "/local-pos-z",
1678       getprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "/location-z-in") * in2m );
1679
1680 } # End function assignHitchLocations
1681
1682
1683 # ######################################################################################################################
1684 #                                                      point transformation
1685 # ######################################################################################################################
1686
1687 var PointRotate3D = func (x,y,z,xr,yr,zr,alpha_deg,beta_deg,gamma_deg){
1688
1689   # ---------------------------------------------------------------------------------
1690   #   rotates point (x,y,z) about all 3 cartesian axis
1691   #   center of rotation (xr,yr,zr)
1692   #   angle of rotation about x-axis = alpha
1693   #   angle of rotation about y-axis = beta
1694   #   angle of rotation about z-axis = gamma
1695   #   delivers new point coordinates (x_new,y_new,z_new)
1696   # ---------------------------------------------------------------------------------
1697   #
1698   #
1699   # Definitions:
1700   # ----------------
1701   #
1702   # x        y           z
1703   # alpha    beta        gamma
1704   #
1705   #
1706   #       z
1707   #       |  y
1708   #       | /
1709   #       |/
1710   #       ----->x
1711   #
1712   #----------------------------------------------------------------------------------
1713
1714   # Transformation in rotation-system X_rel = X-Xr = (x-xr, y-yr, z-zr)
1715   var x_rel = x-xr;
1716   var y_rel = y-yr;
1717   var z_rel = z-zr;
1718
1719   # Trigonometry
1720   var deg2rad = math.pi / 180.;
1721
1722   var alpha_rad    = deg2rad * alpha_deg;
1723   var beta_rad     = deg2rad * beta_deg;
1724   var gamma_rad    = deg2rad * gamma_deg;
1725
1726   var sin_alpha = math.sin(alpha_rad);
1727   var cos_alpha = math.cos(alpha_rad);
1728
1729   var sin_beta  = math.sin(beta_rad);
1730   var cos_beta  = math.cos(beta_rad);
1731
1732   var sin_gamma = math.sin(gamma_rad);
1733   var cos_gamma = math.cos(gamma_rad);
1734
1735   # Matrices
1736   #
1737   # Rotate about x-axis Rx(alpha)
1738   #
1739   #             Rx11 Rx12 Rx13      1     0            0
1740   # Rx(alpha)=  Rx21 Rx22 Rx23   =  0  cos(alpha)  -sin(alpha)
1741   #             Rx31 Rx32 Rx33      0  sin(alpha)   cos(alpha)
1742   #
1743   var Rx11 = 1.;
1744   var Rx12 = 0.;
1745   var Rx13 = 0.;
1746   var Rx21 = 0.;
1747   var Rx22 = cos_alpha;
1748   var Rx23 = - sin_alpha;
1749   var Rx31 = 0.;
1750   var Rx32 = sin_alpha;
1751   var Rx33 = cos_alpha;
1752   #
1753   # Rotate about y-axis Ry(beta)
1754   #
1755   #            Ry11 Ry12 Ry13      cos(beta)  0   sin(beta)
1756   # Ry(beta)=  Ry21 Ry22 Ry23   =      0      1      0
1757   #            Ry31 Ry32 Ry33     -sin(beta)  0   cos(beta)
1758   #
1759   var Ry11 = cos_beta;
1760   var Ry12 = 0.;
1761   var Ry13 = sin_beta;
1762   var Ry21 = 0.;
1763   var Ry22 = 1.;
1764   var Ry23 = 0.;
1765   var Ry31 = - sin_beta;
1766   var Ry32 = 0.;
1767   var Ry33 = cos_beta;
1768   #
1769   # Rotate about z-axis Rz(gamma)
1770   #
1771   #            Rz11 Rz12 Rz13      cos(gamma)  -sin(gamma)  0
1772   # Rz(gamma)= Rz21 Rz22 Rz23   =  sin(gamma)   cos(gamma)  0
1773   #            Rz31 Rz32 Rz33          0            0       1
1774   #
1775   var Rz11 = cos_gamma;
1776   var Rz12 = - sin_gamma;
1777   var Rz13 = 0.;
1778   var Rz21 = sin_gamma;
1779   var Rz22 = cos_gamma;
1780   var Rz23 = 0.;
1781   var Rz31 = 0.;
1782   var Rz32 = 0.;
1783   var Rz33 = 1.;
1784   #
1785   # First rotation about x-axis
1786   # X_x = Rx*X_rel
1787   var x_x = Rx11 * x_rel + Rx12 * y_rel + Rx13 * z_rel;
1788   var y_x = Rx21 * x_rel + Rx22 * y_rel + Rx23 * z_rel;
1789   var z_x = Rx31 * x_rel + Rx32 * y_rel + Rx33 * z_rel;
1790   #
1791   # subsequent rotation about y-axis
1792   # X_xy = Ry*X_x
1793   var x_xy = Ry11 * x_x + Ry12 * y_x + Ry13 * z_x;
1794   var y_xy = Ry21 * x_x + Ry22 * y_x + Ry23 * z_x;
1795   var z_xy = Ry31 * x_x + Ry32 * y_x + Ry33 * z_x;
1796   #
1797   # subsequent rotation about z-axis:
1798   # X_xyz = Rz*X_xy
1799   var x_xyz = Rz11 * x_xy + Rz12 * y_xy + Rz13 * z_xy;
1800   var y_xyz = Rz21 * x_xy + Rz22 * y_xy + Rz23 * z_xy;
1801   var z_xyz = Rz31 * x_xy + Rz32 * y_xy + Rz33 * z_xy;
1802
1803   # Back transformation  X_rel = X-Xr = (x-xr, y-yr, z-zr)
1804   var xn = xr + x_xyz;
1805   var yn = yr + y_xyz;
1806   var zn = zr + z_xyz;
1807
1808   var Xn = [xn,yn,zn];
1809
1810   return Xn;
1811
1812 }
1813
1814 ##################################################################################################################################
1815
1816
1817 # todo:
1818 # ------
1819 #
1820 # - animate rope slack
1821 # - pull in towrope: take sink rate of rope into account
1822 # - dynamic ID for ai-rope-model
1823 #
1824 # Please contact D_NXKT at yahoo.de for bug-reports, suggestions, ...
1825 #