Aircraft/DG-101G/Nasal/winch.nas

   1 # ####################################################################################
   2 # ####################################################################################
   3 # Nasal script to manage winch-launch for JSBSIM gliders (like DG-101G)
   4 #
   5 # ####################################################################################
   6 # Author: Klaus Kerner
   7 # Version: 2012-07-11
   8 #
   9 # ####################################################################################
  10 # To Do's
  11 #
  12 # - animation of winch rope with parachute
  13 # - currently the winch and rope are removed after a certain time after release,
  14 #   for the future the removal should be done when the rope is completely fed in
  15 #
  16 # ####################################################################################
  17 # Concepts:
  18 # 1. check, whether the initial conditions are fullfilled:
  19 #    - we are on ground
  20 #    - we have no speed
  21 #    - up to now there is no winch placed
  22 #    and if we are fine,place a winch in front of the plane and
  23 #    initialize all relevant parameters
  24 # 2. attach to and run winch
  25 #    The winch applies a certain force on the rope contact location at the plane. This
  26 #    force is dependent on several factors: rope speed, rope angle (glider altitude),
  27 #    nominal pull force (dependend on glider type), ...
  28 #    Rope speed: The force is constant up to 85% of max rope speed.
  29 #                From 85% up to 100% of max. rope speed the force decreases to 0%.
  30 #    Rope angle: Up to 70% the force does not change. From 70% up to 100% (the
  31 #                automatic release angle) the force will decrease to 30%.
  32 # 3. release from winch
  33 #    - by reaching conditions
  34 #    - manually
  35 # 4. and finally remove it
  36
  37
  38 # ###  IMPORTANT  ###  IMPORTANT  ###  IMPORTANT #####################################
  39 # ## essential proterties from the jsbsim FDM, that are required for working winch
  40 # /fdm/jsbsim/fcs/winch-cmd-norm                         created by jsbsim config file
  41 #                                                          1: winch is engaged
  42 #                                                          0: winch is not engaged
  43 # /fdm/jsbsim/external_reactions/winchx/magnitude        created by jsbsim config file
  44 # /fdm/jsbsim/external_reactions/winchy/magnitude        created by jsbsim config file
  45 # /fdm/jsbsim/external_reactions/winchz/magnitude        created by jsbsim config file
  46
  47 # ## existing proterties, that are used to handle the winch
  48 # /gear/gear/wow                                         indicating contact to ground
  49 # /orientation/heading-deg                               indicating current heading
  50 # /orientation/pitch-deg                                 indicating current pitch
  51 # /orientation/roll-deg                                  indicating current roll
  52 # /sim/presets/heading-deg                               initial heading at runway
  53 # /sim/presets/latitude-deg                              initial position at runway
  54 # /sim/presets/longitude-deg                             initial position at runway
  55
  56 # ## new properties, used to manage the winch
  57 # /sim/glider/winch/conf/rope_initial_length_m           initial rope length
  58 # /sim/glider/winch/conf/pull_max_lbs                    max. pulling force
  59 # /sim/glider/winch/conf/pull_max_speed_mps              max. pulling speed
  60 # /sim/glider/winch/conf/k_speed_x1                      parameter for force reduction
  61 # /sim/glider/winch/conf/k_speed_y1                      parameter for force reduction
  62 # /sim/glider/winch/conf/k_speed_x2                      parameter for force reduction
  63 # /sim/glider/winch/conf/k_speed_y2                      parameter for force reduction
  64 # /sim/glider/winch/conf/k_angle_x1                      parameter for force reduction
  65 # /sim/glider/winch/conf/k_angle_y1                      parameter for force reduction
  66 # /sim/glider/winch/conf/k_angle_x2                      parameter for force reduction
  67 # /sim/glider/winch/conf/k_angle_y2                      parameter for force reduction
  68 # /sim/glider/winch/glob/rope_initial_length_m           global rope length
  69 # /sim/glider/winch/glob/pull_max_lbs                    global max. pulling force
  70 # /sim/glider/winch/glob/pull_max_speed_mps              global max. pulling speed
  71 # /sim/glider/winch/glob/k_speed_x1                      parameter for force reduction
  72 # /sim/glider/winch/glob/k_speed_y1                      parameter for force reduction
  73 # /sim/glider/winch/glob/k_speed_x2                      parameter for force reduction
  74 # /sim/glider/winch/glob/k_speed_y2                      parameter for force reduction
  75 # /sim/glider/winch/glob/k_angle_x1                      parameter for force reduction
  76 # /sim/glider/winch/glob/k_angle_y1                      parameter for force reduction
  77 # /sim/glider/winch/glob/k_angle_x2                      parameter for force reduction
  78 # /sim/glider/winch/glob/k_angle_y2                      parameter for force reduction
  79 # /sim/glider/winch/work/wp-lat-deg                      storing winch position
  80 # /sim/glider/winch/work/wp-lon-deg                      storing winch position
  81 # /sim/glider/winch/work/wp-alti-m                       storing winch position
  82 # /sim/glider/winch/work/speed                           current speed of rope
  83 # /sim/glider/winch/work/rope_m                          current length of rope
  84 # /sim/glider/winch/flags/placed                          flag for existence of winch
  85 #                                                          1: winch placed
  86 #                                                          0: winch not placed
  87 # /sim/glider/winch/flags/used                            bool storing allready used
  88 #                                                          winch to prevent against
  89 #                                                          reconnecting
  90 # /sim/glider/winch/flags/pull                            bool storing working of winch
  91 #                                                          1: winch is working
  92 #                                                          0: winch is not working
  93 # /sim/glider/winch/flags/hooked                          bool storing release from tow
  94 #                                                          5: tow allready released
  95 #                                                          1. tow not released
  96 # /sim/glider/winchrope/flags/exist                       bool storing existence of rope
  97 #
  98 #
  99 # flags matrix
 100 #  function()      placed used  pull  hooked  exist
 101 #  at startup      0      -     -     -       -
 102 #  createWinch()   1      0     -     1       1
 103 #  startWinch()   (1)     1     1    (1)     (1)
 104 #  runWinch()     (1)    (1)   (1)   (1)     (1)
 105 #  releaseWinch() (1)    (1)   (1)    0      (1)
 106 #  removeWinch()   0     (1)    0    (0)      0
 107
 108
 109 # ####################################################################################
 110 # ####################################################################################
 111 # global variables for this script
 112   var winch_timeincrement_s = 0;                       # timer increment
 113
 114
 115
 116 # ####################################################################################
 117 # ####################################################################################
 118 # set winch parameters to global values, if not properly defined by plane setup-file
 119 # store global values or plane-specific values to prepare for reset option
 120 var globalsWinch = func {
 121   var glob_rope_initial_length_m = 800;                 # default length 800m
 122   var glob_pull_max_lbs = 1102;                         # default force 1102lbs=500daN
 123   var glob_pull_max_speed_mps = 40;                     # default speed 40m/s
 124   var glob_k_speed_x1 = 0.85;
 125   var glob_k_speed_y1 = 1.00;
 126   var glob_k_speed_x2 = 1.00;
 127   var glob_k_speed_y2 = 0.00;
 128   var glob_k_angle_x1 = 0.75;
 129   var glob_k_angle_y1 = 1.00;
 130   var glob_k_angle_x2 = 1.00;
 131   var glob_k_angle_y2 = 0.30;
 132   # set initial rope length if not defined from "plane"-set.xml
 133   if ( getprop("sim/glider/winch/conf/rope_initial_length_m") == nil ) {
 134     setprop("sim/glider/winch/conf/rope_initial_length_m",
 135              glob_rope_initial_length_m);
 136     setprop("sim/glider/winch/glob/rope_initial_length_m",
 137              glob_rope_initial_length_m);
 138   }
 139   else { # if defined, set global to plane specific for reset option
 140     setprop("sim/glider/winch/glob/rope_initial_length_m",
 141             getprop("sim/glider/winch/conf/rope_initial_length_m"));
 142   }
 143
 144   # set max force for pulling, if not defined from "plane"-set.xml
 145   if ( getprop("sim/glider/winch/conf/pull_max_lbs") == nil ) {
 146     setprop("sim/glider/winch/conf/pull_max_lbs", glob_pull_max_lbs);
 147     setprop("sim/glider/winch/glob/pull_max_lbs", glob_pull_max_lbs);
 148   }
 149   else { # if defined, set global to plane specific for reset option
 150     setprop("sim/glider/winch/glob/pull_max_lbs",
 151             getprop("sim/glider/winch/conf/pull_max_lbs"));
 152   }
 153
 154   # set max speed for pulling, if not defined from "plane"-set.xml
 155   if ( getprop("sim/glider/winch/conf/pull_max_speed_mps") == nil ) {
 156     setprop("sim/glider/winch/conf/pull_max_speed_mps", glob_pull_max_speed_mps);
 157     setprop("sim/glider/winch/glob/pull_max_speed_mps", glob_pull_max_speed_mps);
 158   }
 159   else { # if defined, set global to plane specific for reset option
 160     setprop("sim/glider/winch/glob/pull_max_speed_mps",
 161             getprop("sim/glider/winch/conf/pull_max_speed_mps"));
 162   }
 163
 164   # set ref-poing x1 for speed correction, if not defined from "plane"-set.xml
 165   if ( getprop("sim/glider/winch/conf/k_speed_x1") == nil ) {
 166     setprop("sim/glider/winch/conf/k_speed_x1", glob_k_speed_x1);
 167     setprop("sim/glider/winch/glob/k_speed_x1", glob_k_speed_x1);
 168   }
 169   else { # if defined, set global to plane specific for reset option
 170     setprop("sim/glider/winch/glob/k_speed_x1",
 171             getprop("sim/glider/winch/conf/k_speed_x1"));
 172   }
 173
 174   # set ref-poing y1 for speed correction, if not defined from "plane"-set.xml
 175   if ( getprop("sim/glider/winch/conf/k_speed_y1") == nil ) {
 176     setprop("sim/glider/winch/conf/k_speed_y1", glob_k_speed_y1);
 177     setprop("sim/glider/winch/glob/k_speed_y1", glob_k_speed_y1);
 178   }
 179   else { # if defined, set global to plane specific for reset option
 180     setprop("sim/glider/winch/glob/k_speed_y1",
 181             getprop("sim/glider/winch/conf/k_speed_y1"));
 182   }
 183
 184   # set ref-poing x2 for speed correction, if not defined from "plane"-set.xml
 185   if ( getprop("sim/glider/winch/conf/k_speed_x2") == nil ) {
 186     setprop("sim/glider/winch/conf/k_speed_x2", glob_k_speed_x2);
 187     setprop("sim/glider/winch/glob/k_speed_x2", glob_k_speed_x2);
 188   }
 189   else { # if defined, set global to plane specific for reset option
 190     setprop("sim/glider/winch/glob/k_speed_x2",
 191             getprop("sim/glider/winch/conf/k_speed_x2"));
 192   }
 193
 194   # set ref-poing y2 for speed correction, if not defined from "plane"-set.xml
 195   if ( getprop("sim/glider/winch/conf/k_speed_y2") == nil ) {
 196     setprop("sim/glider/winch/conf/k_speed_y2", glob_k_speed_y2);
 197     setprop("sim/glider/winch/glob/k_speed_y2", glob_k_speed_y2);
 198   }
 199   else { # if defined, set global to plane specific for reset option
 200     setprop("sim/glider/winch/glob/k_speed_y2",
 201             getprop("sim/glider/winch/conf/k_speed_y2"));
 202   }
 203
 204   # set ref-poing x1 for angle correction, if not defined from "plane"-set.xml
 205   if ( getprop("sim/glider/winch/conf/k_angle_x1") == nil ) {
 206     setprop("sim/glider/winch/conf/k_angle_x1", glob_k_angle_x1);
 207     setprop("sim/glider/winch/glob/k_angle_x1", glob_k_angle_x1);
 208   }
 209   else { # if defined, set global to plane specific for reset option
 210     setprop("sim/glider/winch/glob/k_angle_x1",
 211             getprop("sim/glider/winch/conf/k_angle_x1"));
 212   }
 213
 214   # set ref-poing y1 for angle correction, if not defined from "plane"-set.xml
 215   if ( getprop("sim/glider/winch/conf/k_angle_y1") == nil ) {
 216     setprop("sim/glider/winch/conf/k_angle_y1", glob_k_angle_y1);
 217     setprop("sim/glider/winch/glob/k_angle_y1", glob_k_angle_y1);
 218   }
 219   else { # if defined, set global to plane specific for reset option
 220     setprop("sim/glider/winch/glob/k_angle_y1",
 221             getprop("sim/glider/winch/conf/k_angle_y1"));
 222   }
 223
 224   # set ref-poing x2 for angle correction, if not defined from "plane"-set.xml
 225   if ( getprop("sim/glider/winch/conf/k_angle_x2") == nil ) {
 226     setprop("sim/glider/winch/conf/k_angle_x2", glob_k_angle_x2);
 227     setprop("sim/glider/winch/glob/k_angle_x2", glob_k_angle_x2);
 228   }
 229   else { # if defined, set global to plane specific for reset option
 230     setprop("sim/glider/winch/glob/k_angle_x2",
 231             getprop("sim/glider/winch/conf/k_angle_x2"));
 232   }
 233
 234   # set ref-poing y2 for angle correction, if not defined from "plane"-set.xml
 235   if ( getprop("sim/glider/winch/conf/k_angle_y2") == nil ) {
 236     setprop("sim/glider/winch/conf/k_angle_y2", glob_k_angle_y2);
 237     setprop("sim/glider/winch/glob/k_angle_y2", glob_k_angle_y2);
 238   }
 239   else { # if defined, set global to plane specific for reset option
 240     setprop("sim/glider/winch/glob/k_angle_y2",
 241             getprop("sim/glider/winch/conf/k_angle_y2"));
 242   }
 243
 244 } # End Function globalsWinch
 245
 246
 247
 248 # ####################################################################################
 249 # ####################################################################################
 250 # reset winch parameters to global values
 251 var resetWinch = func {
 252   # set rope length to global
 253   setprop("sim/glider/winch/conf/rope_initial_length_m",
 254             getprop("sim/glider/winch/glob/rope_initial_length_m"));
 255
 256   # set max force for pulling to global
 257   setprop("sim/glider/winch/conf/pull_max_lbs",
 258             getprop("sim/glider/winch/glob/pull_max_lbs"));
 259
 260   # set max speed for pulling to global
 261   setprop("sim/glider/winch/conf/pull_max_speed_mps",
 262             getprop("sim/glider/winch/glob/pull_max_speed_mps"));
 263
 264   # set speed correction for pulling to global
 265   setprop("sim/glider/winch/conf/k_speed_x1",
 266             getprop("sim/glider/winch/glob/k_speed_x1"));
 267   setprop("sim/glider/winch/conf/k_speed_y1",
 268             getprop("sim/glider/winch/glob/k_speed_y1"));
 269   setprop("sim/glider/winch/conf/k_speed_x2",
 270             getprop("sim/glider/winch/glob/k_speed_x2"));
 271   setprop("sim/glider/winch/conf/k_speed_y2",
 272             getprop("sim/glider/winch/glob/k_speed_y2"));
 273
 274   # set angle correction for pulling to global
 275   setprop("sim/glider/winch/conf/k_angle_x1",
 276             getprop("sim/glider/winch/glob/k_angle_x1"));
 277   setprop("sim/glider/winch/conf/k_angle_y1",
 278             getprop("sim/glider/winch/glob/k_angle_y1"));
 279   setprop("sim/glider/winch/conf/k_angle_x2",
 280             getprop("sim/glider/winch/glob/k_angle_x2"));
 281   setprop("sim/glider/winch/conf/k_angle_y2",
 282             getprop("sim/glider/winch/glob/k_angle_y2"));
 283
 284 } # End Function resetWinch
 285
 286
 287
 288 # ####################################################################################
 289 # ####################################################################################
 290 # Place winch model in correct location
 291 var placeWinch = func {
 292   # first check for an existing winch
 293   # if: winch exists,
 294   #   print message and exit
 295   # else: (winch does not exist, )
 296   #   if plane is on ground
 297   #     get the position of the plane
 298   #     if plane is close enough to initial position on runway
 299   #       calculate position of winch relative to initial position on runway,
 300   #     else: (plane is too far away from initial position)
 301   #       calculate position of winch relative to current position of plane
 302   #     place model
 303   #     set properties winch/flags/placed and jsbsim/fcs/winch-cmd-norm
 304   #     print message
 305   #   else: (plane is in air)
 306   #     print message and exit
 307
 308   var rope_initial_length_m = getprop("sim/glider/winch/conf/rope_initial_length_m");
 309
 310
 311   if ( getprop("sim/glider/winch/flags/placed") == 1 ) {
 312     atc_msg("Winch allready placed");
 313   }
 314   else {
 315     if ( getprop("gear/gear/wow") ) {
 316      # common variables
 317       var ac_pos = geo.aircraft_position();
 318       var ac_hd  = getprop("orientation/heading-deg");       # get heading of aircraft
 319
 320      # setup winch
 321       # set pathes to relevant nodes in the property tree
 322       var winch_ai  = props.globals.getNode("ai/models/winch", 1);
 323       var winch_mod = props.globals.getNode("models", 1);
 324       var winch_sim = props.globals.getNode("sim/glider/winch/work", 1);
 325       var winch_flg = props.globals.getNode("sim/glider/winch/flags", 1);
 326       # get initial runway position
 327       var ipos_lat_deg = getprop("sim/presets/latitude-deg");
 328       var ipos_lon_deg = getprop("sim/presets/longitude-deg");
 329       var ipos_hd_deg  = getprop("sim/presets/heading-deg");
 330       var ipos_alt_m = geo.elevation(ipos_lat_deg,ipos_lon_deg);
 331       var ipos_geo = geo.Coord.new().set_latlon(
 332                       ipos_lat_deg, ipos_lon_deg, ipos_alt_m);
 333       # offset to initial position
 334       # if deviation is too much, locate winch in front of glider, otherwise locate
 335       # winch to end of runway
 336       var deviation = (ac_pos.distance_to(ipos_geo));
 337       if ( deviation > 200) {
 338         var wpos_geo = ac_pos.apply_course_distance( ac_hd ,
 339                    rope_initial_length_m );
 340       }
 341       else {
 342         var wpos_geo = ipos_geo.apply_course_distance( ipos_hd_deg ,
 343                    rope_initial_length_m );
 344       }
 345       var wpalt = geo.elevation(wpos_geo.lat(), wpos_geo.lon());
 346       wpos_geo.set_alt(wpalt);
 347       # get the next free ai id and model id
 348       var freeModelid = getFreeModelID();
 349       freeModelid = 9997;
 350       # setup working properties
 351       winch_sim.getNode("id_AI", 1).setIntValue(9997);
 352       winch_sim.getNode("id_model", 1).setIntValue(freeModelid);
 353       winch_sim.getNode("wp-lat-deg", 1).setValue(wpos_geo.lat());
 354       winch_sim.getNode("wp-lon-deg", 1).setValue(wpos_geo.lon());
 355       winch_sim.getNode("wp-alti-m", 1).setValue(wpos_geo.alt());
 356       # setup ai properties
 357       winch_ai.getNode("id", 1).setIntValue(9997);
 358       winch_ai.getNode("callsign", 1).setValue("winch");
 359       winch_ai.getNode("valid", 1).setBoolValue(1);
 360       winch_ai.getNode("position/latitude-deg", 1).setValue(wpos_geo.lat());
 361       winch_ai.getNode("position/longitude-deg", 1).setValue(wpos_geo.lon());
 362       winch_ai.getNode("position/altitude-ft", 1).setValue(wpos_geo.alt() * M2FT);
 363       winch_ai.getNode("orientation/true-heading-deg", 1).setValue(ac_hd);
 364       winch_ai.getNode("orientation/pitch-deg", 1).setValue(0);
 365       # setup model properties
 366       winch_mod.model = winch_mod.getChild("model", freeModelid, 1);
 367       winch_mod.model.getNode("path", 1).setValue(
 368             "/Models/Airport/supacat_winch.ac");
 369       winch_mod.model.getNode("longitude-deg-prop", 1).setValue(
 370             "ai/models/winch/position/longitude-deg");
 371       winch_mod.model.getNode("latitude-deg-prop", 1).setValue(
 372             "ai/models/winch/position/latitude-deg");
 373       winch_mod.model.getNode("elevation-ft-prop", 1).setValue(
 374             "ai/models/winch/position/altitude-ft");
 375       winch_mod.model.getNode("heading-deg-prop", 1).setValue(
 376             "ai/models/winch/orientation/true-heading-deg");
 377       winch_mod.model.getNode("pitch-deg-prop", 1).setValue(
 378             "ai/models/winch/orientation/pitch-deg");
 379       winch_mod.model.getNode("load", 1).remove();
 380       # set flags for placed winch and prevent against additional winches
 381       winch_flg.getNode("placed", 1).setIntValue(1);
 382       winch_flg.getNode("used", 1).setIntValue(0);
 383       winch_flg.getNode("hooked", 1).setIntValue(1);
 384
 385      # setup rope and parachute for animation
 386       # set pathes to relevant nodes in the property tree
 387       var winchrope_ai  = props.globals.getNode("ai/models/winchrope", 1);
 388       var winchrope_mod = props.globals.getNode("models", 1);
 389       var winchrope_sim = props.globals.getNode("sim/glider/winchrope/work", 1);
 390       var winchrope_flg = props.globals.getNode("sim/glider/winchrope/flags", 1);
 391       # initial rope position, at nose of glider
 392       var rope_length_m = (ac_pos.direct_distance_to(wpos_geo));
 393       var rope_heading_deg = (ac_pos.course_to(wpos_geo));
 394       var rope_pitch_deg = 10; # must be corrected by arcsin function for glider to winch height relation
 395       var install_distance_m = 0.05; # 0.05m in front of ref-point of glider, must be tuned
 396       var install_alt_m = -1; # 1m below ref-point of glider, must be tuned
 397       var rope_pos    = ac_pos.apply_course_distance( ac_hd , install_distance_m );
 398       rope_pos.set_alt(ac_pos.alt() + install_alt_m);       # correct hight by pitch
 399       # get the next free ai id and model id
 400       var freeModelid = getFreeModelID();
 401       freeModelid = 9996;
 402       # setup working properties
 403       winchrope_sim.getNode("id_AI", 1).setIntValue(9996);
 404       winchrope_sim.getNode("id_model", 1).setIntValue(freeModelid);
 405       winchrope_sim.getNode("rope_length_m", 1).setValue(rope_length_m);
 406       winchrope_sim.getNode("rope_heading_deg", 1).setValue(rope_heading_deg);
 407       winchrope_sim.getNode("rope_pitch_deg", 1).setValue(rope_pitch_deg);
 408       winchrope_sim.getNode("hook_x_m", 1).setValue(install_distance_m);
 409       winchrope_sim.getNode("hook_z_m", 1).setValue(install_alt_m);
 410       winchrope_sim.getNode("scale_x", 1).setValue(1);
 411       winchrope_sim.getNode("scale_yz", 1).setValue(1);
 412       # setup ai properties
 413       winchrope_ai.getNode("id", 1).setIntValue(9996);
 414       winchrope_ai.getNode("callsign", 1).setValue("winchrope");
 415       winchrope_ai.getNode("valid", 1).setBoolValue(1);
 416       winchrope_ai.getNode("position/latitude-deg", 1).setValue(rope_pos.lat());
 417       winchrope_ai.getNode("position/longitude-deg", 1).setValue(rope_pos.lon());
 418       winchrope_ai.getNode("position/altitude-ft", 1).setValue(rope_pos.alt() * M2FT);
 419       winchrope_ai.getNode("orientation/true-heading-deg", 1).setValue(rope_heading_deg);
 420       winchrope_ai.getNode("orientation/pitch-deg", 1).setValue(0);
 421       # setup model properties
 422       winchrope_mod.model = winchrope_mod.getChild("model", freeModelid, 1);
 423       winchrope_mod.model.getNode("path", 1).setValue(
 424             "Aircraft/DG-101G/Models/Ropes/winchrope.xml");
 425       winchrope_mod.model.getNode("longitude-deg-prop", 1).setValue(
 426             "ai/models/winchrope/position/longitude-deg");
 427       winchrope_mod.model.getNode("latitude-deg-prop", 1).setValue(
 428             "ai/models/winchrope/position/latitude-deg");
 429       winchrope_mod.model.getNode("elevation-ft-prop", 1).setValue(
 430             "ai/models/winchrope/position/altitude-ft");
 431       winchrope_mod.model.getNode("heading-deg-prop", 1).setValue(
 432             "ai/models/winchrope/orientation/true-heading-deg");
 433       winchrope_mod.model.getNode("pitch-deg-prop", 1).setValue(
 434             "ai/models/winchrope/orientation/pitch-deg");
 435       winchrope_mod.model.getNode("load", 1).remove();
 436       # setup flags
 437       winchrope_flg.getNode("exist", 1).setIntValue(1);
 438
 439      # finally send message
 440       atc_msg("Winch placed in front of you");
 441     }
 442     else {
 443       atc_msg("winch in air useless, no winch placed");
 444     }
 445   }
 446 } # End Function placeWinch
 447
 448
 449
 450
 451 # ####################################################################################
 452 # get the next free id of models/model members
 453 # required for animation of towing rope
 454 # should be shifted to a generic module as same function exists in dragrobot.nas
 455 var getFreeModelID = func {
 456
 457   #local variables
 458   var modelid = 0;                                 # for the next unsused id
 459   var modelobjects = {};                           # vector to keep all model objects
 460
 461   modelobjects = props.globals.getNode("models", 1).getChildren(); # get model objects
 462   foreach ( var member; modelobjects ) {
 463     # get data from member
 464     if ( (var c = member.getNode("id")) != nil) {
 465       var id = c.getValue();
 466       if ( modelid <= id ) {
 467         modelid = id +1;
 468       }
 469     }
 470   }
 471   return(modelid);
 472 }
 473
 474
 475
 476
 477 # ####################################################################################
 478 # ####################################################################################
 479 # starts the winch
 480 var startWinch = func {
 481   # first check for an existing winch
 482   # if the winch exists
 483   #   if the winch was never used
 484   #     close the hook,
 485   #     level the plane
 486   #     gets positions of plane and winch
 487   #     calculates distance and assign to property rope_m
 488   #     sets property speed
 489   #     set flags used and pull
 490   #   if used exit
 491   # if not exit
 492
 493   if ( getprop("sim/glider/winch/flags/placed") == 1 ) {     # check for placed winch
 494     if ( getprop("sim/glider/winch/flags/used") == 0 ) {     # check for unused winch
 495       setprop("fdm/jsbsim/fcs/winch-cmd-norm",1);           # closes the hook
 496       atc_msg("hook closed");
 497       setprop("orientation/roll-deg",0);                    # level the plane
 498       atc_msg("glider leveled");
 499       atc_msg("winch starts running");
 500       var wp = geo.Coord.new().set_latlon(
 501           (getprop("sim/glider/winch/work/wp-lat-deg")),
 502           (getprop("sim/glider/winch/work/wp-lon-deg")),
 503           (getprop("sim/glider/winch/work/wp-alti-m")));    # gets winch position
 504       var ac = geo.aircraft_position();                     # gets aircraft position
 505       var dd_m = (ac.direct_distance_to(wp));               # gets distance
 506       setprop("sim/glider/winch/work/rope_m", dd_m );       # set the rope length
 507       setprop("sim/glider/winch/work/speed",0);             # winch has speed 0
 508       setprop("sim/glider/winch/flags/used", 1);             # one time hooked, never
 509                                                             # hook again
 510       setprop("sim/glider/winch/flags/pull",1);              # winch is pulling
 511     }
 512     else {
 513       atc_msg("Sorry, only one time hooking");
 514     }
 515   }
 516   else {                                                    # failure: no winch placed
 517     atc_msg("no winch");
 518   }
 519
 520 } # End Function startWinch
 521
 522
 523
 524 # ####################################################################################
 525 # ####################################################################################
 526 # release the winch
 527 var releaseWinch = func {
 528   # first check for the winch is pulling
 529   # if yes,
 530   #   opens the hook,
 531   #   sets the forces to zero
 532   #   print a message
 533   # if no,
 534   #   print a message and exit
 535
 536   if ( getprop("sim/glider/winch/flags/pull") ) {           # is the winch pulling
 537     setprop("fdm/jsbsim/fcs/winch-cmd-norm",0);                 # opens the hook
 538     setprop("fdm/jsbsim/external_reactions/winchx/magnitude", 0);  # set the
 539     setprop("fdm/jsbsim/external_reactions/winchy/magnitude", 0);  # forces
 540     setprop("fdm/jsbsim/external_reactions/winchz/magnitude", 0);  # to zero
 541     setprop("sim/glider/winchrope/work/scale_x", 0.5);
 542     setprop("sim/glider/winchrope/work/scale_yz", 2);
 543     setprop("sim/glider/winch/flags/hooked",0);
 544     atc_msg("Hook opened, tow released");
 545     print("Hook opened, tow released");
 546   }
 547   else {                                                        # winch not working
 548     atc_msg("not hooked to a winch");
 549   }
 550
 551 } # End Function releaseWinch
 552
 553
 554
 555 # ####################################################################################
 556 # ####################################################################################
 557 # remove the winch
 558 var removeWinch = func {
 559   # local variables
 560   var modelsNode = {};
 561
 562   # look for allready existing ai object with callsign "winchrope"
 563   # check for the winch rope is still existent
 564   # if yes,
 565   #   remove the winch rope from the property tree ai/models
 566   #   remove the winch rope from the property tree models/
 567   #   remove the winch rope working properties
 568   # if no,
 569   #   do nothing
 570
 571   if ( getprop("/sim/glider/winchrope/flags/exist") == 1 ) {   # does the winch rope exist?
 572     # remove 3d model from scenery
 573     # identification is /models/model[x] with x=id_model
 574     var id_model = getprop("sim/glider/winchrope/work/id_model");
 575     modelsNode = "models/model[" ~ id_model ~ "]";
 576     props.globals.getNode(modelsNode).remove();
 577     props.globals.getNode("ai/models/winchrope").remove();
 578     props.globals.getNode("sim/glider/winchrope/work").remove();
 579     atc_msg("winch rope removed");
 580     setprop("/sim/glider/winchrope/flags/exist", 0);
 581   }
 582   else {                                                     # do nothing
 583     atc_msg("winch rope does not exist");
 584   }
 585
 586
 587   # look for allready existing ai object with callsign "winch"
 588   # check for the winch is still existent
 589   # if yes,
 590   #   remove the winch from the property tree ai/models
 591   #   remove the winch from the property tree models/
 592   #   remove the winch working properties
 593   # if no,
 594   #   do nothing
 595
 596   if ( getprop("/sim/glider/winch/flags/placed") == 1 ) {   # does the winch exist?
 597     # remove 3d model from scenery
 598     # identification is /models/model[x] with x=id_model
 599     var id_model = getprop("sim/glider/winch/work/id_model");
 600     modelsNode = "models/model[" ~ id_model ~ "]";
 601     props.globals.getNode(modelsNode).remove();
 602     props.globals.getNode("ai/models/winch").remove();
 603     props.globals.getNode("sim/glider/winch/work").remove();
 604     atc_msg("winch rope removed");
 605     setprop("/sim/glider/winch/flags/pull", 0);
 606     setprop("/sim/glider/winch/flags/placed", 0);
 607   }
 608   else {                                                     # do nothing
 609     atc_msg("winch does not exist");
 610   }
 611
 612 } # End Function removeWinch
 613
 614
 615
 616 # ####################################################################################
 617 # ####################################################################################
 618 # let the winch pull the plane up into the sky
 619 var runWinch = func {
 620   # strategy,
 621   # - pull with max. force
 622   # - pilot to control speed by aoa
 623
 624   # if the winch is pulling
 625   #   get the position of the winch
 626   #   get the position of the glider
 627   #   get heading of glider
 628   #   get heading to winch of glider
 629   #   get direct distance (hypothenuse)
 630   #   get distance (kathete)
 631   #   get pitch of glider
 632   #   get roll of glider
 633   #   get the old rope-length
 634   #   calculate current rope-speed
 635   #   corredt false length
 636   #   calculate relevant angles for force-transformation
 637   #   calculate according sine and cosine values
 638   #   calculate the force vectors at the plane
 639   #   calculate the angle of resulting force vector an glider
 640   #   if angle is high enough
 641   #     release winch
 642   #   if angle is flat
 643   #     apply values to coresponding properties
 644
 645
 646   var roperelease_deg = 70;  # release winch automatically
 647   var install_distance_m = 0.05; # 0.05m in front of ref-point of glider, must be tuned
 648   var install_alt_m = -1; # 1m below ref-point of glider, must be tuned
 649
 650   var pullmax = getprop("sim/glider/winch/conf/pull_max_lbs");
 651   var speedmax = getprop("sim/glider/winch/conf/pull_max_speed_mps");
 652
 653   var k_speed_x1 = getprop("sim/glider/winch/conf/k_speed_x1");
 654   var k_speed_y1 = getprop("sim/glider/winch/conf/k_speed_y1");
 655   var k_speed_x2 = getprop("sim/glider/winch/conf/k_speed_x2");
 656   var k_speed_y2 = getprop("sim/glider/winch/conf/k_speed_y2");
 657   var k_angle_x1 = getprop("sim/glider/winch/conf/k_angle_x1");
 658   var k_angle_y1 = getprop("sim/glider/winch/conf/k_angle_y1");
 659   var k_angle_x2 = getprop("sim/glider/winch/conf/k_angle_x2");
 660   var k_angle_y2 = getprop("sim/glider/winch/conf/k_angle_y2");
 661
 662
 663   if ( winch_timeincrement_s == 0 ) {
 664     var deltatime_s = getprop("sim/time/delta-sec");
 665   }
 666   else {
 667     var deltatime_s = winch_timeincrement_s;
 668   }
 669
 670   if (getprop ("sim/glider/winch/flags/pull")) {        # is a winch placed and working
 671     var wp = geo.Coord.new().set_latlon(
 672         (getprop("sim/glider/winch/work/wp-lat-deg")),
 673         (getprop("sim/glider/winch/work/wp-lon-deg")),
 674         (getprop("sim/glider/winch/work/wp-alti-m"))); # gets winch position
 675     # if hooked, calculate forces and update rope animation
 676     if (getprop("sim/glider/winch/flags/hooked")) {
 677         var ac = geo.aircraft_position();                  # aircraft position
 678         var hd_deg = getprop("orientation/heading-deg");   # heading of aircraft
 679         var hw_deg = (ac.course_to(wp));                   # heading to winch
 680         var dd_m = (ac.direct_distance_to(wp));            # the actual rope length
 681       # calculate forces
 682         var dp_m = (ac.distance_to(wp));                   # projected distance to winch
 683         var pt_deg = getprop("orientation/pitch-deg");     # pitch of aircraft
 684         var rt_deg = getprop("orientation/roll-deg");      # roll of aircraft
 685         var dd_old_m = getprop("sim/glider/winch/work/rope_m");  # last rope length
 686         var ropespeed = (dd_old_m - dd_m)/deltatime_s;     # the speed of the rope
 687         # correct a failure, if the projected length is larger than direct length
 688         if (dp_m > dd_m) { dp_m = dd_m;}
 689         # calculate main angles
 690         var alpha = math.acos( (dp_m / dd_m) );
 691         var beta = ( hw_deg - hd_deg ) * 0.01745;
 692         var gamma = pt_deg * 0.01745;
 693         var delta = rt_deg * 0.01745;
 694         # get trigonometric values from angles
 695         var sina = math.sin(alpha);
 696         var cosa = math.cos(alpha);
 697         var sinb = math.sin(beta);
 698         var cosb = math.cos(beta);
 699         var sing = math.sin(gamma);
 700         var cosg = math.cos(gamma);
 701         var sind = math.sin(delta);
 702         var cosd = math.cos(delta);
 703         # calculate force correction factors due to speed and rope angle
 704         var k_force_speed = k_speed_y1 + (k_speed_y2 - k_speed_y1)/
 705                                          (k_speed_x2 - k_speed_x1)*
 706                                          (ropespeed/speedmax - k_speed_x1);
 707         var k_force_angle = k_angle_y1 + (k_angle_y2 - k_angle_y1)/
 708                                          (k_angle_x2 - k_angle_x1)*
 709                                          (alpha / 0.01745 / roperelease_deg - k_angle_x1);
 710         if ( k_force_speed > k_speed_y1 ) { k_force_speed = k_speed_y1; }
 711         if ( k_force_speed < k_speed_y2 ) { k_force_speed = k_speed_y2; }
 712         if ( k_force_angle > k_angle_y1 ) { k_force_angle = k_angle_y1; }
 713         if ( k_force_angle < k_angle_y2 ) { k_force_angle = k_angle_y2; }
 714         # global forces dependent on: alpha beta
 715         var fglobalx = pullmax * cosa * cosb;
 716         var fglobaly = pullmax * cosa * sinb;
 717         var fglobalz = pullmax * sina;
 718         # local forces dependent on pitch: gamma
 719         var flpitchx = fglobalx * cosg - fglobalz * sing;
 720         var flpitchy = fglobaly;
 721         var flpitchz = fglobalx * sing + fglobalz * cosg;
 722         # local forces dependent on roll: delta
 723         var flrollx  = flpitchx;
 724         var flrolly  = flpitchy * cosd + flpitchz * sind;
 725         var flrollz  = flpitchy * sind + flpitchz * cosd;
 726         # asigning to LOCAL coord of plane
 727         var forcex = flrollx;
 728         var forcey = flrolly;
 729         var forcez = flrollz;
 730         # calculates the force angle, used for automatic release
 731         var force_deg = math.asin( math.sqrt(forcey * forcey + forcez * forcez)
 732           / math.sqrt(forcex * forcex + forcey * forcey + forcez * forcez) )
 733           / 0.01745;
 734         # automatic release if criteria is reached
 735         if (force_deg > roperelease_deg) {
 736           releaseWinch();
 737         }
 738         # otherwise set the current forces
 739         else  {
 740           forcex = forcex * k_force_speed * k_force_angle;
 741           forcey = forcey * k_force_speed * k_force_angle;
 742           forcez = forcez * k_force_speed * k_force_angle;
 743           setprop("fdm/jsbsim/external_reactions/winchx/magnitude",  forcex );
 744           setprop("fdm/jsbsim/external_reactions/winchy/magnitude",  forcey );
 745           setprop("fdm/jsbsim/external_reactions/winchz/magnitude",  forcez );
 746           setprop("sim/glider/winch/work/speed", ropespeed);
 747           setprop("sim/glider/winch/work/rope_m", dd_m );
 748         }
 749       # update rope animation
 750         var hook_pos    = ac.apply_course_distance( hd_deg , install_distance_m );
 751         hook_pos.set_alt(ac.alt() + install_alt_m);
 752         var height = ac.alt() - wp.alt();
 753         if ( ac.alt() > wp.alt() ) {
 754           var wnpitchto = -math.asin((ac.alt()-wp.alt())/dd_m) / 0.01745;
 755         }
 756         else {
 757           var wnpitchto =  math.asin((ac.alt()-wp.alt())/dd_m) / 0.01745;
 758         }
 759         # update position of rope
 760         setprop("ai/models/winchrope/position/latitude-deg", hook_pos.lat());
 761         setprop("ai/models/winchrope/position/longitude-deg", hook_pos.lon());
 762         setprop("ai/models/winchrope/position/altitude-ft", hook_pos.alt() * M2FT);
 763         setprop("sim/glider/winchrope/work/latitude-deg", hook_pos.lat());
 764         setprop("sim/glider/winchrope/work/longitude-deg", hook_pos.lon());
 765         setprop("sim/glider/winchrope/work/altitude-m", hook_pos.alt());
 766         # update length of rope
 767         setprop("sim/glider/winchrope/work/xstretch_rel", dd_m);
 768         # update pitch and heading of rope
 769         setprop("sim/glider/winchrope/work/rope_heading_deg", hw_deg);
 770         setprop("sim/glider/winchrope/work/rope_pitch_deg", wnpitchto);
 771         setprop("ai/models/winchrope/orientation/true-heading-deg", hw_deg);
 772         setprop("ai/models/winchrope/orientation/pitch-deg", wnpitchto);
 773 print("hook: ", hook_pos.lat(), " ", hook_pos.lon(), " ", hook_pos.alt() );
 774     }
 775     else {
 776       # and finally update the rope
 777       var para = geo.Coord.new().set_latlon(
 778           (getprop("sim/glider/winchrope/work/latitude-deg")),
 779           (getprop("sim/glider/winchrope/work/longitude-deg")),
 780           (getprop("sim/glider/winchrope/work/altitude-m"))); # gets parachute position
 781 print("para: ", para.lat(), " ", para.lon(), " ", para.alt() );
 782       var lati = para.lat();
 783           var long = para.lon();
 784       var ground = geo.elevation(lati, long);
 785 print(" elev: ", ground);
 786       if ( para.alt() > 0.0) { # at the moment only dirty workaround as elevation() seems to report elevation of parachute, and not the elevation above ground
 787         # update position of rope
 788         var sink = para.alt() - deltatime_s * 5.0;
 789         setprop("ai/models/winchrope/position/altitude-ft", sink * M2FT);
 790         setprop("sim/glider/winchrope/work/altitude-m", sink);
 791       }
 792       else {
 793         # remove winch
 794         removeWinch();
 795       }
 796     }
 797
 798     settimer(runWinch, winch_timeincrement_s);
 799
 800   }
 801
 802 } # End Function runWinch
 803
 804
 805
 806
 807 var pulling = setlistener("sim/glider/winch/flags/pull", runWinch);
 808 var initializing_winch = setlistener("sim/signals/fdm-initialized", globalsWinch);